diff --git a/.appveyor.yml b/.appveyor.yml index 594ba5a33..e75d122e1 100644 --- a/.appveyor.yml +++ b/.appveyor.yml @@ -7,10 +7,10 @@ os: Visual Studio 2015 clone_folder: C:\projects\fcl shallow_clone: true -# branches: -# only: -# - master -platform: x64 +# build platform, i.e. Win32 (instead of x86), x64, Any CPU. This setting is optional. +platform: + - Win32 + - x64 environment: CTEST_OUTPUT_ON_FAILURE: 1 @@ -23,11 +23,15 @@ configuration: - Release before_build: + - cmd: if "%platform%"=="Win32" set CMAKE_GENERATOR_NAME=Visual Studio 14 2015 + - cmd: if "%platform%"=="x64" set CMAKE_GENERATOR_NAME=Visual Studio 14 2015 Win64 + - cmd: if "%platform%"=="Win32" set PROGRAM_FILES_PATH=Program Files (x86) + - cmd: if "%platform%"=="x64" set PROGRAM_FILES_PATH=Program Files - cmd: if not exist C:\"Program Files"\libccd\lib\ccd.lib ( curl -L -o libccd-2.0.tar.gz https://github.com/danfis/libccd/archive/v2.0.tar.gz && cmake -E tar zxf libccd-2.0.tar.gz && cd libccd-2.0 && - cmake -G "Visual Studio 14 2015 Win64" -DCMAKE_BUILD_TYPE=%Configuration% . && + cmake -G "%CMAKE_GENERATOR_NAME%" -DCMAKE_BUILD_TYPE=%Configuration% . && cmake --build . --target install --config %Configuration% && cd .. ) else (echo Using cached libccd) @@ -37,19 +41,18 @@ before_build: cd eigen-eigen-dc6cfdf9bcec && mkdir build && cd build && - cmake -G "Visual Studio 14 2015 Win64" -DCMAKE_BUILD_TYPE=%Configuration% .. && + cmake -G "%CMAKE_GENERATOR_NAME%" -DCMAKE_BUILD_TYPE=%Configuration% .. && cmake --build . --target install --config %Configuration% && cd ..\.. ) else (echo Using cached Eigen3) - cmd: set - cmd: mkdir build - cmd: cd build - - cmd: cmake -G "Visual Studio 14 2015 Win64" -DCMAKE_BUILD_TYPE=%Configuration% -DCCD_INCLUDE_DIRS="C:\Program Files\libccd\include" -DCCD_LIBRARY="C:\Program Files\libccd\lib\ccd.lib" -DEIGEN3_INCLUDE_DIR="C:\Program Files\Eigen\include\eigen3" .. + - cmd: cmake -G "%CMAKE_GENERATOR_NAME%" -DCMAKE_BUILD_TYPE=%Configuration% -DCCD_INCLUDE_DIRS="C:\%PROGRAM_FILES_PATH%\libccd\include" -DCCD_LIBRARY="C:\%PROGRAM_FILES_PATH%\libccd\lib\ccd.lib" -DEIGEN3_INCLUDE_DIR="C:\%PROGRAM_FILES_PATH%\Eigen\include\eigen3" .. build: project: C:\projects\fcl\build\fcl.sln parallel: true -# tests seem to hang -# test_script: -# - cmd: ctest -C %Configuration% +test_script: + - cmd: ctest -C %Configuration% diff --git a/.travis.yml b/.travis.yml index 7d100241f..956e57913 100644 --- a/.travis.yml +++ b/.travis.yml @@ -22,11 +22,6 @@ matrix: - os: osx compiler: gcc -addons: - apt: - packages: - - libboost-all-dev - install: # Install dependencies for FCL - if [ "$TRAVIS_OS_NAME" = "linux" ]; then 'ci/install_linux.sh' ; fi @@ -41,7 +36,7 @@ script: - cmake -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DFCL_COVERALLS=$COVERALLS .. # Build - - make -j4 + - make -j2 - if [ $COVERALLS = ON ] && [ "$TRAVIS_OS_NAME" = "linux" ]; then make coveralls; fi # Run unit tests diff --git a/CHANGELOG.md b/CHANGELOG.md index 6354a12dd..a389a59a5 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -2,8 +2,10 @@ ### FCL 0.6.0 (2016-XX-XX) -* Switched to Eigen for math operations: [#150](https://github.com/flexible-collision-library/fcl/pull/150), [#96](https://github.com/flexible-collision-library/fcl/issues/96) * Added missing copyright headers: [#149](https://github.com/flexible-collision-library/fcl/pull/149) +* Enabled build with SSE option by default: [#159](https://github.com/flexible-collision-library/fcl/pull/159) +* Switched to Eigen for math operations: [#150](https://github.com/flexible-collision-library/fcl/pull/150), [#96](https://github.com/flexible-collision-library/fcl/issues/96) +* Fixed redundant pair checking of SpatialHashingCollisionManager: [#156](https://github.com/flexible-collision-library/fcl/pull/156) * Removed dependency on boost: [#148](https://github.com/flexible-collision-library/fcl/pull/148), [#147](https://github.com/flexible-collision-library/fcl/pull/147), [#146](https://github.com/flexible-collision-library/fcl/pull/146), [#140](https://github.com/flexible-collision-library/fcl/pull/140) ### FCL 0.5.0 (2016-07-19) diff --git a/CMakeLists.txt b/CMakeLists.txt index 443e7a788..d07f9a44d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -9,24 +9,12 @@ endif() project(fcl CXX C) +option(FCL_ENABLE_PROFILING "Enable profiling" OFF) option(FCL_TREAT_WARNINGS_AS_ERRORS "Treat warnings as errors" OFF) # set the default build type -if (NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE Release) -endif() - -# Set build type variable -set(FCL_BUILD_TYPE_RELEASE FALSE) -set(FCL_BUILD_TYPE_DEBUG FALSE) - -string(TOUPPER ${CMAKE_BUILD_TYPE} CMAKE_BUILD_TYPE_UPPERCASE) -if("${CMAKE_BUILD_TYPE_UPPERCASE}" STREQUAL "RELEASE") - set(FCL_BUILD_TYPE_RELEASE TRUE) -elseif("${CMAKE_BUILD_TYPE_UPPERCASE}" STREQUAL "DEBUG") - set(FCL_BUILD_TYPE_DEBUG TRUE) -else() - message(STATUS "CMAKE_BUILD_TYPE ${CMAKE_BUILD_TYPE} unknown. Valid options are: Debug Release") +if (NOT MSVC AND NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE Release) endif() # This shouldn't be necessary, but there has been trouble @@ -48,14 +36,13 @@ else() endif() # Whether to enable SSE -option(FCL_USE_SSE "Whether FCL should SSE instructions" OFF) -set(FCL_HAVE_SSE 0) +option(FCL_USE_SSE "Whether FCL should SSE instructions" ON) if(FCL_USE_SSE) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID STREQUAL "Clang") - set(FCL_HAVE_SSE 0) #always disable, for now add_definitions(-march=native) + elseif(MSVC) + add_definitions(/arch:SSE2) endif() - # TODO: do something similar for other compilers endif() # Coveralls support diff --git a/CMakeModules/CompilerSettings.cmake b/CMakeModules/CompilerSettings.cmake index 93565ab99..f57e8d4d6 100644 --- a/CMakeModules/CompilerSettings.cmake +++ b/CMakeModules/CompilerSettings.cmake @@ -1,6 +1,6 @@ # GCC if(CMAKE_COMPILER_IS_GNUCXX) - add_definitions(-std=c++11 -W -Wall -g -Wextra -Wno-missing-field-initializers -Wno-unused-parameter) + add_definitions(-std=c++11 -W -Wall -g -Wextra -Wpedantic -Wno-missing-field-initializers -Wno-unused-parameter) if(FCL_TREAT_WARNINGS_AS_ERRORS) add_definitions(-Werror) endif(FCL_TREAT_WARNINGS_AS_ERRORS) @@ -16,7 +16,7 @@ endif() # Visual Studio if(MSVC OR MSVC90 OR MSVC10) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /EHsc /MP /W1") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /EHsc /MP /W1 /bigobj") if(FCL_TREAT_WARNINGS_AS_ERRORS) add_definitions(/WX) endif(FCL_TREAT_WARNINGS_AS_ERRORS) diff --git a/ci/install_osx.sh b/ci/install_osx.sh index 4f35dbdc7..8af429a86 100755 --- a/ci/install_osx.sh +++ b/ci/install_osx.sh @@ -6,3 +6,13 @@ brew install git brew install cmake brew install eigen brew install libccd + +# Octomap +git clone https://github.com/OctoMap/octomap +cd octomap +git checkout tags/v1.8.0 +mkdir build +cd build +cmake .. +make +sudo make install diff --git a/include/fcl/BV/AABB.h b/include/fcl/BV/AABB.h index a43472098..dd97f134f 100644 --- a/include/fcl/BV/AABB.h +++ b/include/fcl/BV/AABB.h @@ -35,214 +35,424 @@ /** \author Jia Pan */ -#ifndef FCL_AABB_H -#define FCL_AABB_H +#ifndef FCL_BV_AABB_H +#define FCL_BV_AABB_H #include "fcl/data_types.h" namespace fcl { -/// @brief A class describing the AABB collision structure, which is a box in 3D space determined by two diagonal points +/// @brief A class describing the AABB collision structure, which is a box in 3D +/// space determined by two diagonal points +template class AABB { public: + + using S = S_; + /// @brief The min point in the AABB - Vector3d min_; + Vector3 min_; + /// @brief The max point in the AABB - Vector3d max_; + Vector3 max_; /// @brief Creating an AABB with zero size (low bound +inf, upper bound -inf) AABB(); /// @brief Creating an AABB at position v with zero size - AABB(const Vector3d& v) : min_(v), max_(v) - { - } + AABB(const Vector3& v); /// @brief Creating an AABB with two endpoints a and b - AABB(const Vector3d& a, const Vector3d&b) : min_(a.cwiseMin(b)), - max_(a.cwiseMax(b)) - { - } + AABB(const Vector3& a, const Vector3&b); /// @brief Creating an AABB centered as core and is of half-dimension delta - AABB(const AABB& core, const Vector3d& delta) : min_(core.min_ - delta), - max_(core.max_ + delta) - { - } + AABB(const AABB& core, const Vector3& delta); /// @brief Creating an AABB contains three points - AABB(const Vector3d& a, const Vector3d& b, const Vector3d& c) : min_(a.cwiseMin(b).cwiseMin(c)), - max_(a.cwiseMax(b).cwiseMax(c)) - { - } + AABB(const Vector3& a, const Vector3& b, const Vector3& c); /// @brief Check whether two AABB are overlap - inline bool overlap(const AABB& other) const - { - if ((min_.array() > other.max_.array()).any()) - return false; - - if ((max_.array() < other.min_.array()).any()) - return false; - - return true; - } + bool overlap(const AABB& other) const; /// @brief Check whether the AABB contains another AABB - inline bool contain(const AABB& other) const - { - if ((min_.array() > other.min_.array()).any()) - return false; - - if ((max_.array() < other.max_.array()).any()) - return false; - - return true; - } - + bool contain(const AABB& other) const; /// @brief Check whether two AABB are overlapped along specific axis - inline bool axisOverlap(const AABB& other, int axis_id) const - { - if(min_[axis_id] > other.max_[axis_id]) return false; - - if(max_[axis_id] < other.min_[axis_id]) return false; - - return true; - } + bool axisOverlap(const AABB& other, int axis_id) const; /// @brief Check whether two AABB are overlap and return the overlap part - inline bool overlap(const AABB& other, AABB& overlap_part) const - { - if(!overlap(other)) - { - return false; - } - - overlap_part.min_ = min_.cwiseMax(other.min_); - overlap_part.max_ = max_.cwiseMin(other.max_); - return true; - } - + bool overlap(const AABB& other, AABB& overlap_part) const; /// @brief Check whether the AABB contains a point - inline bool contain(const Vector3d& p) const - { - if ((min_.array() > p.array()).any()) - return false; - - if ((max_.array() < p.array()).any()) - return false; - - return true; - } + bool contain(const Vector3& p) const; /// @brief Merge the AABB and a point - inline AABB& operator += (const Vector3d& p) - { - min_ = min_.cwiseMin(p); - max_ = max_.cwiseMax(p); - return *this; - } + AABB& operator += (const Vector3& p); /// @brief Merge the AABB and another AABB - inline AABB& operator += (const AABB& other) - { - min_ = min_.cwiseMin(other.min_); - max_ = max_.cwiseMax(other.max_); - return *this; - } + AABB& operator += (const AABB& other); /// @brief Return the merged AABB of current AABB and the other one - inline AABB operator + (const AABB& other) const - { - AABB res(*this); - return res += other; - } + AABB operator + (const AABB& other) const; /// @brief Width of the AABB - inline FCL_REAL width() const - { - return max_[0] - min_[0]; - } + S width() const; /// @brief Height of the AABB - inline FCL_REAL height() const - { - return max_[1] - min_[1]; - } + S height() const; /// @brief Depth of the AABB - inline FCL_REAL depth() const - { - return max_[2] - min_[2]; - } + S depth() const; /// @brief Volume of the AABB - inline FCL_REAL volume() const - { - return width() * height() * depth(); - } + S volume() const; /// @brief Size of the AABB (used in BV_Splitter to order two AABBs) - inline FCL_REAL size() const - { - return (max_ - min_).squaredNorm(); - } + S size() const; /// @brief Radius of the AABB - inline FCL_REAL radius() const - { - return (max_ - min_).norm() / 2; - } + S radius() const; /// @brief Center of the AABB - inline Vector3d center() const - { - return (min_ + max_) * 0.5; - } + Vector3 center() const; - /// @brief Distance between two AABBs; P and Q, should not be NULL, return the nearest points - FCL_REAL distance(const AABB& other, Vector3d* P, Vector3d* Q) const; + /// @brief Distance between two AABBs; P and Q, should not be nullptr, return the nearest points + S distance( + const AABB& other, Vector3* P, Vector3* Q) const; /// @brief Distance between two AABBs - FCL_REAL distance(const AABB& other) const; + S distance(const AABB& other) const; /// @brief whether two AABB are equal - inline bool equal(const AABB& other) const + bool equal(const AABB& other) const; + + /// @brief expand the half size of the AABB by delta, and keep the center unchanged. + AABB& expand(const Vector3& delta); + + /// @brief expand the aabb by increase the thickness of the plate by a ratio + AABB& expand(const AABB& core, S ratio); +}; + +using AABBf = AABB; +using AABBd = AABB; + +/// @brief translate the center of AABB by t +template +AABB translate( + const AABB& aabb, const Eigen::MatrixBase& t); + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +AABB::AABB() + : min_(Vector3::Constant(std::numeric_limits::max())), + max_(Vector3::Constant(-std::numeric_limits::max())) +{ + // Do nothing +} + +//============================================================================== +template +AABB::AABB(const Vector3& v) : min_(v), max_(v) +{ + // Do nothing +} + +//============================================================================== +template +AABB::AABB(const Vector3& a, const Vector3& b) + : min_(a.cwiseMin(b)), + max_(a.cwiseMax(b)) +{ + // Do nothing +} + +//============================================================================== +template +AABB::AABB(const AABB& core, const Vector3& delta) + : min_(core.min_ - delta), + max_(core.max_ + delta) +{ + // Do nothing +} + +//============================================================================== +template +AABB::AABB( + const Vector3& a, + const Vector3& b, + const Vector3& c) + : min_(a.cwiseMin(b).cwiseMin(c)), + max_(a.cwiseMax(b).cwiseMax(c)) +{ + // Do nothing +} + +//============================================================================== +template +bool AABB::overlap(const AABB& other) const +{ + if ((min_.array() > other.max_.array()).any()) + return false; + + if ((max_.array() < other.min_.array()).any()) + return false; + + return true; +} + +//============================================================================== +template +bool AABB::contain(const AABB& other) const +{ + if ((min_.array() > other.min_.array()).any()) + return false; + + if ((max_.array() < other.max_.array()).any()) + return false; + + return true; +} + +//============================================================================== +template +bool AABB::axisOverlap(const AABB& other, int axis_id) const +{ + if(min_[axis_id] > other.max_[axis_id]) return false; + + if(max_[axis_id] < other.min_[axis_id]) return false; + + return true; +} + +//============================================================================== +template +bool AABB::overlap(const AABB& other, AABB& overlap_part) const +{ + if(!overlap(other)) { - return min_.isApprox(other.min_, std::numeric_limits::epsilon() * 100) - && max_.isApprox(other.max_, std::numeric_limits::epsilon() * 100); + return false; } - /// @brief expand the half size of the AABB by delta, and keep the center unchanged. - inline AABB& expand(const Vector3d& delta) + overlap_part.min_ = min_.cwiseMax(other.min_); + overlap_part.max_ = max_.cwiseMin(other.max_); + return true; +} + +//============================================================================== +template +bool AABB::contain(const Vector3& p) const +{ + if ((min_.array() > p.array()).any()) + return false; + + if ((max_.array() < p.array()).any()) + return false; + + return true; +} + +//============================================================================== +template +AABB& AABB::operator +=(const Vector3& p) +{ + min_ = min_.cwiseMin(p); + max_ = max_.cwiseMax(p); + return *this; +} + +//============================================================================== +template +AABB& AABB::operator +=(const AABB& other) +{ + min_ = min_.cwiseMin(other.min_); + max_ = max_.cwiseMax(other.max_); + return *this; +} + +//============================================================================== +template +AABB AABB::operator +(const AABB& other) const +{ + AABB res(*this); + return res += other; +} + +//============================================================================== +template +S AABB::width() const +{ + return max_[0] - min_[0]; +} + +//============================================================================== +template +S AABB::height() const +{ + return max_[1] - min_[1]; +} + +//============================================================================== +template +S AABB::depth() const +{ + return max_[2] - min_[2]; +} + +//============================================================================== +template +S AABB::volume() const +{ + return width() * height() * depth(); +} + +//============================================================================== +template +S AABB::size() const +{ + return (max_ - min_).squaredNorm(); +} + +//============================================================================== +template +S AABB::radius() const +{ + return (max_ - min_).norm() / 2; +} + +//============================================================================== +template +Vector3 AABB::center() const +{ + return (min_ + max_) * 0.5; +} + +//============================================================================== +template +S AABB::distance(const AABB& other, Vector3* P, Vector3* Q) const +{ + S result = 0; + for(std::size_t i = 0; i < 3; ++i) { - min_ -= delta; - max_ += delta; - return *this; + const S& amin = min_[i]; + const S& amax = max_[i]; + const S& bmin = other.min_[i]; + const S& bmax = other.max_[i]; + + if(amin > bmax) + { + S delta = bmax - amin; + result += delta * delta; + if(P && Q) + { + (*P)[i] = amin; + (*Q)[i] = bmax; + } + } + else if(bmin > amax) + { + S delta = amax - bmin; + result += delta * delta; + if(P && Q) + { + (*P)[i] = amax; + (*Q)[i] = bmin; + } + } + else + { + if(P && Q) + { + if(bmin >= amin) + { + S t = 0.5 * (amax + bmin); + (*P)[i] = t; + (*Q)[i] = t; + } + else + { + S t = 0.5 * (amin + bmax); + (*P)[i] = t; + (*Q)[i] = t; + } + } + } } - /// @brief expand the aabb by increase the thickness of the plate by a ratio - inline AABB& expand(const AABB& core, FCL_REAL ratio) + return std::sqrt(result); +} + +//============================================================================== +template +S AABB::distance(const AABB& other) const +{ + S result = 0; + for(std::size_t i = 0; i < 3; ++i) { - min_ = min_ * ratio - core.min_; - max_ = max_ * ratio - core.max_; - return *this; - } -}; + const S& amin = min_[i]; + const S& amax = max_[i]; + const S& bmin = other.min_[i]; + const S& bmax = other.max_[i]; -/// @brief translate the center of AABB by t -static inline AABB translate(const AABB& aabb, const Vector3d& t) + if(amin > bmax) + { + S delta = bmax - amin; + result += delta * delta; + } + else if(bmin > amax) + { + S delta = amax - bmin; + result += delta * delta; + } + } + + return std::sqrt(result); +} + +//============================================================================== +template +bool AABB::equal(const AABB& other) const { - AABB res(aabb); + return min_.isApprox(other.min_, std::numeric_limits::epsilon() * 100) + && max_.isApprox(other.max_, std::numeric_limits::epsilon() * 100); +} + +//============================================================================== +template +AABB& AABB::expand(const Vector3& delta) +{ + min_ -= delta; + max_ += delta; + return *this; +} + +//============================================================================== +template +AABB& AABB::expand(const AABB& core, S ratio) +{ + min_ = min_ * ratio - core.min_; + max_ = max_ * ratio - core.max_; + return *this; +} + +//============================================================================== +template +AABB translate( + const AABB& aabb, const Eigen::MatrixBase& t) +{ + AABB res(aabb); res.min_ += t; res.max_ += t; return res; } -} +} // namespace fcl #endif diff --git a/include/fcl/BV/BV.h b/include/fcl/BV/BV.h index 119d5a046..2ad71d222 100644 --- a/include/fcl/BV/BV.h +++ b/include/fcl/BV/BV.h @@ -35,263 +35,10 @@ /** \author Jia Pan */ -#ifndef FCL_BV_H -#define FCL_BV_H +#ifndef FCL_BV_BV_H +#define FCL_BV_BV_H - -#include "fcl/BV/kDOP.h" -#include "fcl/BV/AABB.h" -#include "fcl/BV/OBB.h" -#include "fcl/BV/RSS.h" -#include "fcl/BV/OBBRSS.h" -#include "fcl/BV/kIOS.h" - -/** \brief Main namespace */ -namespace fcl -{ - -/// @cond IGNORE -namespace details -{ - -/// @brief Convert a bounding volume of type BV1 in configuration tf1 to a bounding volume of type BV2 in I configuration. -template -class Converter -{ -private: - static void convert(const BV1& bv1, const Transform3d& tf1, BV2& bv2) - { - // should only use the specialized version, so it is private. - } -}; - - -/// @brief Convert from AABB to AABB, not very tight but is fast. -template<> -class Converter -{ -public: - static void convert(const AABB& bv1, const Transform3d& tf1, AABB& bv2) - { - const Vector3d& center = bv1.center(); - FCL_REAL r = (bv1.max_ - bv1.min_).norm() * 0.5; - Vector3d center2 = tf1 * center; - Vector3d delta(r, r, r); - bv2.min_ = center2 - delta; - bv2.max_ = center2 + delta; - } -}; - -template<> -class Converter -{ -public: - static void convert(const AABB& bv1, const Transform3d& tf1, OBB& bv2) - { - /* - bv2.To = tf1 * bv1.center()); - - /// Sort the AABB edges so that AABB extents are ordered. - FCL_REAL d[3] = {bv1.width(), bv1.height(), bv1.depth() }; - std::size_t id[3] = {0, 1, 2}; - - for(std::size_t i = 1; i < 3; ++i) - { - for(std::size_t j = i; j > 0; --j) - { - if(d[j] > d[j-1]) - { - { - FCL_REAL tmp = d[j]; - d[j] = d[j-1]; - d[j-1] = tmp; - } - { - std::size_t tmp = id[j]; - id[j] = id[j-1]; - id[j-1] = tmp; - } - } - } - } - - Vector3d extent = (bv1.max_ - bv1.min_) * 0.5; - bv2.extent = Vector3d(extent[id[0]], extent[id[1]], extent[id[2]]); - const Matrix3d& R = tf1.linear(); - bool left_hand = (id[0] == (id[1] + 1) % 3); - bv2.axis[0] = left_hand ? -R.col(id[0]) : R.col(id[0]); - bv2.axis[1] = R.col(id[1]); - bv2.axis[2] = R.col(id[2]); - */ - - bv2.To = tf1 * bv1.center(); - bv2.extent = (bv1.max_ - bv1.min_) * 0.5; - bv2.axis = tf1.linear(); - } -}; - -template<> -class Converter -{ -public: - static void convert(const OBB& bv1, const Transform3d& tf1, OBB& bv2) - { - bv2.extent = bv1.extent; - bv2.To = tf1 * bv1.To; - bv2.axis = tf1.linear() * bv1.axis; - } -}; - -template<> -class Converter -{ -public: - static void convert(const OBBRSS& bv1, const Transform3d& tf1, OBB& bv2) - { - Converter::convert(bv1.obb, tf1, bv2); - } -}; - -template<> -class Converter -{ -public: - static void convert(const RSS& bv1, const Transform3d& tf1, OBB& bv2) - { - bv2.extent = Vector3d(bv1.l[0] * 0.5 + bv1.r, bv1.l[1] * 0.5 + bv1.r, bv1.r); - bv2.To = tf1 * bv1.Tr; - bv2.axis = tf1.linear() * bv1.axis; - } -}; - - -template -class Converter -{ -public: - static void convert(const BV1& bv1, const Transform3d& tf1, AABB& bv2) - { - const Vector3d& center = bv1.center(); - FCL_REAL r = Vector3d(bv1.width(), bv1.height(), bv1.depth()).norm() * 0.5; - Vector3d delta(r, r, r); - Vector3d center2 = tf1 * center; - bv2.min_ = center2 - delta; - bv2.max_ = center2 + delta; - } -}; - -template -class Converter -{ -public: - static void convert(const BV1& bv1, const Transform3d& tf1, OBB& bv2) - { - AABB bv; - Converter::convert(bv1, Transform3d::Identity(), bv); - Converter::convert(bv, tf1, bv2); - } -}; - -template<> -class Converter -{ -public: - static void convert(const OBB& bv1, const Transform3d& tf1, RSS& bv2) - { - bv2.Tr = tf1 * bv1.To; - bv2.axis = tf1.linear() * bv1.axis; - - bv2.r = bv1.extent[2]; - bv2.l[0] = 2 * (bv1.extent[0] - bv2.r); - bv2.l[1] = 2 * (bv1.extent[1] - bv2.r); - } -}; - -template<> -class Converter -{ -public: - static void convert(const RSS& bv1, const Transform3d& tf1, RSS& bv2) - { - bv2.Tr = tf1 * bv1.Tr; - bv2.axis = tf1.linear() * bv1.axis; - - bv2.r = bv1.r; - bv2.l[0] = bv1.l[0]; - bv2.l[1] = bv1.l[1]; - } -}; - -template<> -class Converter -{ -public: - static void convert(const OBBRSS& bv1, const Transform3d& tf1, RSS& bv2) - { - Converter::convert(bv1.rss, tf1, bv2); - } -}; - -template<> -class Converter -{ -public: - static void convert(const AABB& bv1, const Transform3d& tf1, RSS& bv2) - { - bv2.Tr = tf1 * bv1.center(); - - /// Sort the AABB edges so that AABB extents are ordered. - FCL_REAL d[3] = {bv1.width(), bv1.height(), bv1.depth() }; - std::size_t id[3] = {0, 1, 2}; - - for(std::size_t i = 1; i < 3; ++i) - { - for(std::size_t j = i; j > 0; --j) - { - if(d[j] > d[j-1]) - { - { - FCL_REAL tmp = d[j]; - d[j] = d[j-1]; - d[j-1] = tmp; - } - { - std::size_t tmp = id[j]; - id[j] = id[j-1]; - id[j-1] = tmp; - } - } - } - } - - Vector3d extent = (bv1.max_ - bv1.min_) * 0.5; - bv2.r = extent[id[2]]; - bv2.l[0] = (extent[id[0]] - bv2.r) * 2; - bv2.l[1] = (extent[id[1]] - bv2.r) * 2; - - const Matrix3d& R = tf1.linear(); - bool left_hand = (id[0] == (id[1] + 1) % 3); - if (left_hand) - bv2.axis.col(0) = -R.col(id[0]); - else - bv2.axis.col(0) = R.col(id[0]); - bv2.axis.col(1) = R.col(id[1]); - bv2.axis.col(2) = R.col(id[2]); - } -}; - -} - -/// @endcond - - -/// @brief Convert a bounding volume of type BV1 in configuration tf1 to bounding volume of type BV2 in identity configuration. -template -static inline void convertBV(const BV1& bv1, const Transform3d& tf1, BV2& bv2) -{ - details::Converter::convert(bv1, tf1, bv2); -} - -} +#warning "This header has been deprecated in FCL 0.6. "\ + "Please include fcl/BV/convert_bv.h instead." #endif diff --git a/include/fcl/BV/BV_node.h b/include/fcl/BV/BV_node.h index 140d756c4..fa6f82e0a 100644 --- a/include/fcl/BV/BV_node.h +++ b/include/fcl/BV/BV_node.h @@ -36,10 +36,10 @@ /** \author Jia Pan */ -#ifndef FCL_BV_NODE_H -#define FCL_BV_NODE_H +#ifndef FCL_BV_BVNODE_H +#define FCL_BV_BVNODE_H -#include "fcl/BV/BV.h" +#include "fcl/BV/convert_bv.h" #include namespace fcl @@ -62,63 +62,143 @@ struct BVNodeBase int num_primitives; /// @brief Whether current node is a leaf node (i.e. contains a primitive index - inline bool isLeaf() const { return first_child < 0; } + bool isLeaf() const; /// @brief Return the primitive index. The index is referred to the original data (i.e. vertices or tri_indices) in BVHModel - inline int primitiveId() const { return -(first_child + 1); } + int primitiveId() const; /// @brief Return the index of the first child. The index is referred to the bounding volume array (i.e. bvs) in BVHModel - inline int leftChild() const { return first_child; } + int leftChild() const; /// @brief Return the index of the second child. The index is referred to the bounding volume array (i.e. bvs) in BVHModel - inline int rightChild() const { return first_child + 1; } + int rightChild() const; }; /// @brief A class describing a bounding volume node. It includes the tree structure providing in BVNodeBase and also the geometry data provided in BV template parameter. -template +template struct BVNode : public BVNodeBase { + using S = typename BV::S; + /// @brief bounding volume storing the geometry BV bv; /// @brief Check whether two BVNode collide - bool overlap(const BVNode& other) const - { - return bv.overlap(other.bv); - } + bool overlap(const BVNode& other) const; - /// @brief Compute the distance between two BVNode. P1 and P2, if not NULL and the underlying BV supports distance, return the nearest points. - FCL_REAL distance(const BVNode& other, Vector3d* P1 = NULL, Vector3d* P2 = NULL) const - { - return bv.distance(other.bv, P1, P2); - } + /// @brief Compute the distance between two BVNode. P1 and P2, if not nullptr and + /// the underlying BV supports distance, return the nearest points. + S distance( + const BVNode& other, + Vector3* P1 = nullptr, + Vector3* P2 = nullptr) const; /// @brief Access the center of the BV - Vector3d getCenter() const { return bv.center(); } + Vector3 getCenter() const; /// @brief Access the orientation of the BV - Matrix3d getOrientation() const { return Matrix3d::Identity(); } + Matrix3 getOrientation() const; }; -template<> -inline Matrix3d BVNode::getOrientation() const +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +inline bool BVNodeBase::isLeaf() const +{ + return first_child < 0; +} + +//============================================================================== +inline int BVNodeBase::primitiveId() const +{ + return -(first_child + 1); +} + +//============================================================================== +inline int BVNodeBase::leftChild() const { - return bv.axis; + return first_child; } -template<> -inline Matrix3d BVNode::getOrientation() const +//============================================================================== +inline int BVNodeBase::rightChild() const { - return bv.axis; + return first_child + 1; } -template<> -inline Matrix3d BVNode::getOrientation() const +//============================================================================== +template +bool BVNode::overlap(const BVNode& other) const { - return bv.obb.axis; + return bv.overlap(other.bv); } +//============================================================================== +template +typename BVNode::S BVNode::distance( + const BVNode& other, Vector3* P1, Vector3* P2) const +{ + return bv.distance(other.bv, P1, P2); +} +//============================================================================== +template +Vector3::S> BVNode::getCenter() const +{ + return bv.center(); } +//============================================================================== +template +struct GetOrientationImpl +{ + static Matrix3 run(const BVNode& /*node*/) + { + return Matrix3::Identity(); + } +}; + +//============================================================================== +template +Matrix3 BVNode::getOrientation() const +{ + return GetOrientationImpl::run(bv); +} + +//============================================================================== +template +struct GetOrientationImpl> +{ + static Matrix3 run(const OBB& bv) + { + return bv.axis; + } +}; + +//============================================================================== +template +struct GetOrientationImpl> +{ + static Matrix3 run(const RSS& bv) + { + return bv.axis; + } +}; + +//============================================================================== +template +struct GetOrientationImpl> +{ + static Matrix3 run(const OBBRSS& bv) + { + return bv.obb.axis; + } +}; + +} // namespace fcl + #endif diff --git a/include/fcl/BV/OBB.h b/include/fcl/BV/OBB.h index 657b5ef5a..25f0cdb39 100644 --- a/include/fcl/BV/OBB.h +++ b/include/fcl/BV/OBB.h @@ -35,106 +35,731 @@ /** \author Jia Pan */ -#ifndef FCL_OBB_H -#define FCL_OBB_H +#ifndef FCL_BV_OBB_H +#define FCL_BV_OBB_H + +#include #include "fcl/data_types.h" +#include "fcl/math/geometry.h" namespace fcl { /// @brief Oriented bounding box class +template class OBB { public: - /// @brief Orientation of OBB. axis[i] is the ith column of the orientation matrix for the box; it is also the i-th principle direction of the box. - /// We assume that axis[0] corresponds to the axis with the longest box edge, axis[1] corresponds to the shorter one and axis[2] corresponds to the shortest one. - Matrix3d axis; + + using S = S_; + + /// @brief Orientation of OBB. The axes of the rotation matrix are the + /// principle directions of the box. We assume that the first column + /// corresponds to the axis with the longest box edge, second column + /// corresponds to the shorter one and the third coulumn corresponds to the + /// shortest one. + Matrix3 axis; /// @brief Center of OBB - Vector3d To; - + Vector3 To; + /// @brief Half dimensions of OBB - Vector3d extent; + Vector3 extent; - /// @brief Check collision between two OBB, return true if collision happens. - bool overlap(const OBB& other) const; + /// @brief Constructor + OBB(); + + /// @brief Constructor + OBB(const Matrix3& axis, + const Vector3& center, + const Vector3& extent); + /// @brief Check collision between two OBB, return true if collision happens. + bool overlap(const OBB& other) const; /// @brief Check collision between two OBB and return the overlap part. For OBB, the overlap_part return value is NOT used as the overlap part of two obbs usually is not an obb. - bool overlap(const OBB& other, OBB& overlap_part) const - { - return overlap(other); - } + bool overlap(const OBB& other, OBB& overlap_part) const; /// @brief Check whether the OBB contains a point. - bool contain(const Vector3d& p) const; + bool contain(const Vector3& p) const; /// @brief A simple way to merge the OBB and a point (the result is not compact). - OBB& operator += (const Vector3d& p); + OBB& operator +=(const Vector3& p); /// @brief Merge the OBB and another OBB (the result is not compact). - OBB& operator += (const OBB& other) - { - *this = *this + other; - return *this; - } + OBB& operator += (const OBB& other); /// @brief Return the merged OBB of current OBB and the other one (the result is not compact). - OBB operator + (const OBB& other) const; + OBB operator + (const OBB& other) const; /// @brief Width of the OBB. - inline FCL_REAL width() const + S width() const; + + /// @brief Height of the OBB. + S height() const; + + /// @brief Depth of the OBB + S depth() const; + + /// @brief Volume of the OBB + S volume() const; + + /// @brief Size of the OBB (used in BV_Splitter to order two OBBs) + S size() const; + + /// @brief Center of the OBB + const Vector3 center() const; + + /// @brief Distance between two OBBs, not implemented. + S distance(const OBB& other, Vector3* P = nullptr, + Vector3* Q = nullptr) const; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + +}; + +using OBBf = OBB; +using OBBd = OBB; + +/// @brief Compute the 8 vertices of a OBBd +template +void computeVertices(const OBB& b, Vector3 vertices[8]); + +/// @brief OBBd merge method when the centers of two smaller OBBd are far away +template +OBB merge_largedist(const OBB& b1, const OBB& b2); + +/// @brief OBBd merge method when the centers of two smaller OBBd are close +template +OBB merge_smalldist(const OBB& b1, const OBB& b2); + +/// @brief Translate the OBB bv +template +OBB translate( + const OBB& bv, const Eigen::MatrixBase& t); + +/// @brief Check collision between two obbs, b1 is in configuration (R0, T0) and +/// b2 is in identity. +template +bool overlap(const Eigen::MatrixBase& R0, + const Eigen::MatrixBase& T0, + const OBB& b1, const OBB& b2); + +/// @brief Check collision between two obbs, b1 is in configuration (R0, T0) and +/// b2 is in identity. +template +bool overlap( + const Transform3& tf, + const OBB& b1, + const OBB& b2); + +/// @brief Check collision between two boxes: the first box is in configuration +/// (R, T) and its half dimension is set by a; the second box is in identity +/// configuration and its half dimension is set by b. +template +bool obbDisjoint( + const Matrix3& B, + const Vector3& T, + const Vector3& a, + const Vector3& b); + +/// @brief Check collision between two boxes: the first box is in configuration +/// (R, T) and its half dimension is set by a; the second box is in identity +/// configuration and its half dimension is set by b. +template +bool obbDisjoint( + const Transform3& tf, + const Vector3& a, + const Vector3& b); + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +OBB::OBB() +{ + // Do nothing +} + +//============================================================================== +template +OBB::OBB(const Matrix3& axis_, + const Vector3& center_, + const Vector3& extent_) + : axis(axis_), To(center_), extent(extent_) +{ + // Do nothing +} + +//============================================================================== +template +bool OBB::overlap(const OBB& other) const +{ + /// compute the relative transform that takes us from this->frame to + /// other.frame + + Vector3 t = other.To - To; + Vector3 T( + axis.col(0).dot(t), axis.col(1).dot(t), axis.col(2).dot(t)); + Matrix3 R = axis.transpose() * other.axis; + + return !obbDisjoint(R, T, extent, other.extent); +} + +//============================================================================== +template +bool OBB::overlap(const OBB& other, OBB& overlap_part) const +{ + return overlap(other); +} + +//============================================================================== +template +bool OBB::contain(const Vector3& p) const +{ + Vector3 local_p = p - To; + S proj = local_p.dot(axis.col(0)); + if((proj > extent[0]) || (proj < -extent[0])) + return false; + + proj = local_p.dot(axis.col(1)); + if((proj > extent[1]) || (proj < -extent[1])) + return false; + + proj = local_p.dot(axis.col(2)); + if((proj > extent[2]) || (proj < -extent[2])) + return false; + + return true; +} + +//============================================================================== +template +OBB& OBB::operator +=(const Vector3& p) +{ + OBB bvp(axis, p, Vector3::Zero()); + *this += bvp; + + return *this; +} + +//============================================================================== +template +OBB& OBB::operator +=(const OBB& other) +{ + *this = *this + other; + + return *this; +} + +//============================================================================== +template +OBB OBB::operator +(const OBB& other) const +{ + Vector3 center_diff = To - other.To; + S max_extent = std::max(std::max(extent[0], extent[1]), extent[2]); + S max_extent2 = std::max(std::max(other.extent[0], other.extent[1]), other.extent[2]); + if(center_diff.norm() > 2 * (max_extent + max_extent2)) { - return 2 * extent[0]; + return merge_largedist(*this, other); } + else + { + return merge_smalldist(*this, other); + } +} - /// @brief Height of the OBB. - inline FCL_REAL height() const +//============================================================================== +template +S OBB::width() const +{ + return 2 * extent[0]; +} + +//============================================================================== +template +S OBB::height() const +{ + return 2 * extent[1]; +} + +//============================================================================== +template +S OBB::depth() const +{ + return 2 * extent[2]; +} + +//============================================================================== +template +S OBB::volume() const +{ + return width() * height() * depth(); +} + +//============================================================================== +template +S OBB::size() const +{ + return extent.squaredNorm(); +} + +//============================================================================== +template +const Vector3 OBB::center() const +{ + return To; +} + +//============================================================================== +template +S OBB::distance(const OBB& other, Vector3* P, + Vector3* Q) const +{ + std::cerr << "OBB distance not implemented!" << std::endl; + return 0.0; +} + +//============================================================================== +template +void computeVertices(const OBB& b, Vector3 vertices[8]) +{ + const Vector3& extent = b.extent; + const Vector3& To = b.To; + + Vector3 extAxis0 = b.axis.col(0) * extent[0]; + Vector3 extAxis1 = b.axis.col(1) * extent[1]; + Vector3 extAxis2 = b.axis.col(2) * extent[2]; + + vertices[0] = To - extAxis0 - extAxis1 - extAxis2; + vertices[1] = To + extAxis0 - extAxis1 - extAxis2; + vertices[2] = To + extAxis0 + extAxis1 - extAxis2; + vertices[3] = To - extAxis0 + extAxis1 - extAxis2; + vertices[4] = To - extAxis0 - extAxis1 + extAxis2; + vertices[5] = To + extAxis0 - extAxis1 + extAxis2; + vertices[6] = To + extAxis0 + extAxis1 + extAxis2; + vertices[7] = To - extAxis0 + extAxis1 + extAxis2; +} + +//============================================================================== +template +OBB merge_largedist(const OBB& b1, const OBB& b2) +{ + Vector3 vertex[16]; + computeVertices(b1, vertex); + computeVertices(b2, vertex + 8); + Matrix3 M; + Matrix3 E; + Vector3 s(0, 0, 0); + + OBB b; + b.axis.col(0) = b1.To - b2.To; + b.axis.col(0).normalize(); + + Vector3 vertex_proj[16]; + for(int i = 0; i < 16; ++i) { - return 2 * extent[1]; + vertex_proj[i] = vertex[i]; + vertex_proj[i].noalias() -= b.axis.col(0) * vertex[i].dot(b.axis.col(0)); } - /// @brief Depth of the OBB - inline FCL_REAL depth() const + getCovariance(vertex_proj, nullptr, nullptr, nullptr, 16, M); + eigen_old(M, s, E); + + int min, mid, max; + if (s[0] > s[1]) { - return 2 * extent[2]; + max = 0; + min = 1; + } + else + { + min = 0; + max = 1; } - /// @brief Volume of the OBB - inline FCL_REAL volume() const + if (s[2] < s[min]) + { + mid = min; + min = 2; + } + else if (s[2] > s[max]) { - return width() * height() * depth(); + mid = max; + max = 2; + } + else + { + mid = 2; } - /// @brief Size of the OBB (used in BV_Splitter to order two OBBs) - inline FCL_REAL size() const + b.axis.col(1) << E.col(0)[max], E.col(1)[max], E.col(2)[max]; + b.axis.col(2) << E.col(0)[mid], E.col(1)[mid], E.col(2)[mid]; + + // set obb centers and extensions + getExtentAndCenter( + vertex, nullptr, nullptr, nullptr, 16, b.axis, b.To, b.extent); + + return b; +} + +//============================================================================== +template +OBB merge_smalldist(const OBB& b1, const OBB& b2) +{ + OBB b; + b.To = (b1.To + b2.To) * 0.5; + Quaternion q0(b1.axis); + Quaternion q1(b2.axis); + if(q0.dot(q1) < 0) + q1.coeffs() = -q1.coeffs(); + + Quaternion q(q0.coeffs() + q1.coeffs()); + q.normalize(); + b.axis = q.toRotationMatrix(); + + + Vector3 vertex[8], diff; + S real_max = std::numeric_limits::max(); + Vector3 pmin(real_max, real_max, real_max); + Vector3 pmax(-real_max, -real_max, -real_max); + + computeVertices(b1, vertex); + for(int i = 0; i < 8; ++i) { - return extent.squaredNorm(); + diff = vertex[i] - b.To; + for(int j = 0; j < 3; ++j) + { + S dot = diff.dot(b.axis.col(j)); + if(dot > pmax[j]) + pmax[j] = dot; + else if(dot < pmin[j]) + pmin[j] = dot; + } } - /// @brief Center of the OBB - inline const Vector3d& center() const + computeVertices(b2, vertex); + for(int i = 0; i < 8; ++i) { - return To; + diff = vertex[i] - b.To; + for(int j = 0; j < 3; ++j) + { + S dot = diff.dot(b.axis.col(j)); + if(dot > pmax[j]) + pmax[j] = dot; + else if(dot < pmin[j]) + pmin[j] = dot; + } } + for(int j = 0; j < 3; ++j) + { + b.To += (b.axis.col(j) * (0.5 * (pmax[j] + pmin[j]))); + b.extent[j] = 0.5 * (pmax[j] - pmin[j]); + } - /// @brief Distance between two OBBs, not implemented. - FCL_REAL distance(const OBB& other, Vector3d* P = NULL, Vector3d* Q = NULL) const; -}; + return b; +} +//============================================================================== +template +OBB translate( + const OBB& bv, const Eigen::MatrixBase& t) +{ + OBB res(bv); + res.To += t; + return res; +} -/// @brief Translate the OBB bv -OBB translate(const OBB& bv, const Vector3d& t); +//============================================================================== +template +bool overlap(const Eigen::MatrixBase& R0, + const Eigen::MatrixBase& T0, + const OBB& b1, const OBB& b2) +{ + typename DerivedA::PlainObject R0b2 = R0 * b2.axis; + typename DerivedA::PlainObject R = b1.axis.transpose() * R0b2; -/// @brief Check collision between two obbs, b1 is in configuration (R0, T0) and b2 is in identity. -bool overlap(const Matrix3d& R0, const Vector3d& T0, const OBB& b1, const OBB& b2); + typename DerivedB::PlainObject Ttemp = R0 * b2.To + T0 - b1.To; + typename DerivedB::PlainObject T = Ttemp.transpose() * b1.axis; + return !obbDisjoint(R, T, b1.extent, b2.extent); +} -/// @brief Check collision between two boxes: the first box is in configuration (R, T) and its half dimension is set by a; -/// the second box is in identity configuration and its half dimension is set by b. -bool obbDisjoint(const Matrix3d& B, const Vector3d& T, const Vector3d& a, const Vector3d& b); +//============================================================================== +template +bool overlap( + const Transform3& tf, + const OBB& b1, + const OBB& b2) +{ + return !obbDisjoint( + b1.frame.inverse(Eigen::Isometry) * tf * b2.frame, b1.extent, b2.extent); } +//============================================================================== +template +bool obbDisjoint(const Matrix3& B, const Vector3& T, + const Vector3& a, const Vector3& b) +{ + register S t, s; + const S reps = 1e-6; + + Matrix3 Bf = B.cwiseAbs(); + Bf.array() += reps; + + // if any of these tests are one-sided, then the polyhedra are disjoint + + // A1 x A2 = A0 + t = ((T[0] < 0.0) ? -T[0] : T[0]); + + if(t > (a[0] + Bf.row(0).dot(b))) + return true; + + // B1 x B2 = B0 + s = B.col(0).dot(T); + t = ((s < 0.0) ? -s : s); + + if(t > (b[0] + Bf.col(0).dot(a))) + return true; + + // A2 x A0 = A1 + t = ((T[1] < 0.0) ? -T[1] : T[1]); + + if(t > (a[1] + Bf.row(1).dot(b))) + return true; + + // A0 x A1 = A2 + t =((T[2] < 0.0) ? -T[2] : T[2]); + + if(t > (a[2] + Bf.row(2).dot(b))) + return true; + + // B2 x B0 = B1 + s = B.col(1).dot(T); + t = ((s < 0.0) ? -s : s); + + if(t > (b[1] + Bf.col(1).dot(a))) + return true; + + // B0 x B1 = B2 + s = B.col(2).dot(T); + t = ((s < 0.0) ? -s : s); + + if(t > (b[2] + Bf.col(2).dot(a))) + return true; + + // A0 x B0 + s = T[2] * B(1, 0) - T[1] * B(2, 0); + t = ((s < 0.0) ? -s : s); + + if(t > (a[1] * Bf(2, 0) + a[2] * Bf(1, 0) + + b[1] * Bf(0, 2) + b[2] * Bf(0, 1))) + return true; + + // A0 x B1 + s = T[2] * B(1, 1) - T[1] * B(2, 1); + t = ((s < 0.0) ? -s : s); + + if(t > (a[1] * Bf(2, 1) + a[2] * Bf(1, 1) + + b[0] * Bf(0, 2) + b[2] * Bf(0, 0))) + return true; + + // A0 x B2 + s = T[2] * B(1, 2) - T[1] * B(2, 2); + t = ((s < 0.0) ? -s : s); + + if(t > (a[1] * Bf(2, 2) + a[2] * Bf(1, 2) + + b[0] * Bf(0, 1) + b[1] * Bf(0, 0))) + return true; + + // A1 x B0 + s = T[0] * B(2, 0) - T[2] * B(0, 0); + t = ((s < 0.0) ? -s : s); + + if(t > (a[0] * Bf(2, 0) + a[2] * Bf(0, 0) + + b[1] * Bf(1, 2) + b[2] * Bf(1, 1))) + return true; + + // A1 x B1 + s = T[0] * B(2, 1) - T[2] * B(0, 1); + t = ((s < 0.0) ? -s : s); + + if(t > (a[0] * Bf(2, 1) + a[2] * Bf(0, 1) + + b[0] * Bf(1, 2) + b[2] * Bf(1, 0))) + return true; + + // A1 x B2 + s = T[0] * B(2, 2) - T[2] * B(0, 2); + t = ((s < 0.0) ? -s : s); + + if(t > (a[0] * Bf(2, 2) + a[2] * Bf(0, 2) + + b[0] * Bf(1, 1) + b[1] * Bf(1, 0))) + return true; + + // A2 x B0 + s = T[1] * B(0, 0) - T[0] * B(1, 0); + t = ((s < 0.0) ? -s : s); + + if(t > (a[0] * Bf(1, 0) + a[1] * Bf(0, 0) + + b[1] * Bf(2, 2) + b[2] * Bf(2, 1))) + return true; + + // A2 x B1 + s = T[1] * B(0, 1) - T[0] * B(1, 1); + t = ((s < 0.0) ? -s : s); + + if(t > (a[0] * Bf(1, 1) + a[1] * Bf(0, 1) + + b[0] * Bf(2, 2) + b[2] * Bf(2, 0))) + return true; + + // A2 x B2 + s = T[1] * B(0, 2) - T[0] * B(1, 2); + t = ((s < 0.0) ? -s : s); + + if(t > (a[0] * Bf(1, 2) + a[1] * Bf(0, 2) + + b[0] * Bf(2, 1) + b[1] * Bf(2, 0))) + return true; + + return false; + +} + +//============================================================================== +template +bool obbDisjoint( + const Transform3& tf, + const Vector3& a, + const Vector3& b) +{ + register S t, s; + const S reps = 1e-6; + + Matrix3 Bf = tf.linear().cwiseAbs(); + Bf.array() += reps; + + // if any of these tests are one-sided, then the polyhedra are disjoint + + // A1 x A2 = A0 + t = ((tf.translation()[0] < 0.0) ? -tf.translation()[0] : tf.translation()[0]); + + if(t > (a[0] + Bf.row(0).dot(b))) + return true; + + // B1 x B2 = B0 + s = tf.linear().col(0).dot(tf.translation()); + t = ((s < 0.0) ? -s : s); + + if(t > (b[0] + Bf.col(0).dot(a))) + return true; + + // A2 x A0 = A1 + t = ((tf.translation()[1] < 0.0) ? -tf.translation()[1] : tf.translation()[1]); + + if(t > (a[1] + Bf.row(1).dot(b))) + return true; + + // A0 x A1 = A2 + t =((tf.translation()[2] < 0.0) ? -tf.translation()[2] : tf.translation()[2]); + + if(t > (a[2] + Bf.row(2).dot(b))) + return true; + + // B2 x B0 = B1 + s = tf.linear().col(1).dot(tf.translation()); + t = ((s < 0.0) ? -s : s); + + if(t > (b[1] + Bf.col(1).dot(a))) + return true; + + // B0 x B1 = B2 + s = tf.linear().col(2).dot(tf.translation()); + t = ((s < 0.0) ? -s : s); + + if(t > (b[2] + Bf.col(2).dot(a))) + return true; + + // A0 x B0 + s = tf.translation()[2] * tf.linear()(1, 0) - tf.translation()[1] * tf.linear()(2, 0); + t = ((s < 0.0) ? -s : s); + + if(t > (a[1] * Bf(2, 0) + a[2] * Bf(1, 0) + + b[1] * Bf(0, 2) + b[2] * Bf(0, 1))) + return true; + + // A0 x B1 + s = tf.translation()[2] * tf.linear()(1, 1) - tf.translation()[1] * tf.linear()(2, 1); + t = ((s < 0.0) ? -s : s); + + if(t > (a[1] * Bf(2, 1) + a[2] * Bf(1, 1) + + b[0] * Bf(0, 2) + b[2] * Bf(0, 0))) + return true; + + // A0 x B2 + s = tf.translation()[2] * tf.linear()(1, 2) - tf.translation()[1] * tf.linear()(2, 2); + t = ((s < 0.0) ? -s : s); + + if(t > (a[1] * Bf(2, 2) + a[2] * Bf(1, 2) + + b[0] * Bf(0, 1) + b[1] * Bf(0, 0))) + return true; + + // A1 x B0 + s = tf.translation()[0] * tf.linear()(2, 0) - tf.translation()[2] * tf.linear()(0, 0); + t = ((s < 0.0) ? -s : s); + + if(t > (a[0] * Bf(2, 0) + a[2] * Bf(0, 0) + + b[1] * Bf(1, 2) + b[2] * Bf(1, 1))) + return true; + + // A1 x B1 + s = tf.translation()[0] * tf.linear()(2, 1) - tf.translation()[2] * tf.linear()(0, 1); + t = ((s < 0.0) ? -s : s); + + if(t > (a[0] * Bf(2, 1) + a[2] * Bf(0, 1) + + b[0] * Bf(1, 2) + b[2] * Bf(1, 0))) + return true; + + // A1 x B2 + s = tf.translation()[0] * tf.linear()(2, 2) - tf.translation()[2] * tf.linear()(0, 2); + t = ((s < 0.0) ? -s : s); + + if(t > (a[0] * Bf(2, 2) + a[2] * Bf(0, 2) + + b[0] * Bf(1, 1) + b[1] * Bf(1, 0))) + return true; + + // A2 x B0 + s = tf.translation()[1] * tf.linear()(0, 0) - tf.translation()[0] * tf.linear()(1, 0); + t = ((s < 0.0) ? -s : s); + + if(t > (a[0] * Bf(1, 0) + a[1] * Bf(0, 0) + + b[1] * Bf(2, 2) + b[2] * Bf(2, 1))) + return true; + + // A2 x B1 + s = tf.translation()[1] * tf.linear()(0, 1) - tf.translation()[0] * tf.linear()(1, 1); + t = ((s < 0.0) ? -s : s); + + if(t > (a[0] * Bf(1, 1) + a[1] * Bf(0, 1) + + b[0] * Bf(2, 2) + b[2] * Bf(2, 0))) + return true; + + // A2 x B2 + s = tf.translation()[1] * tf.linear()(0, 2) - tf.translation()[0] * tf.linear()(1, 2); + t = ((s < 0.0) ? -s : s); + + if(t > (a[0] * Bf(1, 2) + a[1] * Bf(0, 2) + + b[0] * Bf(2, 1) + b[1] * Bf(2, 0))) + return true; + + return false; + +} + +} // namespace fcl + #endif diff --git a/include/fcl/BV/OBBRSS.h b/include/fcl/BV/OBBRSS.h index 1e5ff14d8..02f111d5b 100644 --- a/include/fcl/BV/OBBRSS.h +++ b/include/fcl/BV/OBBRSS.h @@ -35,9 +35,8 @@ /** \author Jia Pan */ -#ifndef FCL_OBBRSS_H -#define FCL_OBBRSS_H - +#ifndef FCL_BV_OBBRSS_H +#define FCL_BV_OBBRSS_H #include "fcl/BV/OBB.h" #include "fcl/BV/RSS.h" @@ -45,113 +44,263 @@ namespace fcl { - -/// @brief Class merging the OBB and RSS, can handle collision and distance simultaneously +/// @brief Class merging the OBBd and RSS, can handle collision and distance +/// simultaneously +template class OBBRSS { public: - /// @brief OBB member, for rotation - OBB obb; + using S = S_; - /// @brief RSS member, for distance - RSS rss; + /// @brief OBBd member, for rotation + OBB obb; + + /// @brief RSSd member, for distance + RSS rss; /// @brief Check collision between two OBBRSS - bool overlap(const OBBRSS& other) const - { - return obb.overlap(other.obb); - } + bool overlap(const OBBRSS& other) const; /// @brief Check collision between two OBBRSS and return the overlap part. - bool overlap(const OBBRSS& other, OBBRSS& overlap_part) const - { - return overlap(other); - } + bool overlap(const OBBRSS& other, OBBRSS& overlap_part) const; /// @brief Check whether the OBBRSS contains a point - inline bool contain(const Vector3d& p) const - { - return obb.contain(p); - } + bool contain(const Vector3& p) const; /// @brief Merge the OBBRSS and a point - OBBRSS& operator += (const Vector3d& p) - { - obb += p; - rss += p; - return *this; - } + OBBRSS& operator += (const Vector3& p); /// @brief Merge two OBBRSS - OBBRSS& operator += (const OBBRSS& other) - { - *this = *this + other; - return *this; - } + OBBRSS& operator += (const OBBRSS& other); /// @brief Merge two OBBRSS - OBBRSS operator + (const OBBRSS& other) const - { - OBBRSS result; - result.obb = obb + other.obb; - result.rss = rss + other.rss; - return result; - } + OBBRSS operator + (const OBBRSS& other) const; /// @brief Width of the OBRSS - inline FCL_REAL width() const - { - return obb.width(); - } + S width() const; /// @brief Height of the OBBRSS - inline FCL_REAL height() const - { - return obb.height(); - } + S height() const; /// @brief Depth of the OBBRSS - inline FCL_REAL depth() const - { - return obb.depth(); - } + S depth() const; /// @brief Volume of the OBBRSS - inline FCL_REAL volume() const - { - return obb.volume(); - } + S volume() const; /// @brief Size of the OBBRSS (used in BV_Splitter to order two OBBRSS) - inline FCL_REAL size() const - { - return obb.size(); - } + S size() const; /// @brief Center of the OBBRSS - inline const Vector3d& center() const - { - return obb.center(); - } - - /// @brief Distance between two OBBRSS; P and Q , is not NULL, returns the nearest points - FCL_REAL distance(const OBBRSS& other, Vector3d* P = NULL, Vector3d* Q = NULL) const - { - return rss.distance(other.rss, P, Q); - } + const Vector3 center() const; + + /// @brief Distance between two OBBRSS; P and Q , is not nullptr, returns the nearest points + S distance(const OBBRSS& other, + Vector3* P = nullptr, Vector3* Q = nullptr) const; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; +using OBBRSSf = OBBRSS; +using OBBRSSd = OBBRSS; + /// @brief Translate the OBBRSS bv -OBBRSS translate(const OBBRSS& bv, const Vector3d& t); +template +OBBRSS translate(const OBBRSS& bv, const Vector3& t); + +/// @brief Check collision between two OBBRSS, b1 is in configuration (R0, T0) +/// and b2 is in indentity +template +bool overlap(const Eigen::MatrixBase& R0, + const Eigen::MatrixBase& T0, + const OBBRSS& b1, const OBBRSS& b2); + +/// @brief Check collision between two OBBRSS, b1 is in configuration (R0, T0) +/// and b2 is in indentity +template +bool overlap( + const Transform3& tf, + const OBBRSS& b1, + const OBBRSS& b2); + +/// @brief Computate distance between two OBBRSS, b1 is in configuation (R0, T0) +/// and b2 is in indentity; P and Q, is not nullptr, returns the nearest points +template +S distance( + const Eigen::MatrixBase& R0, + const Eigen::MatrixBase& T0, + const OBBRSS& b1, const OBBRSS& b2, + Vector3* P = nullptr, Vector3* Q = nullptr); + +/// @brief Computate distance between two OBBRSS, b1 is in configuation (R0, T0) +/// and b2 is in indentity; P and Q, is not nullptr, returns the nearest points +template +S distance( + const Transform3& tf, + const OBBRSS& b1, + const OBBRSS& b2, + Vector3* P = nullptr, + Vector3* Q = nullptr); + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +bool OBBRSS::overlap(const OBBRSS& other) const +{ + return obb.overlap(other.obb); +} + +//============================================================================== +template +bool OBBRSS::overlap(const OBBRSS& other, + OBBRSS& /*overlap_part*/) const +{ + return overlap(other); +} + +//============================================================================== +template +bool OBBRSS::contain(const Vector3& p) const +{ + return obb.contain(p); +} + +//============================================================================== +template +OBBRSS& OBBRSS::operator +=(const Vector3& p) +{ + obb += p; + rss += p; + return *this; +} + +//============================================================================== +template +OBBRSS& OBBRSS::operator +=(const OBBRSS& other) +{ + *this = *this + other; + return *this; +} + +//============================================================================== +template +OBBRSS OBBRSS::operator +(const OBBRSS& other) const +{ + OBBRSS result; + result.obb = obb + other.obb; + result.rss = rss + other.rss; + return result; +} + +//============================================================================== +template +S OBBRSS::width() const +{ + return obb.width(); +} + +//============================================================================== +template +S OBBRSS::height() const +{ + return obb.height(); +} + +//============================================================================== +template +S OBBRSS::depth() const +{ + return obb.depth(); +} -/// @brief Check collision between two OBBRSS, b1 is in configuration (R0, T0) and b2 is in indentity -bool overlap(const Matrix3d& R0, const Vector3d& T0, const OBBRSS& b1, const OBBRSS& b2); +//============================================================================== +template +S OBBRSS::volume() const +{ + return obb.volume(); +} + +//============================================================================== +template +S OBBRSS::size() const +{ + return obb.size(); +} -/// @brief Computate distance between two OBBRSS, b1 is in configuation (R0, T0) and b2 is in indentity; P and Q, is not NULL, returns the nearest points -FCL_REAL distance(const Matrix3d& R0, const Vector3d& T0, const OBBRSS& b1, const OBBRSS& b2, Vector3d* P = NULL, Vector3d* Q = NULL); +//============================================================================== +template +const Vector3 OBBRSS::center() const +{ + return obb.center(); +} +//============================================================================== +template +S OBBRSS::distance(const OBBRSS& other, + Vector3* P, Vector3* Q) const +{ + return rss.distance(other.rss, P, Q); +} + +//============================================================================== +template +bool overlap(const Eigen::MatrixBase& R0, + const Eigen::MatrixBase& T0, + const OBBRSS& b1, const OBBRSS& b2) +{ + return overlap(R0, T0, b1.obb, b2.obb); +} + +//============================================================================== +template +bool overlap( + const Transform3& tf, + const OBBRSS& b1, + const OBBRSS& b2) +{ + return overlap(tf, b1.obb, b2.obb); +} + +//============================================================================== +template +S distance( + const Eigen::MatrixBase& R0, + const Eigen::MatrixBase& T0, + const OBBRSS& b1, const OBBRSS& b2, + Vector3* P, Vector3* Q) +{ + return distance(R0, T0, b1.rss, b2.rss, P, Q); +} + +//============================================================================== +template +S distance( + const Transform3& tf, + const OBBRSS& b1, + const OBBRSS& b2, + Vector3* P, + Vector3* Q) +{ + return distance(tf, b1.rss, b2.rss, P, Q); +} + +//============================================================================== +template +OBBRSS translate(const OBBRSS& bv, const Vector3& t) +{ + OBBRSS res(bv); + res.obb.To += t; + res.rss.To += t; + return res; } +} // namespace fcl #endif diff --git a/include/fcl/BV/RSS.h b/include/fcl/BV/RSS.h index 3db8018eb..e04b964b0 100644 --- a/include/fcl/BV/RSS.h +++ b/include/fcl/BV/RSS.h @@ -35,117 +35,2103 @@ /** \author Jia Pan */ -#ifndef FCL_RSS_H -#define FCL_RSS_H +#ifndef FCL_BV_RSS_H +#define FCL_BV_RSS_H #include "fcl/math/constants.h" +#include "fcl/math/geometry.h" +#include "fcl/BV/utility.h" namespace fcl { /// @brief A class for rectangle sphere-swept bounding volume +template class RSS { public: - /// @brief Orientation of RSS. axis[i] is the ith column of the orientation matrix for the RSS; it is also the i-th principle direction of the RSS. - /// We assume that axis[0] corresponds to the axis with the longest length, axis[1] corresponds to the shorter one and axis[2] corresponds to the shortest one. - Matrix3d axis; + + using S = S_; + + /// @brief Orientation of RSS. The axes of the rotation matrix are the + /// principle directions of the box. We assume that the first column + /// corresponds to the axis with the longest box edge, second column + /// corresponds to the shorter one and the third coulumn corresponds to the + /// shortest one. + Matrix3 axis; /// @brief Origin of the rectangle in RSS - Vector3d Tr; + Vector3 To; /// @brief Side lengths of rectangle - FCL_REAL l[2]; + S l[2]; /// @brief Radius of sphere summed with rectangle to form RSS - FCL_REAL r; + S r; /// Constructor RSS(); /// @brief Check collision between two RSS - bool overlap(const RSS& other) const; + bool overlap(const RSS& other) const; /// @brief Check collision between two RSS and return the overlap part. /// For RSS, we return nothing, as the overlap part of two RSSs usually is not a RSS. - bool overlap(const RSS& other, RSS& overlap_part) const - { - return overlap(other); - } + bool overlap(const RSS& other, RSS& overlap_part) const; /// @brief Check whether the RSS contains a point - inline bool contain(const Vector3d& p) const; + bool contain(const Vector3& p) const; /// @brief A simple way to merge the RSS and a point, not compact. /// @todo This function may have some bug. - RSS& operator += (const Vector3d& p); + RSS& operator += (const Vector3& p); /// @brief Merge the RSS and another RSS - inline RSS& operator += (const RSS& other) - { - *this = *this + other; - return *this; - } + RSS& operator += (const RSS& other); /// @brief Return the merged RSS of current RSS and the other one - RSS operator + (const RSS& other) const; + RSS operator + (const RSS& other) const; /// @brief Width of the RSS - inline FCL_REAL width() const + S width() const; + + /// @brief Height of the RSS + S height() const; + + /// @brief Depth of the RSS + S depth() const; + + /// @brief Volume of the RSS + S volume() const; + + /// @brief Size of the RSS (used in BV_Splitter to order two RSSs) + S size() const; + + /// @brief The RSS center + const Vector3 center() const; + + /// @brief the distance between two RSS; P and Q, if not nullptr, return the nearest points + S distance(const RSS& other, + Vector3* P = nullptr, + Vector3* Q = nullptr) const; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + +}; + +using RSSf = RSS; +using RSSd = RSS; + +/// @brief Clip value between a and b +template +void clipToRange(S& val, S a, S b); + +/// @brief Finds the parameters t & u corresponding to the two closest points on +/// a pair of line segments. The first segment is defined as +/// Pa + A*t, 0 <= t <= a, where "Pa" is one endpoint of the segment, "A" is a +/// unit vector pointing to the other endpoint, and t is a scalar that produces +/// all the points between the two endpoints. Since "A" is a unit vector, "a" is +/// the segment's length. The second segment is defined as +/// Pb + B*u, 0 <= u <= b. Many of the terms needed by the algorithm are already +/// computed for other purposes,so we just pass these terms into the function +/// instead of complete specifications of each segment. "T" in the dot products +/// is the vector betweeen Pa and Pb. +/// Reference: "On fast computation of distance between line segments." Vladimir +/// J. Lumelsky, in Information Processing Letters, no. 21, pages 55-61, 1985. +template +void segCoords(S& t, S& u, S a, S b, + S A_dot_B, S A_dot_T, S B_dot_T); + +/// @brief Returns whether the nearest point on rectangle edge +/// Pb + B*u, 0 <= u <= b, to the rectangle edge, +/// Pa + A*t, 0 <= t <= a, is within the half space +/// determined by the point Pa and the direction Anorm. +/// A,B, and Anorm are unit vectors. T is the vector between Pa and Pb. +template +bool inVoronoi(S a, S b, + S Anorm_dot_B, S Anorm_dot_T, + S A_dot_B, S A_dot_T, S B_dot_T); + +/// @brief Distance between two oriented rectangles; P and Q (optional return +/// values) are the closest points in the rectangles, both are in the local +/// frame of the first rectangle. +template +S rectDistance( + const Matrix3& Rab, + const Vector3& Tab, + const S a[2], + const S b[2], + Vector3* P = nullptr, + Vector3* Q = nullptr); + +/// @brief Distance between two oriented rectangles; P and Q (optional return +/// values) are the closest points in the rectangles, both are in the local +/// frame of the first rectangle. +template +S rectDistance( + const Transform3& tfab, + const S a[2], + const S b[2], + Vector3* P = nullptr, + Vector3* Q = nullptr); + +/// @brief distance between two RSS bounding volumes +/// P and Q (optional return values) are the closest points in the rectangles, +/// not the RSS. But the direction P - Q is the correct direction for cloest +/// points. Notice that P and Q are both in the local frame of the first RSS +/// (not global frame and not even the local frame of object 1) +template +S distance( + const Eigen::MatrixBase& R0, + const Eigen::MatrixBase& T0, + const RSS& b1, + const RSS& b2, + Vector3* P = nullptr, + Vector3* Q = nullptr); + +/// @brief distance between two RSS bounding volumes +/// P and Q (optional return values) are the closest points in the rectangles, +/// not the RSS. But the direction P - Q is the correct direction for cloest +/// points. Notice that P and Q are both in the local frame of the first RSS +/// (not global frame and not even the local frame of object 1) +template +S distance( + const Transform3& tf, + const RSS& b1, + const RSS& b2, + Vector3* P = nullptr, + Vector3* Q = nullptr); + +/// @brief Check collision between two RSSs, b1 is in configuration (R0, T0) and +/// b2 is in identity. +template +bool overlap( + const Eigen::MatrixBase& R0, + const Eigen::MatrixBase& T0, + const RSS& b1, + const RSS& b2); + +/// @brief Check collision between two RSSs, b1 is in configuration (R0, T0) and +/// b2 is in identity. +template +bool overlap( + const Transform3& tf, + const RSS& b1, + const RSS& b2); + +/// @brief Translate the RSS bv +template +RSS translate(const RSS& bv, const Vector3& t); + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +RSS::RSS() + : axis(Matrix3::Identity()), To(Vector3::Zero()) +{ + // Do nothing +} + +//============================================================================== +template +bool RSS::overlap(const RSS& other) const +{ + Vector3 t = other.To - To; + Vector3 T( + axis.col(0).dot(t), axis.col(1).dot(t), axis.col(2).dot(t)); + Matrix3 R = axis.transpose() * other.axis; + + S dist = rectDistance(R, T, l, other.l); + return (dist <= (r + other.r)); +} + +//============================================================================== +template +bool RSS::overlap(const RSS& other, + RSS& /*overlap_part*/) const +{ + return overlap(other); +} + +//============================================================================== +template +bool RSS::contain(const Vector3& p) const +{ + Vector3 local_p = p - To; + Vector3 proj( + axis.col(0).dot(local_p), + axis.col(1).dot(local_p), + axis.col(2).dot(local_p)); + S abs_proj2 = fabs(proj[2]); + + /// projection is within the rectangle + if((proj[0] < l[0]) && (proj[0] > 0) && (proj[1] < l[1]) && (proj[1] > 0)) { - return l[0] + 2 * r; + return (abs_proj2 < r); } + else if((proj[0] < l[0]) && (proj[0] > 0) && ((proj[1] < 0) || (proj[1] > l[1]))) + { + S y = (proj[1] > 0) ? l[1] : 0; + Vector3 v(proj[0], y, 0); + return ((proj - v).squaredNorm() < r * r); + } + else if((proj[1] < l[1]) && (proj[1] > 0) && ((proj[0] < 0) || (proj[0] > l[0]))) + { + S x = (proj[0] > 0) ? l[0] : 0; + Vector3 v(x, proj[1], 0); + return ((proj - v).squaredNorm() < r * r); + } + else + { + S x = (proj[0] > 0) ? l[0] : 0; + S y = (proj[1] > 0) ? l[1] : 0; + Vector3 v(x, y, 0); + return ((proj - v).squaredNorm() < r * r); + } +} - /// @brief Height of the RSS - inline FCL_REAL height() const +//============================================================================== +template +RSS& RSS::operator +=(const Vector3& p) + +{ + Vector3 local_p = p - To; + Vector3 proj( + axis.col(0).dot(local_p), + axis.col(1).dot(local_p), + axis.col(2).dot(local_p)); + S abs_proj2 = fabs(proj[2]); + + // projection is within the rectangle + if((proj[0] < l[0]) && (proj[0] > 0) && (proj[1] < l[1]) && (proj[1] > 0)) + { + if(abs_proj2 < r) + ; // do nothing + else + { + r = 0.5 * (r + abs_proj2); // enlarge the r + // change RSS origin position + if(proj[2] > 0) + To[2] += 0.5 * (abs_proj2 - r); + else + To[2] -= 0.5 * (abs_proj2 - r); + } + } + else if((proj[0] < l[0]) && (proj[0] > 0) && ((proj[1] < 0) || (proj[1] > l[1]))) { - return l[1] + 2 * r; + S y = (proj[1] > 0) ? l[1] : 0; + Vector3 v(proj[0], y, 0); + S new_r_sqr = (proj - v).squaredNorm(); + if(new_r_sqr < r * r) + ; // do nothing + else + { + if(abs_proj2 < r) + { + S delta_y = - std::sqrt(r * r - proj[2] * proj[2]) + fabs(proj[1] - y); + l[1] += delta_y; + if(proj[1] < 0) + To[1] -= delta_y; + } + else + { + S delta_y = fabs(proj[1] - y); + l[1] += delta_y; + if(proj[1] < 0) + To[1] -= delta_y; + + if(proj[2] > 0) + To[2] += 0.5 * (abs_proj2 - r); + else + To[2] -= 0.5 * (abs_proj2 - r); + } + } } + else if((proj[1] < l[1]) && (proj[1] > 0) && ((proj[0] < 0) || (proj[0] > l[0]))) + { + S x = (proj[0] > 0) ? l[0] : 0; + Vector3 v(x, proj[1], 0); + S new_r_sqr = (proj - v).squaredNorm(); + if(new_r_sqr < r * r) + ; // do nothing + else + { + if(abs_proj2 < r) + { + S delta_x = - std::sqrt(r * r - proj[2] * proj[2]) + fabs(proj[0] - x); + l[0] += delta_x; + if(proj[0] < 0) + To[0] -= delta_x; + } + else + { + S delta_x = fabs(proj[0] - x); + l[0] += delta_x; + if(proj[0] < 0) + To[0] -= delta_x; - /// @brief Depth of the RSS - inline FCL_REAL depth() const + if(proj[2] > 0) + To[2] += 0.5 * (abs_proj2 - r); + else + To[2] -= 0.5 * (abs_proj2 - r); + } + } + } + else { - return 2 * r; + S x = (proj[0] > 0) ? l[0] : 0; + S y = (proj[1] > 0) ? l[1] : 0; + Vector3 v(x, y, 0); + S new_r_sqr = (proj - v).squaredNorm(); + if(new_r_sqr < r * r) + ; // do nothing + else + { + if(abs_proj2 < r) + { + S diag = std::sqrt(new_r_sqr - proj[2] * proj[2]); + S delta_diag = - std::sqrt(r * r - proj[2] * proj[2]) + diag; + + S delta_x = delta_diag / diag * fabs(proj[0] - x); + S delta_y = delta_diag / diag * fabs(proj[1] - y); + l[0] += delta_x; + l[1] += delta_y; + + if(proj[0] < 0 && proj[1] < 0) + { + To[0] -= delta_x; + To[1] -= delta_y; + } + } + else + { + S delta_x = fabs(proj[0] - x); + S delta_y = fabs(proj[1] - y); + + l[0] += delta_x; + l[1] += delta_y; + + if(proj[0] < 0 && proj[1] < 0) + { + To[0] -= delta_x; + To[1] -= delta_y; + } + + if(proj[2] > 0) + To[2] += 0.5 * (abs_proj2 - r); + else + To[2] -= 0.5 * (abs_proj2 - r); + } + } } - /// @brief Volume of the RSS - inline FCL_REAL volume() const + return *this; +} + +//============================================================================== +template +RSS& RSS::operator +=(const RSS& other) +{ + *this = *this + other; + return *this; +} + +//============================================================================== +template +RSS RSS::operator +(const RSS& other) const +{ + RSS bv; + + Vector3 v[16]; + + Vector3 d0_pos = other.axis.col(0) * (other.l[0] + other.r); + Vector3 d1_pos = other.axis.col(1) * (other.l[1] + other.r); + + Vector3 d0_neg = other.axis.col(0) * (-other.r); + Vector3 d1_neg = other.axis.col(1) * (-other.r); + + Vector3 d2_pos = other.axis.col(2) * other.r; + Vector3 d2_neg = other.axis.col(2) * (-other.r); + + v[0] = other.To + d0_pos + d1_pos + d2_pos; + v[1] = other.To + d0_pos + d1_pos + d2_neg; + v[2] = other.To + d0_pos + d1_neg + d2_pos; + v[3] = other.To + d0_pos + d1_neg + d2_neg; + v[4] = other.To + d0_neg + d1_pos + d2_pos; + v[5] = other.To + d0_neg + d1_pos + d2_neg; + v[6] = other.To + d0_neg + d1_neg + d2_pos; + v[7] = other.To + d0_neg + d1_neg + d2_neg; + + d0_pos.noalias() = axis.col(0) * (l[0] + r); + d1_pos.noalias() = axis.col(1) * (l[1] + r); + d0_neg.noalias() = axis.col(0) * (-r); + d1_neg.noalias() = axis.col(1) * (-r); + d2_pos.noalias() = axis.col(2) * r; + d2_neg.noalias() = axis.col(2) * (-r); + + v[8] = To + d0_pos + d1_pos + d2_pos; + v[9] = To + d0_pos + d1_pos + d2_neg; + v[10] = To + d0_pos + d1_neg + d2_pos; + v[11] = To + d0_pos + d1_neg + d2_neg; + v[12] = To + d0_neg + d1_pos + d2_pos; + v[13] = To + d0_neg + d1_pos + d2_neg; + v[14] = To + d0_neg + d1_neg + d2_pos; + v[15] = To + d0_neg + d1_neg + d2_neg; + + + Matrix3 M; // row first matrix + Matrix3 E; // row first eigen-vectors + Vector3 s(0, 0, 0); + + getCovariance(v, nullptr, nullptr, nullptr, 16, M); + eigen_old(M, s, E); + + int min, mid, max; + if(s[0] > s[1]) { max = 0; min = 1; } + else { min = 0; max = 1; } + if(s[2] < s[min]) { mid = min; min = 2; } + else if(s[2] > s[max]) { mid = max; max = 2; } + else { mid = 2; } + + // column first matrix, as the axis in RSS + bv.axis.col(0) = E.col(max); + bv.axis.col(1) = E.col(mid); + bv.axis.col(2).noalias() = axis.col(0).cross(axis.col(1)); + + // set rss origin, rectangle size and radius + getRadiusAndOriginAndRectangleSize(v, nullptr, nullptr, nullptr, 16, bv.axis, bv.To, bv.l, bv.r); + + return bv; +} + +//============================================================================== +template +S RSS::width() const +{ + return l[0] + 2 * r; +} + +//============================================================================== +template +S RSS::height() const +{ + return l[1] + 2 * r; +} + +//============================================================================== +template +S RSS::depth() const +{ + return 2 * r; +} + +//============================================================================== +template +S RSS::volume() const +{ + return (l[0] * l[1] * 2 * r + 4 * constants::pi() * r * r * r); +} + +//============================================================================== +template +S RSS::size() const +{ + return (std::sqrt(l[0] * l[0] + l[1] * l[1]) + 2 * r); +} + +//============================================================================== +template +const Vector3 RSS::center() const +{ + return To; +} + +//============================================================================== +template +S RSS::distance( + const RSS& other, + Vector3* P, + Vector3* Q) const +{ + Vector3 t = other.To - To; + Vector3 T( + axis.col(0).dot(t), axis.col(1).dot(t), axis.col(2).dot(t)); + Matrix3 R = axis.transpose() * other.axis; + + S dist = rectDistance(R, T, l, other.l, P, Q); + dist -= (r + other.r); + return (dist < (S)0.0) ? (S)0.0 : dist; +} + +//============================================================================== +template +void clipToRange(S& val, S a, S b) +{ + if(val < a) val = a; + else if(val > b) val = b; +} + +//============================================================================== +template +void segCoords(S& t, S& u, S a, S b, S A_dot_B, S A_dot_T, S B_dot_T) +{ + S denom = 1 - A_dot_B * A_dot_B; + + if(denom == 0) t = 0; + else { - return (l[0] * l[1] * 2 * r + 4 * constants::pi * r * r * r); + t = (A_dot_T - B_dot_T * A_dot_B) / denom; + clipToRange(t, (S)0.0, a); } - /// @brief Size of the RSS (used in BV_Splitter to order two RSSs) - inline FCL_REAL size() const + u = t * A_dot_B - B_dot_T; + if(u < 0) + { + u = 0; + t = A_dot_T; + clipToRange(t, (S)0.0, a); + } + else if(u > b) { - return (std::sqrt(l[0] * l[0] + l[1] * l[1]) + 2 * r); + u = b; + t = u * A_dot_B + A_dot_T; + clipToRange(t, (S)0.0, a); } +} - /// @brief The RSS center - inline const Vector3d& center() const +//============================================================================== +template +bool inVoronoi(S a, S b, S Anorm_dot_B, S Anorm_dot_T, S A_dot_B, S A_dot_T, S B_dot_T) +{ + if(fabs(Anorm_dot_B) < 1e-7) return false; + + S t, u, v; + + u = -Anorm_dot_T / Anorm_dot_B; + clipToRange(u, (S)0.0, b); + + t = u * A_dot_B + A_dot_T; + clipToRange(t, (S)0.0, a); + + v = t * A_dot_B - B_dot_T; + + if(Anorm_dot_B > 0) + { + if(v > (u + 1e-7)) return true; + } + else { - return Tr; + if(v < (u - 1e-7)) return true; } + return false; +} - /// @brief the distance between two RSS; P and Q, if not NULL, return the nearest points - FCL_REAL distance(const RSS& other, Vector3d* P = NULL, Vector3d* Q = NULL) const; +//============================================================================== +template +S rectDistance(const Matrix3& Rab, Vector3 const& Tab, const S a[2], const S b[2], Vector3* P, Vector3* Q) +{ + S A0_dot_B0, A0_dot_B1, A1_dot_B0, A1_dot_B1; -}; + A0_dot_B0 = Rab(0, 0); + A0_dot_B1 = Rab(0, 1); + A1_dot_B0 = Rab(1, 0); + A1_dot_B1 = Rab(1, 1); + S aA0_dot_B0, aA0_dot_B1, aA1_dot_B0, aA1_dot_B1; + S bA0_dot_B0, bA0_dot_B1, bA1_dot_B0, bA1_dot_B1; -/// @brief Translate the RSS bv -RSS translate(const RSS& bv, const Vector3d& t); + aA0_dot_B0 = a[0] * A0_dot_B0; + aA0_dot_B1 = a[0] * A0_dot_B1; + aA1_dot_B0 = a[1] * A1_dot_B0; + aA1_dot_B1 = a[1] * A1_dot_B1; + bA0_dot_B0 = b[0] * A0_dot_B0; + bA1_dot_B0 = b[0] * A1_dot_B0; + bA0_dot_B1 = b[1] * A0_dot_B1; + bA1_dot_B1 = b[1] * A1_dot_B1; -/// @brief distance between two RSS bounding volumes -/// P and Q (optional return values) are the closest points in the rectangles, not the RSS. But the direction P - Q is the correct direction for cloest points -/// Notice that P and Q are both in the local frame of the first RSS (not global frame and not even the local frame of object 1) -FCL_REAL distance(const Matrix3d& R0, const Vector3d& T0, const RSS& b1, const RSS& b2, Vector3d* P = NULL, Vector3d* Q = NULL); + Vector3 Tba = Rab.transpose() * Tab; + + Vector3 D; + S t, u; + + // determine if any edge pair contains the closest points + + S ALL_x, ALU_x, AUL_x, AUU_x; + S BLL_x, BLU_x, BUL_x, BUU_x; + S LA1_lx, LA1_ux, UA1_lx, UA1_ux, LB1_lx, LB1_ux, UB1_lx, UB1_ux; + + ALL_x = -Tba[0]; + ALU_x = ALL_x + aA1_dot_B0; + AUL_x = ALL_x + aA0_dot_B0; + AUU_x = ALU_x + aA0_dot_B0; + + if(ALL_x < ALU_x) + { + LA1_lx = ALL_x; + LA1_ux = ALU_x; + UA1_lx = AUL_x; + UA1_ux = AUU_x; + } + else + { + LA1_lx = ALU_x; + LA1_ux = ALL_x; + UA1_lx = AUU_x; + UA1_ux = AUL_x; + } + + BLL_x = Tab[0]; + BLU_x = BLL_x + bA0_dot_B1; + BUL_x = BLL_x + bA0_dot_B0; + BUU_x = BLU_x + bA0_dot_B0; + + if(BLL_x < BLU_x) + { + LB1_lx = BLL_x; + LB1_ux = BLU_x; + UB1_lx = BUL_x; + UB1_ux = BUU_x; + } + else + { + LB1_lx = BLU_x; + LB1_ux = BLL_x; + UB1_lx = BUU_x; + UB1_ux = BUL_x; + } + + // UA1, UB1 + + if((UA1_ux > b[0]) && (UB1_ux > a[0])) + { + if(((UA1_lx > b[0]) || + inVoronoi(b[1], a[1], A1_dot_B0, aA0_dot_B0 - b[0] - Tba[0], + A1_dot_B1, aA0_dot_B1 - Tba[1], + -Tab[1] - bA1_dot_B0)) + && + ((UB1_lx > a[0]) || + inVoronoi(a[1], b[1], A0_dot_B1, Tab[0] + bA0_dot_B0 - a[0], + A1_dot_B1, Tab[1] + bA1_dot_B0, Tba[1] - aA0_dot_B1))) + { + segCoords(t, u, a[1], b[1], A1_dot_B1, Tab[1] + bA1_dot_B0, + Tba[1] - aA0_dot_B1); + + D[0] = Tab[0] + Rab(0, 0) * b[0] + Rab(0, 1) * u - a[0] ; + D[1] = Tab[1] + Rab(1, 0) * b[0] + Rab(1, 1) * u - t; + D[2] = Tab[2] + Rab(2, 0) * b[0] + Rab(2, 1) * u; + + if(P && Q) + { + *P << a[0], t, 0; + *Q = D + (*P); + } + + return D.norm(); + } + } + + + // UA1, LB1 + + if((UA1_lx < 0) && (LB1_ux > a[0])) + { + if(((UA1_ux < 0) || + inVoronoi(b[1], a[1], -A1_dot_B0, Tba[0] - aA0_dot_B0, + A1_dot_B1, aA0_dot_B1 - Tba[1], -Tab[1])) + && + ((LB1_lx > a[0]) || + inVoronoi(a[1], b[1], A0_dot_B1, Tab[0] - a[0], + A1_dot_B1, Tab[1], Tba[1] - aA0_dot_B1))) + { + segCoords(t, u, a[1], b[1], A1_dot_B1, Tab[1], Tba[1] - aA0_dot_B1); + + D[0] = Tab[0] + Rab(0, 1) * u - a[0]; + D[1] = Tab[1] + Rab(1, 1) * u - t; + D[2] = Tab[2] + Rab(2, 1) * u; + + if(P && Q) + { + *P << a[0], t, 0; + *Q = D + (*P); + } + + return D.norm(); + } + } + + // LA1, UB1 + + if((LA1_ux > b[0]) && (UB1_lx < 0)) + { + if(((LA1_lx > b[0]) || + inVoronoi(b[1], a[1], A1_dot_B0, -Tba[0] - b[0], + A1_dot_B1, -Tba[1], -Tab[1] - bA1_dot_B0)) + && + ((UB1_ux < 0) || + inVoronoi(a[1], b[1], -A0_dot_B1, -Tab[0] - bA0_dot_B0, + A1_dot_B1, Tab[1] + bA1_dot_B0, Tba[1]))) + { + segCoords(t, u, a[1], b[1], A1_dot_B1, Tab[1] + bA1_dot_B0, Tba[1]); + + D[0] = Tab[0] + Rab(0, 0) * b[0] + Rab(0, 1) * u; + D[1] = Tab[1] + Rab(1, 0) * b[0] + Rab(1, 1) * u - t; + D[2] = Tab[2] + Rab(2, 0) * b[0] + Rab(2, 1) * u; + + if(P && Q) + { + *P << 0, t, 0; + *Q = D + (*P); + } + + return D.norm(); + } + } + + // LA1, LB1 + + if((LA1_lx < 0) && (LB1_lx < 0)) + { + if (((LA1_ux < 0) || + inVoronoi(b[1], a[1], -A1_dot_B0, Tba[0], A1_dot_B1, + -Tba[1], -Tab[1])) + && + ((LB1_ux < 0) || + inVoronoi(a[1], b[1], -A0_dot_B1, -Tab[0], A1_dot_B1, + Tab[1], Tba[1]))) + { + segCoords(t, u, a[1], b[1], A1_dot_B1, Tab[1], Tba[1]); + + D[0] = Tab[0] + Rab(0, 1) * u; + D[1] = Tab[1] + Rab(1, 1) * u - t; + D[2] = Tab[2] + Rab(2, 1) * u; + + if(P && Q) + { + *P << 0, t, 0; + *Q = D + (*P); + } + + return D.norm(); + } + } + + S ALL_y, ALU_y, AUL_y, AUU_y; + + ALL_y = -Tba[1]; + ALU_y = ALL_y + aA1_dot_B1; + AUL_y = ALL_y + aA0_dot_B1; + AUU_y = ALU_y + aA0_dot_B1; + + S LA1_ly, LA1_uy, UA1_ly, UA1_uy, LB0_lx, LB0_ux, UB0_lx, UB0_ux; + + if(ALL_y < ALU_y) + { + LA1_ly = ALL_y; + LA1_uy = ALU_y; + UA1_ly = AUL_y; + UA1_uy = AUU_y; + } + else + { + LA1_ly = ALU_y; + LA1_uy = ALL_y; + UA1_ly = AUU_y; + UA1_uy = AUL_y; + } + + if(BLL_x < BUL_x) + { + LB0_lx = BLL_x; + LB0_ux = BUL_x; + UB0_lx = BLU_x; + UB0_ux = BUU_x; + } + else + { + LB0_lx = BUL_x; + LB0_ux = BLL_x; + UB0_lx = BUU_x; + UB0_ux = BLU_x; + } + + // UA1, UB0 + + if((UA1_uy > b[1]) && (UB0_ux > a[0])) + { + if(((UA1_ly > b[1]) || + inVoronoi(b[0], a[1], A1_dot_B1, aA0_dot_B1 - Tba[1] - b[1], + A1_dot_B0, aA0_dot_B0 - Tba[0], -Tab[1] - bA1_dot_B1)) + && + ((UB0_lx > a[0]) || + inVoronoi(a[1], b[0], A0_dot_B0, Tab[0] - a[0] + bA0_dot_B1, + A1_dot_B0, Tab[1] + bA1_dot_B1, Tba[0] - aA0_dot_B0))) + { + segCoords(t, u, a[1], b[0], A1_dot_B0, Tab[1] + bA1_dot_B1, + Tba[0] - aA0_dot_B0); + + D[0] = Tab[0] + Rab(0, 1) * b[1] + Rab(0, 0) * u - a[0] ; + D[1] = Tab[1] + Rab(1, 1) * b[1] + Rab(1, 0) * u - t; + D[2] = Tab[2] + Rab(2, 1) * b[1] + Rab(2, 0) * u; + + if(P && Q) + { + *P << a[0], t, 0; + *Q = D + (*P); + } + + return D.norm(); + } + } + + // UA1, LB0 + + if((UA1_ly < 0) && (LB0_ux > a[0])) + { + if(((UA1_uy < 0) || + inVoronoi(b[0], a[1], -A1_dot_B1, Tba[1] - aA0_dot_B1, A1_dot_B0, + aA0_dot_B0 - Tba[0], -Tab[1])) + && + ((LB0_lx > a[0]) || + inVoronoi(a[1], b[0], A0_dot_B0, Tab[0] - a[0], + A1_dot_B0, Tab[1], Tba[0] - aA0_dot_B0))) + { + segCoords(t, u, a[1], b[0], A1_dot_B0, Tab[1], Tba[0] - aA0_dot_B0); + + D[0] = Tab[0] + Rab(0, 0) * u - a[0]; + D[1] = Tab[1] + Rab(1, 0) * u - t; + D[2] = Tab[2] + Rab(2, 0) * u; + + if(P && Q) + { + *P << a[0], t, 0; + *Q = D + (*P); + } + + return D.norm(); + } + } + + // LA1, UB0 + + if((LA1_uy > b[1]) && (UB0_lx < 0)) + { + if(((LA1_ly > b[1]) || + inVoronoi(b[0], a[1], A1_dot_B1, -Tba[1] - b[1], + A1_dot_B0, -Tba[0], -Tab[1] - bA1_dot_B1)) + && + + ((UB0_ux < 0) || + inVoronoi(a[1], b[0], -A0_dot_B0, -Tab[0] - bA0_dot_B1, A1_dot_B0, + Tab[1] + bA1_dot_B1, Tba[0]))) + { + segCoords(t, u, a[1], b[0], A1_dot_B0, Tab[1] + bA1_dot_B1, Tba[0]); + + D[0] = Tab[0] + Rab(0, 1) * b[1] + Rab(0, 0) * u; + D[1] = Tab[1] + Rab(1, 1) * b[1] + Rab(1, 0) * u - t; + D[2] = Tab[2] + Rab(2, 1) * b[1] + Rab(2, 0) * u; + + if(P && Q) + { + *P << 0, t, 0; + *Q = D + (*P); + } + + + return D.norm(); + } + } + + // LA1, LB0 + + if((LA1_ly < 0) && (LB0_lx < 0)) + { + if(((LA1_uy < 0) || + inVoronoi(b[0], a[1], -A1_dot_B1, Tba[1], A1_dot_B0, + -Tba[0], -Tab[1])) + && + ((LB0_ux < 0) || + inVoronoi(a[1], b[0], -A0_dot_B0, -Tab[0], A1_dot_B0, + Tab[1], Tba[0]))) + { + segCoords(t, u, a[1], b[0], A1_dot_B0, Tab[1], Tba[0]); + + D[0] = Tab[0] + Rab(0, 0) * u; + D[1] = Tab[1] + Rab(1, 0) * u - t; + D[2] = Tab[2] + Rab(2, 0) * u; + + if(P&& Q) + { + *P << 0, t, 0; + *Q = D + (*P); + } + + return D.norm(); + } + } + + S BLL_y, BLU_y, BUL_y, BUU_y; + + BLL_y = Tab[1]; + BLU_y = BLL_y + bA1_dot_B1; + BUL_y = BLL_y + bA1_dot_B0; + BUU_y = BLU_y + bA1_dot_B0; + + S LA0_lx, LA0_ux, UA0_lx, UA0_ux, LB1_ly, LB1_uy, UB1_ly, UB1_uy; + + if(ALL_x < AUL_x) + { + LA0_lx = ALL_x; + LA0_ux = AUL_x; + UA0_lx = ALU_x; + UA0_ux = AUU_x; + } + else + { + LA0_lx = AUL_x; + LA0_ux = ALL_x; + UA0_lx = AUU_x; + UA0_ux = ALU_x; + } + + if(BLL_y < BLU_y) + { + LB1_ly = BLL_y; + LB1_uy = BLU_y; + UB1_ly = BUL_y; + UB1_uy = BUU_y; + } + else + { + LB1_ly = BLU_y; + LB1_uy = BLL_y; + UB1_ly = BUU_y; + UB1_uy = BUL_y; + } + + // UA0, UB1 + + if((UA0_ux > b[0]) && (UB1_uy > a[1])) + { + if(((UA0_lx > b[0]) || + inVoronoi(b[1], a[0], A0_dot_B0, aA1_dot_B0 - Tba[0] - b[0], + A0_dot_B1, aA1_dot_B1 - Tba[1], -Tab[0] - bA0_dot_B0)) + && + ((UB1_ly > a[1]) || + inVoronoi(a[0], b[1], A1_dot_B1, Tab[1] - a[1] + bA1_dot_B0, + A0_dot_B1, Tab[0] + bA0_dot_B0, Tba[1] - aA1_dot_B1))) + { + segCoords(t, u, a[0], b[1], A0_dot_B1, Tab[0] + bA0_dot_B0, + Tba[1] - aA1_dot_B1); + + D[0] = Tab[0] + Rab(0, 0) * b[0] + Rab(0, 1) * u - t; + D[1] = Tab[1] + Rab(1, 0) * b[0] + Rab(1, 1) * u - a[1]; + D[2] = Tab[2] + Rab(2, 0) * b[0] + Rab(2, 1) * u; + + if(P && Q) + { + *P << t, a[1], 0; + *Q = D + (*P); + } + + return D.norm(); + } + } + + // UA0, LB1 + + if((UA0_lx < 0) && (LB1_uy > a[1])) + { + if(((UA0_ux < 0) || + inVoronoi(b[1], a[0], -A0_dot_B0, Tba[0] - aA1_dot_B0, A0_dot_B1, + aA1_dot_B1 - Tba[1], -Tab[0])) + && + ((LB1_ly > a[1]) || + inVoronoi(a[0], b[1], A1_dot_B1, Tab[1] - a[1], A0_dot_B1, Tab[0], + Tba[1] - aA1_dot_B1))) + { + segCoords(t, u, a[0], b[1], A0_dot_B1, Tab[0], Tba[1] - aA1_dot_B1); + D[0] = Tab[0] + Rab(0, 1) * u - t; + D[1] = Tab[1] + Rab(1, 1) * u - a[1]; + D[2] = Tab[2] + Rab(2, 1) * u; -/// @brief Check collision between two RSSs, b1 is in configuration (R0, T0) and b2 is in identity. -bool overlap(const Matrix3d& R0, const Vector3d& T0, const RSS& b1, const RSS& b2); + if(P && Q) + { + *P << t, a[1], 0; + *Q = D + (*P); + } + return D.norm(); + } + } + + // LA0, UB1 + + if((LA0_ux > b[0]) && (UB1_ly < 0)) + { + if(((LA0_lx > b[0]) || + inVoronoi(b[1], a[0], A0_dot_B0, -b[0] - Tba[0], A0_dot_B1, -Tba[1], + -bA0_dot_B0 - Tab[0])) + && + ((UB1_uy < 0) || + inVoronoi(a[0], b[1], -A1_dot_B1, -Tab[1] - bA1_dot_B0, A0_dot_B1, + Tab[0] + bA0_dot_B0, Tba[1]))) + { + segCoords(t, u, a[0], b[1], A0_dot_B1, Tab[0] + bA0_dot_B0, Tba[1]); + + D[0] = Tab[0] + Rab(0, 0) * b[0] + Rab(0, 1) * u - t; + D[1] = Tab[1] + Rab(1, 0) * b[0] + Rab(1, 1) * u; + D[2] = Tab[2] + Rab(2, 0) * b[0] + Rab(2, 1) * u; + + if(P && Q) + { + *P << t, 0, 0; + *Q = D + (*P); + } + + return D.norm(); + } + } + // LA0, LB1 + + if((LA0_lx < 0) && (LB1_ly < 0)) + { + if(((LA0_ux < 0) || + inVoronoi(b[1], a[0], -A0_dot_B0, Tba[0], A0_dot_B1, -Tba[1], + -Tab[0])) + && + ((LB1_uy < 0) || + inVoronoi(a[0], b[1], -A1_dot_B1, -Tab[1], A0_dot_B1, + Tab[0], Tba[1]))) + { + segCoords(t, u, a[0], b[1], A0_dot_B1, Tab[0], Tba[1]); + + D[0] = Tab[0] + Rab(0, 1) * u - t; + D[1] = Tab[1] + Rab(1, 1) * u; + D[2] = Tab[2] + Rab(2, 1) * u; + + if(P && Q) + { + *P << t, 0, 0; + *Q = D + (*P); + } + + return D.norm(); + } + } + + S LA0_ly, LA0_uy, UA0_ly, UA0_uy, LB0_ly, LB0_uy, UB0_ly, UB0_uy; + + if(ALL_y < AUL_y) + { + LA0_ly = ALL_y; + LA0_uy = AUL_y; + UA0_ly = ALU_y; + UA0_uy = AUU_y; + } + else + { + LA0_ly = AUL_y; + LA0_uy = ALL_y; + UA0_ly = AUU_y; + UA0_uy = ALU_y; + } + + if(BLL_y < BUL_y) + { + LB0_ly = BLL_y; + LB0_uy = BUL_y; + UB0_ly = BLU_y; + UB0_uy = BUU_y; + } + else + { + LB0_ly = BUL_y; + LB0_uy = BLL_y; + UB0_ly = BUU_y; + UB0_uy = BLU_y; + } + + // UA0, UB0 + + if((UA0_uy > b[1]) && (UB0_uy > a[1])) + { + if(((UA0_ly > b[1]) || + inVoronoi(b[0], a[0], A0_dot_B1, aA1_dot_B1 - Tba[1] - b[1], + A0_dot_B0, aA1_dot_B0 - Tba[0], -Tab[0] - bA0_dot_B1)) + && + ((UB0_ly > a[1]) || + inVoronoi(a[0], b[0], A1_dot_B0, Tab[1] - a[1] + bA1_dot_B1, A0_dot_B0, + Tab[0] + bA0_dot_B1, Tba[0] - aA1_dot_B0))) + { + segCoords(t, u, a[0], b[0], A0_dot_B0, Tab[0] + bA0_dot_B1, + Tba[0] - aA1_dot_B0); + + D[0] = Tab[0] + Rab(0, 1) * b[1] + Rab(0, 0) * u - t; + D[1] = Tab[1] + Rab(1, 1) * b[1] + Rab(1, 0) * u - a[1]; + D[2] = Tab[2] + Rab(2, 1) * b[1] + Rab(2, 0) * u; + + if(P && Q) + { + *P << t, a[1], 0; + *Q = D + (*P); + } + + return D.norm(); + } + } + + // UA0, LB0 + + if((UA0_ly < 0) && (LB0_uy > a[1])) + { + if(((UA0_uy < 0) || + inVoronoi(b[0], a[0], -A0_dot_B1, Tba[1] - aA1_dot_B1, A0_dot_B0, + aA1_dot_B0 - Tba[0], -Tab[0])) + && + ((LB0_ly > a[1]) || + inVoronoi(a[0], b[0], A1_dot_B0, Tab[1] - a[1], + A0_dot_B0, Tab[0], Tba[0] - aA1_dot_B0))) + { + segCoords(t, u, a[0], b[0], A0_dot_B0, Tab[0], Tba[0] - aA1_dot_B0); + + D[0] = Tab[0] + Rab(0, 0) * u - t; + D[1] = Tab[1] + Rab(1, 0) * u - a[1]; + D[2] = Tab[2] + Rab(2, 0) * u; + + if(P && Q) + { + *P << t, a[1], 0; + *Q = D + (*P); + } + + return D.norm(); + } + } + + // LA0, UB0 + + if((LA0_uy > b[1]) && (UB0_ly < 0)) + { + if(((LA0_ly > b[1]) || + inVoronoi(b[0], a[0], A0_dot_B1, -Tba[1] - b[1], A0_dot_B0, -Tba[0], + -Tab[0] - bA0_dot_B1)) + && + + ((UB0_uy < 0) || + inVoronoi(a[0], b[0], -A1_dot_B0, -Tab[1] - bA1_dot_B1, A0_dot_B0, + Tab[0] + bA0_dot_B1, Tba[0]))) + { + segCoords(t, u, a[0], b[0], A0_dot_B0, Tab[0] + bA0_dot_B1, Tba[0]); + + D[0] = Tab[0] + Rab(0, 1) * b[1] + Rab(0, 0) * u - t; + D[1] = Tab[1] + Rab(1, 1) * b[1] + Rab(1, 0) * u; + D[2] = Tab[2] + Rab(2, 1) * b[1] + Rab(2, 0) * u; + + if(P && Q) + { + *P << t, 0, 0; + *Q = D + (*P); + } + + return D.norm(); + } + } + + // LA0, LB0 + + if((LA0_ly < 0) && (LB0_ly < 0)) + { + if(((LA0_uy < 0) || + inVoronoi(b[0], a[0], -A0_dot_B1, Tba[1], A0_dot_B0, + -Tba[0], -Tab[0])) + && + ((LB0_uy < 0) || + inVoronoi(a[0], b[0], -A1_dot_B0, -Tab[1], A0_dot_B0, + Tab[0], Tba[0]))) + { + segCoords(t, u, a[0], b[0], A0_dot_B0, Tab[0], Tba[0]); + + D[0] = Tab[0] + Rab(0, 0) * u - t; + D[1] = Tab[1] + Rab(1, 0) * u; + D[2] = Tab[2] + Rab(2, 0) * u; + + if(P && Q) + { + *P << t, 0, 0; + *Q = D + (*P); + } + + return D.norm(); + } + } + + // no edges passed, take max separation along face normals + + S sep1, sep2; + + if(Tab[2] > 0.0) + { + sep1 = Tab[2]; + if (Rab(2, 0) < 0.0) sep1 += b[0] * Rab(2, 0); + if (Rab(2, 1) < 0.0) sep1 += b[1] * Rab(2, 1); + } + else + { + sep1 = -Tab[2]; + if (Rab(2, 0) > 0.0) sep1 -= b[0] * Rab(2, 0); + if (Rab(2, 1) > 0.0) sep1 -= b[1] * Rab(2, 1); + } + + if(Tba[2] < 0) + { + sep2 = -Tba[2]; + if (Rab(0, 2) < 0.0) sep2 += a[0] * Rab(0, 2); + if (Rab(1, 2) < 0.0) sep2 += a[1] * Rab(1, 2); + } + else + { + sep2 = Tba[2]; + if (Rab(0, 2) > 0.0) sep2 -= a[0] * Rab(0, 2); + if (Rab(1, 2) > 0.0) sep2 -= a[1] * Rab(1, 2); + } + + if(sep1 >= sep2 && sep1 >= 0) + { + if(Tab[2] > 0) + D << 0, 0, sep1; + else + D << 0, 0, -sep1; + + if(P && Q) + { + *Q = D; + P->setZero(); + } + } + + if(sep2 >= sep1 && sep2 >= 0) + { + Vector3 Q_(Tab[0], Tab[1], Tab[2]); + Vector3 P_; + if(Tba[2] < 0) + { + P_[0] = Rab(0, 2) * sep2 + Tab[0]; + P_[1] = Rab(1, 2) * sep2 + Tab[1]; + P_[2] = Rab(2, 2) * sep2 + Tab[2]; + } + else + { + P_[0] = -Rab(0, 2) * sep2 + Tab[0]; + P_[1] = -Rab(1, 2) * sep2 + Tab[1]; + P_[2] = -Rab(2, 2) * sep2 + Tab[2]; + } + + D = Q_ - P_; + + if(P && Q) + { + *P = P_; + *Q = Q_; + } + } + + S sep = (sep1 > sep2 ? sep1 : sep2); + return (sep > 0 ? sep : 0); +} + +//============================================================================== +template +S rectDistance( + const Transform3& tfab, + const S a[2], + const S b[2], + Vector3* P, + Vector3* Q) +{ + S A0_dot_B0 = tfab.linear()(0, 0); + S A0_dot_B1 = tfab.linear()(0, 1); + S A1_dot_B0 = tfab.linear()(1, 0); + S A1_dot_B1 = tfab.linear()(1, 1); + + S aA0_dot_B0 = a[0] * A0_dot_B0; + S aA0_dot_B1 = a[0] * A0_dot_B1; + S aA1_dot_B0 = a[1] * A1_dot_B0; + S aA1_dot_B1 = a[1] * A1_dot_B1; + S bA0_dot_B0 = b[0] * A0_dot_B0; + S bA1_dot_B0 = b[0] * A1_dot_B0; + S bA0_dot_B1 = b[1] * A0_dot_B1; + S bA1_dot_B1 = b[1] * A1_dot_B1; + + Vector3 Tba = tfab.linear().transpose() * tfab.translation(); + + Vector3 D; + S t, u; + + // determine if any edge pair contains the closest points + + S LA1_lx, LA1_ux, UA1_lx, UA1_ux, LB1_lx, LB1_ux, UB1_lx, UB1_ux; + + S ALL_x = -Tba[0]; + S ALU_x = ALL_x + aA1_dot_B0; + S AUL_x = ALL_x + aA0_dot_B0; + S AUU_x = ALU_x + aA0_dot_B0; + + if(ALL_x < ALU_x) + { + LA1_lx = ALL_x; + LA1_ux = ALU_x; + UA1_lx = AUL_x; + UA1_ux = AUU_x; + } + else + { + LA1_lx = ALU_x; + LA1_ux = ALL_x; + UA1_lx = AUU_x; + UA1_ux = AUL_x; + } + + S BLL_x = tfab.translation()[0]; + S BLU_x = BLL_x + bA0_dot_B1; + S BUL_x = BLL_x + bA0_dot_B0; + S BUU_x = BLU_x + bA0_dot_B0; + + if(BLL_x < BLU_x) + { + LB1_lx = BLL_x; + LB1_ux = BLU_x; + UB1_lx = BUL_x; + UB1_ux = BUU_x; + } + else + { + LB1_lx = BLU_x; + LB1_ux = BLL_x; + UB1_lx = BUU_x; + UB1_ux = BUL_x; + } + + // UA1, UB1 + + if((UA1_ux > b[0]) && (UB1_ux > a[0])) + { + if(((UA1_lx > b[0]) || + inVoronoi(b[1], a[1], A1_dot_B0, aA0_dot_B0 - b[0] - Tba[0], + A1_dot_B1, aA0_dot_B1 - Tba[1], + -tfab.translation()[1] - bA1_dot_B0)) + && + ((UB1_lx > a[0]) || + inVoronoi(a[1], b[1], A0_dot_B1, tfab.translation()[0] + bA0_dot_B0 - a[0], + A1_dot_B1, tfab.translation()[1] + bA1_dot_B0, Tba[1] - aA0_dot_B1))) + { + segCoords(t, u, a[1], b[1], A1_dot_B1, tfab.translation()[1] + bA1_dot_B0, + Tba[1] - aA0_dot_B1); + + D[0] = tfab.translation()[0] + tfab.linear()(0, 0) * b[0] + tfab.linear()(0, 1) * u - a[0] ; + D[1] = tfab.translation()[1] + tfab.linear()(1, 0) * b[0] + tfab.linear()(1, 1) * u - t; + D[2] = tfab.translation()[2] + tfab.linear()(2, 0) * b[0] + tfab.linear()(2, 1) * u; + + if(P && Q) + { + *P << a[0], t, 0; + *Q = D + (*P); + } + + return D.norm(); + } + } + + + // UA1, LB1 + + if((UA1_lx < 0) && (LB1_ux > a[0])) + { + if(((UA1_ux < 0) || + inVoronoi(b[1], a[1], -A1_dot_B0, Tba[0] - aA0_dot_B0, + A1_dot_B1, aA0_dot_B1 - Tba[1], -tfab.translation()[1])) + && + ((LB1_lx > a[0]) || + inVoronoi(a[1], b[1], A0_dot_B1, tfab.translation()[0] - a[0], + A1_dot_B1, tfab.translation()[1], Tba[1] - aA0_dot_B1))) + { + segCoords(t, u, a[1], b[1], A1_dot_B1, tfab.translation()[1], Tba[1] - aA0_dot_B1); + + D[0] = tfab.translation()[0] + tfab.linear()(0, 1) * u - a[0]; + D[1] = tfab.translation()[1] + tfab.linear()(1, 1) * u - t; + D[2] = tfab.translation()[2] + tfab.linear()(2, 1) * u; + + if(P && Q) + { + *P << a[0], t, 0; + *Q = D + (*P); + } + + return D.norm(); + } + } + + // LA1, UB1 + + if((LA1_ux > b[0]) && (UB1_lx < 0)) + { + if(((LA1_lx > b[0]) || + inVoronoi(b[1], a[1], A1_dot_B0, -Tba[0] - b[0], + A1_dot_B1, -Tba[1], -tfab.translation()[1] - bA1_dot_B0)) + && + ((UB1_ux < 0) || + inVoronoi(a[1], b[1], -A0_dot_B1, -tfab.translation()[0] - bA0_dot_B0, + A1_dot_B1, tfab.translation()[1] + bA1_dot_B0, Tba[1]))) + { + segCoords(t, u, a[1], b[1], A1_dot_B1, tfab.translation()[1] + bA1_dot_B0, Tba[1]); + + D[0] = tfab.translation()[0] + tfab.linear()(0, 0) * b[0] + tfab.linear()(0, 1) * u; + D[1] = tfab.translation()[1] + tfab.linear()(1, 0) * b[0] + tfab.linear()(1, 1) * u - t; + D[2] = tfab.translation()[2] + tfab.linear()(2, 0) * b[0] + tfab.linear()(2, 1) * u; + + if(P && Q) + { + *P << 0, t, 0; + *Q = D + (*P); + } + + return D.norm(); + } + } + + // LA1, LB1 + + if((LA1_lx < 0) && (LB1_lx < 0)) + { + if (((LA1_ux < 0) || + inVoronoi(b[1], a[1], -A1_dot_B0, Tba[0], A1_dot_B1, + -Tba[1], -tfab.translation()[1])) + && + ((LB1_ux < 0) || + inVoronoi(a[1], b[1], -A0_dot_B1, -tfab.translation()[0], A1_dot_B1, + tfab.translation()[1], Tba[1]))) + { + segCoords(t, u, a[1], b[1], A1_dot_B1, tfab.translation()[1], Tba[1]); + + D[0] = tfab.translation()[0] + tfab.linear()(0, 1) * u; + D[1] = tfab.translation()[1] + tfab.linear()(1, 1) * u - t; + D[2] = tfab.translation()[2] + tfab.linear()(2, 1) * u; + + if(P && Q) + { + *P << 0, t, 0; + *Q = D + (*P); + } + + return D.norm(); + } + } + + S ALL_y, ALU_y, AUL_y, AUU_y; + + ALL_y = -Tba[1]; + ALU_y = ALL_y + aA1_dot_B1; + AUL_y = ALL_y + aA0_dot_B1; + AUU_y = ALU_y + aA0_dot_B1; + + S LA1_ly, LA1_uy, UA1_ly, UA1_uy, LB0_lx, LB0_ux, UB0_lx, UB0_ux; + + if(ALL_y < ALU_y) + { + LA1_ly = ALL_y; + LA1_uy = ALU_y; + UA1_ly = AUL_y; + UA1_uy = AUU_y; + } + else + { + LA1_ly = ALU_y; + LA1_uy = ALL_y; + UA1_ly = AUU_y; + UA1_uy = AUL_y; + } + + if(BLL_x < BUL_x) + { + LB0_lx = BLL_x; + LB0_ux = BUL_x; + UB0_lx = BLU_x; + UB0_ux = BUU_x; + } + else + { + LB0_lx = BUL_x; + LB0_ux = BLL_x; + UB0_lx = BUU_x; + UB0_ux = BLU_x; + } + + // UA1, UB0 + + if((UA1_uy > b[1]) && (UB0_ux > a[0])) + { + if(((UA1_ly > b[1]) || + inVoronoi(b[0], a[1], A1_dot_B1, aA0_dot_B1 - Tba[1] - b[1], + A1_dot_B0, aA0_dot_B0 - Tba[0], -tfab.translation()[1] - bA1_dot_B1)) + && + ((UB0_lx > a[0]) || + inVoronoi(a[1], b[0], A0_dot_B0, tfab.translation()[0] - a[0] + bA0_dot_B1, + A1_dot_B0, tfab.translation()[1] + bA1_dot_B1, Tba[0] - aA0_dot_B0))) + { + segCoords(t, u, a[1], b[0], A1_dot_B0, tfab.translation()[1] + bA1_dot_B1, + Tba[0] - aA0_dot_B0); + + D[0] = tfab.translation()[0] + tfab.linear()(0, 1) * b[1] + tfab.linear()(0, 0) * u - a[0] ; + D[1] = tfab.translation()[1] + tfab.linear()(1, 1) * b[1] + tfab.linear()(1, 0) * u - t; + D[2] = tfab.translation()[2] + tfab.linear()(2, 1) * b[1] + tfab.linear()(2, 0) * u; + + if(P && Q) + { + *P << a[0], t, 0; + *Q = D + (*P); + } + + return D.norm(); + } + } + + // UA1, LB0 + + if((UA1_ly < 0) && (LB0_ux > a[0])) + { + if(((UA1_uy < 0) || + inVoronoi(b[0], a[1], -A1_dot_B1, Tba[1] - aA0_dot_B1, A1_dot_B0, + aA0_dot_B0 - Tba[0], -tfab.translation()[1])) + && + ((LB0_lx > a[0]) || + inVoronoi(a[1], b[0], A0_dot_B0, tfab.translation()[0] - a[0], + A1_dot_B0, tfab.translation()[1], Tba[0] - aA0_dot_B0))) + { + segCoords(t, u, a[1], b[0], A1_dot_B0, tfab.translation()[1], Tba[0] - aA0_dot_B0); + + D[0] = tfab.translation()[0] + tfab.linear()(0, 0) * u - a[0]; + D[1] = tfab.translation()[1] + tfab.linear()(1, 0) * u - t; + D[2] = tfab.translation()[2] + tfab.linear()(2, 0) * u; + + if(P && Q) + { + *P << a[0], t, 0; + *Q = D + (*P); + } + + return D.norm(); + } + } + + // LA1, UB0 + + if((LA1_uy > b[1]) && (UB0_lx < 0)) + { + if(((LA1_ly > b[1]) || + inVoronoi(b[0], a[1], A1_dot_B1, -Tba[1] - b[1], + A1_dot_B0, -Tba[0], -tfab.translation()[1] - bA1_dot_B1)) + && + + ((UB0_ux < 0) || + inVoronoi(a[1], b[0], -A0_dot_B0, -tfab.translation()[0] - bA0_dot_B1, A1_dot_B0, + tfab.translation()[1] + bA1_dot_B1, Tba[0]))) + { + segCoords(t, u, a[1], b[0], A1_dot_B0, tfab.translation()[1] + bA1_dot_B1, Tba[0]); + + D[0] = tfab.translation()[0] + tfab.linear()(0, 1) * b[1] + tfab.linear()(0, 0) * u; + D[1] = tfab.translation()[1] + tfab.linear()(1, 1) * b[1] + tfab.linear()(1, 0) * u - t; + D[2] = tfab.translation()[2] + tfab.linear()(2, 1) * b[1] + tfab.linear()(2, 0) * u; + + if(P && Q) + { + *P << 0, t, 0; + *Q = D + (*P); + } + + + return D.norm(); + } + } + + // LA1, LB0 + + if((LA1_ly < 0) && (LB0_lx < 0)) + { + if(((LA1_uy < 0) || + inVoronoi(b[0], a[1], -A1_dot_B1, Tba[1], A1_dot_B0, + -Tba[0], -tfab.translation()[1])) + && + ((LB0_ux < 0) || + inVoronoi(a[1], b[0], -A0_dot_B0, -tfab.translation()[0], A1_dot_B0, + tfab.translation()[1], Tba[0]))) + { + segCoords(t, u, a[1], b[0], A1_dot_B0, tfab.translation()[1], Tba[0]); + + D[0] = tfab.translation()[0] + tfab.linear()(0, 0) * u; + D[1] = tfab.translation()[1] + tfab.linear()(1, 0) * u - t; + D[2] = tfab.translation()[2] + tfab.linear()(2, 0) * u; + + if(P&& Q) + { + *P << 0, t, 0; + *Q = D + (*P); + } + + return D.norm(); + } + } + + S BLL_y, BLU_y, BUL_y, BUU_y; + + BLL_y = tfab.translation()[1]; + BLU_y = BLL_y + bA1_dot_B1; + BUL_y = BLL_y + bA1_dot_B0; + BUU_y = BLU_y + bA1_dot_B0; + + S LA0_lx, LA0_ux, UA0_lx, UA0_ux, LB1_ly, LB1_uy, UB1_ly, UB1_uy; + + if(ALL_x < AUL_x) + { + LA0_lx = ALL_x; + LA0_ux = AUL_x; + UA0_lx = ALU_x; + UA0_ux = AUU_x; + } + else + { + LA0_lx = AUL_x; + LA0_ux = ALL_x; + UA0_lx = AUU_x; + UA0_ux = ALU_x; + } + + if(BLL_y < BLU_y) + { + LB1_ly = BLL_y; + LB1_uy = BLU_y; + UB1_ly = BUL_y; + UB1_uy = BUU_y; + } + else + { + LB1_ly = BLU_y; + LB1_uy = BLL_y; + UB1_ly = BUU_y; + UB1_uy = BUL_y; + } + + // UA0, UB1 + + if((UA0_ux > b[0]) && (UB1_uy > a[1])) + { + if(((UA0_lx > b[0]) || + inVoronoi(b[1], a[0], A0_dot_B0, aA1_dot_B0 - Tba[0] - b[0], + A0_dot_B1, aA1_dot_B1 - Tba[1], -tfab.translation()[0] - bA0_dot_B0)) + && + ((UB1_ly > a[1]) || + inVoronoi(a[0], b[1], A1_dot_B1, tfab.translation()[1] - a[1] + bA1_dot_B0, + A0_dot_B1, tfab.translation()[0] + bA0_dot_B0, Tba[1] - aA1_dot_B1))) + { + segCoords(t, u, a[0], b[1], A0_dot_B1, tfab.translation()[0] + bA0_dot_B0, + Tba[1] - aA1_dot_B1); + + D[0] = tfab.translation()[0] + tfab.linear()(0, 0) * b[0] + tfab.linear()(0, 1) * u - t; + D[1] = tfab.translation()[1] + tfab.linear()(1, 0) * b[0] + tfab.linear()(1, 1) * u - a[1]; + D[2] = tfab.translation()[2] + tfab.linear()(2, 0) * b[0] + tfab.linear()(2, 1) * u; + + if(P && Q) + { + *P << t, a[1], 0; + *Q = D + (*P); + } + + return D.norm(); + } + } + + // UA0, LB1 + + if((UA0_lx < 0) && (LB1_uy > a[1])) + { + if(((UA0_ux < 0) || + inVoronoi(b[1], a[0], -A0_dot_B0, Tba[0] - aA1_dot_B0, A0_dot_B1, + aA1_dot_B1 - Tba[1], -tfab.translation()[0])) + && + ((LB1_ly > a[1]) || + inVoronoi(a[0], b[1], A1_dot_B1, tfab.translation()[1] - a[1], A0_dot_B1, tfab.translation()[0], + Tba[1] - aA1_dot_B1))) + { + segCoords(t, u, a[0], b[1], A0_dot_B1, tfab.translation()[0], Tba[1] - aA1_dot_B1); + + D[0] = tfab.translation()[0] + tfab.linear()(0, 1) * u - t; + D[1] = tfab.translation()[1] + tfab.linear()(1, 1) * u - a[1]; + D[2] = tfab.translation()[2] + tfab.linear()(2, 1) * u; + + if(P && Q) + { + *P << t, a[1], 0; + *Q = D + (*P); + } + + return D.norm(); + } + } + + // LA0, UB1 + + if((LA0_ux > b[0]) && (UB1_ly < 0)) + { + if(((LA0_lx > b[0]) || + inVoronoi(b[1], a[0], A0_dot_B0, -b[0] - Tba[0], A0_dot_B1, -Tba[1], + -bA0_dot_B0 - tfab.translation()[0])) + && + ((UB1_uy < 0) || + inVoronoi(a[0], b[1], -A1_dot_B1, -tfab.translation()[1] - bA1_dot_B0, A0_dot_B1, + tfab.translation()[0] + bA0_dot_B0, Tba[1]))) + { + segCoords(t, u, a[0], b[1], A0_dot_B1, tfab.translation()[0] + bA0_dot_B0, Tba[1]); + + D[0] = tfab.translation()[0] + tfab.linear()(0, 0) * b[0] + tfab.linear()(0, 1) * u - t; + D[1] = tfab.translation()[1] + tfab.linear()(1, 0) * b[0] + tfab.linear()(1, 1) * u; + D[2] = tfab.translation()[2] + tfab.linear()(2, 0) * b[0] + tfab.linear()(2, 1) * u; + + if(P && Q) + { + *P << t, 0, 0; + *Q = D + (*P); + } + + return D.norm(); + } + } + + // LA0, LB1 + + if((LA0_lx < 0) && (LB1_ly < 0)) + { + if(((LA0_ux < 0) || + inVoronoi(b[1], a[0], -A0_dot_B0, Tba[0], A0_dot_B1, -Tba[1], + -tfab.translation()[0])) + && + ((LB1_uy < 0) || + inVoronoi(a[0], b[1], -A1_dot_B1, -tfab.translation()[1], A0_dot_B1, + tfab.translation()[0], Tba[1]))) + { + segCoords(t, u, a[0], b[1], A0_dot_B1, tfab.translation()[0], Tba[1]); + + D[0] = tfab.translation()[0] + tfab.linear()(0, 1) * u - t; + D[1] = tfab.translation()[1] + tfab.linear()(1, 1) * u; + D[2] = tfab.translation()[2] + tfab.linear()(2, 1) * u; + + if(P && Q) + { + *P << t, 0, 0; + *Q = D + (*P); + } + + return D.norm(); + } + } + + S LA0_ly, LA0_uy, UA0_ly, UA0_uy, LB0_ly, LB0_uy, UB0_ly, UB0_uy; + + if(ALL_y < AUL_y) + { + LA0_ly = ALL_y; + LA0_uy = AUL_y; + UA0_ly = ALU_y; + UA0_uy = AUU_y; + } + else + { + LA0_ly = AUL_y; + LA0_uy = ALL_y; + UA0_ly = AUU_y; + UA0_uy = ALU_y; + } + + if(BLL_y < BUL_y) + { + LB0_ly = BLL_y; + LB0_uy = BUL_y; + UB0_ly = BLU_y; + UB0_uy = BUU_y; + } + else + { + LB0_ly = BUL_y; + LB0_uy = BLL_y; + UB0_ly = BUU_y; + UB0_uy = BLU_y; + } + + // UA0, UB0 + + if((UA0_uy > b[1]) && (UB0_uy > a[1])) + { + if(((UA0_ly > b[1]) || + inVoronoi(b[0], a[0], A0_dot_B1, aA1_dot_B1 - Tba[1] - b[1], + A0_dot_B0, aA1_dot_B0 - Tba[0], -tfab.translation()[0] - bA0_dot_B1)) + && + ((UB0_ly > a[1]) || + inVoronoi(a[0], b[0], A1_dot_B0, tfab.translation()[1] - a[1] + bA1_dot_B1, A0_dot_B0, + tfab.translation()[0] + bA0_dot_B1, Tba[0] - aA1_dot_B0))) + { + segCoords(t, u, a[0], b[0], A0_dot_B0, tfab.translation()[0] + bA0_dot_B1, + Tba[0] - aA1_dot_B0); + + D[0] = tfab.translation()[0] + tfab.linear()(0, 1) * b[1] + tfab.linear()(0, 0) * u - t; + D[1] = tfab.translation()[1] + tfab.linear()(1, 1) * b[1] + tfab.linear()(1, 0) * u - a[1]; + D[2] = tfab.translation()[2] + tfab.linear()(2, 1) * b[1] + tfab.linear()(2, 0) * u; + + if(P && Q) + { + *P << t, a[1], 0; + *Q = D + (*P); + } + + return D.norm(); + } + } + + // UA0, LB0 + + if((UA0_ly < 0) && (LB0_uy > a[1])) + { + if(((UA0_uy < 0) || + inVoronoi(b[0], a[0], -A0_dot_B1, Tba[1] - aA1_dot_B1, A0_dot_B0, + aA1_dot_B0 - Tba[0], -tfab.translation()[0])) + && + ((LB0_ly > a[1]) || + inVoronoi(a[0], b[0], A1_dot_B0, tfab.translation()[1] - a[1], + A0_dot_B0, tfab.translation()[0], Tba[0] - aA1_dot_B0))) + { + segCoords(t, u, a[0], b[0], A0_dot_B0, tfab.translation()[0], Tba[0] - aA1_dot_B0); + + D[0] = tfab.translation()[0] + tfab.linear()(0, 0) * u - t; + D[1] = tfab.translation()[1] + tfab.linear()(1, 0) * u - a[1]; + D[2] = tfab.translation()[2] + tfab.linear()(2, 0) * u; + + if(P && Q) + { + *P << t, a[1], 0; + *Q = D + (*P); + } + + return D.norm(); + } + } + + // LA0, UB0 + + if((LA0_uy > b[1]) && (UB0_ly < 0)) + { + if(((LA0_ly > b[1]) || + inVoronoi(b[0], a[0], A0_dot_B1, -Tba[1] - b[1], A0_dot_B0, -Tba[0], + -tfab.translation()[0] - bA0_dot_B1)) + && + + ((UB0_uy < 0) || + inVoronoi(a[0], b[0], -A1_dot_B0, -tfab.translation()[1] - bA1_dot_B1, A0_dot_B0, + tfab.translation()[0] + bA0_dot_B1, Tba[0]))) + { + segCoords(t, u, a[0], b[0], A0_dot_B0, tfab.translation()[0] + bA0_dot_B1, Tba[0]); + + D[0] = tfab.translation()[0] + tfab.linear()(0, 1) * b[1] + tfab.linear()(0, 0) * u - t; + D[1] = tfab.translation()[1] + tfab.linear()(1, 1) * b[1] + tfab.linear()(1, 0) * u; + D[2] = tfab.translation()[2] + tfab.linear()(2, 1) * b[1] + tfab.linear()(2, 0) * u; + + if(P && Q) + { + *P << t, 0, 0; + *Q = D + (*P); + } + + return D.norm(); + } + } + + // LA0, LB0 + + if((LA0_ly < 0) && (LB0_ly < 0)) + { + if(((LA0_uy < 0) || + inVoronoi(b[0], a[0], -A0_dot_B1, Tba[1], A0_dot_B0, + -Tba[0], -tfab.translation()[0])) + && + ((LB0_uy < 0) || + inVoronoi(a[0], b[0], -A1_dot_B0, -tfab.translation()[1], A0_dot_B0, + tfab.translation()[0], Tba[0]))) + { + segCoords(t, u, a[0], b[0], A0_dot_B0, tfab.translation()[0], Tba[0]); + + D[0] = tfab.translation()[0] + tfab.linear()(0, 0) * u - t; + D[1] = tfab.translation()[1] + tfab.linear()(1, 0) * u; + D[2] = tfab.translation()[2] + tfab.linear()(2, 0) * u; + + if(P && Q) + { + *P << t, 0, 0; + *Q = D + (*P); + } + + return D.norm(); + } + } + + // no edges passed, take max separation along face normals + + S sep1, sep2; + + if(tfab.translation()[2] > 0.0) + { + sep1 = tfab.translation()[2]; + if (tfab.linear()(2, 0) < 0.0) sep1 += b[0] * tfab.linear()(2, 0); + if (tfab.linear()(2, 1) < 0.0) sep1 += b[1] * tfab.linear()(2, 1); + } + else + { + sep1 = -tfab.translation()[2]; + if (tfab.linear()(2, 0) > 0.0) sep1 -= b[0] * tfab.linear()(2, 0); + if (tfab.linear()(2, 1) > 0.0) sep1 -= b[1] * tfab.linear()(2, 1); + } + + if(Tba[2] < 0) + { + sep2 = -Tba[2]; + if (tfab.linear()(0, 2) < 0.0) sep2 += a[0] * tfab.linear()(0, 2); + if (tfab.linear()(1, 2) < 0.0) sep2 += a[1] * tfab.linear()(1, 2); + } + else + { + sep2 = Tba[2]; + if (tfab.linear()(0, 2) > 0.0) sep2 -= a[0] * tfab.linear()(0, 2); + if (tfab.linear()(1, 2) > 0.0) sep2 -= a[1] * tfab.linear()(1, 2); + } + + if(sep1 >= sep2 && sep1 >= 0) + { + if(tfab.translation()[2] > 0) + D << 0, 0, sep1; + else + D << 0, 0, -sep1; + + if(P && Q) + { + *Q = D; + P->setZero(); + } + } + + if(sep2 >= sep1 && sep2 >= 0) + { + Vector3 Q_(tfab.translation()); + Vector3 P_; + if(Tba[2] < 0) + { + P_.noalias() = tfab.linear().col(2) * sep2; + P_.noalias() += tfab.translation(); + } + else + { + P_.noalias() = tfab.linear().col(2) * -sep2; + P_.noalias() += tfab.translation(); + } + + D = Q_ - P_; + + if(P && Q) + { + *P = P_; + *Q = Q_; + } + } + + S sep = (sep1 > sep2 ? sep1 : sep2); + return (sep > 0 ? sep : 0); +} + +//============================================================================== +template +bool overlap( + const Eigen::MatrixBase& R0, + const Eigen::MatrixBase& T0, + const RSS& b1, + const RSS& b2) +{ + Matrix3 R0b2 = R0 * b2.axis; + Matrix3 R = b1.axis.transpose() * R0b2; + + Vector3 Ttemp = R0 * b2.To + T0 - b1.To; + Vector3 T = Ttemp.transpose() * b1.axis; + + S dist = rectDistance(R, T, b1.l, b2.l); + return (dist <= (b1.r + b2.r)); +} + +//============================================================================== +template +bool overlap( + const Transform3& tf, + const RSS& b1, + const RSS& b2) +{ + S dist = rectDistance(b1.frame.inverse(Eigen::Isometry) * tf * b2.frame, b1.l, b2.l); + return (dist <= (b1.r + b2.r)); +} + +//============================================================================== +template +S distance( + const Eigen::MatrixBase& R0, + const Eigen::MatrixBase& T0, + const RSS& b1, + const RSS& b2, + Vector3* P, + Vector3* Q) +{ + Matrix3 R0b2 = R0 * b2.axis; + Matrix3 R = b1.axis.transpose() * R0b2; + + Vector3 Ttemp = R0 * b2.To + T0 - b1.To; + Vector3 T = Ttemp.transpose() * b1.axis; + + S dist = rectDistance(R, T, b1.l, b2.l, P, Q); + dist -= (b1.r + b2.r); + return (dist < (S)0.0) ? (S)0.0 : dist; +} + +//============================================================================== +template +S distance( + const Transform3& tf, + const RSS& b1, + const RSS& b2, + Vector3* P, + Vector3* Q) +{ + const Transform3 tf1 = b1.frame.inverse(Eigen::Isometry) * tf * b2.frame; + + S dist = rectDistance(tf1, b1.l, b2.l, P, Q); + dist -= (b1.r + b2.r); + return (dist < (S)0.0) ? (S)0.0 : dist; +} + +//============================================================================== +template +RSS translate(const RSS& bv, const Vector3& t) +{ + RSS res(bv); + res.To += t; + return res; } +} // namespace fcl #endif diff --git a/src/traversal/traversal_node_base.cpp b/include/fcl/BV/convert_bv.h similarity index 59% rename from src/traversal/traversal_node_base.cpp rename to include/fcl/BV/convert_bv.h index 2dae61aa3..30e682e1b 100644 --- a/src/traversal/traversal_node_base.cpp +++ b/include/fcl/BV/convert_bv.h @@ -35,87 +35,38 @@ /** \author Jia Pan */ +#ifndef FCL_BV_CONVERTBV_H +#define FCL_BV_CONVERTBV_H -#include "fcl/traversal/traversal_node_base.h" -#include +#include "fcl/BV/detail/converter.h" +/** \brief Main namespace */ namespace fcl { -TraversalNodeBase::~TraversalNodeBase() -{ -} - -bool TraversalNodeBase::isFirstNodeLeaf(int b) const -{ - return true; -} - -bool TraversalNodeBase::isSecondNodeLeaf(int b) const -{ - return true; -} - -bool TraversalNodeBase::firstOverSecond(int b1, int b2) const -{ - return true; -} +/// @brief Convert a bounding volume of type BV1 in configuration tf1 to +/// bounding volume of type BV2 in identity configuration. +template +void convertBV( + const BV1& bv1, const Transform3& tf1, BV2& bv2); -int TraversalNodeBase::getFirstLeftChild(int b) const -{ - return b; -} +//============================================================================// +// // +// Implementations // +// // +//============================================================================// -int TraversalNodeBase::getFirstRightChild(int b) const +//============================================================================== +template +void convertBV( + const BV1& bv1, const Transform3& tf1, BV2& bv2) { - return b; -} + static_assert(std::is_same::value, + "The scalar type of BV1 and BV2 should be the same"); -int TraversalNodeBase::getSecondLeftChild(int b) const -{ - return b; + detail::Converter::convert(bv1, tf1, bv2); } -int TraversalNodeBase::getSecondRightChild(int b) const -{ - return b; -} +} // namespace fcl -CollisionTraversalNodeBase::~CollisionTraversalNodeBase() -{ -} - -bool CollisionTraversalNodeBase::BVTesting(int b1, int b2) const -{ - return true; -} - -void CollisionTraversalNodeBase::leafTesting(int b1, int b2) const -{ -} - -bool CollisionTraversalNodeBase::canStop() const -{ - return false; -} - - -DistanceTraversalNodeBase::~DistanceTraversalNodeBase() -{ -} - -FCL_REAL DistanceTraversalNodeBase::BVTesting(int b1, int b2) const -{ - return std::numeric_limits::max(); -} - -void DistanceTraversalNodeBase::leafTesting(int b1, int b2) const -{ -} - -bool DistanceTraversalNodeBase::canStop(FCL_REAL c) const -{ - return false; -} - -} +#endif diff --git a/include/fcl/BV/detail/converter.h b/include/fcl/BV/detail/converter.h new file mode 100644 index 000000000..2f40eef1b --- /dev/null +++ b/include/fcl/BV/detail/converter.h @@ -0,0 +1,287 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_BV_DETAIL_CONVERTER_H +#define FCL_BV_DETAIL_CONVERTER_H + +#include "fcl/BV/kDOP.h" +#include "fcl/BV/AABB.h" +#include "fcl/BV/OBB.h" +#include "fcl/BV/RSS.h" +#include "fcl/BV/OBBRSS.h" +#include "fcl/BV/kIOS.h" + +/** \brief Main namespace */ +namespace fcl +{ + +/// @cond IGNORE +namespace detail +{ + +/// @brief Convert a bounding volume of type BV1 in configuration tf1 to a bounding volume of type BV2 in I configuration. +template +class Converter +{ +private: + static void convert(const BV1& bv1, const Transform3& tf1, BV2& bv2) + { + // should only use the specialized version, so it is private. + } +}; + +/// @brief Convert from AABB to AABB, not very tight but is fast. +template +class Converter, AABB> +{ +public: + static void convert(const AABB& bv1, const Transform3& tf1, AABB& bv2) + { + const Vector3 center = bv1.center(); + S r = (bv1.max_ - bv1.min_).norm() * 0.5; + Vector3 center2 = tf1 * center; + Vector3 delta(r, r, r); + bv2.min_ = center2 - delta; + bv2.max_ = center2 + delta; + } +}; + +template +class Converter, OBB> +{ +public: + static void convert(const AABB& bv1, const Transform3& tf1, OBB& bv2) + { + /* + bv2.To = tf1 * bv1.center()); + + /// Sort the AABB edges so that AABB extents are ordered. + S d[3] = {bv1.width(), bv1.height(), bv1.depth() }; + std::size_t id[3] = {0, 1, 2}; + + for(std::size_t i = 1; i < 3; ++i) + { + for(std::size_t j = i; j > 0; --j) + { + if(d[j] > d[j-1]) + { + { + S tmp = d[j]; + d[j] = d[j-1]; + d[j-1] = tmp; + } + { + std::size_t tmp = id[j]; + id[j] = id[j-1]; + id[j-1] = tmp; + } + } + } + } + + Vector3 extent = (bv1.max_ - bv1.min_) * 0.5; + bv2.extent = Vector3(extent[id[0]], extent[id[1]], extent[id[2]]); + const Matrix3& R = tf1.linear(); + bool left_hand = (id[0] == (id[1] + 1) % 3); + bv2.axis[0] = left_hand ? -R.col(id[0]) : R.col(id[0]); + bv2.axis[1] = R.col(id[1]); + bv2.axis[2] = R.col(id[2]); + */ + + bv2.To.noalias() = tf1 * bv1.center(); + bv2.extent.noalias() = (bv1.max_ - bv1.min_) * 0.5; + bv2.axis = tf1.linear(); + } +}; + +template +class Converter, OBB> +{ +public: + static void convert(const OBB& bv1, const Transform3& tf1, OBB& bv2) + { + bv2.extent = bv1.extent; + bv2.To.noalias() = tf1 * bv1.To; + bv2.axis.noalias() = tf1.linear() * bv1.axis; + } +}; + +template +class Converter, OBB> +{ +public: + static void convert(const OBBRSS& bv1, const Transform3& tf1, OBB& bv2) + { + Converter, OBB>::convert(bv1.obb, tf1, bv2); + } +}; + +template +class Converter, OBB> +{ +public: + static void convert(const RSS& bv1, const Transform3& tf1, OBB& bv2) + { + bv2.extent << bv1.l[0] * 0.5 + bv1.r, bv1.l[1] * 0.5 + bv1.r, bv1.r; + bv2.To.noalias() = tf1 * bv1.To; + bv2.axis.noalias() = tf1.linear() * bv1.axis; + } +}; + + +template +class Converter> +{ +public: + static void convert(const BV1& bv1, const Transform3& tf1, AABB& bv2) + { + const Vector3 center = bv1.center(); + S r = Vector3(bv1.width(), bv1.height(), bv1.depth()).norm() * 0.5; + Vector3 delta(r, r, r); + Vector3 center2 = tf1 * center; + bv2.min_ = center2 - delta; + bv2.max_ = center2 + delta; + } +}; + +template +class Converter> +{ +public: + static void convert(const BV1& bv1, const Transform3& tf1, OBB& bv2) + { + AABB bv; + Converter>::convert(bv1, Transform3::Identity(), bv); + Converter, OBB>::convert(bv, tf1, bv2); + } +}; + +template +class Converter, RSS> +{ +public: + static void convert(const OBB& bv1, const Transform3& tf1, RSS& bv2) + { + bv2.To.noalias() = tf1 * bv1.To; + bv2.axis.noalias() = tf1.linear() * bv1.axis; + + bv2.r = bv1.extent[2]; + bv2.l[0] = 2 * (bv1.extent[0] - bv2.r); + bv2.l[1] = 2 * (bv1.extent[1] - bv2.r); + } +}; + +template +class Converter, RSS> +{ +public: + static void convert(const RSS& bv1, const Transform3& tf1, RSS& bv2) + { + bv2.To.noalias() = tf1 * bv1.To; + bv2.axis.noalias() = tf1.linear() * bv1.axis; + + bv2.r = bv1.r; + bv2.l[0] = bv1.l[0]; + bv2.l[1] = bv1.l[1]; + } +}; + +template +class Converter, RSS> +{ +public: + static void convert(const OBBRSS& bv1, const Transform3& tf1, RSS& bv2) + { + Converter, RSS>::convert(bv1.rss, tf1, bv2); + } +}; + +template +class Converter, RSS> +{ +public: + static void convert(const AABB& bv1, const Transform3& tf1, RSS& bv2) + { + bv2.To.noalias() = tf1 * bv1.center(); + + /// Sort the AABB edges so that AABB extents are ordered. + S d[3] = {bv1.width(), bv1.height(), bv1.depth() }; + std::size_t id[3] = {0, 1, 2}; + + for(std::size_t i = 1; i < 3; ++i) + { + for(std::size_t j = i; j > 0; --j) + { + if(d[j] > d[j-1]) + { + { + S tmp = d[j]; + d[j] = d[j-1]; + d[j-1] = tmp; + } + { + std::size_t tmp = id[j]; + id[j] = id[j-1]; + id[j-1] = tmp; + } + } + } + } + + Vector3 extent = (bv1.max_ - bv1.min_) * 0.5; + bv2.r = extent[id[2]]; + bv2.l[0] = (extent[id[0]] - bv2.r) * 2; + bv2.l[1] = (extent[id[1]] - bv2.r) * 2; + + const Matrix3& R = tf1.linear(); + bool left_hand = (id[0] == (id[1] + 1) % 3); + if (left_hand) + bv2.axis.col(0) = -R.col(id[0]); + else + bv2.axis.col(0) = R.col(id[0]); + bv2.axis.col(1) = R.col(id[1]); + bv2.axis.col(2) = R.col(id[2]); + } +}; + +} // namespace detail + +/// @endcond + +} // namespace fcl + +#endif diff --git a/include/fcl/BV/detail/fitter.h b/include/fcl/BV/detail/fitter.h new file mode 100644 index 000000000..79c70d4d6 --- /dev/null +++ b/include/fcl/BV/detail/fitter.h @@ -0,0 +1,516 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_BV_DETAIL_FITTER_H +#define FCL_BV_DETAIL_FITTER_H + +#include "fcl/BV/OBB.h" +#include "fcl/BV/RSS.h" +#include "fcl/BV/OBBRSS.h" +#include "fcl/BV/kIOS.h" + +namespace fcl { +namespace detail { + +//============================================================================== +template +struct Fitter +{ + static void fit(Vector3* ps, int n, BV& bv) + { + for(int i = 0; i < n; ++i) + bv += ps[i]; + } +}; + +namespace OBB_fit_functions +{ + +template +void fit1(Vector3* ps, OBB& bv) +{ + bv.To = ps[0]; + bv.axis.setIdentity(); + bv.extent.setZero(); +} + +template +void fit2(Vector3* ps, OBB& bv) +{ + const Vector3& p1 = ps[0]; + const Vector3& p2 = ps[1]; + Vector3 p1p2 = p1 - p2; + S len_p1p2 = p1p2.norm(); + p1p2.normalize(); + + bv.axis.col(0) = p1p2; + generateCoordinateSystem(bv.axis); + + bv.extent << len_p1p2 * 0.5, 0, 0; + bv.To.noalias() = 0.5 * (p1 + p2); +} + +template +void fit3(Vector3* ps, OBB& bv) +{ + const Vector3& p1 = ps[0]; + const Vector3& p2 = ps[1]; + const Vector3& p3 = ps[2]; + Vector3 e[3]; + e[0] = p1 - p2; + e[1] = p2 - p3; + e[2] = p3 - p1; + S len[3]; + len[0] = e[0].squaredNorm(); + len[1] = e[1].squaredNorm(); + len[2] = e[2].squaredNorm(); + + int imax = 0; + if(len[1] > len[0]) imax = 1; + if(len[2] > len[imax]) imax = 2; + + bv.axis.col(2).noalias() = e[0].cross(e[1]); + bv.axis.col(2).normalize(); + bv.axis.col(0) = e[imax]; + bv.axis.col(0).normalize(); + bv.axis.col(1).noalias() = bv.axis.col(2).cross(bv.axis.col(0)); + + getExtentAndCenter(ps, nullptr, nullptr, nullptr, 3, bv.axis, bv.To, bv.extent); +} + +template +void fit6(Vector3* ps, OBB& bv) +{ + OBB bv1, bv2; + fit3(ps, bv1); + fit3(ps + 3, bv2); + bv = bv1 + bv2; +} + +template +void fitn(Vector3* ps, int n, OBB& bv) +{ + Matrix3 M; + Matrix3 E; + Vector3 s = Vector3::Zero(); // three eigen values + + getCovariance(ps, nullptr, nullptr, nullptr, n, M); + eigen_old(M, s, E); + axisFromEigen(E, s, bv.axis); + + // set obb centers and extensions + getExtentAndCenter(ps, nullptr, nullptr, nullptr, n, bv.axis, bv.To, bv.extent); +} + +} // namespace OBB_fit_functions + + +namespace RSS_fit_functions { + +template +void fit1(Vector3* ps, RSS& bv) +{ + bv.To = ps[0]; + bv.axis.setIdentity(); + bv.l[0] = 0; + bv.l[1] = 0; + bv.r = 0; +} + +template +void fit2(Vector3* ps, RSS& bv) +{ + const Vector3& p1 = ps[0]; + const Vector3& p2 = ps[1]; + Vector3 p1p2 = p1 - p2; + S len_p1p2 = p1p2.norm(); + p1p2.normalize(); + + bv.axis.col(0) = p1p2; + generateCoordinateSystem(bv.axis); + bv.l[0] = len_p1p2; + bv.l[1] = 0; + + bv.To = p2; + bv.r = 0; +} + +template +void fit3(Vector3* ps, RSS& bv) +{ + const Vector3& p1 = ps[0]; + const Vector3& p2 = ps[1]; + const Vector3& p3 = ps[2]; + Vector3 e[3]; + e[0] = p1 - p2; + e[1] = p2 - p3; + e[2] = p3 - p1; + S len[3]; + len[0] = e[0].squaredNorm(); + len[1] = e[1].squaredNorm(); + len[2] = e[2].squaredNorm(); + + int imax = 0; + if(len[1] > len[0]) imax = 1; + if(len[2] > len[imax]) imax = 2; + + bv.axis.col(2).noalias() = e[0].cross(e[1]).normalized(); + bv.axis.col(0).noalias() = e[imax].normalized(); + bv.axis.col(1).noalias() = bv.axis.col(2).cross(bv.axis.col(0)); + + getRadiusAndOriginAndRectangleSize(ps, nullptr, nullptr, nullptr, 3, bv.axis, bv.To, bv.l, bv.r); +} + +template +void fit6(Vector3* ps, RSS& bv) +{ + RSS bv1, bv2; + fit3(ps, bv1); + fit3(ps + 3, bv2); + bv = bv1 + bv2; +} + +template +void fitn(Vector3* ps, int n, RSS& bv) +{ + Matrix3 M; // row first matrix + Matrix3 E; // row first eigen-vectors + Vector3 s = Vector3::Zero(); + + getCovariance(ps, nullptr, nullptr, nullptr, n, M); + eigen_old(M, s, E); + axisFromEigen(E, s, bv.axis); + + // set rss origin, rectangle size and radius + getRadiusAndOriginAndRectangleSize(ps, nullptr, nullptr, nullptr, n, bv.axis, bv.To, bv.l, bv.r); +} + +} // namespace RSS_fit_functions + +namespace kIOS_fit_functions { + +template +void fit1(Vector3* ps, kIOS& bv) +{ + bv.num_spheres = 1; + bv.spheres[0].o = ps[0]; + bv.spheres[0].r = 0; + + bv.obb.axis.setIdentity(); + bv.obb.extent.setZero(); + bv.obb.To = ps[0]; +} + +template +void fit2(Vector3* ps, kIOS& bv) +{ + bv.num_spheres = 5; + + const Vector3& p1 = ps[0]; + const Vector3& p2 = ps[1]; + Vector3 p1p2 = p1 - p2; + S len_p1p2 = p1p2.norm(); + p1p2.normalize(); + + bv.obb.axis.col(0) = p1p2; + generateCoordinateSystem(bv.obb.axis); + + S r0 = len_p1p2 * 0.5; + bv.obb.extent << r0, 0, 0; + bv.obb.To.noalias() = (p1 + p2) * 0.5; + + bv.spheres[0].o = bv.obb.To; + bv.spheres[0].r = r0; + + S r1 = r0 * kIOS::invSinA(); + S r1cosA = r1 * kIOS::cosA(); + bv.spheres[1].r = r1; + bv.spheres[2].r = r1; + Vector3 delta = bv.obb.axis.col(1) * r1cosA; + bv.spheres[1].o = bv.spheres[0].o - delta; + bv.spheres[2].o = bv.spheres[0].o + delta; + + bv.spheres[3].r = r1; + bv.spheres[4].r = r1; + delta.noalias() = bv.obb.axis.col(2) * r1cosA; + bv.spheres[3].o = bv.spheres[0].o - delta; + bv.spheres[4].o = bv.spheres[0].o + delta; +} + +template +void fit3(Vector3* ps, kIOS& bv) +{ + bv.num_spheres = 3; + + const Vector3& p1 = ps[0]; + const Vector3& p2 = ps[1]; + const Vector3& p3 = ps[2]; + Vector3 e[3]; + e[0] = p1 - p2; + e[1] = p2 - p3; + e[2] = p3 - p1; + S len[3]; + len[0] = e[0].squaredNorm(); + len[1] = e[1].squaredNorm(); + len[2] = e[2].squaredNorm(); + + int imax = 0; + if(len[1] > len[0]) imax = 1; + if(len[2] > len[imax]) imax = 2; + + bv.obb.axis.col(2).noalias() = e[0].cross(e[1]).normalized(); + bv.obb.axis.col(0) = e[imax].normalized(); + bv.obb.axis.col(1).noalias() = bv.obb.axis.col(2).cross(bv.obb.axis.col(0)); + + getExtentAndCenter(ps, nullptr, nullptr, nullptr, 3, bv.obb.axis, bv.obb.To, bv.obb.extent); + + // compute radius and center + S r0; + Vector3 center; + circumCircleComputation(p1, p2, p3, center, r0); + + bv.spheres[0].o = center; + bv.spheres[0].r = r0; + + S r1 = r0 * kIOS::invSinA(); + Vector3 delta = bv.obb.axis.col(2) * (r1 * kIOS::cosA()); + + bv.spheres[1].r = r1; + bv.spheres[1].o = center - delta; + bv.spheres[2].r = r1; + bv.spheres[2].o = center + delta; +} + +template +void fitn(Vector3* ps, int n, kIOS& bv) +{ + Matrix3 M; + Matrix3 E; + Vector3 s = Vector3::Zero(); // three eigen values; + + getCovariance(ps, nullptr, nullptr, nullptr, n, M); + eigen_old(M, s, E); + axisFromEigen(E, s, bv.obb.axis); + + getExtentAndCenter(ps, nullptr, nullptr, nullptr, n, bv.obb.axis, bv.obb.To, bv.obb.extent); + + // get center and extension + const Vector3& center = bv.obb.To; + const Vector3& extent = bv.obb.extent; + S r0 = maximumDistance(ps, nullptr, nullptr, nullptr, n, center); + + // decide the k in kIOS + if(extent[0] > kIOS::ratio() * extent[2]) + { + if(extent[0] > kIOS::ratio() * extent[1]) bv.num_spheres = 5; + else bv.num_spheres = 3; + } + else bv.num_spheres = 1; + + + bv.spheres[0].o = center; + bv.spheres[0].r = r0; + + if(bv.num_spheres >= 3) + { + S r10 = sqrt(r0 * r0 - extent[2] * extent[2]) * kIOS::invSinA(); + Vector3 delta = bv.obb.axis.col(2) * (r10 * kIOS::cosA() - extent[2]); + bv.spheres[1].o = center - delta; + bv.spheres[2].o = center + delta; + + S r11 = 0, r12 = 0; + r11 = maximumDistance(ps, nullptr, nullptr, nullptr, n, bv.spheres[1].o); + r12 = maximumDistance(ps, nullptr, nullptr, nullptr, n, bv.spheres[2].o); + bv.spheres[1].o.noalias() += bv.obb.axis.col(2) * (-r10 + r11); + bv.spheres[2].o.noalias() += bv.obb.axis.col(2) * (r10 - r12); + + bv.spheres[1].r = r10; + bv.spheres[2].r = r10; + } + + if(bv.num_spheres >= 5) + { + S r10 = bv.spheres[1].r; + Vector3 delta = bv.obb.axis.col(1) * (sqrt(r10 * r10 - extent[0] * extent[0] - extent[2] * extent[2]) - extent[1]); + bv.spheres[3].o = bv.spheres[0].o - delta; + bv.spheres[4].o = bv.spheres[0].o + delta; + + S r21 = 0, r22 = 0; + r21 = maximumDistance(ps, nullptr, nullptr, nullptr, n, bv.spheres[3].o); + r22 = maximumDistance(ps, nullptr, nullptr, nullptr, n, bv.spheres[4].o); + + bv.spheres[3].o.noalias() += bv.obb.axis.col(1) * (-r10 + r21); + bv.spheres[4].o.noalias() += bv.obb.axis.col(1) * (r10 - r22); + + bv.spheres[3].r = r10; + bv.spheres[4].r = r10; + } +} + +} // namespace kIOS_fit_functions + +namespace OBBRSS_fit_functions { + +template +void fit1(Vector3* ps, OBBRSS& bv) +{ + OBB_fit_functions::fit1(ps, bv.obb); + RSS_fit_functions::fit1(ps, bv.rss); +} + +template +void fit2(Vector3* ps, OBBRSS& bv) +{ + OBB_fit_functions::fit2(ps, bv.obb); + RSS_fit_functions::fit2(ps, bv.rss); +} + +template +void fit3(Vector3* ps, OBBRSS& bv) +{ + OBB_fit_functions::fit3(ps, bv.obb); + RSS_fit_functions::fit3(ps, bv.rss); +} + +template +void fitn(Vector3* ps, int n, OBBRSS& bv) +{ + OBB_fit_functions::fitn(ps, n, bv.obb); + RSS_fit_functions::fitn(ps, n, bv.rss); +} + +} // namespace OBBRSS_fit_functions + +//============================================================================== +template +struct Fitter> +{ + static void fit(Vector3* ps, int n, OBB& bv) + { + switch(n) + { + case 1: + OBB_fit_functions::fit1(ps, bv); + break; + case 2: + OBB_fit_functions::fit2(ps, bv); + break; + case 3: + OBB_fit_functions::fit3(ps, bv); + break; + case 6: + OBB_fit_functions::fit6(ps, bv); + break; + default: + OBB_fit_functions::fitn(ps, n, bv); + } + } +}; + +//============================================================================== +template +struct Fitter> +{ + static void fit(Vector3* ps, int n, RSS& bv) + { + switch(n) + { + case 1: + RSS_fit_functions::fit1(ps, bv); + break; + case 2: + RSS_fit_functions::fit2(ps, bv); + break; + case 3: + RSS_fit_functions::fit3(ps, bv); + break; + default: + RSS_fit_functions::fitn(ps, n, bv); + } + } +}; + +//============================================================================== +template +struct Fitter> +{ + static void fit(Vector3* ps, int n, kIOS& bv) + { + switch(n) + { + case 1: + kIOS_fit_functions::fit1(ps, bv); + break; + case 2: + kIOS_fit_functions::fit2(ps, bv); + break; + case 3: + kIOS_fit_functions::fit3(ps, bv); + break; + default: + kIOS_fit_functions::fitn(ps, n, bv); + } + } +}; + +//============================================================================== +template +struct Fitter> +{ + static void fit(Vector3* ps, int n, OBBRSS& bv) + { + switch(n) + { + case 1: + OBBRSS_fit_functions::fit1(ps, bv); + break; + case 2: + OBBRSS_fit_functions::fit2(ps, bv); + break; + case 3: + OBBRSS_fit_functions::fit3(ps, bv); + break; + default: + OBBRSS_fit_functions::fitn(ps, n, bv); + } + } +}; + +} // namespace detail +} // namespace fcl + +#endif diff --git a/include/fcl/BV/detail/utility.h b/include/fcl/BV/detail/utility.h new file mode 100644 index 000000000..a39341307 --- /dev/null +++ b/include/fcl/BV/detail/utility.h @@ -0,0 +1,374 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + + +#ifndef FCL_BV_DETAIL_UTILITY_H +#define FCL_BV_DETAIL_UTILITY_H + +#include "fcl/data_types.h" +#include "fcl/math/triangle.h" + +namespace fcl +{ +namespace detail +{ + +/// \brief Compute the bounding volume extent and center for a set or subset of +/// points. The bounding volume axes are known. +template +void getExtentAndCenter_pointcloud( + Vector3* ps, + Vector3* ps2, + Triangle* ts, + unsigned int* indices, + int n, + const Matrix3& axis, + Vector3& center, + Vector3& extent); + +/// \brief Compute the bounding volume extent and center for a set or subset of +/// points. The bounding volume axes are known. +template +void getExtentAndCenter_pointcloud( + Vector3* ps, + Vector3* ps2, + Triangle* ts, + unsigned int* indices, + int n, + Transform3& tf, + Vector3& extent); + +/// \brief Compute the bounding volume extent and center for a set or subset of +/// points. The bounding volume axes are known. +template +void getExtentAndCenter_mesh( + Vector3* ps, + Vector3* ps2, + Triangle* ts, + unsigned int* indices, + int n, + const Matrix3& axis, + Vector3& center, + Vector3& extent); + +/// \brief Compute the bounding volume extent and center for a set or subset of +/// points. The bounding volume axes are known. +template +void getExtentAndCenter_mesh( + Vector3* ps, + Vector3* ps2, + Triangle* ts, + unsigned int* indices, + int n, + Transform3& tf, + Vector3& extent); + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +void getExtentAndCenter_pointcloud( + Vector3* ps, + Vector3* ps2, + unsigned int* indices, + int n, + const Matrix3& axis, + Vector3& center, + Vector3& extent) +{ + bool indirect_index = true; + if(!indices) indirect_index = false; + + auto real_max = std::numeric_limits::max(); + + Vector3 min_coord = Vector3::Constant(real_max); + Vector3 max_coord = Vector3::Constant(-real_max); + + for(int i = 0; i < n; ++i) + { + int index = indirect_index ? indices[i] : i; + + const Vector3& p = ps[index]; + Vector3 proj( + axis.col(0).dot(p), + axis.col(1).dot(p), + axis.col(2).dot(p)); + + for(int j = 0; j < 3; ++j) + { + if(proj[j] > max_coord[j]) + max_coord[j] = proj[j]; + + if(proj[j] < min_coord[j]) + min_coord[j] = proj[j]; + } + + if(ps2) + { + const Vector3& v = ps2[index]; + Vector3 proj( + axis.col(0).dot(v), + axis.col(1).dot(v), + axis.col(2).dot(v)); + + for(int j = 0; j < 3; ++j) + { + if(proj[j] > max_coord[j]) + max_coord[j] = proj[j]; + + if(proj[j] < min_coord[j]) + min_coord[j] = proj[j]; + } + } + } + + const Vector3 o = (max_coord + min_coord) / 2; + center.noalias() = axis * o; + extent.noalias() = (max_coord - min_coord) * 0.5; +} + +//============================================================================== +template +void getExtentAndCenter_pointcloud( + Vector3* ps, + Vector3* ps2, + unsigned int* indices, + int n, + Transform3& tf, + Vector3& extent) +{ + bool indirect_index = true; + if(!indices) indirect_index = false; + + auto real_max = std::numeric_limits::max(); + + Vector3 min_coord = Vector3::Constant(real_max); + Vector3 max_coord = Vector3::Constant(-real_max); + + for(int i = 0; i < n; ++i) + { + int index = indirect_index ? indices[i] : i; + + const Vector3& p = ps[index]; + Vector3 proj( + tf.linear().col(0).dot(p), + tf.linear().col(1).dot(p), + tf.linear().col(2).dot(p)); + + for(int j = 0; j < 3; ++j) + { + if(proj[j] > max_coord[j]) + max_coord[j] = proj[j]; + + if(proj[j] < min_coord[j]) + min_coord[j] = proj[j]; + } + + if(ps2) + { + const Vector3& v = ps2[index]; + Vector3 proj( + tf.linear().col(0).dot(v), + tf.linear().col(1).dot(v), + tf.linear().col(2).dot(v)); + + for(int j = 0; j < 3; ++j) + { + if(proj[j] > max_coord[j]) + max_coord[j] = proj[j]; + + if(proj[j] < min_coord[j]) + min_coord[j] = proj[j]; + } + } + } + + const Vector3 o = (max_coord + min_coord) / 2; + tf.translation().noalias() = tf.linear() * o; + extent.noalias() = (max_coord - min_coord) / 2; +} + +//============================================================================== +template +void getExtentAndCenter_mesh(Vector3* ps, + Vector3* ps2, + Triangle* ts, + unsigned int* indices, + int n, + const Matrix3& axis, + Vector3& center, + Vector3& extent) +{ + bool indirect_index = true; + if(!indices) indirect_index = false; + + auto real_max = std::numeric_limits::max(); + + Vector3 min_coord = Vector3::Constant(real_max); + Vector3 max_coord = Vector3::Constant(-real_max); + + for(int i = 0; i < n; ++i) + { + unsigned int index = indirect_index? indices[i] : i; + const Triangle& t = ts[index]; + + for(int j = 0; j < 3; ++j) + { + int point_id = t[j]; + const Vector3& p = ps[point_id]; + Vector3 proj( + axis.col(0).dot(p), + axis.col(1).dot(p), + axis.col(2).dot(p)); + + for(int k = 0; k < 3; ++k) + { + if(proj[k] > max_coord[k]) + max_coord[k] = proj[k]; + + if(proj[k] < min_coord[k]) + min_coord[k] = proj[k]; + } + } + + if(ps2) + { + for(int j = 0; j < 3; ++j) + { + int point_id = t[j]; + const Vector3& p = ps2[point_id]; + Vector3 proj( + axis.col(0).dot(p), + axis.col(1).dot(p), + axis.col(2).dot(p)); + + for(int k = 0; k < 3; ++k) + { + if(proj[k] > max_coord[k]) + max_coord[k] = proj[k]; + + if(proj[k] < min_coord[k]) + min_coord[k] = proj[k]; + } + } + } + } + + const Vector3 o = (max_coord + min_coord) / 2; + center.noalias() = axis * o; + extent.noalias() = (max_coord - min_coord) / 2; +} + +//============================================================================== +template +void getExtentAndCenter_mesh( + Vector3* ps, + Vector3* ps2, + Triangle* ts, + unsigned int* indices, + int n, + Transform3& tf, + Vector3& extent) +{ + bool indirect_index = true; + if(!indices) indirect_index = false; + + auto real_max = std::numeric_limits::max(); + + Vector3 min_coord = Vector3::Constant(real_max); + Vector3 max_coord = Vector3::Constant(-real_max); + + for(int i = 0; i < n; ++i) + { + const unsigned int index = indirect_index? indices[i] : i; + const Triangle& t = ts[index]; + + for(int j = 0; j < 3; ++j) + { + const int point_id = t[j]; + const Vector3& p = ps[point_id]; + const Vector3 proj( + tf.linear().col(0).dot(p), + tf.linear().col(1).dot(p), + tf.linear().col(2).dot(p)); + + for(int k = 0; k < 3; ++k) + { + if(proj[k] > max_coord[k]) + max_coord[k] = proj[k]; + + if(proj[k] < min_coord[k]) + min_coord[k] = proj[k]; + } + } + + if(ps2) + { + for(int j = 0; j < 3; ++j) + { + const int point_id = t[j]; + const Vector3& p = ps2[point_id]; + const Vector3 proj( + tf.linear().col(0).dot(p), + tf.linear().col(1).dot(p), + tf.linear().col(2).dot(p)); + + for(int k = 0; k < 3; ++k) + { + if(proj[k] > max_coord[k]) + max_coord[k] = proj[k]; + + if(proj[k] < min_coord[k]) + min_coord[k] = proj[k]; + } + } + } + } + + const Vector3 o = (max_coord + min_coord) * 0.5; + tf.translation().noalias() = tf.linear() * o; + extent.noalias() = (max_coord - min_coord) * 0.5; +} + +} // namespace detail +} // namespace fcl + +#endif diff --git a/src/articulated_model/link.cpp b/include/fcl/BV/fit.h similarity index 66% rename from src/articulated_model/link.cpp rename to include/fcl/BV/fit.h index 59dcd4335..781b51da3 100644 --- a/src/articulated_model/link.cpp +++ b/include/fcl/BV/fit.h @@ -33,52 +33,33 @@ * POSSIBILITY OF SUCH DAMAGE. */ -/** \author Dalibor Matura, Jia Pan */ +/** \author Jia Pan */ -#include "fcl/articulated_model/link.h" +#ifndef FCL_BV_FIT_H +#define FCL_BV_FIT_H -#include "fcl/articulated_model/joint.h" +#include "fcl/BV/detail/fitter.h" namespace fcl { -Link::Link(const std::string& name) : name_(name) -{} +/// @brief Compute a bounding volume that fits a set of n points. +template +void fit(Vector3* ps, int n, BV& bv); -const std::string& Link::getName() const -{ - return name_; -} - -void Link::setName(const std::string& name) -{ - name_ = name; -} - -void Link::addChildJoint(const std::shared_ptr& joint) -{ - children_joints_.push_back(joint); -} - -void Link::setParentJoint(const std::shared_ptr& joint) -{ - parent_joint_ = joint; -} +//============================================================================// +// // +// Implementations // +// // +//============================================================================// -void Link::addObject(const std::shared_ptr& object) +//============================================================================== +template +void fit(Vector3* ps, int n, BV& bv) { - objects_.push_back(object); + detail::Fitter::fit(ps, n, bv); } -std::size_t Link::getNumChildJoints() const -{ - return children_joints_.size(); -} +} // namespace fcl -std::size_t Link::getNumObjects() const -{ - return objects_.size(); -} - - -} +#endif diff --git a/include/fcl/BV/kDOP.h b/include/fcl/BV/kDOP.h index b88e97e48..a2a1bd606 100644 --- a/include/fcl/BV/kDOP.h +++ b/include/fcl/BV/kDOP.h @@ -35,10 +35,11 @@ /** \author Jia Pan */ -#ifndef FCL_KDOP_H -#define FCL_KDOP_H +#ifndef FCL_BV_KDOP_H +#define FCL_BV_KDOP_H #include +#include #include "fcl/data_types.h" @@ -79,97 +80,397 @@ namespace fcl /// (-1, -1, 1) and (1, 1, -1) --> indices 9 and 21 /// (-1, 1, -1) and (1, -1, 1) --> indices 10 and 22 /// (1, -1, -1) and (-1, 1, 1) --> indices 11 and 23 -template +template class KDOP { public: + using S = S_; + /// @brief Creating kDOP containing nothing KDOP(); /// @brief Creating kDOP containing only one point - KDOP(const Vector3d& v); + KDOP(const Vector3& v); /// @brief Creating kDOP containing two points - KDOP(const Vector3d& a, const Vector3d& b); + KDOP(const Vector3& a, const Vector3& b); /// @brief Check whether two KDOPs are overlapped - bool overlap(const KDOP& other) const; + bool overlap(const KDOP& other) const; //// @brief Check whether one point is inside the KDOP - bool inside(const Vector3d& p) const; + bool inside(const Vector3& p) const; /// @brief Merge the point and the KDOP - KDOP& operator += (const Vector3d& p); + KDOP& operator += (const Vector3& p); /// @brief Merge two KDOPs - KDOP& operator += (const KDOP& other); + KDOP& operator += (const KDOP& other); /// @brief Create a KDOP by mergin two KDOPs - KDOP operator + (const KDOP& other) const; + KDOP operator + (const KDOP& other) const; /// @brief The (AABB) width - inline FCL_REAL width() const + S width() const; + + /// @brief The (AABB) height + S height() const; + + /// @brief The (AABB) depth + S depth() const; + + /// @brief The (AABB) volume + S volume() const; + + /// @brief Size of the kDOP (used in BV_Splitter to order two kDOPs) + S size() const; + + /// @brief The (AABB) center + Vector3 center() const; + + /// @brief The distance between two KDOP. Not implemented. + S distance( + const KDOP& other, + Vector3* P = nullptr, Vector3* Q = nullptr) const; + +private: + /// @brief Origin's distances to N KDOP planes + S dist_[N]; + +public: + S dist(std::size_t i) const; + + S& dist(std::size_t i); + +}; + +template +using KDOPf = KDOP; +template +using KDOPd = KDOP; + +/// @brief Find the smaller and larger one of two values +template +void minmax(S a, S b, S& minv, S& maxv); + +/// @brief Merge the interval [minv, maxv] and value p/ +template +void minmax(S p, S& minv, S& maxv); + +/// @brief Compute the distances to planes with normals from KDOP vectors except +/// those of AABB face planes +template +void getDistances(const Vector3& p, S* d); + +/// @brief translate the KDOP BV +template +KDOP translate( + const KDOP& bv, const Eigen::MatrixBase& t); + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +KDOP::KDOP() +{ + static_assert(N == 16 || N == 18 || N == 24, "N should be 16, 18, or 24"); + + S real_max = std::numeric_limits::max(); + for(std::size_t i = 0; i < N / 2; ++i) { - return dist_[N / 2] - dist_[0]; + dist_[i] = real_max; + dist_[i + N / 2] = -real_max; } +} - /// @brief The (AABB) height - inline FCL_REAL height() const +//============================================================================== +template +KDOP::KDOP(const Vector3& v) +{ + for(std::size_t i = 0; i < 3; ++i) { - return dist_[N / 2 + 1] - dist_[1]; + dist_[i] = dist_[N / 2 + i] = v[i]; } - /// @brief The (AABB) depth - inline FCL_REAL depth() const + S d[(N - 6) / 2]; + getDistances(v, d); + for(std::size_t i = 0; i < (N - 6) / 2; ++i) { - return dist_[N / 2 + 2] - dist_[2]; + dist_[3 + i] = dist_[3 + i + N / 2] = d[i]; } +} - /// @brief The (AABB) volume - inline FCL_REAL volume() const +//============================================================================== +template +KDOP::KDOP(const Vector3& a, const Vector3& b) +{ + for(std::size_t i = 0; i < 3; ++i) { - return width() * height() * depth(); + minmax(a[i], b[i], dist_[i], dist_[i + N / 2]); } - /// @brief Size of the kDOP (used in BV_Splitter to order two kDOPs) - inline FCL_REAL size() const + S ad[(N - 6) / 2], bd[(N - 6) / 2]; + getDistances(a, ad); + getDistances(b, bd); + for(std::size_t i = 0; i < (N - 6) / 2; ++i) { - return width() * width() + height() * height() + depth() * depth(); + minmax(ad[i], bd[i], dist_[3 + i], dist_[3 + i + N / 2]); } +} - /// @brief The (AABB) center - inline Vector3d center() const +//============================================================================== +template +bool KDOP::overlap(const KDOP& other) const +{ + for(std::size_t i = 0; i < N / 2; ++i) { - return Vector3d(dist_[0] + dist_[N / 2], dist_[1] + dist_[N / 2 + 1], dist_[2] + dist_[N / 2 + 2]) * 0.5; + if(dist_[i] > other.dist_[i + N / 2]) return false; + if(dist_[i + N / 2] < other.dist_[i]) return false; } - /// @brief The distance between two KDOP. Not implemented. - FCL_REAL distance(const KDOP& other, Vector3d* P = NULL, Vector3d* Q = NULL) const; + return true; +} -private: - /// @brief Origin's distances to N KDOP planes - FCL_REAL dist_[N]; +//============================================================================== +template +bool KDOP::inside(const Vector3& p) const +{ + for(std::size_t i = 0; i < 3; ++i) + { + if(p[i] < dist_[i] || p[i] > dist_[i + N / 2]) + return false; + } -public: - inline FCL_REAL dist(std::size_t i) const + S d[(N - 6) / 2]; + getDistances(p, d); + for(std::size_t i = 0; i < (N - 6) / 2; ++i) { - return dist_[i]; + if(d[i] < dist_[3 + i] || d[i] > dist_[i + 3 + N / 2]) + return false; } - inline FCL_REAL& dist(std::size_t i) + return true; +} + +//============================================================================== +template +KDOP& KDOP::operator += (const Vector3& p) +{ + for(std::size_t i = 0; i < 3; ++i) { - return dist_[i]; + minmax(p[i], dist_[i], dist_[N / 2 + i]); } + S pd[(N - 6) / 2]; + getDistances(p, pd); + for(std::size_t i = 0; i < (N - 6) / 2; ++i) + { + minmax(pd[i], dist_[3 + i], dist_[3 + N / 2 + i]); + } -}; + return *this; +} +//============================================================================== +template +KDOP& KDOP::operator += (const KDOP& other) +{ + for(std::size_t i = 0; i < N / 2; ++i) + { + dist_[i] = std::min(other.dist_[i], dist_[i]); + dist_[i + N / 2] = std::max(other.dist_[i + N / 2], dist_[i + N / 2]); + } + return *this; +} -/// @brief translate the KDOP BV -template -KDOP translate(const KDOP& bv, const Vector3d& t); +//============================================================================== +template +KDOP KDOP::operator + (const KDOP& other) const +{ + KDOP res(*this); + return res += other; +} +//============================================================================== +template +S KDOP::width() const +{ + return dist_[N / 2] - dist_[0]; } +//============================================================================== +template +S KDOP::height() const +{ + return dist_[N / 2 + 1] - dist_[1]; +} + +//============================================================================== +template +S KDOP::depth() const +{ + return dist_[N / 2 + 2] - dist_[2]; +} + +//============================================================================== +template +S KDOP::volume() const +{ + return width() * height() * depth(); +} + +//============================================================================== +template +S KDOP::size() const +{ + return width() * width() + height() * height() + depth() * depth(); +} + +//============================================================================== +template +Vector3 KDOP::center() const +{ + return Vector3(dist_[0] + dist_[N / 2], dist_[1] + dist_[N / 2 + 1], dist_[2] + dist_[N / 2 + 2]) * 0.5; +} + +//============================================================================== +template +S KDOP::distance(const KDOP& other, Vector3* P, Vector3* Q) const +{ + std::cerr << "KDOP distance not implemented!" << std::endl; + return 0.0; +} + +//============================================================================== +template +S KDOP::dist(std::size_t i) const +{ + return dist_[i]; +} + +//============================================================================== +template +S& KDOP::dist(std::size_t i) +{ + return dist_[i]; +} + +//============================================================================== +template +KDOP translate( + const KDOP& bv, const Eigen::MatrixBase& t) +{ + KDOP res(bv); + for(std::size_t i = 0; i < 3; ++i) + { + res.dist(i) += t[i]; + res.dist(N / 2 + i) += t[i]; + } + + S d[(N - 6) / 2]; + getDistances(t, d); + for(std::size_t i = 0; i < (N - 6) / 2; ++i) + { + res.dist(3 + i) += d[i]; + res.dist(3 + i + N / 2) += d[i]; + } + + return res; +} + +//============================================================================== +template +void minmax(S a, S b, S& minv, S& maxv) +{ + if(a > b) + { + minv = b; + maxv = a; + } + else + { + minv = a; + maxv = b; + } +} + +//============================================================================== +template +void minmax(S p, S& minv, S& maxv) +{ + if(p > maxv) maxv = p; + if(p < minv) minv = p; +} + +//============================================================================== +template +struct GetDistancesImpl +{ + static void run(const Vector3& /*p*/, S* /*d*/) + { + // Do nothing + } +}; + +//============================================================================== +template +void getDistances(const Vector3& p, S* d) +{ + GetDistancesImpl::run(p, d); +} + +//============================================================================== +template +struct GetDistancesImpl +{ + static void run(const Vector3& p, S* d) + { + d[0] = p[0] + p[1]; + d[1] = p[0] + p[2]; + d[2] = p[1] + p[2]; + d[3] = p[0] - p[1]; + d[4] = p[0] - p[2]; + } +}; + +//============================================================================== +template +struct GetDistancesImpl +{ + static void run(const Vector3& p, S* d) + { + d[0] = p[0] + p[1]; + d[1] = p[0] + p[2]; + d[2] = p[1] + p[2]; + d[3] = p[0] - p[1]; + d[4] = p[0] - p[2]; + d[5] = p[1] - p[2]; + } +}; + +//============================================================================== +template +struct GetDistancesImpl +{ + static void run(const Vector3& p, S* d) + { + d[0] = p[0] + p[1]; + d[1] = p[0] + p[2]; + d[2] = p[1] + p[2]; + d[3] = p[0] - p[1]; + d[4] = p[0] - p[2]; + d[5] = p[1] - p[2]; + d[6] = p[0] + p[1] - p[2]; + d[7] = p[0] + p[2] - p[1]; + d[8] = p[1] + p[2] - p[0]; + } +}; + +} // namespace fcl + #endif diff --git a/include/fcl/BV/kIOS.h b/include/fcl/BV/kIOS.h index 0882815ad..8bc6e6841 100644 --- a/include/fcl/BV/kIOS.h +++ b/include/fcl/BV/kIOS.h @@ -35,125 +35,428 @@ /** \author Jia Pan */ -#ifndef FCL_KIOS_H -#define FCL_KIOS_H +#ifndef FCL_BV_KIOS_H +#define FCL_BV_KIOS_H #include "fcl/BV/OBB.h" - namespace fcl { /// @brief A class describing the kIOS collision structure, which is a set of spheres. +template class kIOS { /// @brief One sphere in kIOS struct kIOS_Sphere { - Vector3d o; - FCL_REAL r; + Vector3 o; + S_ r; }; /// @brief generate one sphere enclosing two spheres - static kIOS_Sphere encloseSphere(const kIOS_Sphere& s0, const kIOS_Sphere& s1) - { - Vector3d d = s1.o - s0.o; - FCL_REAL dist2 = d.squaredNorm(); - FCL_REAL diff_r = s1.r - s0.r; - - /** The sphere with the larger radius encloses the other */ - if(diff_r * diff_r >= dist2) - { - if(s1.r > s0.r) return s1; - else return s0; - } - else /** spheres partially overlapping or disjoint */ - { - float dist = std::sqrt(dist2); - kIOS_Sphere s; - s.r = dist + s0.r + s1.r; - if(dist > 0) - s.o = s0.o + d * ((s.r - s0.r) / dist); - else - s.o = s0.o; - return s; - } - } + static kIOS_Sphere encloseSphere( + const kIOS_Sphere& s0, const kIOS_Sphere& s1); + public: - + + using S = S_; + /// @brief The (at most) five spheres for intersection kIOS_Sphere spheres[5]; /// @brief The number of spheres, no larger than 5 unsigned int num_spheres; - /// @ OBB related with kIOS - OBB obb; + /// @brief OBB related with kIOS + OBB obb; /// @brief Check collision between two kIOS - bool overlap(const kIOS& other) const; + bool overlap(const kIOS& other) const; /// @brief Check collision between two kIOS and return the overlap part. - /// For kIOS, we return nothing, as the overlappart of two kIOS usually is not an kIOS - /// @todo Not efficient. It first checks the sphere collisions and then use OBB for further culling. - bool overlap(const kIOS& other, kIOS& overlap_part) const - { - return overlap(other); - } + /// For kIOS, we return nothing, as the overlappart of two kIOS usually is not + /// an kIOS + /// @todo Not efficient. It first checks the sphere collisions and then use + /// OBB for further culling. + bool overlap(const kIOS& other, kIOS& overlap_part) const; /// @brief Check whether the kIOS contains a point - inline bool contain(const Vector3d& p) const; + inline bool contain(const Vector3& p) const; /// @brief A simple way to merge the kIOS and a point - kIOS& operator += (const Vector3d& p); + kIOS& operator += (const Vector3& p); /// @brief Merge the kIOS and another kIOS - kIOS& operator += (const kIOS& other) - { - *this = *this + other; - return *this; - } + kIOS& operator += (const kIOS& other); /// @brief Return the merged kIOS of current kIOS and the other one - kIOS operator + (const kIOS& other) const; + kIOS operator + (const kIOS& other) const; /// @brief Center of the kIOS - const Vector3d& center() const - { - return spheres[0].o; - } + const Vector3& center() const; /// @brief Width of the kIOS - FCL_REAL width() const; + S width() const; /// @brief Height of the kIOS - FCL_REAL height() const; + S height() const; /// @brief Depth of the kIOS - FCL_REAL depth() const; + S depth() const; /// @brief Volume of the kIOS - FCL_REAL volume() const; + S volume() const; /// @brief size of the kIOS (used in BV_Splitter to order two kIOSs) - FCL_REAL size() const; + S size() const; /// @brief The distance between two kIOS - FCL_REAL distance(const kIOS& other, Vector3d* P = NULL, Vector3d* Q = NULL) const; + S distance( + const kIOS& other, + Vector3* P = nullptr, Vector3* Q = nullptr) const; + + static constexpr S ratio() { return 1.5; } + static constexpr S invSinA() { return 2; } + static S cosA() { return std::sqrt(3.0) / 2.0; } }; +using kIOSf = kIOS; +using kIOSd = kIOS; /// @brief Translate the kIOS BV -kIOS translate(const kIOS& bv, const Vector3d& t); +template +kIOS translate( + const kIOS& bv, const Eigen::MatrixBase& t); + +/// @brief Check collision between two kIOSs, b1 is in configuration (R0, T0) +/// and b2 is in identity. +/// @todo Not efficient +template +bool overlap( + const Eigen::MatrixBase& R0, + const Eigen::MatrixBase& T0, + const kIOS& b1, const kIOS& b2); -/// @brief Check collision between two kIOSs, b1 is in configuration (R0, T0) and b2 is in identity. +/// @brief Check collision between two kIOSs, b1 is in configuration (R0, T0) +/// and b2 is in identity. /// @todo Not efficient -bool overlap(const Matrix3d& R0, const Vector3d& T0, const kIOS& b1, const kIOS& b2); +template +bool overlap( + const Transform3& tf, + const kIOS& b1, + const kIOS& b2); + +/// @brief Approximate distance between two kIOS bounding volumes +/// @todo P and Q is not returned, need implementation +template +S distance( + const Eigen::MatrixBase& R0, + const Eigen::MatrixBase& T0, + const kIOS& b1, + const kIOS& b2, + Vector3* P = nullptr, + Vector3* Q = nullptr); /// @brief Approximate distance between two kIOS bounding volumes /// @todo P and Q is not returned, need implementation -FCL_REAL distance(const Matrix3d& R0, const Vector3d& T0, const kIOS& b1, const kIOS& b2, Vector3d* P = NULL, Vector3d* Q = NULL); +template +S distance( + const Transform3& tf, + const kIOS& b1, + const kIOS& b2, + Vector3* P = nullptr, + Vector3* Q = nullptr); + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +typename kIOS::kIOS_Sphere kIOS::encloseSphere( + const typename kIOS::kIOS_Sphere& s0, const typename kIOS::kIOS_Sphere& s1) +{ + Vector3 d = s1.o - s0.o; + S dist2 = d.squaredNorm(); + S diff_r = s1.r - s0.r; + /** The sphere with the larger radius encloses the other */ + if(diff_r * diff_r >= dist2) + { + if(s1.r > s0.r) return s1; + else return s0; + } + else /** spheres partially overlapping or disjoint */ + { + float dist = std::sqrt(dist2); + kIOS_Sphere s; + s.r = dist + s0.r + s1.r; + if(dist > 0) + s.o = s0.o + d * ((s.r - s0.r) / dist); + else + s.o = s0.o; + return s; + } } +//============================================================================== +template +bool kIOS::overlap(const kIOS& other) const +{ + for(unsigned int i = 0; i < num_spheres; ++i) + { + for(unsigned int j = 0; j < other.num_spheres; ++j) + { + S o_dist = (spheres[i].o - other.spheres[j].o).squaredNorm(); + S sum_r = spheres[i].r + other.spheres[j].r; + if(o_dist > sum_r * sum_r) + return false; + } + } + + return obb.overlap(other.obb); + + return true; +} + +//============================================================================== +template +bool kIOS::overlap( + const kIOS& other, kIOS& /*overlap_part*/) const +{ + return overlap(other); +} + +//============================================================================== +template +bool kIOS::contain(const Vector3& p) const +{ + for(unsigned int i = 0; i < num_spheres; ++i) + { + S r = spheres[i].r; + if((spheres[i].o - p).squaredNorm() > r * r) + return false; + } + + return true; +} + +//============================================================================== +template +kIOS& kIOS::operator += (const Vector3& p) +{ + for(unsigned int i = 0; i < num_spheres; ++i) + { + S r = spheres[i].r; + S new_r_sqr = (p - spheres[i].o).squaredNorm(); + if(new_r_sqr > r * r) + { + spheres[i].r = sqrt(new_r_sqr); + } + } + + obb += p; + return *this; +} + +//============================================================================== +template +kIOS& kIOS::operator +=(const kIOS& other) +{ + *this = *this + other; + return *this; +} + +//============================================================================== +template +kIOS kIOS::operator + (const kIOS& other) const +{ + kIOS result; + unsigned int new_num_spheres = std::min(num_spheres, other.num_spheres); + for(unsigned int i = 0; i < new_num_spheres; ++i) + { + result.spheres[i] = encloseSphere(spheres[i], other.spheres[i]); + } + + result.num_spheres = new_num_spheres; + + result.obb = obb + other.obb; + + return result; +} + +//============================================================================== +template +const Vector3& kIOS::center() const +{ + return spheres[0].o; +} + +//============================================================================== +template +S kIOS::width() const +{ + return obb.width(); +} + +//============================================================================== +template +S kIOS::height() const +{ + return obb.height(); +} + +//============================================================================== +template +S kIOS::depth() const +{ + return obb.depth(); +} + +//============================================================================== +template +S kIOS::volume() const +{ + return obb.volume(); +} + +//============================================================================== +template +S kIOS::size() const +{ + return volume(); +} + +//============================================================================== +template +S kIOS::distance( + const kIOS& other, + Vector3* P, + Vector3* Q) const +{ + S d_max = 0; + int id_a = -1, id_b = -1; + for(unsigned int i = 0; i < num_spheres; ++i) + { + for(unsigned int j = 0; j < other.num_spheres; ++j) + { + S d = (spheres[i].o - other.spheres[j].o).norm() - (spheres[i].r + other.spheres[j].r); + if(d_max < d) + { + d_max = d; + if(P && Q) + { + id_a = i; id_b = j; + } + } + } + } + + if(P && Q) + { + if(id_a != -1 && id_b != -1) + { + Vector3 v = spheres[id_a].o - spheres[id_b].o; + S len_v = v.norm(); + *P = spheres[id_a].o; + (*P).noalias() -= v * (spheres[id_a].r / len_v); + *Q = spheres[id_b].o; + (*Q).noalias() += v * (spheres[id_b].r / len_v); + } + } + + return d_max; +} + +//============================================================================== +template +bool overlap( + const Eigen::MatrixBase& R0, + const Eigen::MatrixBase& T0, + const kIOS& b1, const kIOS& b2) +{ + kIOS b2_temp = b2; + for(unsigned int i = 0; i < b2_temp.num_spheres; ++i) + b2_temp.spheres[i].o = R0 * b2_temp.spheres[i].o + T0; + + b2_temp.obb.To = R0 * b2_temp.obb.To + T0; + b2_temp.obb.axis = R0 * b2_temp.obb.axis; + + return b1.overlap(b2_temp); +} + +//============================================================================== +template +bool overlap( + const Transform3& tf, + const kIOS& b1, + const kIOS& b2) +{ + kIOS b2_temp = b2; + for(unsigned int i = 0; i < b2_temp.num_spheres; ++i) + b2_temp.spheres[i].o = tf * b2_temp.spheres[i].o; + + b2_temp.obb.frame = tf * b2_temp.obb.frame; + + return b1.overlap(b2_temp); +} + +//============================================================================== +template +S distance( + const Eigen::MatrixBase& R0, + const Eigen::MatrixBase& T0, + const kIOS& b1, const kIOS& b2, + Vector3* P, Vector3* Q) +{ + kIOS b2_temp = b2; + for(unsigned int i = 0; i < b2_temp.num_spheres; ++i) + b2_temp.spheres[i].o = R0 * b2_temp.spheres[i].o + T0; + + return b1.distance(b2_temp, P, Q); +} + +//============================================================================== +template +S distance( + const Transform3& tf, + const kIOS& b1, + const kIOS& b2, + Vector3* P, + Vector3* Q) +{ + kIOS b2_temp = b2; + + for(unsigned int i = 0; i < b2_temp.num_spheres; ++i) + b2_temp.spheres[i].o = tf * b2_temp.spheres[i].o; + + return b1.distance(b2_temp, P, Q); +} + +//============================================================================== +template +kIOS translate( + const kIOS& bv, const Eigen::MatrixBase& t) +{ + EIGEN_STATIC_ASSERT( + Derived::RowsAtCompileTime == 3 + && Derived::ColsAtCompileTime == 1, + THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE); + + kIOS res(bv); + for(size_t i = 0; i < res.num_spheres; ++i) + { + res.spheres[i].o += t; + } + + translate(res.obb, t); + return res; +} + + +} // namespace fcl + #endif diff --git a/include/fcl/BV/utility.h b/include/fcl/BV/utility.h new file mode 100644 index 000000000..274cba23d --- /dev/null +++ b/include/fcl/BV/utility.h @@ -0,0 +1,217 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + + +#ifndef FCL_BV_UTILITY_H +#define FCL_BV_UTILITY_H + +#include "fcl/data_types.h" +#include "fcl/math/triangle.h" +#include "fcl/BV/detail/utility.h" + +namespace fcl +{ + +/// @brief Compute the bounding volume extent and center for a set or subset of points, given the BV axises. +template +void getExtentAndCenter( + Vector3* ps, + Vector3* ps2, + Triangle* ts, + unsigned int* indices, + int n, + const Matrix3& axis, + Vector3& center, + Vector3& extent); + +/// @brief Compute the bounding volume extent and center for a set or subset of +/// points, given the BV axises. +template +void getExtentAndCenter( + Vector3* ps, + Vector3* ps2, + Triangle* ts, + unsigned int* indices, + int n, + Transform3& tf, + Vector3& extent); + +/// @brief Compute the covariance matrix for a set or subset of points. if +/// ts = null, then indices refer to points directly; otherwise refer to +/// triangles +template +void getCovariance( + Vector3* ps, + Vector3* ps2, + Triangle* ts, + unsigned int* indices, + int n, + Matrix3& M); + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +void getExtentAndCenter( + Vector3* ps, + Vector3* ps2, + Triangle* ts, + unsigned int* indices, + int n, + const Matrix3& axis, + Vector3& center, + Vector3& extent) +{ + if(ts) + detail::getExtentAndCenter_mesh(ps, ps2, ts, indices, n, axis, center, extent); + else + detail::getExtentAndCenter_pointcloud(ps, ps2, indices, n, axis, center, extent); +} + +//============================================================================== +template +void getExtentAndCenter( + Vector3* ps, + Vector3* ps2, + Triangle* ts, + unsigned int* indices, + int n, + Transform3& tf, + Vector3& extent) +{ + if(ts) + detail::getExtentAndCenter_mesh(ps, ps2, ts, indices, n, tf, extent); + else + detail::getExtentAndCenter_pointcloud(ps, ps2, indices, n, tf, extent); +} + +//============================================================================== +template +void getCovariance(Vector3* ps, + Vector3* ps2, + Triangle* ts, + unsigned int* indices, + int n, Matrix3& M) +{ + Vector3 S1 = Vector3::Zero(); + Vector3 S2[3] = { + Vector3::Zero(), Vector3::Zero(), Vector3::Zero() + }; + + if(ts) + { + for(int i = 0; i < n; ++i) + { + const Triangle& t = (indices) ? ts[indices[i]] : ts[i]; + + const Vector3& p1 = ps[t[0]]; + const Vector3& p2 = ps[t[1]]; + const Vector3& p3 = ps[t[2]]; + + S1 += (p1 + p2 + p3).eval(); + + S2[0][0] += (p1[0] * p1[0] + p2[0] * p2[0] + p3[0] * p3[0]); + S2[1][1] += (p1[1] * p1[1] + p2[1] * p2[1] + p3[1] * p3[1]); + S2[2][2] += (p1[2] * p1[2] + p2[2] * p2[2] + p3[2] * p3[2]); + S2[0][1] += (p1[0] * p1[1] + p2[0] * p2[1] + p3[0] * p3[1]); + S2[0][2] += (p1[0] * p1[2] + p2[0] * p2[2] + p3[0] * p3[2]); + S2[1][2] += (p1[1] * p1[2] + p2[1] * p2[2] + p3[1] * p3[2]); + + if(ps2) + { + const Vector3& p1 = ps2[t[0]]; + const Vector3& p2 = ps2[t[1]]; + const Vector3& p3 = ps2[t[2]]; + + S1 += (p1 + p2 + p3).eval(); + + S2[0][0] += (p1[0] * p1[0] + p2[0] * p2[0] + p3[0] * p3[0]); + S2[1][1] += (p1[1] * p1[1] + p2[1] * p2[1] + p3[1] * p3[1]); + S2[2][2] += (p1[2] * p1[2] + p2[2] * p2[2] + p3[2] * p3[2]); + S2[0][1] += (p1[0] * p1[1] + p2[0] * p2[1] + p3[0] * p3[1]); + S2[0][2] += (p1[0] * p1[2] + p2[0] * p2[2] + p3[0] * p3[2]); + S2[1][2] += (p1[1] * p1[2] + p2[1] * p2[2] + p3[1] * p3[2]); + } + } + } + else + { + for(int i = 0; i < n; ++i) + { + const Vector3& p = (indices) ? ps[indices[i]] : ps[i]; + S1 += p; + S2[0][0] += (p[0] * p[0]); + S2[1][1] += (p[1] * p[1]); + S2[2][2] += (p[2] * p[2]); + S2[0][1] += (p[0] * p[1]); + S2[0][2] += (p[0] * p[2]); + S2[1][2] += (p[1] * p[2]); + + if(ps2) // another frame + { + const Vector3& p = (indices) ? ps2[indices[i]] : ps2[i]; + S1 += p; + S2[0][0] += (p[0] * p[0]); + S2[1][1] += (p[1] * p[1]); + S2[2][2] += (p[2] * p[2]); + S2[0][1] += (p[0] * p[1]); + S2[0][2] += (p[0] * p[2]); + S2[1][2] += (p[1] * p[2]); + } + } + } + + int n_points = ((ps2) ? 2 : 1) * ((ts) ? 3 : 1) * n; + + M(0, 0) = S2[0][0] - S1[0]*S1[0] / n_points; + M(1, 1) = S2[1][1] - S1[1]*S1[1] / n_points; + M(2, 2) = S2[2][2] - S1[2]*S1[2] / n_points; + M(0, 1) = S2[0][1] - S1[0]*S1[1] / n_points; + M(1, 2) = S2[1][2] - S1[1]*S1[2] / n_points; + M(0, 2) = S2[0][2] - S1[0]*S1[2] / n_points; + M(1, 0) = M(0, 1); + M(2, 0) = M(0, 2); + M(2, 1) = M(1, 2); +} + +} // namespace fcl + +#endif diff --git a/include/fcl/BVH/BVH_front.h b/include/fcl/BVH/BVH_front.h index a6a740864..7378212a1 100644 --- a/include/fcl/BVH/BVH_front.h +++ b/include/fcl/BVH/BVH_front.h @@ -38,7 +38,6 @@ #ifndef FCL_BVH_FRONT_H #define FCL_BVH_FRONT_H - #include namespace fcl @@ -46,33 +45,47 @@ namespace fcl /// @brief Front list acceleration for collision /// Front list is a set of internal and leaf nodes in the BVTT hierarchy, where -/// the traversal terminates while performing a query during a given time instance. The front list reflects the subset of a -/// BVTT that is traversed for that particular proximity query. +/// the traversal terminates while performing a query during a given time +/// instance. The front list reflects the subset of a BVTT that is traversed for +/// that particular proximity query. struct BVHFrontNode { - /// @brief The nodes to start in the future, i.e. the wave front of the traversal tree. + /// @brief The nodes to start in the future, i.e. the wave front of the + /// traversal tree. int left, right; - /// @brief The front node is not valid when collision is detected on the front node. + /// @brief The front node is not valid when collision is detected on the front + /// node. bool valid; - BVHFrontNode(int left_, int right_) : left(left_), - right(right_), - valid(true) - { - } + BVHFrontNode(int left_, int right_); }; /// @brief BVH front list is a list of front nodes. -typedef std::list BVHFrontList; +using BVHFrontList = std::list; /// @brief Add new front node into the front list -inline void updateFrontList(BVHFrontList* front_list, int b1, int b2) +void updateFrontList(BVHFrontList* front_list, int b1, int b2); + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +inline BVHFrontNode::BVHFrontNode(int left_, int right_) + : left(left_), right(right_), valid(true) { - if(front_list) front_list->push_back(BVHFrontNode(b1, b2)); + // Do nothing } - +//============================================================================== +inline void updateFrontList(BVHFrontList* front_list, int b1, int b2) +{ + if(front_list) front_list->emplace_back(b1, b2); } +} // namespace fcl + #endif diff --git a/include/fcl/BVH/BVH_model.h b/include/fcl/BVH/BVH_model.h index 06d4d3c3c..09d3437d6 100644 --- a/include/fcl/BVH/BVH_model.h +++ b/include/fcl/BVH/BVH_model.h @@ -38,110 +38,75 @@ #ifndef FCL_BVH_MODEL_H #define FCL_BVH_MODEL_H -#include "fcl/collision_object.h" -#include "fcl/BVH/BVH_internal.h" +#include +#include + +#include "fcl/collision_geometry.h" #include "fcl/BV/BV_node.h" +#include "fcl/BV/OBB.h" +#include "fcl/BV/fit.h" +#include "fcl/BVH/BVH_internal.h" #include "fcl/BVH/BV_splitter.h" #include "fcl/BVH/BV_fitter.h" -#include -#include namespace fcl { /// @brief A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as a degraded version of mesh) -template -class BVHModel : public CollisionGeometry +template +class BVHModel : public CollisionGeometry { - public: + + using S = typename BV::S; + /// @brief Model type described by the instance - BVHModelType getModelType() const - { - if(num_tris && num_vertices) - return BVH_MODEL_TRIANGLES; - else if(num_vertices) - return BVH_MODEL_POINTCLOUD; - else - return BVH_MODEL_UNKNOWN; - } + BVHModelType getModelType() const; /// @brief Constructing an empty BVH - BVHModel() : vertices(NULL), - tri_indices(NULL), - prev_vertices(NULL), - num_tris(0), - num_vertices(0), - build_state(BVH_BUILD_STATE_EMPTY), - bv_splitter(new BVSplitter(SPLIT_METHOD_MEAN)), - bv_fitter(new BVFitter()), - num_tris_allocated(0), - num_vertices_allocated(0), - num_bvs_allocated(0), - num_vertex_updated(0), - primitive_indices(NULL), - bvs(NULL), - num_bvs(0) - { - } + BVHModel(); /// @brief copy from another BVH BVHModel(const BVHModel& other); /// @brief deconstruction, delete mesh data related. - ~BVHModel() - { - delete [] vertices; - delete [] tri_indices; - delete [] bvs; - - delete [] prev_vertices; - delete [] primitive_indices; - } + ~BVHModel(); - /// @brief We provide getBV() and getNumBVs() because BVH may be compressed (in future), so we must provide some flexibility here + /// @brief We provide getBV() and getNumBVs() because BVH may be compressed + /// (in future), so we must provide some flexibility here /// @brief Access the bv giving the its index - const BVNode& getBV(int id) const - { - return bvs[id]; - } + const BVNode& getBV(int id) const; /// @brief Access the bv giving the its index - BVNode& getBV(int id) - { - return bvs[id]; - } + BVNode& getBV(int id); /// @brief Get the number of bv in the BVH - int getNumBVs() const - { - return num_bvs; - } + int getNumBVs() const; /// @brief Get the object type: it is a BVH - OBJECT_TYPE getObjectType() const { return OT_BVH; } + OBJECT_TYPE getObjectType() const override; /// @brief Get the BV type: default is unknown - NODE_TYPE getNodeType() const { return BV_UNKNOWN; } + NODE_TYPE getNodeType() const override; /// @brief Compute the AABB for the BVH, used for broad-phase collision - void computeLocalAABB(); + void computeLocalAABB() override; /// @brief Begin a new BVH model int beginModel(int num_tris = 0, int num_vertices = 0); /// @brief Add one point in the new BVH model - int addVertex(const Vector3d& p); + int addVertex(const Vector3& p); /// @brief Add one triangle in the new BVH model - int addTriangle(const Vector3d& p1, const Vector3d& p2, const Vector3d& p3); + int addTriangle(const Vector3& p1, const Vector3& p2, const Vector3& p3); /// @brief Add a set of triangles in the new BVH model - int addSubModel(const std::vector& ps, const std::vector& ts); + int addSubModel(const std::vector>& ps, const std::vector& ts); /// @brief Add a set of points in the new BVH model - int addSubModel(const std::vector& ps); + int addSubModel(const std::vector>& ps); /// @brief End BVH model construction, will build the bounding volume hierarchy int endModel(); @@ -151,13 +116,13 @@ class BVHModel : public CollisionGeometry int beginReplaceModel(); /// @brief Replace one point in the old BVH model - int replaceVertex(const Vector3d& p); + int replaceVertex(const Vector3& p); /// @brief Replace one triangle in the old BVH model - int replaceTriangle(const Vector3d& p1, const Vector3d& p2, const Vector3d& p3); + int replaceTriangle(const Vector3& p1, const Vector3& p2, const Vector3& p3); /// @brief Replace a set of points in the old BVH model - int replaceSubModel(const std::vector& ps); + int replaceSubModel(const std::vector>& ps); /// @brief End BVH model replacement, will also refit or rebuild the bounding volume hierarchy int endReplaceModel(bool refit = true, bool bottomup = true); @@ -168,13 +133,13 @@ class BVHModel : public CollisionGeometry int beginUpdateModel(); /// @brief Update one point in the old BVH model - int updateVertex(const Vector3d& p); + int updateVertex(const Vector3& p); /// @brief Update one triangle in the old BVH model - int updateTriangle(const Vector3d& p1, const Vector3d& p2, const Vector3d& p3); + int updateTriangle(const Vector3& p1, const Vector3& p2, const Vector3& p3); /// @brief Update a set of points in the old BVH model - int updateSubModel(const std::vector& ps); + int updateSubModel(const std::vector>& ps); /// @brief End BVH model update, will also refit or rebuild the bounding volume hierarchy int endUpdateModel(bool refit = true, bool bottomup = true); @@ -184,81 +149,23 @@ class BVHModel : public CollisionGeometry /// @brief This is a special acceleration: BVH_model default stores the BV's transform in world coordinate. However, we can also store each BV's transform related to its parent /// BV node. When traversing the BVH, this can save one matrix transformation. - void makeParentRelative() - { - makeParentRelativeRecurse(0, Matrix3d::Identity(), Vector3d::Zero()); - } - - Vector3d computeCOM() const - { - FCL_REAL vol = 0; - Vector3d com = Vector3d::Zero(); - for(int i = 0; i < num_tris; ++i) - { - const Triangle& tri = tri_indices[i]; - FCL_REAL d_six_vol = (vertices[tri[0]].cross(vertices[tri[1]])).dot(vertices[tri[2]]); - vol += d_six_vol; - com += (vertices[tri[0]] + vertices[tri[1]] + vertices[tri[2]]) * d_six_vol; - } - - return com / (vol * 4); - } - - FCL_REAL computeVolume() const - { - FCL_REAL vol = 0; - for(int i = 0; i < num_tris; ++i) - { - const Triangle& tri = tri_indices[i]; - FCL_REAL d_six_vol = (vertices[tri[0]].cross(vertices[tri[1]])).dot(vertices[tri[2]]); - vol += d_six_vol; - } - - return vol / 6; - } - - Matrix3d computeMomentofInertia() const - { - Matrix3d C = Matrix3d::Zero(); - - Matrix3d C_canonical; - C_canonical << 1/ 60.0, 1/120.0, 1/120.0, - 1/120.0, 1/ 60.0, 1/120.0, - 1/120.0, 1/120.0, 1/ 60.0; + void makeParentRelative(); - for(int i = 0; i < num_tris; ++i) - { - const Triangle& tri = tri_indices[i]; - const Vector3d& v1 = vertices[tri[0]]; - const Vector3d& v2 = vertices[tri[1]]; - const Vector3d& v3 = vertices[tri[2]]; - FCL_REAL d_six_vol = (v1.cross(v2)).dot(v3); - Matrix3d A; - A.row(0) = v1; - A.row(1) = v2; - A.row(2) = v3; - C += A.transpose() * C_canonical * A * d_six_vol; - } + Vector3 computeCOM() const override; - FCL_REAL trace_C = C(0, 0) + C(1, 1) + C(2, 2); + S computeVolume() const override; - Matrix3d m; - m << trace_C - C(0, 0), -C(0, 1), -C(0, 2), - -C(1, 0), trace_C - C(1, 1), -C(1, 2), - -C(2, 0), -C(2, 1), trace_C - C(2, 2); - - return m; - } + Matrix3 computeMomentofInertia() const override; public: /// @brief Geometry point data - Vector3d* vertices; + Vector3* vertices; - /// @brief Geometry triangle index data, will be NULL for point clouds + /// @brief Geometry triangle index data, will be nullptr for point clouds Triangle* tri_indices; /// @brief Geometry point data in previous frame - Vector3d* prev_vertices; + Vector3* prev_vertices; /// @brief Number of triangles int num_tris; @@ -270,10 +177,10 @@ class BVHModel : public CollisionGeometry BVHBuildState build_state; /// @brief Split rule to split one BV node into two children - std::shared_ptr > bv_splitter; + std::shared_ptr> bv_splitter; /// @brief Fitting rule to fit a BV node to a set of geometry primitives - std::shared_ptr > bv_fitter; + std::shared_ptr> bv_fitter; private: @@ -309,57 +216,1222 @@ class BVHModel : public CollisionGeometry /// @brief Recursive kernel for bottomup refitting int recursiveRefitTree_bottomup(int bv_id); - /// @recursively compute each bv's transform related to its parent. For default BV, only the translation works. - /// For oriented BV (OBB, RSS, OBBRSS), special implementation is provided. - void makeParentRelativeRecurse(int bv_id, const Matrix3d& parent_axis, const Vector3d& parent_c) + /// @recursively compute each bv's transform related to its parent. For + /// default BV, only the translation works. For oriented BV (OBB, RSS, + /// OBBRSS), special implementation is provided. + void makeParentRelativeRecurse( + int bv_id, + const Matrix3& parent_axis, + const Vector3& parent_c); + + template + friend struct MakeParentRelativeRecurseImpl; +}; + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +BVHModelType BVHModel::getModelType() const +{ + if(num_tris && num_vertices) + return BVH_MODEL_TRIANGLES; + else if(num_vertices) + return BVH_MODEL_POINTCLOUD; + else + return BVH_MODEL_UNKNOWN; +} + +//============================================================================== +template +BVHModel::BVHModel() : vertices(nullptr), + tri_indices(nullptr), + prev_vertices(nullptr), + num_tris(0), + num_vertices(0), + build_state(BVH_BUILD_STATE_EMPTY), + bv_splitter(new BVSplitter(SPLIT_METHOD_MEAN)), + bv_fitter(new BVFitter()), + num_tris_allocated(0), + num_vertices_allocated(0), + num_bvs_allocated(0), + num_vertex_updated(0), + primitive_indices(nullptr), + bvs(nullptr), + num_bvs(0) +{ + // Do nothing +} + +//============================================================================== +template +BVHModel::BVHModel(const BVHModel& other) + : CollisionGeometry(other), + num_tris(other.num_tris), + num_vertices(other.num_vertices), + build_state(other.build_state), + bv_splitter(other.bv_splitter), + bv_fitter(other.bv_fitter), + num_tris_allocated(other.num_tris), + num_vertices_allocated(other.num_vertices) +{ + if(other.vertices) { - if(!bvs[bv_id].isLeaf()) - { - makeParentRelativeRecurse(bvs[bv_id].first_child, parent_axis, bvs[bv_id].getCenter()); + vertices = new Vector3[num_vertices]; + memcpy(vertices, other.vertices, sizeof(Vector3) * num_vertices); + } + else + vertices = nullptr; - makeParentRelativeRecurse(bvs[bv_id].first_child + 1, parent_axis, bvs[bv_id].getCenter()); + if(other.tri_indices) + { + tri_indices = new Triangle[num_tris]; + memcpy(tri_indices, other.tri_indices, sizeof(Triangle) * num_tris); + } + else + tri_indices = nullptr; + + if(other.prev_vertices) + { + prev_vertices = new Vector3[num_vertices]; + memcpy(prev_vertices, other.prev_vertices, sizeof(Vector3) * num_vertices); + } + else + prev_vertices = nullptr; + + if(other.primitive_indices) + { + int num_primitives = 0; + switch(other.getModelType()) + { + case BVH_MODEL_TRIANGLES: + num_primitives = num_tris; + break; + case BVH_MODEL_POINTCLOUD: + num_primitives = num_vertices; + break; + default: + ; } - bvs[bv_id].bv = translate(bvs[bv_id].bv, -parent_c); + primitive_indices = new unsigned int[num_primitives]; + memcpy(primitive_indices, other.primitive_indices, sizeof(unsigned int) * num_primitives); + } + else + primitive_indices = nullptr; + + num_bvs = num_bvs_allocated = other.num_bvs; + if(other.bvs) + { + bvs = new BVNode[num_bvs]; + memcpy(bvs, other.bvs, sizeof(BVNode) * num_bvs); + } + else + bvs = nullptr; +} + +//============================================================================== +template +BVHModel::~BVHModel() +{ + delete [] vertices; + delete [] tri_indices; + delete [] bvs; + + delete [] prev_vertices; + delete [] primitive_indices; +} + +//============================================================================== +template +const BVNode& BVHModel::getBV(int id) const +{ + return bvs[id]; +} + +//============================================================================== +template +BVNode& BVHModel::getBV(int id) +{ + return bvs[id]; +} + +//============================================================================== +template +int BVHModel::getNumBVs() const +{ + return num_bvs; +} + +//============================================================================== +template +OBJECT_TYPE BVHModel::getObjectType() const +{ + return OT_BVH; +} + +//============================================================================== +template +struct GetNodeTypeImpl +{ + static NODE_TYPE run() + { + return BV_UNKNOWN; } }; +//============================================================================== +template +NODE_TYPE BVHModel::getNodeType() const +{ + return GetNodeTypeImpl::run(); +} + +//============================================================================== +template +int BVHModel::beginModel(int num_tris_, int num_vertices_) +{ + if(build_state != BVH_BUILD_STATE_EMPTY) + { + delete [] vertices; vertices = nullptr; + delete [] tri_indices; tri_indices = nullptr; + delete [] bvs; bvs = nullptr; + delete [] prev_vertices; prev_vertices = nullptr; + delete [] primitive_indices; primitive_indices = nullptr; + + num_vertices_allocated = num_vertices = num_tris_allocated = num_tris = num_bvs_allocated = num_bvs = 0; + } + + if(num_tris_ <= 0) num_tris_ = 8; + if(num_vertices_ <= 0) num_vertices_ = 8; + + num_vertices_allocated = num_vertices_; + num_tris_allocated = num_tris_; + + tri_indices = new Triangle[num_tris_allocated]; + vertices = new Vector3[num_vertices_allocated]; + + if(!tri_indices) + { + std::cerr << "BVH Error! Out of memory for tri_indices array on BeginModel() call!" << std::endl; + return BVH_ERR_MODEL_OUT_OF_MEMORY; + } + if(!vertices) + { + std::cerr << "BVH Error! Out of memory for vertices array on BeginModel() call!" << std::endl; + return BVH_ERR_MODEL_OUT_OF_MEMORY; + } + + if(build_state != BVH_BUILD_STATE_EMPTY) + { + std::cerr << "BVH Warning! Call beginModel() on a BVHModel that is not empty. This model was cleared and previous triangles/vertices were lost." << std::endl; + build_state = BVH_BUILD_STATE_EMPTY; + return BVH_ERR_BUILD_OUT_OF_SEQUENCE; + } + + build_state = BVH_BUILD_STATE_BEGUN; + + return BVH_OK; +} + +//============================================================================== +template +int BVHModel::addVertex(const Vector3& p) +{ + if(build_state != BVH_BUILD_STATE_BEGUN) + { + std::cerr << "BVH Warning! Call addVertex() in a wrong order. addVertex() was ignored. Must do a beginModel() to clear the model for addition of new vertices." << std::endl; + return BVH_ERR_BUILD_OUT_OF_SEQUENCE; + } + + if(num_vertices >= num_vertices_allocated) + { + Vector3* temp = new Vector3[num_vertices_allocated * 2]; + if(!temp) + { + std::cerr << "BVH Error! Out of memory for vertices array on addVertex() call!" << std::endl; + return BVH_ERR_MODEL_OUT_OF_MEMORY; + } + + memcpy(temp, vertices, sizeof(Vector3) * num_vertices); + delete [] vertices; + vertices = temp; + num_vertices_allocated *= 2; + } + + vertices[num_vertices] = p; + num_vertices += 1; + + return BVH_OK; +} + +//============================================================================== +template +int BVHModel::addTriangle(const Vector3& p1, const Vector3& p2, const Vector3& p3) +{ + if(build_state == BVH_BUILD_STATE_PROCESSED) + { + std::cerr << "BVH Warning! Call addTriangle() in a wrong order. addTriangle() was ignored. Must do a beginModel() to clear the model for addition of new triangles." << std::endl; + return BVH_ERR_BUILD_OUT_OF_SEQUENCE; + } + + if(num_vertices + 2 >= num_vertices_allocated) + { + Vector3* temp = new Vector3[num_vertices_allocated * 2 + 2]; + if(!temp) + { + std::cerr << "BVH Error! Out of memory for vertices array on addTriangle() call!" << std::endl; + return BVH_ERR_MODEL_OUT_OF_MEMORY; + } + + memcpy(temp, vertices, sizeof(Vector3) * num_vertices); + delete [] vertices; + vertices = temp; + num_vertices_allocated = num_vertices_allocated * 2 + 2; + } + + int offset = num_vertices; + + vertices[num_vertices] = p1; + num_vertices++; + vertices[num_vertices] = p2; + num_vertices++; + vertices[num_vertices] = p3; + num_vertices++; + + if(num_tris >= num_tris_allocated) + { + Triangle* temp = new Triangle[num_tris_allocated * 2]; + if(!temp) + { + std::cerr << "BVH Error! Out of memory for tri_indices array on addTriangle() call!" << std::endl; + return BVH_ERR_MODEL_OUT_OF_MEMORY; + } + + memcpy(temp, tri_indices, sizeof(Triangle) * num_tris); + delete [] tri_indices; + tri_indices = temp; + num_tris_allocated *= 2; + } + + tri_indices[num_tris].set(offset, offset + 1, offset + 2); + num_tris++; + + return BVH_OK; +} + +//============================================================================== +template +int BVHModel::addSubModel(const std::vector>& ps) +{ + if(build_state == BVH_BUILD_STATE_PROCESSED) + { + std::cerr << "BVH Warning! Call addSubModel() in a wrong order. addSubModel() was ignored. Must do a beginModel() to clear the model for addition of new vertices." << std::endl; + return BVH_ERR_BUILD_OUT_OF_SEQUENCE; + } + + int num_vertices_to_add = ps.size(); + + if(num_vertices + num_vertices_to_add - 1 >= num_vertices_allocated) + { + Vector3* temp = new Vector3[num_vertices_allocated * 2 + num_vertices_to_add - 1]; + if(!temp) + { + std::cerr << "BVH Error! Out of memory for vertices array on addSubModel() call!" << std::endl; + return BVH_ERR_MODEL_OUT_OF_MEMORY; + } + + memcpy(temp, vertices, sizeof(Vector3) * num_vertices); + delete [] vertices; + vertices = temp; + num_vertices_allocated = num_vertices_allocated * 2 + num_vertices_to_add - 1; + } + + for(int i = 0; i < num_vertices_to_add; ++i) + { + vertices[num_vertices] = ps[i]; + num_vertices++; + } + + return BVH_OK; +} + +//============================================================================== +template +int BVHModel::addSubModel(const std::vector>& ps, const std::vector& ts) +{ + if(build_state == BVH_BUILD_STATE_PROCESSED) + { + std::cerr << "BVH Warning! Call addSubModel() in a wrong order. addSubModel() was ignored. Must do a beginModel() to clear the model for addition of new vertices." << std::endl; + return BVH_ERR_BUILD_OUT_OF_SEQUENCE; + } + + int num_vertices_to_add = ps.size(); + + if(num_vertices + num_vertices_to_add - 1 >= num_vertices_allocated) + { + Vector3* temp = new Vector3[num_vertices_allocated * 2 + num_vertices_to_add - 1]; + if(!temp) + { + std::cerr << "BVH Error! Out of memory for vertices array on addSubModel() call!" << std::endl; + return BVH_ERR_MODEL_OUT_OF_MEMORY; + } + + memcpy(temp, vertices, sizeof(Vector3) * num_vertices); + delete [] vertices; + vertices = temp; + num_vertices_allocated = num_vertices_allocated * 2 + num_vertices_to_add - 1; + } + + int offset = num_vertices; + + for(int i = 0; i < num_vertices_to_add; ++i) + { + vertices[num_vertices] = ps[i]; + num_vertices++; + } + + + int num_tris_to_add = ts.size(); + + if(num_tris + num_tris_to_add - 1 >= num_tris_allocated) + { + Triangle* temp = new Triangle[num_tris_allocated * 2 + num_tris_to_add - 1]; + if(!temp) + { + std::cerr << "BVH Error! Out of memory for tri_indices array on addSubModel() call!" << std::endl; + return BVH_ERR_MODEL_OUT_OF_MEMORY; + } + + memcpy(temp, tri_indices, sizeof(Triangle) * num_tris); + delete [] tri_indices; + tri_indices = temp; + num_tris_allocated = num_tris_allocated * 2 + num_tris_to_add - 1; + } + + for(int i = 0; i < num_tris_to_add; ++i) + { + const Triangle& t = ts[i]; + tri_indices[num_tris].set(t[0] + offset, t[1] + offset, t[2] + offset); + num_tris++; + } + + return BVH_OK; +} + +//============================================================================== +template +int BVHModel::endModel() +{ + if(build_state != BVH_BUILD_STATE_BEGUN) + { + std::cerr << "BVH Warning! Call endModel() in wrong order. endModel() was ignored." << std::endl; + return BVH_ERR_BUILD_OUT_OF_SEQUENCE; + } + + if(num_tris == 0 && num_vertices == 0) + { + std::cerr << "BVH Error! endModel() called on model with no triangles and vertices." << std::endl; + return BVH_ERR_BUILD_EMPTY_MODEL; + } + + if(num_tris_allocated > num_tris) + { + Triangle* new_tris = new Triangle[num_tris]; + if(!new_tris) + { + std::cerr << "BVH Error! Out of memory for tri_indices array in endModel() call!" << std::endl; + return BVH_ERR_MODEL_OUT_OF_MEMORY; + } + memcpy(new_tris, tri_indices, sizeof(Triangle) * num_tris); + delete [] tri_indices; + tri_indices = new_tris; + num_tris_allocated = num_tris; + } + + if(num_vertices_allocated > num_vertices) + { + Vector3* new_vertices = new Vector3[num_vertices]; + if(!new_vertices) + { + std::cerr << "BVH Error! Out of memory for vertices array in endModel() call!" << std::endl; + return BVH_ERR_MODEL_OUT_OF_MEMORY; + } + memcpy(new_vertices, vertices, sizeof(Vector3) * num_vertices); + delete [] vertices; + vertices = new_vertices; + num_vertices_allocated = num_vertices; + } + + + // construct BVH tree + int num_bvs_to_be_allocated = 0; + if(num_tris == 0) + num_bvs_to_be_allocated = 2 * num_vertices - 1; + else + num_bvs_to_be_allocated = 2 * num_tris - 1; + + + bvs = new BVNode [num_bvs_to_be_allocated]; + primitive_indices = new unsigned int [num_bvs_to_be_allocated]; + if(!bvs || !primitive_indices) + { + std::cerr << "BVH Error! Out of memory for BV array in endModel()!" << std::endl; + return BVH_ERR_MODEL_OUT_OF_MEMORY; + } + num_bvs_allocated = num_bvs_to_be_allocated; + num_bvs = 0; + + buildTree(); + + // finish constructing + build_state = BVH_BUILD_STATE_PROCESSED; + + return BVH_OK; +} + +//============================================================================== +template +int BVHModel::beginReplaceModel() +{ + if(build_state != BVH_BUILD_STATE_PROCESSED) + { + std::cerr << "BVH Error! Call beginReplaceModel() on a BVHModel that has no previous frame." << std::endl; + return BVH_ERR_BUILD_EMPTY_PREVIOUS_FRAME; + } + + if(prev_vertices) delete [] prev_vertices; prev_vertices = nullptr; + + num_vertex_updated = 0; + + build_state = BVH_BUILD_STATE_REPLACE_BEGUN; + + return BVH_OK; +} + +//============================================================================== +template +int BVHModel::replaceVertex(const Vector3& p) +{ + if(build_state != BVH_BUILD_STATE_REPLACE_BEGUN) + { + std::cerr << "BVH Warning! Call replaceVertex() in a wrong order. replaceVertex() was ignored. Must do a beginReplaceModel() for initialization." << std::endl; + return BVH_ERR_BUILD_OUT_OF_SEQUENCE; + } + + vertices[num_vertex_updated] = p; + num_vertex_updated++; + + return BVH_OK; +} + +//============================================================================== +template +int BVHModel::replaceTriangle(const Vector3& p1, const Vector3& p2, const Vector3& p3) +{ + if(build_state != BVH_BUILD_STATE_REPLACE_BEGUN) + { + std::cerr << "BVH Warning! Call replaceTriangle() in a wrong order. replaceTriangle() was ignored. Must do a beginReplaceModel() for initialization." << std::endl; + return BVH_ERR_BUILD_OUT_OF_SEQUENCE; + } + + vertices[num_vertex_updated] = p1; num_vertex_updated++; + vertices[num_vertex_updated] = p2; num_vertex_updated++; + vertices[num_vertex_updated] = p3; num_vertex_updated++; + return BVH_OK; +} + +//============================================================================== +template +int BVHModel::replaceSubModel(const std::vector>& ps) +{ + if(build_state != BVH_BUILD_STATE_REPLACE_BEGUN) + { + std::cerr << "BVH Warning! Call replaceSubModel() in a wrong order. replaceSubModel() was ignored. Must do a beginReplaceModel() for initialization." << std::endl; + return BVH_ERR_BUILD_OUT_OF_SEQUENCE; + } + + for(unsigned int i = 0; i < ps.size(); ++i) + { + vertices[num_vertex_updated] = ps[i]; + num_vertex_updated++; + } + return BVH_OK; +} -template<> -void BVHModel::makeParentRelativeRecurse(int bv_id, const Matrix3d& parent_axis, const Vector3d& parent_c); +//============================================================================== +template +int BVHModel::endReplaceModel(bool refit, bool bottomup) +{ + if(build_state != BVH_BUILD_STATE_REPLACE_BEGUN) + { + std::cerr << "BVH Warning! Call endReplaceModel() in a wrong order. endReplaceModel() was ignored. " << std::endl; + return BVH_ERR_BUILD_OUT_OF_SEQUENCE; + } -template<> -void BVHModel::makeParentRelativeRecurse(int bv_id, const Matrix3d& parent_axis, const Vector3d& parent_c); + if(num_vertex_updated != num_vertices) + { + std::cerr << "BVH Error! The replaced model should have the same number of vertices as the old model." << std::endl; + return BVH_ERR_INCORRECT_DATA; + } -template<> -void BVHModel::makeParentRelativeRecurse(int bv_id, const Matrix3d& parent_axis, const Vector3d& parent_c); + if(refit) // refit, do not change BVH structure + { + refitTree(bottomup); + } + else // reconstruct bvh tree based on current frame data + { + buildTree(); + } + + build_state = BVH_BUILD_STATE_PROCESSED; + + return BVH_OK; +} + +//============================================================================== +template +int BVHModel::beginUpdateModel() +{ + if(build_state != BVH_BUILD_STATE_PROCESSED && build_state != BVH_BUILD_STATE_UPDATED) + { + std::cerr << "BVH Error! Call beginUpdatemodel() on a BVHModel that has no previous frame." << std::endl; + return BVH_ERR_BUILD_EMPTY_PREVIOUS_FRAME; + } + + if(prev_vertices) + { + Vector3* temp = prev_vertices; + prev_vertices = vertices; + vertices = temp; + } + else + { + prev_vertices = vertices; + vertices = new Vector3[num_vertices]; + } + num_vertex_updated = 0; -/// @brief Specialization of getNodeType() for BVHModel with different BV types -template<> -NODE_TYPE BVHModel::getNodeType() const; + build_state = BVH_BUILD_STATE_UPDATE_BEGUN; -template<> -NODE_TYPE BVHModel::getNodeType() const; + return BVH_OK; +} -template<> -NODE_TYPE BVHModel::getNodeType() const; +//============================================================================== +template +int BVHModel::updateVertex(const Vector3& p) +{ + if(build_state != BVH_BUILD_STATE_UPDATE_BEGUN) + { + std::cerr << "BVH Warning! Call updateVertex() in a wrong order. updateVertex() was ignored. Must do a beginUpdateModel() for initialization." << std::endl; + return BVH_ERR_BUILD_OUT_OF_SEQUENCE; + } -template<> -NODE_TYPE BVHModel::getNodeType() const; + vertices[num_vertex_updated] = p; + num_vertex_updated++; -template<> -NODE_TYPE BVHModel::getNodeType() const; + return BVH_OK; +} -template<> -NODE_TYPE BVHModel >::getNodeType() const; +//============================================================================== +template +int BVHModel::updateTriangle(const Vector3& p1, const Vector3& p2, const Vector3& p3) +{ + if(build_state != BVH_BUILD_STATE_UPDATE_BEGUN) + { + std::cerr << "BVH Warning! Call updateTriangle() in a wrong order. updateTriangle() was ignored. Must do a beginUpdateModel() for initialization." << std::endl; + return BVH_ERR_BUILD_OUT_OF_SEQUENCE; + } -template<> -NODE_TYPE BVHModel >::getNodeType() const; + vertices[num_vertex_updated] = p1; num_vertex_updated++; + vertices[num_vertex_updated] = p2; num_vertex_updated++; + vertices[num_vertex_updated] = p3; num_vertex_updated++; + return BVH_OK; +} -template<> -NODE_TYPE BVHModel >::getNodeType() const; +//============================================================================== +template +int BVHModel::updateSubModel(const std::vector>& ps) +{ + if(build_state != BVH_BUILD_STATE_UPDATE_BEGUN) + { + std::cerr << "BVH Warning! Call updateSubModel() in a wrong order. updateSubModel() was ignored. Must do a beginUpdateModel() for initialization." << std::endl; + return BVH_ERR_BUILD_OUT_OF_SEQUENCE; + } + for(unsigned int i = 0; i < ps.size(); ++i) + { + vertices[num_vertex_updated] = ps[i]; + num_vertex_updated++; + } + return BVH_OK; } +//============================================================================== +template +int BVHModel::endUpdateModel(bool refit, bool bottomup) +{ + if(build_state != BVH_BUILD_STATE_UPDATE_BEGUN) + { + std::cerr << "BVH Warning! Call endUpdateModel() in a wrong order. endUpdateModel() was ignored. " << std::endl; + return BVH_ERR_BUILD_OUT_OF_SEQUENCE; + } + + if(num_vertex_updated != num_vertices) + { + std::cerr << "BVH Error! The updated model should have the same number of vertices as the old model." << std::endl; + return BVH_ERR_INCORRECT_DATA; + } + + if(refit) // refit, do not change BVH structure + { + refitTree(bottomup); + } + else // reconstruct bvh tree based on current frame data + { + buildTree(); + + // then refit + + refitTree(bottomup); + } + + + build_state = BVH_BUILD_STATE_UPDATED; + + return BVH_OK; +} + +//============================================================================== +template +int BVHModel::memUsage(int msg) const +{ + int mem_bv_list = sizeof(BV) * num_bvs; + int mem_tri_list = sizeof(Triangle) * num_tris; + int mem_vertex_list = sizeof(Vector3) * num_vertices; + + int total_mem = mem_bv_list + mem_tri_list + mem_vertex_list + sizeof(BVHModel); + if(msg) + { + std::cerr << "Total for model " << total_mem << " bytes." << std::endl; + std::cerr << "BVs: " << num_bvs << " allocated." << std::endl; + std::cerr << "Tris: " << num_tris << " allocated." << std::endl; + std::cerr << "Vertices: " << num_vertices << " allocated." << std::endl; + } + + return BVH_OK; +} + +//============================================================================== +template +void BVHModel::makeParentRelative() +{ + makeParentRelativeRecurse( + 0, Matrix3::Identity(), Vector3::Zero()); +} + +//============================================================================== +template +Vector3 BVHModel::computeCOM() const +{ + S vol = 0; + Vector3 com = Vector3::Zero(); + for(int i = 0; i < num_tris; ++i) + { + const Triangle& tri = tri_indices[i]; + S d_six_vol = (vertices[tri[0]].cross(vertices[tri[1]])).dot(vertices[tri[2]]); + vol += d_six_vol; + com.noalias() += (vertices[tri[0]] + vertices[tri[1]] + vertices[tri[2]]) * d_six_vol; + } + + return com / (vol * 4); +} + +//============================================================================== +template +typename BV::S BVHModel::computeVolume() const +{ + S vol = 0; + for(int i = 0; i < num_tris; ++i) + { + const Triangle& tri = tri_indices[i]; + S d_six_vol = (vertices[tri[0]].cross(vertices[tri[1]])).dot(vertices[tri[2]]); + vol += d_six_vol; + } + + return vol / 6; +} + +//============================================================================== +template +Matrix3 BVHModel::computeMomentofInertia() const +{ + Matrix3 C = Matrix3::Zero(); + + Matrix3 C_canonical; + C_canonical << 1/ 60.0, 1/120.0, 1/120.0, + 1/120.0, 1/ 60.0, 1/120.0, + 1/120.0, 1/120.0, 1/ 60.0; + + for(int i = 0; i < num_tris; ++i) + { + const Triangle& tri = tri_indices[i]; + const Vector3& v1 = vertices[tri[0]]; + const Vector3& v2 = vertices[tri[1]]; + const Vector3& v3 = vertices[tri[2]]; + S d_six_vol = (v1.cross(v2)).dot(v3); + Matrix3 A; + A.row(0) = v1; + A.row(1) = v2; + A.row(2) = v3; + C.noalias() += A.transpose() * C_canonical * A * d_six_vol; + } + + S trace_C = C(0, 0) + C(1, 1) + C(2, 2); + + Matrix3 m; + m << trace_C - C(0, 0), -C(0, 1), -C(0, 2), + -C(1, 0), trace_C - C(1, 1), -C(1, 2), + -C(2, 0), -C(2, 1), trace_C - C(2, 2); + + return m; +} + +//============================================================================== +template +int BVHModel::buildTree() +{ + // set BVFitter + bv_fitter->set(vertices, tri_indices, getModelType()); + // set SplitRule + bv_splitter->set(vertices, tri_indices, getModelType()); + + num_bvs = 1; + + int num_primitives = 0; + switch(getModelType()) + { + case BVH_MODEL_TRIANGLES: + num_primitives = num_tris; + break; + case BVH_MODEL_POINTCLOUD: + num_primitives = num_vertices; + break; + default: + std::cerr << "BVH Error: Model type not supported!" << std::endl; + return BVH_ERR_UNSUPPORTED_FUNCTION; + } + + for(int i = 0; i < num_primitives; ++i) + primitive_indices[i] = i; + recursiveBuildTree(0, 0, num_primitives); + + bv_fitter->clear(); + bv_splitter->clear(); + + return BVH_OK; +} + +//============================================================================== +template +int BVHModel::recursiveBuildTree(int bv_id, int first_primitive, int num_primitives) +{ + BVHModelType type = getModelType(); + BVNode* bvnode = bvs + bv_id; + unsigned int* cur_primitive_indices = primitive_indices + first_primitive; + + // constructing BV + BV bv = bv_fitter->fit(cur_primitive_indices, num_primitives); + bv_splitter->computeRule(bv, cur_primitive_indices, num_primitives); + + bvnode->bv = bv; + bvnode->first_primitive = first_primitive; + bvnode->num_primitives = num_primitives; + + if(num_primitives == 1) + { + bvnode->first_child = -((*cur_primitive_indices) + 1); + } + else + { + bvnode->first_child = num_bvs; + num_bvs += 2; + + int c1 = 0; + for(int i = 0; i < num_primitives; ++i) + { + Vector3 p; + if(type == BVH_MODEL_POINTCLOUD) p = vertices[cur_primitive_indices[i]]; + else if(type == BVH_MODEL_TRIANGLES) + { + const Triangle& t = tri_indices[cur_primitive_indices[i]]; + const Vector3& p1 = vertices[t[0]]; + const Vector3& p2 = vertices[t[1]]; + const Vector3& p3 = vertices[t[2]]; + p.noalias() = (p1 + p2 + p3) / 3.0; + } + else + { + std::cerr << "BVH Error: Model type not supported!" << std::endl; + return BVH_ERR_UNSUPPORTED_FUNCTION; + } + + + // loop invariant: up to (but not including) index c1 in group 1, + // then up to (but not including) index i in group 2 + // + // [1] [1] [1] [1] [2] [2] [2] [x] [x] ... [x] + // c1 i + // + if(bv_splitter->apply(p)) // in the right side + { + // do nothing + } + else + { + std::swap(cur_primitive_indices[i], cur_primitive_indices[c1]); + c1++; + } + } + + + if((c1 == 0) || (c1 == num_primitives)) c1 = num_primitives / 2; + + int num_first_half = c1; + + recursiveBuildTree(bvnode->leftChild(), first_primitive, num_first_half); + recursiveBuildTree(bvnode->rightChild(), first_primitive + num_first_half, num_primitives - num_first_half); + } + + return BVH_OK; +} + +//============================================================================== +template +int BVHModel::refitTree(bool bottomup) +{ + if(bottomup) + return refitTree_bottomup(); + else + return refitTree_topdown(); +} + +//============================================================================== +template +int BVHModel::refitTree_bottomup() +{ + int res = recursiveRefitTree_bottomup(0); + + return res; +} + +//============================================================================== +template +int BVHModel::recursiveRefitTree_bottomup(int bv_id) +{ + BVNode* bvnode = bvs + bv_id; + if(bvnode->isLeaf()) + { + BVHModelType type = getModelType(); + int primitive_id = -(bvnode->first_child + 1); + if(type == BVH_MODEL_POINTCLOUD) + { + BV bv; + + if(prev_vertices) + { + Vector3 v[2]; + v[0] = prev_vertices[primitive_id]; + v[1] = vertices[primitive_id]; + fit(v, 2, bv); + } + else + fit(vertices + primitive_id, 1, bv); + + bvnode->bv = bv; + } + else if(type == BVH_MODEL_TRIANGLES) + { + BV bv; + const Triangle& triangle = tri_indices[primitive_id]; + + if(prev_vertices) + { + Vector3 v[6]; + for(int i = 0; i < 3; ++i) + { + v[i] = prev_vertices[triangle[i]]; + v[i + 3] = vertices[triangle[i]]; + } + + fit(v, 6, bv); + } + else + { + Vector3 v[3]; + for(int i = 0; i < 3; ++i) + { + v[i] = vertices[triangle[i]]; + } + + fit(v, 3, bv); + } + + bvnode->bv = bv; + } + else + { + std::cerr << "BVH Error: Model type not supported!" << std::endl; + return BVH_ERR_UNSUPPORTED_FUNCTION; + } + } + else + { + recursiveRefitTree_bottomup(bvnode->leftChild()); + recursiveRefitTree_bottomup(bvnode->rightChild()); + bvnode->bv = bvs[bvnode->leftChild()].bv + bvs[bvnode->rightChild()].bv; + } + + return BVH_OK; +} + +//============================================================================== +template +struct MakeParentRelativeRecurseImpl +{ + static void run(BVHModel& model, + int bv_id, + const Matrix3& parent_axis, + const Vector3& parent_c) + { + if(!model.bvs[bv_id].isLeaf()) + { + MakeParentRelativeRecurseImpl tmp1; + tmp1(model, model.bvs[bv_id].first_child, parent_axis, model.bvs[bv_id].getCenter()); + + MakeParentRelativeRecurseImpl tmp2; + tmp2(model, model.bvs[bv_id].first_child + 1, parent_axis, model.bvs[bv_id].getCenter()); + } + + model.bvs[bv_id].bv = translate(model.bvs[bv_id].bv, -parent_c); + } +}; + +//============================================================================== +template +void BVHModel::makeParentRelativeRecurse( + int bv_id, + const Matrix3& parent_axis, + const Vector3& parent_c) +{ + MakeParentRelativeRecurseImpl::run( + *this, bv_id, parent_axis, parent_c); +} + +//============================================================================== +template +int BVHModel::refitTree_topdown() +{ + bv_fitter->set(vertices, prev_vertices, tri_indices, getModelType()); + for(int i = 0; i < num_bvs; ++i) + { + BV bv = bv_fitter->fit(primitive_indices + bvs[i].first_primitive, bvs[i].num_primitives); + bvs[i].bv = bv; + } + + bv_fitter->clear(); + + return BVH_OK; +} + +//============================================================================== +template +void BVHModel::computeLocalAABB() +{ + AABB aabb_; + for(int i = 0; i < num_vertices; ++i) + { + aabb_ += vertices[i]; + } + + this->aabb_center = aabb_.center(); + + this->aabb_radius = 0; + for(int i = 0; i < num_vertices; ++i) + { + S r = (this->aabb_center - vertices[i]).squaredNorm(); + if(r > this->aabb_radius) this->aabb_radius = r; + } + + this->aabb_radius = sqrt(this->aabb_radius); + + this->aabb_local = aabb_; +} + +//============================================================================== +template +struct MakeParentRelativeRecurseImpl> +{ + static void run(BVHModel>& model, + int bv_id, + const Matrix3& parent_axis, + const Vector3& parent_c) + { + OBB& obb = model.bvs[bv_id].bv; + if(!model.bvs[bv_id].isLeaf()) + { + MakeParentRelativeRecurseImpl> tmp1; + tmp1(model, model.bvs[bv_id].first_child, obb.axis, obb.To); + + MakeParentRelativeRecurseImpl> tmp2; + tmp2(model, model.bvs[bv_id].first_child + 1, obb.axis, obb.To); + } + + // make self parent relative + obb.axis = parent_axis.transpose() * obb.axis; + obb.To = (obb.To - parent_c).transpose() * parent_axis; + } +}; + +//============================================================================== +template +struct MakeParentRelativeRecurseImpl> +{ + static void run(BVHModel>& model, + int bv_id, + const Matrix3& parent_axis, + const Vector3& parent_c) + { + RSS& rss = model.bvs[bv_id].bv; + if(!model.bvs[bv_id].isLeaf()) + { + MakeParentRelativeRecurseImpl> tmp1; + tmp1(model, model.bvs[bv_id].first_child, rss.axis, rss.To); + + MakeParentRelativeRecurseImpl> tmp2; + tmp2(model, model.bvs[bv_id].first_child + 1, rss.axis, rss.To); + } + + // make self parent relative + rss.axis = parent_axis.transpose() * rss.axis; + rss.To = (rss.To - parent_c).transpose() * parent_axis; + } +}; + +//============================================================================== +template +struct MakeParentRelativeRecurseImpl> +{ + static void run(BVHModel>& model, + int bv_id, + const Matrix3& parent_axis, + const Vector3& parent_c) + { + OBB& obb = model.bvs[bv_id].bv.obb; + RSS& rss = model.bvs[bv_id].bv.rss; + if(!model.bvs[bv_id].isLeaf()) + { + MakeParentRelativeRecurseImpl> tmp1; + tmp1(model, model.bvs[bv_id].first_child, obb.axis, obb.To); + + MakeParentRelativeRecurseImpl> tmp2; + tmp2(model, model.bvs[bv_id].first_child + 1, obb.axis, obb.To); + } + + // make self parent relative + obb.axis = parent_axis.transpose() * obb.axis; + rss.axis = obb.axis; + + obb.To = (obb.To - parent_c).transpose() * parent_axis; + rss.To = obb.To; + } +}; + +//============================================================================== +template +struct GetNodeTypeImpl> +{ + static NODE_TYPE run() + { + return BV_AABB; + } +}; + +//============================================================================== +template +struct GetNodeTypeImpl> +{ + static NODE_TYPE run() + { + return BV_OBB; + } +}; + +//============================================================================== +template +struct GetNodeTypeImpl> +{ + static NODE_TYPE run() + { + return BV_RSS; + } +}; + +//============================================================================== +template +struct GetNodeTypeImpl> +{ + static NODE_TYPE run() + { + return BV_kIOS; + } +}; + +//============================================================================== +template +struct GetNodeTypeImpl> +{ + static NODE_TYPE run() + { + return BV_OBBRSS; + } +}; + +//============================================================================== +template +struct GetNodeTypeImpl> +{ + static NODE_TYPE run() + { + return BV_KDOP16; + } +}; + +//============================================================================== +template +struct GetNodeTypeImpl> +{ + static NODE_TYPE run() + { + return BV_KDOP18; + } +}; + +//============================================================================== +template +struct GetNodeTypeImpl> +{ + static NODE_TYPE run() + { + return BV_KDOP24; + } +}; + +} // namespace fcl + #endif diff --git a/include/fcl/BVH/BVH_utility.h b/include/fcl/BVH/BVH_utility.h index cb1468ce5..c57f52a76 100644 --- a/include/fcl/BVH/BVH_utility.h +++ b/include/fcl/BVH/BVH_utility.h @@ -39,15 +39,46 @@ #ifndef FCL_BVH_UTILITY_H #define FCL_BVH_UTILITY_H -#include "fcl/math/variance.h" +#include "fcl/math/variance3.h" #include "fcl/BVH/BVH_model.h" namespace fcl { -/// @brief Expand the BVH bounding boxes according to the variance matrix corresponding to the data stored within each BV node -template -void BVHExpand(BVHModel& model, const Variance3f* ucs, FCL_REAL r) +/// @brief Expand the BVH bounding boxes according to the variance matrix +/// corresponding to the data stored within each BV node +template +void BVHExpand( + BVHModel& model, + const Variance3* ucs, + typename BV::S r); + +/// @brief Expand the BVH bounding boxes according to the corresponding variance +/// information, for OBB +template +void BVHExpand( + BVHModel>& model, const Variance3* ucs, S r = 1.0); + +/// @brief Expand the BVH bounding boxes according to the corresponding variance +/// information, for RSS +template +void BVHExpand( + BVHModel>& model, const Variance3* ucs, S r = 1.0); + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +void BVHExpand( + BVHModel& model, + const Variance3* ucs, + typename BV::S r) { + using S = typename BV::S; + for(int i = 0; i < model.num_bvs; ++i) { BVNode& bvnode = model.getBV(i); @@ -56,9 +87,9 @@ void BVHExpand(BVHModel& model, const Variance3f* ucs, FCL_REAL r) for(int j = 0; j < bvnode.num_primitives; ++j) { int v_id = bvnode.first_primitive + j; - const Variance3f& uc = ucs[v_id]; + const Variance3& uc = ucs[v_id]; - Vector3d& v = model.vertices[bvnode.first_primitive + j]; + Vector3& v = model.vertices[bvnode.first_primitive + j]; for(int k = 0; k < 3; ++k) { @@ -71,28 +102,88 @@ void BVHExpand(BVHModel& model, const Variance3f* ucs, FCL_REAL r) } } -/// @brief Expand the BVH bounding boxes according to the corresponding variance information, for OBB -void BVHExpand(BVHModel& model, const Variance3f* ucs, FCL_REAL r); +//============================================================================== +template +void BVHExpand( + BVHModel>& model, + const Variance3* ucs, + S r) +{ + for(int i = 0; i < model.getNumBVs(); ++i) + { + BVNode>& bvnode = model.getBV(i); + + Vector3* vs = new Vector3[bvnode.num_primitives * 6]; + // TODO(JS): We could use one std::vector outside of the outter for-loop, + // and reuse it rather than creating and destructing the array every + // iteration. + + for(int j = 0; j < bvnode.num_primitives; ++j) + { + int v_id = bvnode.first_primitive + j; + const Variance3& uc = ucs[v_id]; + + Vector3&v = model.vertices[bvnode.first_primitive + j]; + + for(int k = 0; k < 3; ++k) + { + const auto index1 = 6 * j + 2 * k; + const auto index2 = index1 + 1; + vs[index1] = v; + vs[index1].noalias() += uc.axis.col(k) * (r * uc.sigma[k]); + vs[index2] = v; + vs[index2].noalias() -= uc.axis.col(k) * (r * uc.sigma[k]); + } + } + + OBB bv; + fit(vs, bvnode.num_primitives * 6, bv); + + delete [] vs; -/// @brief Expand the BVH bounding boxes according to the corresponding variance information, for RSS -void BVHExpand(BVHModel& model, const Variance3f* ucs, FCL_REAL r); + bvnode.bv = bv; + } +} + +//============================================================================== +template +void BVHExpand( + BVHModel>& model, + const Variance3* ucs, + S r) +{ + for(int i = 0; i < model.getNumBVs(); ++i) + { + BVNode>& bvnode = model.getBV(i); -/// @brief Compute the covariance matrix for a set or subset of points. if ts = null, then indices refer to points directly; otherwise refer to triangles -void getCovariance(Vector3d* ps, Vector3d* ps2, Triangle* ts, unsigned int* indices, int n, Matrix3d& M); + Vector3* vs = new Vector3[bvnode.num_primitives * 6]; + // TODO(JS): We could use one std::vector outside of the outter for-loop, + // and reuse it rather than creating and destructing the array every + // iteration. -/// @brief Compute the RSS bounding volume parameters: radius, rectangle size and the origin, given the BV axises. -void getRadiusAndOriginAndRectangleSize(Vector3d* ps, Vector3d* ps2, Triangle* ts, unsigned int* indices, int n, const Matrix3d& axis, Vector3d& origin, FCL_REAL l[2], FCL_REAL& r); + for(int j = 0; j < bvnode.num_primitives; ++j) + { + int v_id = bvnode.first_primitive + j; + const Variance3& uc = ucs[v_id]; -/// @brief Compute the bounding volume extent and center for a set or subset of points, given the BV axises. -void getExtentAndCenter(Vector3d* ps, Vector3d* ps2, Triangle* ts, unsigned int* indices, int n, const Matrix3d& axis, Vector3d& center, Vector3d& extent); + Vector3&v = model.vertices[bvnode.first_primitive + j]; -/// @brief Compute the center and radius for a triangle's circumcircle -void circumCircleComputation(const Vector3d& a, const Vector3d& b, const Vector3d& c, Vector3d& center, FCL_REAL& radius); + for(int k = 0; k < 3; ++k) + { + vs[6 * j + 2 * k] = v + uc.axis.col(k) * (r * uc.sigma[k]); + vs[6 * j + 2 * k + 1] = v - uc.axis.col(k) * (r * uc.sigma[k]); + } + } -/// @brief Compute the maximum distance from a given center point to a point cloud -FCL_REAL maximumDistance(Vector3d* ps, Vector3d* ps2, Triangle* ts, unsigned int* indices, int n, const Vector3d& query); + RSS bv; + fit(vs, bvnode.num_primitives * 6, bv); + delete [] vs; + bvnode.bv = bv; + } } +} // namespace fcl + #endif diff --git a/include/fcl/BVH/BV_fitter.h b/include/fcl/BVH/BV_fitter.h index cfe26da90..137643e07 100644 --- a/include/fcl/BVH/BV_fitter.h +++ b/include/fcl/BVH/BV_fitter.h @@ -47,39 +47,19 @@ namespace fcl { -/// @brief Compute a bounding volume that fits a set of n points. -template -void fit(Vector3d* ps, int n, BV& bv) -{ - for(int i = 0; i < n; ++i) - { - bv += ps[i]; - } -} - -template<> -void fit(Vector3d* ps, int n, OBB& bv); - -template<> -void fit(Vector3d* ps, int n, RSS& bv); - -template<> -void fit(Vector3d* ps, int n, kIOS& bv); - -template<> -void fit(Vector3d* ps, int n, OBBRSS& bv); - - /// @brief Interface for fitting a bv given the triangles or points inside it. -template +template class BVFitterBase { public: + + using S = typename BV::S; + /// @brief Set the primitives to be processed by the fitter - virtual void set(Vector3d* vertices_, Triangle* tri_indices_, BVHModelType type_) = 0; + virtual void set(Vector3* vertices_, Triangle* tri_indices_, BVHModelType type_) = 0; /// @brief Set the primitives to be processed by the fitter, for deformable mesh. - virtual void set(Vector3d* vertices_, Vector3d* prev_vertices_, Triangle* tri_indices_, BVHModelType type_) = 0; + virtual void set(Vector3* vertices_, Vector3* prev_vertices_, Triangle* tri_indices_, BVHModelType type_) = 0; /// @brief Compute the fitting BV virtual BV fit(unsigned int* primitive_indices, int num_primitives) = 0; @@ -89,266 +69,485 @@ class BVFitterBase }; /// @brief The class for the default algorithm fitting a bounding volume to a set of points -template +template class BVFitter : public BVFitterBase { public: + + using S = typename BVFitterBase::S; + /// @brief default deconstructor - virtual ~BVFitter() {} + virtual ~BVFitter(); /// @brief Prepare the geometry primitive data for fitting - void set(Vector3d* vertices_, Triangle* tri_indices_, BVHModelType type_) - { - vertices = vertices_; - prev_vertices = NULL; - tri_indices = tri_indices_; - type = type_; - } + void set( + Vector3* vertices_, Triangle* tri_indices_, BVHModelType type_); /// @brief Prepare the geometry primitive data for fitting, for deformable mesh - void set(Vector3d* vertices_, Vector3d* prev_vertices_, Triangle* tri_indices_, BVHModelType type_) - { - vertices = vertices_; - prev_vertices = prev_vertices_; - tri_indices = tri_indices_; - type = type_; - } + void set( + Vector3* vertices_, + Vector3* prev_vertices_, + Triangle* tri_indices_, + BVHModelType type_); /// @brief Compute a bounding volume that fits a set of primitives (points or triangles). /// The primitive data was set by set function and primitive_indices is the primitive index relative to the data - BV fit(unsigned int* primitive_indices, int num_primitives) - { - BV bv; - - if(type == BVH_MODEL_TRIANGLES) /// The primitive is triangle - { - for(int i = 0; i < num_primitives; ++i) - { - Triangle t = tri_indices[primitive_indices[i]]; - bv += vertices[t[0]]; - bv += vertices[t[1]]; - bv += vertices[t[2]]; - - if(prev_vertices) /// can fitting both current and previous frame - { - bv += prev_vertices[t[0]]; - bv += prev_vertices[t[1]]; - bv += prev_vertices[t[2]]; - } - } - } - else if(type == BVH_MODEL_POINTCLOUD) /// The primitive is point - { - for(int i = 0; i < num_primitives; ++i) - { - bv += vertices[primitive_indices[i]]; - - if(prev_vertices) /// can fitting both current and previous frame - { - bv += prev_vertices[primitive_indices[i]]; - } - } - } - - return bv; - } + BV fit(unsigned int* primitive_indices, int num_primitives); /// @brief Clear the geometry primitive data - void clear() - { - vertices = NULL; - prev_vertices = NULL; - tri_indices = NULL; - type = BVH_MODEL_UNKNOWN; - } + void clear(); private: - Vector3d* vertices; - Vector3d* prev_vertices; + Vector3* vertices; + Vector3* prev_vertices; Triangle* tri_indices; BVHModelType type; + + template + friend struct SetImpl; + + template + friend struct FitImpl; }; +//============================================================================// +// // +// Implementations // +// // +//============================================================================// -/// @brief Specification of BVFitter for OBB bounding volume -template<> -class BVFitter : public BVFitterBase +//============================================================================== +template +BVFitter::~BVFitter() { -public: - /// @brief Prepare the geometry primitive data for fitting - void set(Vector3d* vertices_, Triangle* tri_indices_, BVHModelType type_) - { - vertices = vertices_; - prev_vertices = NULL; - tri_indices = tri_indices_; - type = type_; - } + // Do nothing +} - /// @brief Prepare the geometry primitive data for fitting, for deformable mesh - void set(Vector3d* vertices_, Vector3d* prev_vertices_, Triangle* tri_indices_, BVHModelType type_) - { - vertices = vertices_; - prev_vertices = prev_vertices_; - tri_indices = tri_indices_; - type = type_; - } +//============================================================================== +template +struct SetImpl; - /// @brief Compute a bounding volume that fits a set of primitives (points or triangles). - /// The primitive data was set by set function and primitive_indices is the primitive index relative to the data. - OBB fit(unsigned int* primitive_indices, int num_primitives); +//============================================================================== +template +void BVFitter::set( + Vector3::S>* vertices_, + Triangle* tri_indices_, + BVHModelType type_) +{ + SetImpl::run(*this, vertices_, tri_indices_, type_); +} - /// brief Clear the geometry primitive data - void clear() - { - vertices = NULL; - prev_vertices = NULL; - tri_indices = NULL; - type = BVH_MODEL_UNKNOWN; - } +//============================================================================== +template +void BVFitter::set( + Vector3::S>* vertices_, + Vector3::S>* prev_vertices_, + Triangle* tri_indices_, + BVHModelType type_) +{ + SetImpl::run( + *this, vertices_, prev_vertices_, tri_indices_, type_); +} -private: +//============================================================================== +template +struct FitImpl; - Vector3d* vertices; - Vector3d* prev_vertices; - Triangle* tri_indices; - BVHModelType type; -}; +//============================================================================== +template +BV BVFitter::fit(unsigned int* primitive_indices, int num_primitives) +{ + return FitImpl::run( + *this, primitive_indices, num_primitives); +} +//============================================================================== +template +void BVFitter::clear() +{ + vertices = nullptr; + prev_vertices = nullptr; + tri_indices = nullptr; + type = BVH_MODEL_UNKNOWN; +} -/// @brief Specification of BVFitter for RSS bounding volume -template<> -class BVFitter : public BVFitterBase +//============================================================================== +template +struct SetImpl { -public: - /// brief Prepare the geometry primitive data for fitting - void set(Vector3d* vertices_, Triangle* tri_indices_, BVHModelType type_) + static void run( + BVFitter& fitter, + Vector3* vertices_, + Triangle* tri_indices_, + BVHModelType type_) { - vertices = vertices_; - prev_vertices = NULL; - tri_indices = tri_indices_; - type = type_; + fitter.vertices = vertices_; + fitter.prev_vertices = nullptr; + fitter.tri_indices = tri_indices_; + fitter.type = type_; } - /// @brief Prepare the geometry primitive data for fitting, for deformable mesh - void set(Vector3d* vertices_, Vector3d* prev_vertices_, Triangle* tri_indices_, BVHModelType type_) + static void run( + BVFitter& fitter, + Vector3* vertices_, + Vector3* prev_vertices_, + Triangle* tri_indices_, + BVHModelType type_) { - vertices = vertices_; - prev_vertices = prev_vertices_; - tri_indices = tri_indices_; - type = type_; + fitter.vertices = vertices_; + fitter.prev_vertices = prev_vertices_; + fitter.tri_indices = tri_indices_; + fitter.type = type_; } +}; - /// @brief Compute a bounding volume that fits a set of primitives (points or triangles). - /// The primitive data was set by set function and primitive_indices is the primitive index relative to the data. - RSS fit(unsigned int* primitive_indices, int num_primitives); +//============================================================================== +template +struct SetImpl> +{ + static void run( + BVFitter>& fitter, + Vector3* vertices_, + Triangle* tri_indices_, + BVHModelType type_) + { + fitter.vertices = vertices_; + fitter.prev_vertices = nullptr; + fitter.tri_indices = tri_indices_; + fitter.type = type_; + } - /// @brief Clear the geometry primitive data - void clear() + static void run( + BVFitter>& fitter, + Vector3* vertices_, + Vector3* prev_vertices_, + Triangle* tri_indices_, + BVHModelType type_) { - vertices = NULL; - prev_vertices = NULL; - tri_indices = NULL; - type = BVH_MODEL_UNKNOWN; + fitter.vertices = vertices_; + fitter.prev_vertices = prev_vertices_; + fitter.tri_indices = tri_indices_; + fitter.type = type_; } +}; -private: +//============================================================================== +template +struct SetImpl> +{ + static void run( + BVFitter>& fitter, + Vector3* vertices_, + Triangle* tri_indices_, + BVHModelType type_) + { + fitter.vertices = vertices_; + fitter.prev_vertices = nullptr; + fitter.tri_indices = tri_indices_; + fitter.type = type_; + } - Vector3d* vertices; - Vector3d* prev_vertices; - Triangle* tri_indices; - BVHModelType type; + static void run( + BVFitter>& fitter, + Vector3* vertices_, + Vector3* prev_vertices_, + Triangle* tri_indices_, + BVHModelType type_) + { + fitter.vertices = vertices_; + fitter.prev_vertices = prev_vertices_; + fitter.tri_indices = tri_indices_; + fitter.type = type_; + } }; - -/// @brief Specification of BVFitter for kIOS bounding volume -template<> -class BVFitter : public BVFitterBase +//============================================================================== +template +struct SetImpl> { -public: - /// @brief Prepare the geometry primitive data for fitting - void set(Vector3d* vertices_, Triangle* tri_indices_, BVHModelType type_) + static void run( + BVFitter>& fitter, + Vector3* vertices_, + Triangle* tri_indices_, + BVHModelType type_) { - vertices = vertices_; - prev_vertices = NULL; - tri_indices = tri_indices_; - type = type_; + fitter.vertices = vertices_; + fitter.prev_vertices = nullptr; + fitter.tri_indices = tri_indices_; + fitter.type = type_; } - /// @brief Prepare the geometry primitive data for fitting - void set(Vector3d* vertices_, Vector3d* prev_vertices_, Triangle* tri_indices_, BVHModelType type_) + static void run( + BVFitter>& fitter, + Vector3* vertices_, + Vector3* prev_vertices_, + Triangle* tri_indices_, + BVHModelType type_) { - vertices = vertices_; - prev_vertices = prev_vertices_; - tri_indices = tri_indices_; - type = type_; + fitter.vertices = vertices_; + fitter.prev_vertices = prev_vertices_; + fitter.tri_indices = tri_indices_; + fitter.type = type_; } +}; - /// @brief Compute a bounding volume that fits a set of primitives (points or triangles). - /// The primitive data was set by set function and primitive_indices is the primitive index relative to the data. - kIOS fit(unsigned int* primitive_indices, int num_primitives); - - /// @brief Clear the geometry primitive data - void clear() +//============================================================================== +template +struct SetImpl> +{ + static void run( + BVFitter>& fitter, + Vector3* vertices_, + Triangle* tri_indices_, + BVHModelType type_) { - vertices = NULL; - prev_vertices = NULL; - tri_indices = NULL; - type = BVH_MODEL_UNKNOWN; + fitter.vertices = vertices_; + fitter.prev_vertices = nullptr; + fitter.tri_indices = tri_indices_; + fitter.type = type_; } -private: - Vector3d* vertices; - Vector3d* prev_vertices; - Triangle* tri_indices; - BVHModelType type; + static void run( + BVFitter>& fitter, + Vector3* vertices_, + Vector3* prev_vertices_, + Triangle* tri_indices_, + BVHModelType type_) + { + fitter.vertices = vertices_; + fitter.prev_vertices = prev_vertices_; + fitter.tri_indices = tri_indices_; + fitter.type = type_; + } }; - -/// @brief Specification of BVFitter for OBBRSS bounding volume -template<> -class BVFitter : public BVFitterBase +//============================================================================== +template +struct FitImpl { -public: - /// @brief Prepare the geometry primitive data for fitting - void set(Vector3d* vertices_, Triangle* tri_indices_, BVHModelType type_) + static BV run( + const BVFitter& fitter, + unsigned int* primitive_indices, + int num_primitives) { - vertices = vertices_; - prev_vertices = NULL; - tri_indices = tri_indices_; - type = type_; + BV bv; + + if(fitter.type == BVH_MODEL_TRIANGLES) /// The primitive is triangle + { + for(int i = 0; i < num_primitives; ++i) + { + Triangle t = fitter.tri_indices[primitive_indices[i]]; + bv += fitter.vertices[t[0]]; + bv += fitter.vertices[t[1]]; + bv += fitter.vertices[t[2]]; + + if(fitter.prev_vertices) /// can fitting both current and previous frame + { + bv += fitter.prev_vertices[t[0]]; + bv += fitter.prev_vertices[t[1]]; + bv += fitter.prev_vertices[t[2]]; + } + } + } + else if(fitter.type == BVH_MODEL_POINTCLOUD) /// The primitive is point + { + for(int i = 0; i < num_primitives; ++i) + { + bv += fitter.vertices[primitive_indices[i]]; + + if(fitter.prev_vertices) /// can fitting both current and previous frame + { + bv += fitter.prev_vertices[primitive_indices[i]]; + } + } + } + + return bv; } +}; - /// @brief Prepare the geometry primitive data for fitting - void set(Vector3d* vertices_, Vector3d* prev_vertices_, Triangle* tri_indices_, BVHModelType type_) +//============================================================================== +template +struct FitImpl> +{ + static OBB run( + const BVFitter>& fitter, + unsigned int* primitive_indices, + int num_primitives) { - vertices = vertices_; - prev_vertices = prev_vertices_; - tri_indices = tri_indices_; - type = type_; + OBB bv; + + Matrix3 M; // row first matrix + Matrix3 E; // row first eigen-vectors + Vector3 s; // three eigen values + getCovariance( + fitter.vertices, fitter.prev_vertices, fitter.tri_indices, + primitive_indices, num_primitives, M); + eigen_old(M, s, E); + axisFromEigen(E, s, bv.axis); + + // set obb centers and extensions + getExtentAndCenter( + fitter.vertices, fitter.prev_vertices, fitter.tri_indices, + primitive_indices, num_primitives, + bv.axis, bv.To, bv.extent); + + return bv; } +}; - /// @brief Compute a bounding volume that fits a set of primitives (points or triangles). - /// The primitive data was set by set function and primitive_indices is the primitive index relative to the data. - OBBRSS fit(unsigned int* primitive_indices, int num_primitives); +//============================================================================== +template +struct FitImpl> +{ + static RSS run( + const BVFitter>& fitter, + unsigned int* primitive_indices, + int num_primitives) + { + RSS bv; + + Matrix3 M; // row first matrix + Matrix3 E; // row first eigen-vectors + Vector3 s; // three eigen values + getCovariance( + fitter.vertices, fitter.prev_vertices, fitter.tri_indices, + primitive_indices, num_primitives, M); + eigen_old(M, s, E); + axisFromEigen(E, s, bv.axis); + + // set rss origin, rectangle size and radius + getRadiusAndOriginAndRectangleSize( + fitter.vertices, fitter.prev_vertices, fitter.tri_indices, + primitive_indices, num_primitives, bv.axis, bv.To, bv.l, bv.r); - /// @brief Clear the geometry primitive data - void clear() + return bv; + } +}; + +//============================================================================== +template +struct FitImpl> +{ + static kIOS run( + const BVFitter>& fitter, + unsigned int* primitive_indices, + int num_primitives) { - vertices = NULL; - prev_vertices = NULL; - tri_indices = NULL; - type = BVH_MODEL_UNKNOWN; + kIOS bv; + + Matrix3 M; // row first matrix + Matrix3 E; // row first eigen-vectors + Vector3 s; + getCovariance( + fitter.vertices, fitter.prev_vertices, fitter.tri_indices, + primitive_indices, num_primitives, M); + eigen_old(M, s, E); + axisFromEigen(E, s, bv.obb.axis); + + // get centers and extensions + getExtentAndCenter( + fitter.vertices, fitter.prev_vertices, fitter.tri_indices, + primitive_indices, num_primitives, bv.obb.axis, bv.obb.To, bv.obb.extent); + + const Vector3& center = bv.obb.To; + const Vector3& extent = bv.obb.extent; + S r0 = maximumDistance( + fitter.vertices, fitter.prev_vertices, fitter.tri_indices, + primitive_indices, num_primitives, center); + + // decide k in kIOS + if(extent[0] > kIOS::ratio() * extent[2]) + { + if(extent[0] > kIOS::ratio() * extent[1]) bv.num_spheres = 5; + else bv.num_spheres = 3; + } + else bv.num_spheres = 1; + + bv.spheres[0].o = center; + bv.spheres[0].r = r0; + + if(bv.num_spheres >= 3) + { + S r10 = sqrt(r0 * r0 - extent[2] * extent[2]) * kIOS::invSinA(); + Vector3 delta = bv.obb.axis.col(2) * (r10 * kIOS::cosA() - extent[2]); + bv.spheres[1].o = center - delta; + bv.spheres[2].o = center + delta; + + S r11 = maximumDistance( + fitter.vertices, fitter.prev_vertices, fitter.tri_indices, + primitive_indices, num_primitives, bv.spheres[1].o); + S r12 = maximumDistance( + fitter.vertices, fitter.prev_vertices, fitter.tri_indices, + primitive_indices, num_primitives, bv.spheres[2].o); + + bv.spheres[1].o.noalias() += bv.obb.axis.col(2) * (-r10 + r11); + bv.spheres[2].o.noalias() += bv.obb.axis.col(2) * (r10 - r12); + + bv.spheres[1].r = r10; + bv.spheres[2].r = r10; + } + + if(bv.num_spheres >= 5) + { + S r10 = bv.spheres[1].r; + Vector3 delta = bv.obb.axis.col(1) * (sqrt(r10 * r10 - extent[0] * extent[0] - extent[2] * extent[2]) - extent[1]); + bv.spheres[3].o = bv.spheres[0].o - delta; + bv.spheres[4].o = bv.spheres[0].o + delta; + + S r21 = 0, r22 = 0; + r21 = maximumDistance( + fitter.vertices, fitter.prev_vertices, fitter.tri_indices, + primitive_indices, num_primitives, bv.spheres[3].o); + r22 = maximumDistance( + fitter.vertices, fitter.prev_vertices, fitter.tri_indices, + primitive_indices, num_primitives, bv.spheres[4].o); + + bv.spheres[3].o.noalias() += bv.obb.axis.col(1) * (-r10 + r21); + bv.spheres[4].o.noalias() += bv.obb.axis.col(1) * (r10 - r22); + + bv.spheres[3].r = r10; + bv.spheres[4].r = r10; + } + + return bv; } +}; -private: +//============================================================================== +template +struct FitImpl> +{ + static OBBRSS run( + const BVFitter>& fitter, + unsigned int* primitive_indices, + int num_primitives) + { + OBBRSS bv; + Matrix3 M; + Matrix3 E; + Vector3 s; + getCovariance( + fitter.vertices, fitter.prev_vertices, fitter.tri_indices, + primitive_indices, num_primitives, M); + eigen_old(M, s, E); + axisFromEigen(E, s, bv.obb.axis); + bv.rss.axis = bv.obb.axis; + + getExtentAndCenter( + fitter.vertices, fitter.prev_vertices, fitter.tri_indices, + primitive_indices, num_primitives, bv.obb.axis, bv.obb.To, bv.obb.extent); + + getRadiusAndOriginAndRectangleSize( + fitter.vertices, fitter.prev_vertices, fitter.tri_indices, + primitive_indices, num_primitives, + bv.rss.axis, bv.rss.To, bv.rss.l, bv.rss.r); - Vector3d* vertices; - Vector3d* prev_vertices; - Triangle* tri_indices; - BVHModelType type; + return bv; + } }; -} +} // namespace fcl #endif diff --git a/include/fcl/BVH/BV_splitter.h b/include/fcl/BVH/BV_splitter.h index 31a751fb9..7c1b44b7b 100644 --- a/include/fcl/BVH/BV_splitter.h +++ b/include/fcl/BVH/BV_splitter.h @@ -49,94 +49,83 @@ namespace fcl { /// @brief Base interface for BV splitting algorithm -template +template class BVSplitterBase { public: + + using S = typename BV::S; + /// @brief Set the geometry data needed by the split rule - virtual void set(Vector3d* vertices_, Triangle* tri_indices_, BVHModelType type_) = 0; + virtual void set( + Vector3* vertices_, + Triangle* tri_indices_, + BVHModelType type_) = 0; - /// @brief Compute the split rule according to a subset of geometry and the corresponding BV node - virtual void computeRule(const BV& bv, unsigned int* primitive_indices, int num_primitives) = 0; + /// @brief Compute the split rule according to a subset of geometry and the + /// corresponding BV node + virtual void computeRule( + const BV& bv, unsigned int* primitive_indices, int num_primitives) = 0; /// @brief Apply the split rule on a given point - virtual bool apply(const Vector3d& q) const = 0; + virtual bool apply(const Vector3& q) const = 0; /// @brief Clear the geometry data set before virtual void clear() = 0; }; - /// @brief Three types of split algorithms are provided in FCL as default -enum SplitMethodType {SPLIT_METHOD_MEAN, SPLIT_METHOD_MEDIAN, SPLIT_METHOD_BV_CENTER}; - +enum SplitMethodType +{ + SPLIT_METHOD_MEAN, + SPLIT_METHOD_MEDIAN, + SPLIT_METHOD_BV_CENTER +}; /// @brief A class describing the split rule that splits each BV node -template +template class BVSplitter : public BVSplitterBase { public: - BVSplitter(SplitMethodType method) : split_method(method) - { - } + using S = typename BV::S; + + BVSplitter(SplitMethodType method); /// @brief Default deconstructor - virtual ~BVSplitter() {} + virtual ~BVSplitter(); /// @brief Set the geometry data needed by the split rule - void set(Vector3d* vertices_, Triangle* tri_indices_, BVHModelType type_) - { - vertices = vertices_; - tri_indices = tri_indices_; - type = type_; - } + void set( + Vector3* vertices_, Triangle* tri_indices_, BVHModelType type_); - /// @brief Compute the split rule according to a subset of geometry and the corresponding BV node - void computeRule(const BV& bv, unsigned int* primitive_indices, int num_primitives) - { - switch(split_method) - { - case SPLIT_METHOD_MEAN: - computeRule_mean(bv, primitive_indices, num_primitives); - break; - case SPLIT_METHOD_MEDIAN: - computeRule_median(bv, primitive_indices, num_primitives); - break; - case SPLIT_METHOD_BV_CENTER: - computeRule_bvcenter(bv, primitive_indices, num_primitives); - break; - default: - std::cerr << "Split method not supported" << std::endl; - } - } + /// @brief Compute the split rule according to a subset of geometry and the + /// corresponding BV node + void computeRule( + const BV& bv, unsigned int* primitive_indices, int num_primitives); /// @brief Apply the split rule on a given point - bool apply(const Vector3d& q) const - { - return q[split_axis] > split_value; - } + bool apply(const Vector3& q) const; /// @brief Clear the geometry data set before - void clear() - { - vertices = NULL; - tri_indices = NULL; - type = BVH_MODEL_UNKNOWN; - } + void clear(); private: - /// @brief The axis based on which the split decision is made. For most BV, the axis is aligned with one of the world coordinate, so only split_axis is needed. - /// For oriented node, we can use a vector to make a better split decision. + /// @brief The axis based on which the split decision is made. For most BV, + /// the axis is aligned with one of the world coordinate, so only split_axis + /// is needed. For oriented node, we can use a vector to make a better split + /// decision. int split_axis; - Vector3d split_vector; + Vector3 split_vector; - /// @brief The split threshold, different primitives are splitted according whether their projection on the split_axis is larger or smaller than the threshold - FCL_REAL split_value; + /// @brief The split threshold, different primitives are splitted according + /// whether their projection on the split_axis is larger or smaller than the + /// threshold + S split_value; /// @brief The mesh vertices or points handled by the splitter - Vector3d* vertices; + Vector3* vertices; /// @brief The triangles handled by the splitter Triangle* tri_indices; @@ -148,9 +137,141 @@ class BVSplitter : public BVSplitterBase SplitMethodType split_method; /// @brief Split algorithm 1: Split the node from center - void computeRule_bvcenter(const BV& bv, unsigned int* primitive_indices, int num_primitives) + void computeRule_bvcenter( + const BV& bv, unsigned int* primitive_indices, int num_primitives); + + /// @brief Split algorithm 2: Split the node according to the mean of the data + /// contained + void computeRule_mean( + const BV& bv, unsigned int* primitive_indices, int num_primitives); + + /// @brief Split algorithm 3: Split the node according to the median of the + /// data contained + void computeRule_median( + const BV& bv, unsigned int* primitive_indices, int num_primitives); + + template + friend struct ApplyImpl; + + template + friend struct ComputeRuleCenterImpl; + + template + friend struct ComputeRuleMeanImpl; + + template + friend struct ComputeRuleMedianImpl; +}; + +template +void computeSplitVector(const BV& bv, Vector3& split_vector); + +template +void computeSplitValue_bvcenter(const BV& bv, S& split_value); + +template +void computeSplitValue_mean( + const BV& bv, + Vector3* vertices, + Triangle* triangles, + unsigned int* primitive_indices, + int num_primitives, + BVHModelType type, + const Vector3& split_vector, + S& split_value); + +template +void computeSplitValue_median( + const BV& bv, + Vector3* vertices, + Triangle* triangles, + unsigned int* primitive_indices, + int num_primitives, + BVHModelType type, + const Vector3& split_vector, + S& split_value); + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +BVSplitter::BVSplitter(SplitMethodType method) + : split_method(method) +{ + // Do nothing +} + +//============================================================================== +template +BVSplitter::~BVSplitter() +{ + // Do nothing +} + +//============================================================================== +template +void BVSplitter::set( + Vector3* vertices_, Triangle* tri_indices_, BVHModelType type_) +{ + vertices = vertices_; + tri_indices = tri_indices_; + type = type_; +} + +//============================================================================== +template +void BVSplitter::computeRule( + const BV& bv, unsigned int* primitive_indices, int num_primitives) +{ + switch(split_method) + { + case SPLIT_METHOD_MEAN: + computeRule_mean(bv, primitive_indices, num_primitives); + break; + case SPLIT_METHOD_MEDIAN: + computeRule_median(bv, primitive_indices, num_primitives); + break; + case SPLIT_METHOD_BV_CENTER: + computeRule_bvcenter(bv, primitive_indices, num_primitives); + break; + default: + std::cerr << "Split method not supported" << std::endl; + } +} + +//============================================================================== +template +struct ApplyImpl +{ + static bool run( + const BVSplitter& splitter, const Vector3& q) + { + return q[splitter.split_axis] > splitter.split_value; + } +}; + +//============================================================================== +template +bool BVSplitter::apply(const Vector3& q) const +{ + return ApplyImpl::run(*this, q); +} + +//============================================================================== +template +struct ComputeRuleCenterImpl +{ + static void run( + BVSplitter& splitter, + const BV& bv, + unsigned int* /*primitive_indices*/, + int /*num_primitives*/) { - Vector3d center = bv.center(); + Vector3 center = bv.center(); int axis = 2; if(bv.width() >= bv.height() && bv.width() >= bv.depth()) @@ -158,12 +279,29 @@ class BVSplitter : public BVSplitterBase else if(bv.height() >= bv.width() && bv.height() >= bv.depth()) axis = 1; - split_axis = axis; - split_value = center[axis]; + splitter.split_axis = axis; + splitter.split_value = center[axis]; } +}; - /// @brief Split algorithm 2: Split the node according to the mean of the data contained - void computeRule_mean(const BV& bv, unsigned int* primitive_indices, int num_primitives) +//============================================================================== +template +void BVSplitter::computeRule_bvcenter( + const BV& bv, unsigned int* primitive_indices, int num_primitives) +{ + ComputeRuleCenterImpl::run( + *this, bv, primitive_indices, num_primitives); +} + +//============================================================================== +template +struct ComputeRuleMeanImpl +{ + static void run( + BVSplitter& splitter, + const BV& bv, + unsigned int* primitive_indices, + int num_primitives) { int axis = 2; @@ -172,32 +310,51 @@ class BVSplitter : public BVSplitterBase else if(bv.height() >= bv.width() && bv.height() >= bv.depth()) axis = 1; - split_axis = axis; - FCL_REAL sum = 0; + splitter.split_axis = axis; + S sum = 0; - if(type == BVH_MODEL_TRIANGLES) + if(splitter.type == BVH_MODEL_TRIANGLES) { for(int i = 0; i < num_primitives; ++i) { - const Triangle& t = tri_indices[primitive_indices[i]]; - sum += (vertices[t[0]][split_axis] + vertices[t[1]][split_axis] + vertices[t[2]][split_axis]); + const Triangle& t = splitter.tri_indices[primitive_indices[i]]; + sum += splitter.vertices[t[0]][splitter.split_axis] + + splitter.vertices[t[1]][splitter.split_axis] + + splitter.vertices[t[2]][splitter.split_axis]; } sum /= 3; } - else if(type == BVH_MODEL_POINTCLOUD) + else if(splitter.type == BVH_MODEL_POINTCLOUD) { for(int i = 0; i < num_primitives; ++i) { - sum += vertices[primitive_indices[i]][split_axis]; + sum += splitter.vertices[primitive_indices[i]][splitter.split_axis]; } } - split_value = sum / num_primitives; + splitter.split_value = sum / num_primitives; } +}; - /// @brief Split algorithm 3: Split the node according to the median of the data contained - void computeRule_median(const BV& bv, unsigned int* primitive_indices, int num_primitives) +//============================================================================== +template +void BVSplitter::computeRule_mean( + const BV& bv, unsigned int* primitive_indices, int num_primitives) +{ + ComputeRuleMeanImpl::run( + *this, bv, primitive_indices, num_primitives); +} + +//============================================================================== +template +struct ComputeRuleMedianImpl +{ + static void run( + BVSplitter& splitter, + const BV& bv, + unsigned int* primitive_indices, + int num_primitives) { int axis = 2; @@ -206,85 +363,467 @@ class BVSplitter : public BVSplitterBase else if(bv.height() >= bv.width() && bv.height() >= bv.depth()) axis = 1; - split_axis = axis; - std::vector proj(num_primitives); + splitter.split_axis = axis; + std::vector proj(num_primitives); - if(type == BVH_MODEL_TRIANGLES) + if(splitter.type == BVH_MODEL_TRIANGLES) { for(int i = 0; i < num_primitives; ++i) { - const Triangle& t = tri_indices[primitive_indices[i]]; - proj[i] = (vertices[t[0]][split_axis] + vertices[t[1]][split_axis] + vertices[t[2]][split_axis]) / 3; + const Triangle& t = splitter.tri_indices[primitive_indices[i]]; + proj[i] = (splitter.vertices[t[0]][splitter.split_axis] + + splitter.vertices[t[1]][splitter.split_axis] + + splitter.vertices[t[2]][splitter.split_axis]) / 3; } } - else if(type == BVH_MODEL_POINTCLOUD) + else if(splitter.type == BVH_MODEL_POINTCLOUD) { for(int i = 0; i < num_primitives; ++i) - proj[i] = vertices[primitive_indices[i]][split_axis]; + proj[i] = splitter.vertices[primitive_indices[i]][splitter.split_axis]; } std::sort(proj.begin(), proj.end()); if(num_primitives % 2 == 1) { - split_value = proj[(num_primitives - 1) / 2]; + splitter.split_value = proj[(num_primitives - 1) / 2]; } else { - split_value = (proj[num_primitives / 2] + proj[num_primitives / 2 - 1]) / 2; + splitter.split_value = (proj[num_primitives / 2] + proj[num_primitives / 2 - 1]) / 2; } } }; +//============================================================================== +template +void BVSplitter::computeRule_median( + const BV& bv, unsigned int* primitive_indices, int num_primitives) +{ + ComputeRuleMedianImpl::run( + *this, bv, primitive_indices, num_primitives); +} -template<> -bool BVSplitter::apply(const Vector3d& q) const; +//============================================================================== +template +struct ComputeRuleCenterImpl> +{ + static void run( + BVSplitter>& splitter, + const OBB& bv, + unsigned int* /*primitive_indices*/, + int /*num_primitives*/) + { + computeSplitVector>(bv, splitter.split_vector); + computeSplitValue_bvcenter>(bv, splitter.split_value); + } +}; -template<> -bool BVSplitter::apply(const Vector3d& q) const; +//============================================================================== +template +struct ComputeRuleMeanImpl> +{ + static void run( + BVSplitter>& splitter, + const OBB& bv, + unsigned int* primitive_indices, + int num_primitives) + { + computeSplitVector>(bv, splitter.split_vector); + computeSplitValue_mean>( + bv, splitter.vertices, splitter.tri_indices, primitive_indices, + num_primitives, splitter.type, splitter.split_vector, splitter.split_value); + } +}; -template<> -bool BVSplitter::apply(const Vector3d& q) const; +//============================================================================== +template +struct ComputeRuleMedianImpl> +{ + static void run( + BVSplitter>& splitter, + const OBB& bv, + unsigned int* primitive_indices, + int num_primitives) + { + computeSplitVector>(bv, splitter.split_vector); + computeSplitValue_median>( + bv, splitter.vertices, splitter.tri_indices, primitive_indices, + num_primitives, splitter.type, splitter.split_vector, splitter.split_value); + } +}; -template<> -bool BVSplitter::apply(const Vector3d& q) const; +//============================================================================== +template +struct ComputeRuleCenterImpl> +{ + static void run( + BVSplitter>& splitter, + const RSS& bv, + unsigned int* /*primitive_indices*/, + int /*num_primitives*/) + { + computeSplitVector>(bv, splitter.split_vector); + computeSplitValue_bvcenter>(bv, splitter.split_value); + } +}; -template<> -void BVSplitter::computeRule_bvcenter(const OBB& bv, unsigned int* primitive_indices, int num_primitives); +//============================================================================== +template +struct ComputeRuleMeanImpl> +{ + static void run( + BVSplitter>& splitter, + const RSS& bv, + unsigned int* primitive_indices, + int num_primitives) + { + computeSplitVector>(bv, splitter.split_vector); + computeSplitValue_mean>( + bv, splitter.vertices, splitter.tri_indices, primitive_indices, + num_primitives, splitter.type, splitter.split_vector, splitter.split_value); + } +}; -template<> -void BVSplitter::computeRule_mean(const OBB& bv, unsigned int* primitive_indices, int num_primitives); +//============================================================================== +template +struct ComputeRuleMedianImpl> +{ + static void run( + BVSplitter>& splitter, + const RSS& bv, + unsigned int* primitive_indices, + int num_primitives) + { + computeSplitVector>(bv, splitter.split_vector); + computeSplitValue_median>( + bv, splitter.vertices, splitter.tri_indices, primitive_indices, + num_primitives, splitter.type, splitter.split_vector, splitter.split_value); + } +}; -template<> -void BVSplitter::computeRule_median(const OBB& bv, unsigned int* primitive_indices, int num_primitives); +//============================================================================== +template +struct ComputeRuleCenterImpl> +{ + static void run( + BVSplitter>& splitter, + const kIOS& bv, + unsigned int* /*primitive_indices*/, + int /*num_primitives*/) + { + computeSplitVector>(bv, splitter.split_vector); + computeSplitValue_bvcenter>(bv, splitter.split_value); + } +}; -template<> -void BVSplitter::computeRule_bvcenter(const RSS& bv, unsigned int* primitive_indices, int num_primitives); - -template<> -void BVSplitter::computeRule_mean(const RSS& bv, unsigned int* primitive_indices, int num_primitives); +//============================================================================== +template +struct ComputeRuleMeanImpl> +{ + static void run( + BVSplitter>& splitter, + const kIOS& bv, + unsigned int* primitive_indices, + int num_primitives) + { + computeSplitVector>(bv, splitter.split_vector); + computeSplitValue_mean>( + bv, splitter.vertices, splitter.tri_indices, primitive_indices, + num_primitives, splitter.type, splitter.split_vector, splitter.split_value); + } +}; -template<> -void BVSplitter::computeRule_median(const RSS& bv, unsigned int* primitive_indices, int num_primitives); +//============================================================================== +template +struct ComputeRuleMedianImpl> +{ + static void run( + BVSplitter>& splitter, + const kIOS& bv, + unsigned int* primitive_indices, + int num_primitives) + { + computeSplitVector>(bv, splitter.split_vector); + computeSplitValue_median>( + bv, splitter.vertices, splitter.tri_indices, primitive_indices, + num_primitives, splitter.type, splitter.split_vector, splitter.split_value); + } +}; -template<> -void BVSplitter::computeRule_bvcenter(const kIOS& bv, unsigned int* primitive_indices, int num_primitives); +//============================================================================== +template +struct ComputeRuleCenterImpl> +{ + static void run( + BVSplitter>& splitter, + const OBBRSS& bv, + unsigned int* /*primitive_indices*/, + int /*num_primitives*/) + { + computeSplitVector>(bv, splitter.split_vector); + computeSplitValue_bvcenter>(bv, splitter.split_value); + } +}; -template<> -void BVSplitter::computeRule_mean(const kIOS& bv, unsigned int* primitive_indices, int num_primitives); +//============================================================================== +template +struct ComputeRuleMeanImpl> +{ + static void run( + BVSplitter>& splitter, + const OBBRSS& bv, + unsigned int* primitive_indices, + int num_primitives) + { + computeSplitVector>(bv, splitter.split_vector); + computeSplitValue_mean>( + bv, splitter.vertices, splitter.tri_indices, primitive_indices, + num_primitives, splitter.type, splitter.split_vector, splitter.split_value); + } +}; -template<> -void BVSplitter::computeRule_median(const kIOS& bv, unsigned int* primitive_indices, int num_primitives); +//============================================================================== +template +struct ComputeRuleMedianImpl> +{ + static void run( + BVSplitter>& splitter, + const OBBRSS& bv, + unsigned int* primitive_indices, + int num_primitives) + { + computeSplitVector>(bv, splitter.split_vector); + computeSplitValue_median>( + bv, splitter.vertices, splitter.tri_indices, primitive_indices, + num_primitives, splitter.type, splitter.split_vector, splitter.split_value); + } +}; -template<> -void BVSplitter::computeRule_bvcenter(const OBBRSS& bv, unsigned int* primitive_indices, int num_primitives); +//============================================================================== +template +struct ApplyImpl> +{ + static bool run( + const BVSplitter>& splitter, + const Vector3& q) + { + return splitter.split_vector.dot(q) > splitter.split_value; + } +}; -template<> -void BVSplitter::computeRule_mean(const OBBRSS& bv, unsigned int* primitive_indices, int num_primitives); +//============================================================================== +template +struct ApplyImpl> +{ + static bool run( + const BVSplitter>& splitter, + const Vector3& q) + { + return splitter.split_vector.dot(q) > splitter.split_value; + } +}; -template<> -void BVSplitter::computeRule_median(const OBBRSS& bv, unsigned int* primitive_indices, int num_primitives); +//============================================================================== +template +struct ApplyImpl> +{ + static bool run( + const BVSplitter>& splitter, + const Vector3& q) + { + return splitter.split_vector.dot(q) > splitter.split_value; + } +}; + +//============================================================================== +template +struct ApplyImpl> +{ + static bool run( + const BVSplitter>& splitter, + const Vector3& q) + { + return splitter.split_vector.dot(q) > splitter.split_value; + } +}; + +//============================================================================== +template +void BVSplitter::clear() +{ + vertices = nullptr; + tri_indices = nullptr; + type = BVH_MODEL_UNKNOWN; +} + +//============================================================================== +template +struct ComputeSplitVectorImpl +{ + static void run(const BV& bv, Vector3& split_vector) + { + split_vector = bv.axis.col(0); + } +}; +//============================================================================== +template +void computeSplitVector(const BV& bv, Vector3& split_vector) +{ + ComputeSplitVectorImpl::run(bv, split_vector); } +//============================================================================== +template +struct ComputeSplitVectorImpl> +{ + static void run(const kIOS& bv, Vector3& split_vector) + { + /* + switch(bv.num_spheres) + { + case 1: + split_vector = Vector3(1, 0, 0); + break; + case 3: + { + Vector3 v[3]; + v[0] = bv.spheres[1].o - bv.spheres[0].o; + v[0].normalize(); + generateCoordinateSystem(v[0], v[1], v[2]); + split_vector = v[1]; + } + break; + case 5: + { + Vector3 v[2]; + v[0] = bv.spheres[1].o - bv.spheres[0].o; + v[1] = bv.spheres[3].o - bv.spheres[0].o; + split_vector = v[0].cross(v[1]); + split_vector.normalize(); + } + break; + default: + ; + } + */ + split_vector = bv.obb.axis.col(0); + } +}; + +//============================================================================== +template +struct ComputeSplitVectorImpl> +{ + static void run(const OBBRSS& bv, Vector3& split_vector) + { + split_vector = bv.obb.axis.col(0); + } +}; + +//============================================================================== +template +void computeSplitValue_bvcenter(const BV& bv, S& split_value) +{ + Vector3 center = bv.center(); + split_value = center[0]; +} + +//============================================================================== +template +void computeSplitValue_mean( + const BV& bv, + Vector3* vertices, + Triangle* triangles, + unsigned int* primitive_indices, + int num_primitives, + BVHModelType type, + const Vector3& split_vector, + S& split_value) +{ + S sum = 0.0; + if(type == BVH_MODEL_TRIANGLES) + { + S c[3] = {0.0, 0.0, 0.0}; + + for(int i = 0; i < num_primitives; ++i) + { + const Triangle& t = triangles[primitive_indices[i]]; + const Vector3& p1 = vertices[t[0]]; + const Vector3& p2 = vertices[t[1]]; + const Vector3& p3 = vertices[t[2]]; + + c[0] += (p1[0] + p2[0] + p3[0]); + c[1] += (p1[1] + p2[1] + p3[1]); + c[2] += (p1[2] + p2[2] + p3[2]); + } + split_value = (c[0] * split_vector[0] + c[1] * split_vector[1] + c[2] * split_vector[2]) / (3 * num_primitives); + } + else if(type == BVH_MODEL_POINTCLOUD) + { + for(int i = 0; i < num_primitives; ++i) + { + const Vector3& p = vertices[primitive_indices[i]]; + Vector3 v(p[0], p[1], p[2]); + sum += v.dot(split_vector); + } + + split_value = sum / num_primitives; + } +} + +//============================================================================== +template +void computeSplitValue_median( + const BV& bv, + Vector3* vertices, + Triangle* triangles, + unsigned int* primitive_indices, + int num_primitives, + BVHModelType type, + const Vector3& split_vector, + S& split_value) +{ + std::vector proj(num_primitives); + + if(type == BVH_MODEL_TRIANGLES) + { + for(int i = 0; i < num_primitives; ++i) + { + const Triangle& t = triangles[primitive_indices[i]]; + const Vector3& p1 = vertices[t[0]]; + const Vector3& p2 = vertices[t[1]]; + const Vector3& p3 = vertices[t[2]]; + Vector3 centroid3(p1[0] + p2[0] + p3[0], + p1[1] + p2[1] + p3[1], + p1[2] + p2[2] + p3[2]); + + proj[i] = centroid3.dot(split_vector) / 3; + } + } + else if(type == BVH_MODEL_POINTCLOUD) + { + for(int i = 0; i < num_primitives; ++i) + { + const Vector3& p = vertices[primitive_indices[i]]; + Vector3 v(p[0], p[1], p[2]); + proj[i] = v.dot(split_vector); + } + } + + std::sort(proj.begin(), proj.end()); + + if(num_primitives % 2 == 1) + { + split_value = proj[(num_primitives - 1) / 2]; + } + else + { + split_value = (proj[num_primitives / 2] + proj[num_primitives / 2 - 1]) / 2; + } +} + +} // namespace fcl + #endif diff --git a/include/fcl/articulated_model/joint.h b/include/fcl/articulated_model/joint.h index 98fde51d0..074b506ee 100644 --- a/include/fcl/articulated_model/joint.h +++ b/include/fcl/articulated_model/joint.h @@ -55,12 +55,13 @@ class Link; enum JointType {JT_UNKNOWN, JT_PRISMATIC, JT_REVOLUTE, JT_BALLEULER}; /// @brief Base Joint +template class Joint { public: Joint(const std::shared_ptr& link_parent, const std::shared_ptr& link_child, - const Transform3d& transform_to_parent, + const Transform3& transform_to_parent, const std::string& name); Joint(const std::string& name); @@ -70,7 +71,7 @@ class Joint const std::string& getName() const; void setName(const std::string& name); - virtual Transform3d getLocalTransform() const = 0; + virtual Transform3 getLocalTransform() const = 0; virtual std::size_t getNumDofs() const = 0; @@ -85,8 +86,8 @@ class Joint JointType getJointType() const; - const Transform3d& getTransformToParent() const; - void setTransformToParent(const Transform3d& t); + const Transform3& getTransformToParent() const; + void setTransformToParent(const Transform3& t); protected: @@ -99,67 +100,276 @@ class Joint std::shared_ptr joint_cfg_; - Transform3d transform_to_parent_; -}; + Transform3 transform_to_parent_; +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; +template class PrismaticJoint : public Joint { public: PrismaticJoint(const std::shared_ptr& link_parent, const std::shared_ptr& link_child, - const Transform3d& transform_to_parent, + const Transform3& transform_to_parent, const std::string& name, - const Vector3d& axis); + const Vector3& axis); virtual ~PrismaticJoint() {} - Transform3d getLocalTransform() const; + Transform3 getLocalTransform() const; std::size_t getNumDofs() const; - const Vector3d& getAxis() const; + const Vector3& getAxis() const; protected: - Vector3d axis_; + Vector3 axis_; }; +template class RevoluteJoint : public Joint { public: RevoluteJoint(const std::shared_ptr& link_parent, const std::shared_ptr& link_child, - const Transform3d& transform_to_parent, + const Transform3& transform_to_parent, const std::string& name, - const Vector3d& axis); + const Vector3& axis); virtual ~RevoluteJoint() {} - Transform3d getLocalTransform() const; + Transform3 getLocalTransform() const; std::size_t getNumDofs() const; - const Vector3d& getAxis() const; + const Vector3& getAxis() const; protected: - Vector3d axis_; + Vector3 axis_; }; - - +template class BallEulerJoint : public Joint { public: BallEulerJoint(const std::shared_ptr& link_parent, const std::shared_ptr& link_child, - const Transform3d& transform_to_parent, + const Transform3& transform_to_parent, const std::string& name); virtual ~BallEulerJoint() {} std::size_t getNumDofs() const; - Transform3d getLocalTransform() const; + Transform3 getLocalTransform() const; }; +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +Joint::Joint(const std::shared_ptr& link_parent, const std::shared_ptr& link_child, + const Transform3& transform_to_parent, + const std::string& name) : + link_parent_(link_parent), link_child_(link_child), + name_(name), + transform_to_parent_(transform_to_parent) +{} + +//============================================================================== +template +Joint::Joint(const std::string& name) : + name_(name) +{ +} + +//============================================================================== +template +const std::string& Joint::getName() const +{ + return name_; +} + +//============================================================================== +template +void Joint::setName(const std::string& name) +{ + name_ = name; +} + +//============================================================================== +template +std::shared_ptr Joint::getJointConfig() const +{ + return joint_cfg_; +} + +//============================================================================== +template +void Joint::setJointConfig(const std::shared_ptr& joint_cfg) +{ + joint_cfg_ = joint_cfg; +} + +//============================================================================== +template +std::shared_ptr Joint::getParentLink() const +{ + return link_parent_.lock(); +} + +//============================================================================== +template +std::shared_ptr Joint::getChildLink() const +{ + return link_child_.lock(); +} + +//============================================================================== +template +void Joint::setParentLink(const std::shared_ptr& link) +{ + link_parent_ = link; +} + +//============================================================================== +template +void Joint::setChildLink(const std::shared_ptr& link) +{ + link_child_ = link; +} + +//============================================================================== +template +JointType Joint::getJointType() const +{ + return type_; +} + +//============================================================================== +template +const Transform3& Joint::getTransformToParent() const +{ + return transform_to_parent_; +} + +//============================================================================== +template +void Joint::setTransformToParent(const Transform3& t) +{ + transform_to_parent_ = t; +} + +//============================================================================== +template +PrismaticJoint::PrismaticJoint(const std::shared_ptr& link_parent, const std::shared_ptr& link_child, + const Transform3& transform_to_parent, + const std::string& name, + const Vector3& axis) : + Joint(link_parent, link_child, transform_to_parent, name), + axis_(axis) +{ + type_ = JT_PRISMATIC; +} + +//============================================================================== +template +const Vector3& PrismaticJoint::getAxis() const +{ + return axis_; +} +//============================================================================== +template +std::size_t PrismaticJoint::getNumDofs() const +{ + return 1; } +//============================================================================== +template +Transform3 PrismaticJoint::getLocalTransform() const +{ + const Quaternion quat(transform_to_parent_.linear()); + const Vector3& transl = transform_to_parent_.translation(); + + Transform3 tf = Transform3::Identity(); + tf.linear() = quat.toRotationMatrix(); + tf.translation() = quat * (axis_ * (*joint_cfg_)[0]) + transl; + + return tf; +} + +//============================================================================== +template +RevoluteJoint::RevoluteJoint(const std::shared_ptr& link_parent, const std::shared_ptr& link_child, + const Transform3& transform_to_parent, + const std::string& name, + const Vector3& axis) : + Joint(link_parent, link_child, transform_to_parent, name), + axis_(axis) +{ + type_ = JT_REVOLUTE; +} + +//============================================================================== +template +const Vector3& RevoluteJoint::getAxis() const +{ + return axis_; +} + +//============================================================================== +template +std::size_t RevoluteJoint::getNumDofs() const +{ + return 1; +} + +//============================================================================== +template +Transform3 RevoluteJoint::getLocalTransform() const +{ + Transform3 tf = Transform3::Identity(); + tf.linear() = transform_to_parent_.linear() * AngleAxis((*joint_cfg_)[0], axis_); + tf.translation() = transform_to_parent_.translation(); + + return tf; +} + +//============================================================================== +template +BallEulerJoint::BallEulerJoint(const std::shared_ptr& link_parent, const std::shared_ptr& link_child, + const Transform3& transform_to_parent, + const std::string& name) : + Joint(link_parent, link_child, transform_to_parent, name) +{} + +//============================================================================== +template +std::size_t BallEulerJoint::getNumDofs() const +{ + return 3; +} + +//============================================================================== +template +Transform3 BallEulerJoint::getLocalTransform() const +{ + Matrix3 rot( + AngleAxis((*joint_cfg_)[0], Eigen::Vector3::UnitX()) + * AngleAxis((*joint_cfg_)[1], Eigen::Vector3::UnitY()) + * AngleAxis((*joint_cfg_)[2], Eigen::Vector3::UnitZ())); + + Transform3 tf = Transform3::Identity(); + tf.linear() = rot; + + return transform_to_parent_ * tf; +} + +} // namespace fcl + #endif diff --git a/include/fcl/articulated_model/joint_config.h b/include/fcl/articulated_model/joint_config.h index 11a1ba5b3..5da0a3a51 100644 --- a/include/fcl/articulated_model/joint_config.h +++ b/include/fcl/articulated_model/joint_config.h @@ -45,8 +45,10 @@ namespace fcl { +template class Joint; +template class JointConfig { public: @@ -55,46 +57,133 @@ class JointConfig JointConfig(const JointConfig& joint_cfg); JointConfig(const std::shared_ptr& joint, - FCL_REAL default_value = 0, - FCL_REAL default_value_min = 0, - FCL_REAL default_value_max = 0); + S default_value = 0, + S default_value_min = 0, + S default_value_max = 0); std::size_t getDim() const; - inline FCL_REAL operator [] (std::size_t i) const + inline S operator [] (std::size_t i) const { return values_[i]; } - inline FCL_REAL& operator [] (std::size_t i) + inline S& operator [] (std::size_t i) { return values_[i]; } - FCL_REAL getValue(std::size_t i) const; + S getValue(std::size_t i) const; - FCL_REAL& getValue(std::size_t i); + S& getValue(std::size_t i); - FCL_REAL getLimitMin(std::size_t i) const; + S getLimitMin(std::size_t i) const; - FCL_REAL& getLimitMin(std::size_t i); + S& getLimitMin(std::size_t i); - FCL_REAL getLimitMax(std::size_t i) const; + S getLimitMax(std::size_t i) const; - FCL_REAL& getLimitMax(std::size_t i); + S& getLimitMax(std::size_t i); std::shared_ptr getJoint() const; private: std::weak_ptr joint_; - std::vector values_; - std::vector limits_min_; - std::vector limits_max_; + std::vector values_; + std::vector limits_min_; + std::vector limits_max_; }; +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +JointConfig::JointConfig() {} + +//============================================================================== +template +JointConfig::JointConfig(const JointConfig& joint_cfg) : + joint_(joint_cfg.joint_), + values_(joint_cfg.values_), + limits_min_(joint_cfg.limits_min_), + limits_max_(joint_cfg.limits_max_) +{ +} + +//============================================================================== +template +JointConfig::JointConfig(const std::shared_ptr& joint, + S default_value, + S default_value_min, + S default_value_max) : + joint_(joint) +{ + values_.resize(joint->getNumDofs(), default_value); + limits_min_.resize(joint->getNumDofs(), default_value_min); + limits_max_.resize(joint->getNumDofs(), default_value_max); +} + +//============================================================================== +template +std::size_t JointConfig::getDim() const +{ + return values_.size(); } +//============================================================================== +template +S JointConfig::getValue(std::size_t i) const +{ + return values_[i]; +} + +//============================================================================== +template +S& JointConfig::getValue(std::size_t i) +{ + return values_[i]; +} + +//============================================================================== +template +S JointConfig::getLimitMin(std::size_t i) const +{ + return limits_min_[i]; +} + +//============================================================================== +template +S& JointConfig::getLimitMin(std::size_t i) +{ + return limits_min_[i]; +} + +//============================================================================== +template +S JointConfig::getLimitMax(std::size_t i) const +{ + return limits_max_[i]; +} + +//============================================================================== +template +S& JointConfig::getLimitMax(std::size_t i) +{ + return limits_max_[i]; +} + +//============================================================================== +template +std::shared_ptr JointConfig::getJoint() const +{ + return joint_.lock(); +} +} // namespace fcl #endif diff --git a/include/fcl/articulated_model/link.h b/include/fcl/articulated_model/link.h index 8b6f1da43..0aa0ea24f 100644 --- a/include/fcl/articulated_model/link.h +++ b/include/fcl/articulated_model/link.h @@ -47,8 +47,10 @@ namespace fcl { +template class Joint; +template class Link { public: @@ -62,7 +64,7 @@ class Link void setParentJoint(const std::shared_ptr& joint); - void addObject(const std::shared_ptr& object); + void addObject(const std::shared_ptr>& object); std::size_t getNumChildJoints() const; @@ -71,13 +73,73 @@ class Link protected: std::string name_; - std::vector > objects_; + std::vector> > objects_; std::vector > children_joints_; std::shared_ptr parent_joint_; }; +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +Link::Link(const std::string& name) : name_(name) +{} + +//============================================================================== +template +const std::string& Link::getName() const +{ + return name_; +} + +//============================================================================== +template +void Link::setName(const std::string& name) +{ + name_ = name; +} + +//============================================================================== +template +void Link::addChildJoint(const std::shared_ptr& joint) +{ + children_joints_.push_back(joint); +} + +//============================================================================== +template +void Link::setParentJoint(const std::shared_ptr& joint) +{ + parent_joint_ = joint; +} + +//============================================================================== +template +void Link::addObject(const std::shared_ptr>& object) +{ + objects_.push_back(object); } +//============================================================================== +template +std::size_t Link::getNumChildJoints() const +{ + return children_joints_.size(); +} + +//============================================================================== +template +std::size_t Link::getNumObjects() const +{ + return objects_.size(); +} + +} // namespace fcl + #endif diff --git a/include/fcl/articulated_model/model.h b/include/fcl/articulated_model/model.h index b2fa79d5f..69a7ed0c3 100644 --- a/include/fcl/articulated_model/model.h +++ b/include/fcl/articulated_model/model.h @@ -46,6 +46,7 @@ #include #include + namespace fcl { @@ -55,6 +56,7 @@ class ModelParseError : public std::runtime_error ModelParseError(const std::string& error_msg) : std::runtime_error(error_msg) {} }; +template class Model { public: @@ -93,7 +95,143 @@ class Model }; +//============================================================================== +template +std::shared_ptr Model::getRoot() const +{ + return root_link_; } -#endif +//============================================================================== +template +std::shared_ptr Model::getLink(const std::string& name) const +{ + std::shared_ptr ptr; + std::map >::const_iterator it = links_.find(name); + if(it == links_.end()) + ptr.reset(); + else + ptr = it->second; + return ptr; +} + +//============================================================================== +template +std::shared_ptr Model::getJoint(const std::string& name) const +{ + std::shared_ptr ptr; + std::map >::const_iterator it = joints_.find(name); + if(it == joints_.end()) + ptr.reset(); + else + ptr = it->second; + return ptr; +} + +//============================================================================== +template +const std::string& Model::getName() const +{ + return name_; +} + +//============================================================================== +template +std::vector > Model::getLinks() const +{ + std::vector > links; + for(std::map >::const_iterator it = links_.begin(); it != links_.end(); ++it) + { + links.push_back(it->second); + } + + return links; +} + +//============================================================================== +template +std::size_t Model::getNumLinks() const +{ + return links_.size(); +} + +//============================================================================== +template +std::size_t Model::getNumJoints() const +{ + return joints_.size(); +} +//============================================================================== +template +std::size_t Model::getNumDofs() const +{ + std::size_t dof = 0; + for(std::map >::const_iterator it = joints_.begin(); it != joints_.end(); ++it) + { + dof += it->second->getNumDofs(); + } + + return dof; +} + +//============================================================================== +template +void Model::addLink(const std::shared_ptr& link) +{ + links_[link->getName()] = link; +} + +//============================================================================== +template +void Model::addJoint(const std::shared_ptr& joint) +{ + joints_[joint->getName()] = joint; +} + +//============================================================================== +template +void Model::initRoot(const std::map& link_parent_tree) +{ + root_link_.reset(); + + /// find the links that have no parent in the tree + for(std::map >::const_iterator it = links_.begin(); it != links_.end(); ++it) + { + std::map::const_iterator parent = link_parent_tree.find(it->first); + if(parent == link_parent_tree.end()) + { + if(!root_link_) + { + root_link_ = getLink(it->first); + } + else + { + throw ModelParseError("Two root links found: [" + root_link_->getName() + "] and [" + it->first + "]"); + } + } + } + + if(!root_link_) + throw ModelParseError("No root link found."); +} + +//============================================================================== +template +void Model::initTree(std::map& link_parent_tree) +{ + for(std::map >::iterator it = joints_.begin(); it != joints_.end(); ++it) + { + std::string parent_link_name = it->second->getParentLink()->getName(); + std::string child_link_name = it->second->getChildLink()->getName(); + + it->second->getParentLink()->addChildJoint(it->second); + it->second->getChildLink()->setParentJoint(it->second); + + link_parent_tree[child_link_name] = parent_link_name; + } +} + +} // namespace fcl + +#endif diff --git a/include/fcl/articulated_model/model_config.h b/include/fcl/articulated_model/model_config.h index 1bf4ab802..e7a3cf203 100644 --- a/include/fcl/articulated_model/model_config.h +++ b/include/fcl/articulated_model/model_config.h @@ -38,15 +38,16 @@ #ifndef FCL_ARTICULATED_MODEL_MODEL_CONFIG_H #define FCL_ARTICULATED_MODEL_MODEL_CONFIG_H -#include "fcl/data_types.h" -#include "fcl/articulated_model/joint_config.h" #include #include #include +#include "fcl/data_types.h" +#include "fcl/articulated_model/joint_config.h" namespace fcl { +template class ModelConfig { public: @@ -69,7 +70,68 @@ class ModelConfig std::map joint_cfgs_map_; }; +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +ModelConfig::ModelConfig() +{ + // Do nothing +} + +//============================================================================== +template +ModelConfig::ModelConfig(const ModelConfig& model_cfg) : + joint_cfgs_map_(model_cfg.joint_cfgs_map_) +{} + +//============================================================================== +template +ModelConfig::ModelConfig(std::map > joints_map) +{ + std::map >::iterator it; + for(it = joints_map.begin(); it != joints_map.end(); ++it) + joint_cfgs_map_[it->first] = JointConfig(it->second); +} + +//============================================================================== +template +JointConfig ModelConfig::getJointConfigByJointName(const std::string& joint_name) const +{ + std::map::const_iterator it = joint_cfgs_map_.find(joint_name); + assert(it != joint_cfgs_map_.end()); + + return it->second; +} + +//============================================================================== +template +JointConfig& ModelConfig::getJointConfigByJointName(const std::string& joint_name) +{ + std::map::iterator it = joint_cfgs_map_.find(joint_name); + assert(it != joint_cfgs_map_.end()); + + return it->second; +} + +//============================================================================== +template +JointConfig ModelConfig::getJointConfigByJoint(std::shared_ptr joint) const +{ + return getJointConfigByJointName(joint->getName()); +} +//============================================================================== +template +JointConfig& ModelConfig::getJointConfigByJoint(std::shared_ptr joint) +{ + return getJointConfigByJointName(joint->getName()); } +} // namespace fcl + #endif diff --git a/include/fcl/broadphase/broadphase.h b/include/fcl/broadphase/broadphase.h index ff68bff8b..3f58ed074 100644 --- a/include/fcl/broadphase/broadphase.h +++ b/include/fcl/broadphase/broadphase.h @@ -40,22 +40,33 @@ #ifndef FCL_BROAD_PHASE_H #define FCL_BROAD_PHASE_H -#include "fcl/collision_object.h" #include #include +#include "fcl/collision_object.h" +#include "fcl/continuous_collision_object.h" + namespace fcl { -/// @brief Callback for collision between two objects. Return value is whether can stop now. -typedef bool (*CollisionCallBack)(CollisionObject* o1, CollisionObject* o2, void* cdata); - -/// @brief Callback for distance between two objects, Return value is whether can stop now, also return the minimum distance till now. -typedef bool (*DistanceCallBack)(CollisionObject* o1, CollisionObject* o2, void* cdata, FCL_REAL& dist); +/// @brief Callback for collision between two objects. Return value is whether +/// can stop now. +template +using CollisionCallBack = bool (*)( + CollisionObject* o1, CollisionObject* o2, void* cdata); +/// @brief Callback for distance between two objects, Return value is whether +/// can stop now, also return the minimum distance till now. +template +using DistanceCallBack = bool (*)( + CollisionObject* o1, + CollisionObject* o2, void* cdata, S& dist); -/// @brief Base class for broad phase collision. It helps to accelerate the collision/distance between N objects. Also support self collision, self distance and collision/distance with another M objects. +/// @brief Base class for broad phase collision. It helps to accelerate the +/// collision/distance between N objects. Also support self collision, self +/// distance and collision/distance with another M objects. +template class BroadPhaseCollisionManager { public: @@ -66,17 +77,17 @@ class BroadPhaseCollisionManager virtual ~BroadPhaseCollisionManager() {} /// @brief add objects to the manager - virtual void registerObjects(const std::vector& other_objs) + virtual void registerObjects(const std::vector*>& other_objs) { for(size_t i = 0; i < other_objs.size(); ++i) registerObject(other_objs[i]); } /// @brief add one object to the manager - virtual void registerObject(CollisionObject* obj) = 0; + virtual void registerObject(CollisionObject* obj) = 0; /// @brief remove one object from the manager - virtual void unregisterObject(CollisionObject* obj) = 0; + virtual void unregisterObject(CollisionObject* obj) = 0; /// @brief initialize the manager, related with the specific type of manager virtual void setup() = 0; @@ -85,13 +96,13 @@ class BroadPhaseCollisionManager virtual void update() = 0; /// @brief update the manager by explicitly given the object updated - virtual void update(CollisionObject* updated_obj) + virtual void update(CollisionObject* updated_obj) { update(); } /// @brief update the manager by explicitly given the set of objects update - virtual void update(const std::vector& updated_objs) + virtual void update(const std::vector*>& updated_objs) { update(); } @@ -100,25 +111,25 @@ class BroadPhaseCollisionManager virtual void clear() = 0; /// @brief return the objects managed by the manager - virtual void getObjects(std::vector& objs) const = 0; + virtual void getObjects(std::vector*>& objs) const = 0; /// @brief perform collision test between one object and all the objects belonging to the manager - virtual void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const = 0; + virtual void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const = 0; /// @brief perform distance computation between one object and all the objects belonging to the manager - virtual void distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const = 0; + virtual void distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const = 0; /// @brief perform collision test for the objects belonging to the manager (i.e., N^2 self collision) - virtual void collide(void* cdata, CollisionCallBack callback) const = 0; + virtual void collide(void* cdata, CollisionCallBack callback) const = 0; /// @brief perform distance test for the objects belonging to the manager (i.e., N^2 self distance) - virtual void distance(void* cdata, DistanceCallBack callback) const = 0; + virtual void distance(void* cdata, DistanceCallBack callback) const = 0; /// @brief perform collision test with objects belonging to another manager - virtual void collide(BroadPhaseCollisionManager* other_manager, void* cdata, CollisionCallBack callback) const = 0; + virtual void collide(BroadPhaseCollisionManager* other_manager, void* cdata, CollisionCallBack callback) const = 0; /// @brief perform distance test with objects belonging to another manager - virtual void distance(BroadPhaseCollisionManager* other_manager, void* cdata, DistanceCallBack callback) const = 0; + virtual void distance(BroadPhaseCollisionManager* other_manager, void* cdata, DistanceCallBack callback) const = 0; /// @brief whether the manager is empty virtual bool empty() const = 0; @@ -129,16 +140,16 @@ class BroadPhaseCollisionManager protected: /// @brief tools help to avoid repeating collision or distance callback for the pairs of objects tested before. It can be useful for some of the broadphase algorithms. - mutable std::set > tested_set; + mutable std::set*, CollisionObject*> > tested_set; mutable bool enable_tested_set_; - bool inTestedSet(CollisionObject* a, CollisionObject* b) const + bool inTestedSet(CollisionObject* a, CollisionObject* b) const { if(a < b) return tested_set.find(std::make_pair(a, b)) != tested_set.end(); else return tested_set.find(std::make_pair(b, a)) != tested_set.end(); } - void insertTestedSet(CollisionObject* a, CollisionObject* b) const + void insertTestedSet(CollisionObject* a, CollisionObject* b) const { if(a < b) tested_set.insert(std::make_pair(a, b)); else tested_set.insert(std::make_pair(b, a)); @@ -146,15 +157,27 @@ class BroadPhaseCollisionManager }; - -/// @brief Callback for continuous collision between two objects. Return value is whether can stop now. -typedef bool (*ContinuousCollisionCallBack)(ContinuousCollisionObject* o1, ContinuousCollisionObject* o2, void* cdata); - -/// @brief Callback for continuous distance between two objects, Return value is whether can stop now, also return the minimum distance till now. -typedef bool (*ContinuousDistanceCallBack)(ContinuousCollisionObject* o1, ContinuousCollisionObject* o2, void* cdata, FCL_REAL& dist); - - -/// @brief Base class for broad phase continuous collision. It helps to accelerate the continuous collision/distance between N objects. Also support self collision, self distance and collision/distance with another M objects. +using BroadPhaseCollisionManagerf = BroadPhaseCollisionManager; +using BroadPhaseCollisionManagerd = BroadPhaseCollisionManager; + +/// @brief Callback for continuous collision between two objects. Return value +/// is whether can stop now. +template +using ContinuousCollisionCallBack = bool (*)( + ContinuousCollisionObject* o1, + ContinuousCollisionObject* o2, void* cdata); + +/// @brief Callback for continuous distance between two objects, Return value is +/// whether can stop now, also return the minimum distance till now. +template +using ContinuousDistanceCallBack = bool (*)( + ContinuousCollisionObject* o1, + ContinuousCollisionObject* o2, void* cdata, S& dist); + +/// @brief Base class for broad phase continuous collision. It helps to +/// accelerate the continuous collision/distance between N objects. Also support +/// self collision, self distance and collision/distance with another M objects. +template class BroadPhaseContinuousCollisionManager { public: @@ -165,17 +188,17 @@ class BroadPhaseContinuousCollisionManager virtual ~BroadPhaseContinuousCollisionManager() {} /// @brief add objects to the manager - virtual void registerObjects(const std::vector& other_objs) + virtual void registerObjects(const std::vector*>& other_objs) { for(size_t i = 0; i < other_objs.size(); ++i) registerObject(other_objs[i]); } /// @brief add one object to the manager - virtual void registerObject(ContinuousCollisionObject* obj) = 0; + virtual void registerObject(ContinuousCollisionObject* obj) = 0; /// @brief remove one object from the manager - virtual void unregisterObject(ContinuousCollisionObject* obj) = 0; + virtual void unregisterObject(ContinuousCollisionObject* obj) = 0; /// @brief initialize the manager, related with the specific type of manager virtual void setup() = 0; @@ -184,13 +207,13 @@ class BroadPhaseContinuousCollisionManager virtual void update() = 0; /// @brief update the manager by explicitly given the object updated - virtual void update(ContinuousCollisionObject* updated_obj) + virtual void update(ContinuousCollisionObject* updated_obj) { update(); } /// @brief update the manager by explicitly given the set of objects update - virtual void update(const std::vector& updated_objs) + virtual void update(const std::vector*>& updated_objs) { update(); } @@ -199,25 +222,25 @@ class BroadPhaseContinuousCollisionManager virtual void clear() = 0; /// @brief return the objects managed by the manager - virtual void getObjects(std::vector& objs) const = 0; + virtual void getObjects(std::vector*>& objs) const = 0; /// @brief perform collision test between one object and all the objects belonging to the manager - virtual void collide(ContinuousCollisionObject* obj, void* cdata, CollisionCallBack callback) const = 0; + virtual void collide(ContinuousCollisionObject* obj, void* cdata, CollisionCallBack callback) const = 0; /// @brief perform distance computation between one object and all the objects belonging to the manager - virtual void distance(ContinuousCollisionObject* obj, void* cdata, DistanceCallBack callback) const = 0; + virtual void distance(ContinuousCollisionObject* obj, void* cdata, DistanceCallBack callback) const = 0; /// @brief perform collision test for the objects belonging to the manager (i.e., N^2 self collision) - virtual void collide(void* cdata, CollisionCallBack callback) const = 0; + virtual void collide(void* cdata, CollisionCallBack callback) const = 0; /// @brief perform distance test for the objects belonging to the manager (i.e., N^2 self distance) - virtual void distance(void* cdata, DistanceCallBack callback) const = 0; + virtual void distance(void* cdata, DistanceCallBack callback) const = 0; /// @brief perform collision test with objects belonging to another manager - virtual void collide(BroadPhaseContinuousCollisionManager* other_manager, void* cdata, CollisionCallBack callback) const = 0; + virtual void collide(BroadPhaseContinuousCollisionManager* other_manager, void* cdata, CollisionCallBack callback) const = 0; /// @brief perform distance test with objects belonging to another manager - virtual void distance(BroadPhaseContinuousCollisionManager* other_manager, void* cdata, DistanceCallBack callback) const = 0; + virtual void distance(BroadPhaseContinuousCollisionManager* other_manager, void* cdata, DistanceCallBack callback) const = 0; /// @brief whether the manager is empty virtual bool empty() const = 0; diff --git a/include/fcl/broadphase/broadphase_SSaP.h b/include/fcl/broadphase/broadphase_SSaP.h index 5c7b95ae7..37d956689 100644 --- a/include/fcl/broadphase/broadphase_SSaP.h +++ b/include/fcl/broadphase/broadphase_SSaP.h @@ -45,17 +45,18 @@ namespace fcl { /// @brief Simple SAP collision manager -class SSaPCollisionManager : public BroadPhaseCollisionManager +template +class SSaPCollisionManager : public BroadPhaseCollisionManager { public: SSaPCollisionManager() : setup_(false) {} /// @brief remove one object from the manager - void registerObject(CollisionObject* obj); + void registerObject(CollisionObject* obj); /// @brief add one object to the manager - void unregisterObject(CollisionObject* obj); + void unregisterObject(CollisionObject* obj); /// @brief initialize the manager, related with the specific type of manager void setup(); @@ -67,25 +68,25 @@ class SSaPCollisionManager : public BroadPhaseCollisionManager void clear(); /// @brief return the objects managed by the manager - void getObjects(std::vector& objs) const; + void getObjects(std::vector*>& objs) const; /// @brief perform collision test between one object and all the objects belonging to the manager - void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const; + void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const; /// @brief perform distance computation between one object and all the objects belonging to the manager - void distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const; + void distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const; /// @brief perform collision test for the objects belonging to the manager (i.e., N^2 self collision) - void collide(void* cdata, CollisionCallBack callback) const; + void collide(void* cdata, CollisionCallBack callback) const; /// @brief perform distance test for the objects belonging to the manager (i.e., N^2 self distance) - void distance(void* cdata, DistanceCallBack callback) const; + void distance(void* cdata, DistanceCallBack callback) const; /// @brief perform collision test with objects belonging to another manager - void collide(BroadPhaseCollisionManager* other_manager, void* cdata, CollisionCallBack callback) const; + void collide(BroadPhaseCollisionManager* other_manager, void* cdata, CollisionCallBack callback) const; /// @brief perform distance test with objects belonging to another manager - void distance(BroadPhaseCollisionManager* other_manager, void* cdata, DistanceCallBack callback) const; + void distance(BroadPhaseCollisionManager* other_manager, void* cdata, DistanceCallBack callback) const; /// @brief whether the manager is empty bool empty() const; @@ -95,23 +96,23 @@ class SSaPCollisionManager : public BroadPhaseCollisionManager protected: /// @brief check collision between one object and a list of objects, return value is whether stop is possible - bool checkColl(std::vector::const_iterator pos_start, std::vector::const_iterator pos_end, - CollisionObject* obj, void* cdata, CollisionCallBack callback) const; + bool checkColl(typename std::vector*>::const_iterator pos_start, typename std::vector*>::const_iterator pos_end, + CollisionObject* obj, void* cdata, CollisionCallBack callback) const; /// @brief check distance between one object and a list of objects, return value is whether stop is possible - bool checkDis(std::vector::const_iterator pos_start, std::vector::const_iterator pos_end, - CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const; + bool checkDis(typename std::vector*>::const_iterator pos_start, typename std::vector*>::const_iterator pos_end, + CollisionObject* obj, void* cdata, DistanceCallBack callback, S& min_dist) const; - bool collide_(CollisionObject* obj, void* cdata, CollisionCallBack callback) const; + bool collide_(CollisionObject* obj, void* cdata, CollisionCallBack callback) const; - bool distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const; + bool distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, S& min_dist) const; - static inline size_t selectOptimalAxis(const std::vector& objs_x, const std::vector& objs_y, const std::vector& objs_z, std::vector::const_iterator& it_beg, std::vector::const_iterator& it_end) + static inline size_t selectOptimalAxis(const std::vector*>& objs_x, const std::vector*>& objs_y, const std::vector*>& objs_z, typename std::vector*>::const_iterator& it_beg, typename std::vector*>::const_iterator& it_end) { /// simple sweep and prune method - double delta_x = (objs_x[objs_x.size() - 1])->getAABB().min_[0] - (objs_x[0])->getAABB().min_[0]; - double delta_y = (objs_x[objs_y.size() - 1])->getAABB().min_[1] - (objs_y[0])->getAABB().min_[1]; - double delta_z = (objs_z[objs_z.size() - 1])->getAABB().min_[2] - (objs_z[0])->getAABB().min_[2]; + S delta_x = (objs_x[objs_x.size() - 1])->getAABB().min_[0] - (objs_x[0])->getAABB().min_[0]; + S delta_y = (objs_x[objs_y.size() - 1])->getAABB().min_[1] - (objs_y[0])->getAABB().min_[1]; + S delta_z = (objs_z[objs_z.size() - 1])->getAABB().min_[2] - (objs_z[0])->getAABB().min_[2]; int axis = 0; if(delta_y > delta_x && delta_y > delta_z) @@ -140,19 +141,517 @@ class SSaPCollisionManager : public BroadPhaseCollisionManager /// @brief Objects sorted according to lower x value - std::vector objs_x; + std::vector*> objs_x; /// @brief Objects sorted according to lower y value - std::vector objs_y; + std::vector*> objs_y; /// @brief Objects sorted according to lower z value - std::vector objs_z; + std::vector*> objs_z; /// @brief tag about whether the environment is maintained suitably (i.e., the objs_x, objs_y, objs_z are sorted correctly bool setup_; }; +using SSaPCollisionManagerf = SSaPCollisionManager; +using SSaPCollisionManagerd = SSaPCollisionManager; +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +/** \brief Functor sorting objects according to the AABB lower x bound */ +template +struct SortByXLow +{ + bool operator()(const CollisionObject* a, const CollisionObject* b) const + { + if(a->getAABB().min_[0] < b->getAABB().min_[0]) + return true; + return false; + } +}; + +/** \brief Functor sorting objects according to the AABB lower y bound */ +template +struct SortByYLow +{ + bool operator()(const CollisionObject* a, const CollisionObject* b) const + { + if(a->getAABB().min_[1] < b->getAABB().min_[1]) + return true; + return false; + } +}; + +/** \brief Functor sorting objects according to the AABB lower z bound */ +template +struct SortByZLow +{ + bool operator()(const CollisionObject* a, const CollisionObject* b) const + { + if(a->getAABB().min_[2] < b->getAABB().min_[2]) + return true; + return false; + } +}; + +/** \brief Dummy collision object with a point AABB */ +template +class DummyCollisionObject : public CollisionObject +{ +public: + DummyCollisionObject(const AABB& aabb_) : CollisionObject(std::shared_ptr>()) + { + this->aabb = aabb_; + } + + void computeLocalAABB() {} +}; + +//============================================================================== +template +void SSaPCollisionManager::unregisterObject(CollisionObject* obj) +{ + setup(); + + DummyCollisionObject dummyHigh(AABB(obj->getAABB().max_)); + + auto pos_start1 = objs_x.begin(); + auto pos_end1 = std::upper_bound(pos_start1, objs_x.end(), &dummyHigh, SortByXLow()); + + while(pos_start1 < pos_end1) + { + if(*pos_start1 == obj) + { + objs_x.erase(pos_start1); + break; + } + ++pos_start1; + } + + auto pos_start2 = objs_y.begin(); + auto pos_end2 = std::upper_bound(pos_start2, objs_y.end(), &dummyHigh, SortByYLow()); + + while(pos_start2 < pos_end2) + { + if(*pos_start2 == obj) + { + objs_y.erase(pos_start2); + break; + } + ++pos_start2; + } + + auto pos_start3 = objs_z.begin(); + auto pos_end3 = std::upper_bound(pos_start3, objs_z.end(), &dummyHigh, SortByZLow()); + + while(pos_start3 < pos_end3) + { + if(*pos_start3 == obj) + { + objs_z.erase(pos_start3); + break; + } + ++pos_start3; + } +} + +//============================================================================== +template +void SSaPCollisionManager::registerObject(CollisionObject* obj) +{ + objs_x.push_back(obj); + objs_y.push_back(obj); + objs_z.push_back(obj); + setup_ = false; +} + +//============================================================================== +template +void SSaPCollisionManager::setup() +{ + if(!setup_) + { + std::sort(objs_x.begin(), objs_x.end(), SortByXLow()); + std::sort(objs_y.begin(), objs_y.end(), SortByYLow()); + std::sort(objs_z.begin(), objs_z.end(), SortByZLow()); + setup_ = true; + } +} + +//============================================================================== +template +void SSaPCollisionManager::update() +{ + setup_ = false; + setup(); +} + +//============================================================================== +template +void SSaPCollisionManager::clear() +{ + objs_x.clear(); + objs_y.clear(); + objs_z.clear(); + setup_ = false; +} + +//============================================================================== +template +void SSaPCollisionManager::getObjects(std::vector*>& objs) const +{ + objs.resize(objs_x.size()); + std::copy(objs_x.begin(), objs_x.end(), objs.begin()); +} + +//============================================================================== +template +bool SSaPCollisionManager::checkColl(typename std::vector*>::const_iterator pos_start, typename std::vector*>::const_iterator pos_end, + CollisionObject* obj, void* cdata, CollisionCallBack callback) const +{ + while(pos_start < pos_end) + { + if(*pos_start != obj) // no collision between the same object + { + if((*pos_start)->getAABB().overlap(obj->getAABB())) + { + if(callback(*pos_start, obj, cdata)) + return true; + } + } + pos_start++; + } + return false; +} + +//============================================================================== +template +bool SSaPCollisionManager::checkDis(typename std::vector*>::const_iterator pos_start, typename std::vector*>::const_iterator pos_end, CollisionObject* obj, void* cdata, DistanceCallBack callback, S& min_dist) const +{ + while(pos_start < pos_end) + { + if(*pos_start != obj) // no distance between the same object + { + if((*pos_start)->getAABB().distance(obj->getAABB()) < min_dist) + { + if(callback(*pos_start, obj, cdata, min_dist)) + return true; + } + } + pos_start++; + } + + return false; +} + +//============================================================================== +template +void SSaPCollisionManager::collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const +{ + if(size() == 0) return; + + collide_(obj, cdata, callback); +} + +//============================================================================== +template +bool SSaPCollisionManager::collide_(CollisionObject* obj, void* cdata, CollisionCallBack callback) const +{ + static const unsigned int CUTOFF = 100; + + DummyCollisionObject dummyHigh(AABB(obj->getAABB().max_)); + bool coll_res = false; + + const auto pos_start1 = objs_x.begin(); + const auto pos_end1 = std::upper_bound(pos_start1, objs_x.end(), &dummyHigh, SortByXLow()); + unsigned int d1 = pos_end1 - pos_start1; + + if(d1 > CUTOFF) + { + const auto pos_start2 = objs_y.begin(); + const auto pos_end2 = std::upper_bound(pos_start2, objs_y.end(), &dummyHigh, SortByYLow()); + unsigned int d2 = pos_end2 - pos_start2; + + if(d2 > CUTOFF) + { + const auto pos_start3 = objs_z.begin(); + const auto pos_end3 = std::upper_bound(pos_start3, objs_z.end(), &dummyHigh, SortByZLow()); + unsigned int d3 = pos_end3 - pos_start3; + + if(d3 > CUTOFF) + { + if(d3 <= d2 && d3 <= d1) + coll_res = checkColl(pos_start3, pos_end3, obj, cdata, callback); + else + { + if(d2 <= d3 && d2 <= d1) + coll_res = checkColl(pos_start2, pos_end2, obj, cdata, callback); + else + coll_res = checkColl(pos_start1, pos_end1, obj, cdata, callback); + } + } + else + coll_res = checkColl(pos_start3, pos_end3, obj, cdata, callback); + } + else + coll_res = checkColl(pos_start2, pos_end2, obj, cdata, callback); + } + else + coll_res = checkColl(pos_start1, pos_end1, obj, cdata, callback); + + return coll_res; +} + +//============================================================================== +template +void SSaPCollisionManager::distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const +{ + if(size() == 0) return; + + S min_dist = std::numeric_limits::max(); + distance_(obj, cdata, callback, min_dist); +} + +//============================================================================== +template +bool SSaPCollisionManager::distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, S& min_dist) const +{ + static const unsigned int CUTOFF = 100; + Vector3 delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5; + Vector3 dummy_vector = obj->getAABB().max_; + if(min_dist < std::numeric_limits::max()) + dummy_vector += Vector3(min_dist, min_dist, min_dist); + + typename std::vector*>::const_iterator pos_start1 = objs_x.begin(); + typename std::vector*>::const_iterator pos_start2 = objs_y.begin(); + typename std::vector*>::const_iterator pos_start3 = objs_z.begin(); + typename std::vector*>::const_iterator pos_end1 = objs_x.end(); + typename std::vector*>::const_iterator pos_end2 = objs_y.end(); + typename std::vector*>::const_iterator pos_end3 = objs_z.end(); + + int status = 1; + S old_min_distance; + + while(1) + { + old_min_distance = min_dist; + DummyCollisionObject dummyHigh((AABB(dummy_vector))); + + pos_end1 = std::upper_bound(pos_start1, objs_x.end(), &dummyHigh, SortByXLow()); + unsigned int d1 = pos_end1 - pos_start1; + + bool dist_res = false; + + if(d1 > CUTOFF) + { + pos_end2 = std::upper_bound(pos_start2, objs_y.end(), &dummyHigh, SortByYLow()); + unsigned int d2 = pos_end2 - pos_start2; + + if(d2 > CUTOFF) + { + pos_end3 = std::upper_bound(pos_start3, objs_z.end(), &dummyHigh, SortByZLow()); + unsigned int d3 = pos_end3 - pos_start3; + + if(d3 > CUTOFF) + { + if(d3 <= d2 && d3 <= d1) + dist_res = checkDis(pos_start3, pos_end3, obj, cdata, callback, min_dist); + else + { + if(d2 <= d3 && d2 <= d1) + dist_res = checkDis(pos_start2, pos_end2, obj, cdata, callback, min_dist); + else + dist_res = checkDis(pos_start1, pos_end1, obj, cdata, callback, min_dist); + } + } + else + dist_res = checkDis(pos_start3, pos_end3, obj, cdata, callback, min_dist); + } + else + dist_res = checkDis(pos_start2, pos_end2, obj, cdata, callback, min_dist); + } + else + { + dist_res = checkDis(pos_start1, pos_end1, obj, cdata, callback, min_dist); + } + + if(dist_res) return true; + + if(status == 1) + { + if(old_min_distance < std::numeric_limits::max()) + break; + else + { + // from infinity to a finite one, only need one additional loop + // to check the possible missed ones to the right of the objs array + if(min_dist < old_min_distance) + { + dummy_vector = obj->getAABB().max_ + Vector3(min_dist, min_dist, min_dist); + status = 0; + } + else // need more loop + { + if(dummy_vector.isApprox(obj->getAABB().max_, std::numeric_limits::epsilon() * 100)) + dummy_vector = dummy_vector + delta; + else + dummy_vector = dummy_vector * 2 - obj->getAABB().max_; + } + } + + // yes, following is wrong, will result in too large distance. + // if(pos_end1 != objs_x.end()) pos_start1 = pos_end1; + // if(pos_end2 != objs_y.end()) pos_start2 = pos_end2; + // if(pos_end3 != objs_z.end()) pos_start3 = pos_end3; + } + else if(status == 0) + break; + } + + return false; } +//============================================================================== +template +void SSaPCollisionManager::collide(void* cdata, CollisionCallBack callback) const +{ + if(size() == 0) return; + + typename std::vector*>::const_iterator pos, run_pos, pos_end; + size_t axis = selectOptimalAxis(objs_x, objs_y, objs_z, + pos, pos_end); + size_t axis2 = (axis + 1 > 2) ? 0 : (axis + 1); + size_t axis3 = (axis2 + 1 > 2) ? 0 : (axis2 + 1); + + run_pos = pos; + + while((run_pos < pos_end) && (pos < pos_end)) + { + CollisionObject* obj = *(pos++); + + while(1) + { + if((*run_pos)->getAABB().min_[axis] < obj->getAABB().min_[axis]) + { + run_pos++; + if(run_pos == pos_end) break; + continue; + } + else + { + run_pos++; + break; + } + } + + if(run_pos < pos_end) + { + typename std::vector*>::const_iterator run_pos2 = run_pos; + + while((*run_pos2)->getAABB().min_[axis] <= obj->getAABB().max_[axis]) + { + CollisionObject* obj2 = *run_pos2; + run_pos2++; + + if((obj->getAABB().max_[axis2] >= obj2->getAABB().min_[axis2]) && (obj2->getAABB().max_[axis2] >= obj->getAABB().min_[axis2])) + { + if((obj->getAABB().max_[axis3] >= obj2->getAABB().min_[axis3]) && (obj2->getAABB().max_[axis3] >= obj->getAABB().min_[axis3])) + { + if(callback(obj, obj2, cdata)) + return; + } + } + + if(run_pos2 == pos_end) break; + } + } + } +} + +//============================================================================== +template +void SSaPCollisionManager::distance(void* cdata, DistanceCallBack callback) const +{ + if(size() == 0) return; + + typename std::vector*>::const_iterator it, it_end; + selectOptimalAxis(objs_x, objs_y, objs_z, it, it_end); + + S min_dist = std::numeric_limits::max(); + for(; it != it_end; ++it) + { + if(distance_(*it, cdata, callback, min_dist)) + return; + } +} + +//============================================================================== +template +void SSaPCollisionManager::collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const +{ + SSaPCollisionManager* other_manager = static_cast(other_manager_); + + if((size() == 0) || (other_manager->size() == 0)) return; + + if(this == other_manager) + { + collide(cdata, callback); + return; + } + + + typename std::vector*>::const_iterator it, end; + if(this->size() < other_manager->size()) + { + for(it = objs_x.begin(), end = objs_x.end(); it != end; ++it) + if(other_manager->collide_(*it, cdata, callback)) return; + } + else + { + for(it = other_manager->objs_x.begin(), end = other_manager->objs_x.end(); it != end; ++it) + if(collide_(*it, cdata, callback)) return; + } +} + +//============================================================================== +template +void SSaPCollisionManager::distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const +{ + SSaPCollisionManager* other_manager = static_cast(other_manager_); + + if((size() == 0) || (other_manager->size() == 0)) return; + + if(this == other_manager) + { + distance(cdata, callback); + return; + } + + S min_dist = std::numeric_limits::max(); + typename std::vector*>::const_iterator it, end; + if(this->size() < other_manager->size()) + { + for(it = objs_x.begin(), end = objs_x.end(); it != end; ++it) + if(other_manager->distance_(*it, cdata, callback, min_dist)) return; + } + else + { + for(it = other_manager->objs_x.begin(), end = other_manager->objs_x.end(); it != end; ++it) + if(distance_(*it, cdata, callback, min_dist)) return; + } +} + +//============================================================================== +template +bool SSaPCollisionManager::empty() const +{ + return objs_x.empty(); +} + +} // namespace + #endif diff --git a/include/fcl/broadphase/broadphase_SaP.h b/include/fcl/broadphase/broadphase_SaP.h index f796707cd..4a42b24ec 100644 --- a/include/fcl/broadphase/broadphase_SaP.h +++ b/include/fcl/broadphase/broadphase_SaP.h @@ -47,15 +47,16 @@ namespace fcl { /// @brief Rigorous SAP collision manager -class SaPCollisionManager : public BroadPhaseCollisionManager +template +class SaPCollisionManager : public BroadPhaseCollisionManager { public: SaPCollisionManager() { - elist[0] = NULL; - elist[1] = NULL; - elist[2] = NULL; + elist[0] = nullptr; + elist[1] = nullptr; + elist[2] = nullptr; optimal_axis = 0; } @@ -66,13 +67,13 @@ class SaPCollisionManager : public BroadPhaseCollisionManager } /// @brief add objects to the manager - void registerObjects(const std::vector& other_objs); + void registerObjects(const std::vector*>& other_objs); /// @brief remove one object from the manager - void registerObject(CollisionObject* obj); + void registerObject(CollisionObject* obj); /// @brief add one object to the manager - void unregisterObject(CollisionObject* obj); + void unregisterObject(CollisionObject* obj); /// @brief initialize the manager, related with the specific type of manager void setup(); @@ -81,34 +82,34 @@ class SaPCollisionManager : public BroadPhaseCollisionManager void update(); /// @brief update the manager by explicitly given the object updated - void update(CollisionObject* updated_obj); + void update(CollisionObject* updated_obj); /// @brief update the manager by explicitly given the set of objects update - void update(const std::vector& updated_objs); + void update(const std::vector*>& updated_objs); /// @brief clear the manager void clear(); /// @brief return the objects managed by the manager - void getObjects(std::vector& objs) const; + void getObjects(std::vector*>& objs) const; /// @brief perform collision test between one object and all the objects belonging to the manager - void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const; + void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const; /// @brief perform distance computation between one object and all the objects belonging to the manager - void distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const; + void distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const; /// @brief perform collision test for the objects belonging to the manager (i.e., N^2 self collision) - void collide(void* cdata, CollisionCallBack callback) const; + void collide(void* cdata, CollisionCallBack callback) const; /// @brief perform distance test for the objects belonging to the manager (i.e., N^2 self distance) - void distance(void* cdata, DistanceCallBack callback) const; + void distance(void* cdata, DistanceCallBack callback) const; /// @brief perform collision test with objects belonging to another manager - void collide(BroadPhaseCollisionManager* other_manager, void* cdata, CollisionCallBack callback) const; + void collide(BroadPhaseCollisionManager* other_manager, void* cdata, CollisionCallBack callback) const; /// @brief perform distance test with objects belonging to another manager - void distance(BroadPhaseCollisionManager* other_manager, void* cdata, DistanceCallBack callback) const; + void distance(BroadPhaseCollisionManager* other_manager, void* cdata, DistanceCallBack callback) const; /// @brief whether the manager is empty bool empty() const; @@ -124,7 +125,7 @@ class SaPCollisionManager : public BroadPhaseCollisionManager struct SaPAABB { /// @brief object - CollisionObject* obj; + CollisionObject* obj; /// @brief lower bound end point of the interval EndPoint* lo; @@ -132,8 +133,8 @@ class SaPCollisionManager : public BroadPhaseCollisionManager /// @brief higher bound end point of the interval EndPoint* hi; - /// @brief cached AABB value - AABB cached; + /// @brief cached AABB value + AABB cached; }; /// @brief End point for an interval @@ -151,20 +152,20 @@ class SaPCollisionManager : public BroadPhaseCollisionManager EndPoint* next[3]; /// @brief get the value of the end point - inline const Vector3d& getVal() const + inline const Vector3& getVal() const { if(minmax) return aabb->cached.max_; else return aabb->cached.min_; } /// @brief set the value of the end point - inline Vector3d& getVal() + inline Vector3& getVal() { if(minmax) return aabb->cached.max_; else return aabb->cached.min_; } - inline Vector3d::Scalar getVal(size_t i) const + inline S getVal(size_t i) const { if(minmax) return aabb->cached.max_[i]; @@ -172,7 +173,7 @@ class SaPCollisionManager : public BroadPhaseCollisionManager return aabb->cached.min_[i]; } - inline Vector3d::Scalar& getVal(size_t i) + inline S& getVal(size_t i) { if(minmax) return aabb->cached.max_[i]; @@ -185,7 +186,7 @@ class SaPCollisionManager : public BroadPhaseCollisionManager /// @brief A pair of objects that are not culling away and should further check collision struct SaPPair { - SaPPair(CollisionObject* a, CollisionObject* b) + SaPPair(CollisionObject* a, CollisionObject* b) { if(a < b) { @@ -199,8 +200,8 @@ class SaPCollisionManager : public BroadPhaseCollisionManager } } - CollisionObject* obj1; - CollisionObject* obj2; + CollisionObject* obj1; + CollisionObject* obj2; bool operator == (const SaPPair& other) const { @@ -211,10 +212,10 @@ class SaPCollisionManager : public BroadPhaseCollisionManager /// @brief Functor to help unregister one object class isUnregistered { - CollisionObject* obj; + CollisionObject* obj; public: - isUnregistered(CollisionObject* obj_) : obj(obj_) + isUnregistered(CollisionObject* obj_) : obj(obj_) {} bool operator() (const SaPPair& pair) const @@ -226,11 +227,11 @@ class SaPCollisionManager : public BroadPhaseCollisionManager /// @brief Functor to help remove collision pairs no longer valid (i.e., should be culled away) class isNotValidPair { - CollisionObject* obj1; - CollisionObject* obj2; + CollisionObject* obj1; + CollisionObject* obj2; public: - isNotValidPair(CollisionObject* obj1_, CollisionObject* obj2_) : obj1(obj1_), + isNotValidPair(CollisionObject* obj1_, CollisionObject* obj2_) : obj1(obj1_), obj2(obj2_) {} @@ -272,18 +273,16 @@ class SaPCollisionManager : public BroadPhaseCollisionManager size_t optimal_axis; - std::map obj_aabb_map; + std::map*, SaPAABB*> obj_aabb_map; - bool distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const; + bool distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, S& min_dist) const; - bool collide_(CollisionObject* obj, void* cdata, CollisionCallBack callback) const; + bool collide_(CollisionObject* obj, void* cdata, CollisionCallBack callback) const; void addToOverlapPairs(const SaPPair& p) { bool repeated = false; - for(std::list::iterator it = overlap_pairs.begin(), end = overlap_pairs.end(); - it != end; - ++it) + for(auto it = overlap_pairs.begin(), end = overlap_pairs.end(); it != end; ++it) { if(*it == p) { @@ -298,7 +297,7 @@ class SaPCollisionManager : public BroadPhaseCollisionManager void removeFromOverlapPairs(const SaPPair& p) { - for(std::list::iterator it = overlap_pairs.begin(), end = overlap_pairs.end(); + for(auto it = overlap_pairs.begin(), end = overlap_pairs.end(); it != end; ++it) { @@ -313,9 +312,769 @@ class SaPCollisionManager : public BroadPhaseCollisionManager } }; +using SaPCollisionManagerf = SaPCollisionManager; +using SaPCollisionManagerd = SaPCollisionManager; + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +void SaPCollisionManager::unregisterObject(CollisionObject* obj) +{ + auto it = AABB_arr.begin(); + for(auto end = AABB_arr.end(); it != end; ++it) + { + if((*it)->obj == obj) + break; + } + + AABB_arr.erase(it); + obj_aabb_map.erase(obj); + + if(it == AABB_arr.end()) + return; + + SaPAABB* curr = *it; + *it = nullptr; + + for(int coord = 0; coord < 3; ++coord) + { + //first delete the lo endpoint of the interval. + if(curr->lo->prev[coord] == nullptr) + elist[coord] = curr->lo->next[coord]; + else + curr->lo->prev[coord]->next[coord] = curr->lo->next[coord]; + + curr->lo->next[coord]->prev[coord] = curr->lo->prev[coord]; + + //then, delete the "hi" endpoint. + if(curr->hi->prev[coord] == nullptr) + elist[coord] = curr->hi->next[coord]; + else + curr->hi->prev[coord]->next[coord] = curr->hi->next[coord]; + + if(curr->hi->next[coord] != nullptr) + curr->hi->next[coord]->prev[coord] = curr->hi->prev[coord]; + } + + delete curr->lo; + delete curr->hi; + delete curr; + + overlap_pairs.remove_if(isUnregistered(obj)); +} +\ +//============================================================================== +template +void SaPCollisionManager::registerObjects(const std::vector*>& other_objs) +{ + if(other_objs.empty()) return; + + if(size() > 0) + BroadPhaseCollisionManager::registerObjects(other_objs); + else + { + std::vector endpoints(2 * other_objs.size()); + + for(size_t i = 0; i < other_objs.size(); ++i) + { + SaPAABB* sapaabb = new SaPAABB(); + sapaabb->obj = other_objs[i]; + sapaabb->lo = new EndPoint(); + sapaabb->hi = new EndPoint(); + sapaabb->cached = other_objs[i]->getAABB(); + endpoints[2 * i] = sapaabb->lo; + endpoints[2 * i + 1] = sapaabb->hi; + sapaabb->lo->minmax = 0; + sapaabb->hi->minmax = 1; + sapaabb->lo->aabb = sapaabb; + sapaabb->hi->aabb = sapaabb; + AABB_arr.push_back(sapaabb); + obj_aabb_map[other_objs[i]] = sapaabb; + } + + + S scale[3]; + for(size_t coord = 0; coord < 3; ++coord) + { + std::sort(endpoints.begin(), endpoints.end(), + std::bind(std::less(), + std::bind(static_cast(&EndPoint::getVal), std::placeholders::_1, coord), + std::bind(static_cast(&EndPoint::getVal), std::placeholders::_2, coord))); + + endpoints[0]->prev[coord] = nullptr; + endpoints[0]->next[coord] = endpoints[1]; + for(size_t i = 1; i < endpoints.size() - 1; ++i) + { + endpoints[i]->prev[coord] = endpoints[i-1]; + endpoints[i]->next[coord] = endpoints[i+1]; + } + endpoints[endpoints.size() - 1]->prev[coord] = endpoints[endpoints.size() - 2]; + endpoints[endpoints.size() - 1]->next[coord] = nullptr; + + elist[coord] = endpoints[0]; + + scale[coord] = endpoints.back()->aabb->cached.max_[coord] - endpoints[0]->aabb->cached.min_[coord]; + } + + int axis = 0; + if(scale[axis] < scale[1]) axis = 1; + if(scale[axis] < scale[2]) axis = 2; + + EndPoint* pos = elist[axis]; + while(pos != nullptr) + { + EndPoint* pos_next = nullptr; + SaPAABB* aabb = pos->aabb; + EndPoint* pos_it = pos->next[axis]; + + while(pos_it != nullptr) + { + if(pos_it->aabb == aabb) + { + if(pos_next == nullptr) pos_next = pos_it; + break; + } + + if(pos_it->minmax == 0) + { + if(pos_next == nullptr) pos_next = pos_it; + if(pos_it->aabb->cached.overlap(aabb->cached)) + overlap_pairs.emplace_back(pos_it->aabb->obj, aabb->obj); + } + pos_it = pos_it->next[axis]; + } + + pos = pos_next; + } + } + + updateVelist(); +} + +//============================================================================== +template +void SaPCollisionManager::registerObject(CollisionObject* obj) +{ + SaPAABB* curr = new SaPAABB; + curr->cached = obj->getAABB(); + curr->obj = obj; + curr->lo = new EndPoint; + curr->lo->minmax = 0; + curr->lo->aabb = curr; + + curr->hi = new EndPoint; + curr->hi->minmax = 1; + curr->hi->aabb = curr; + + for(int coord = 0; coord < 3; ++coord) + { + EndPoint* current = elist[coord]; + + // first insert the lo end point + if(current == nullptr) // empty list + { + elist[coord] = curr->lo; + curr->lo->prev[coord] = curr->lo->next[coord] = nullptr; + } + else // otherwise, find the correct location in the list and insert + { + EndPoint* curr_lo = curr->lo; + S curr_lo_val = curr_lo->getVal()[coord]; + while((current->getVal()[coord] < curr_lo_val) && (current->next[coord] != nullptr)) + current = current->next[coord]; + + if(current->getVal()[coord] >= curr_lo_val) + { + curr_lo->prev[coord] = current->prev[coord]; + curr_lo->next[coord] = current; + if(current->prev[coord] == nullptr) + elist[coord] = curr_lo; + else + current->prev[coord]->next[coord] = curr_lo; + + current->prev[coord] = curr_lo; + } + else + { + curr_lo->prev[coord] = current; + curr_lo->next[coord] = nullptr; + current->next[coord] = curr_lo; + } + } + // now insert hi end point + current = curr->lo; + + EndPoint* curr_hi = curr->hi; + S curr_hi_val = curr_hi->getVal()[coord]; + + if(coord == 0) + { + while((current->getVal()[coord] < curr_hi_val) && (current->next[coord] != nullptr)) + { + if(current != curr->lo) + if(current->aabb->cached.overlap(curr->cached)) + overlap_pairs.emplace_back(current->aabb->obj, obj); + + current = current->next[coord]; + } + } + else + { + while((current->getVal()[coord] < curr_hi_val) && (current->next[coord] != nullptr)) + current = current->next[coord]; + } + + if(current->getVal()[coord] >= curr_hi_val) + { + curr_hi->prev[coord] = current->prev[coord]; + curr_hi->next[coord] = current; + if(current->prev[coord] == nullptr) + elist[coord] = curr_hi; + else + current->prev[coord]->next[coord] = curr_hi; + + current->prev[coord] = curr_hi; + } + else + { + curr_hi->prev[coord] = current; + curr_hi->next[coord] = nullptr; + current->next[coord] = curr_hi; + } + } + + AABB_arr.push_back(curr); + + obj_aabb_map[obj] = curr; + + updateVelist(); +} + +//============================================================================== +template +void SaPCollisionManager::setup() +{ + if(size() == 0) return; + + S scale[3]; + scale[0] = (velist[0].back())->getVal(0) - velist[0][0]->getVal(0); + scale[1] = (velist[1].back())->getVal(1) - velist[1][0]->getVal(1);; + scale[2] = (velist[2].back())->getVal(2) - velist[2][0]->getVal(2); + size_t axis = 0; + if(scale[axis] < scale[1]) axis = 1; + if(scale[axis] < scale[2]) axis = 2; + optimal_axis = axis; +} + +//============================================================================== +template +void SaPCollisionManager::update_(SaPAABB* updated_aabb) +{ + if(updated_aabb->cached.equal(updated_aabb->obj->getAABB())) + return; + + SaPAABB* current = updated_aabb; + + Vector3 new_min = current->obj->getAABB().min_; + Vector3 new_max = current->obj->getAABB().max_; + + SaPAABB dummy; + dummy.cached = current->obj->getAABB(); + + for(int coord = 0; coord < 3; ++coord) + { + int direction; // -1 reverse, 0 nochange, 1 forward + EndPoint* temp; + + if(current->lo->getVal(coord) > new_min[coord]) + direction = -1; + else if(current->lo->getVal(coord) < new_min[coord]) + direction = 1; + else direction = 0; + + if(direction == -1) + { + //first update the "lo" endpoint of the interval + if(current->lo->prev[coord] != nullptr) + { + temp = current->lo; + while((temp != nullptr) && (temp->getVal(coord) > new_min[coord])) + { + if(temp->minmax == 1) + if(temp->aabb->cached.overlap(dummy.cached)) + addToOverlapPairs(SaPPair(temp->aabb->obj, current->obj)); + temp = temp->prev[coord]; + } + + if(temp == nullptr) + { + current->lo->prev[coord]->next[coord] = current->lo->next[coord]; + current->lo->next[coord]->prev[coord] = current->lo->prev[coord]; + current->lo->prev[coord] = nullptr; + current->lo->next[coord] = elist[coord]; + elist[coord]->prev[coord] = current->lo; + elist[coord] = current->lo; + } + else + { + current->lo->prev[coord]->next[coord] = current->lo->next[coord]; + current->lo->next[coord]->prev[coord] = current->lo->prev[coord]; + current->lo->prev[coord] = temp; + current->lo->next[coord] = temp->next[coord]; + temp->next[coord]->prev[coord] = current->lo; + temp->next[coord] = current->lo; + } + } + + current->lo->getVal(coord) = new_min[coord]; + + // update hi end point + temp = current->hi; + while(temp->getVal(coord) > new_max[coord]) + { + if((temp->minmax == 0) && (temp->aabb->cached.overlap(current->cached))) + removeFromOverlapPairs(SaPPair(temp->aabb->obj, current->obj)); + temp = temp->prev[coord]; + } + + current->hi->prev[coord]->next[coord] = current->hi->next[coord]; + if(current->hi->next[coord] != nullptr) + current->hi->next[coord]->prev[coord] = current->hi->prev[coord]; + current->hi->prev[coord] = temp; + current->hi->next[coord] = temp->next[coord]; + if(temp->next[coord] != nullptr) + temp->next[coord]->prev[coord] = current->hi; + temp->next[coord] = current->hi; + + current->hi->getVal(coord) = new_max[coord]; + } + else if(direction == 1) + { + //here, we first update the "hi" endpoint. + if(current->hi->next[coord] != nullptr) + { + temp = current->hi; + while((temp->next[coord] != nullptr) && (temp->getVal(coord) < new_max[coord])) + { + if(temp->minmax == 0) + if(temp->aabb->cached.overlap(dummy.cached)) + addToOverlapPairs(SaPPair(temp->aabb->obj, current->obj)); + temp = temp->next[coord]; + } + + if(temp->getVal(coord) < new_max[coord]) + { + current->hi->prev[coord]->next[coord] = current->hi->next[coord]; + current->hi->next[coord]->prev[coord] = current->hi->prev[coord]; + current->hi->prev[coord] = temp; + current->hi->next[coord] = nullptr; + temp->next[coord] = current->hi; + } + else + { + current->hi->prev[coord]->next[coord] = current->hi->next[coord]; + current->hi->next[coord]->prev[coord] = current->hi->prev[coord]; + current->hi->prev[coord] = temp->prev[coord]; + current->hi->next[coord] = temp; + temp->prev[coord]->next[coord] = current->hi; + temp->prev[coord] = current->hi; + } + } + + current->hi->getVal(coord) = new_max[coord]; + + //then, update the "lo" endpoint of the interval. + temp = current->lo; + + while(temp->getVal(coord) < new_min[coord]) + { + if((temp->minmax == 1) && (temp->aabb->cached.overlap(current->cached))) + removeFromOverlapPairs(SaPPair(temp->aabb->obj, current->obj)); + temp = temp->next[coord]; + } + + if(current->lo->prev[coord] != nullptr) + current->lo->prev[coord]->next[coord] = current->lo->next[coord]; + else + elist[coord] = current->lo->next[coord]; + current->lo->next[coord]->prev[coord] = current->lo->prev[coord]; + current->lo->prev[coord] = temp->prev[coord]; + current->lo->next[coord] = temp; + if(temp->prev[coord] != nullptr) + temp->prev[coord]->next[coord] = current->lo; + else + elist[coord] = current->lo; + temp->prev[coord] = current->lo; + current->lo->getVal(coord) = new_min[coord]; + } + } +} + +//============================================================================== +template +void SaPCollisionManager::update(CollisionObject* updated_obj) +{ + update_(obj_aabb_map[updated_obj]); + + updateVelist(); + + setup(); +} + +//============================================================================== +template +void SaPCollisionManager::update(const std::vector*>& updated_objs) +{ + for(size_t i = 0; i < updated_objs.size(); ++i) + update_(obj_aabb_map[updated_objs[i]]); + + updateVelist(); + + setup(); +} + +//============================================================================== +template +void SaPCollisionManager::update() +{ + for(auto it = AABB_arr.cbegin(), end = AABB_arr.cend(); it != end; ++it) + { + update_(*it); + } + + updateVelist(); + + setup(); +} + +//============================================================================== +template +void SaPCollisionManager::clear() +{ + for(auto it = AABB_arr.begin(), end = AABB_arr.end(); it != end; ++it) + { + delete (*it)->hi; + delete (*it)->lo; + delete *it; + *it = nullptr; + } + + AABB_arr.clear(); + overlap_pairs.clear(); + + elist[0] = nullptr; + elist[1] = nullptr; + elist[2] = nullptr; + + velist[0].clear(); + velist[1].clear(); + velist[2].clear(); + + obj_aabb_map.clear(); +} + +//============================================================================== +template +void SaPCollisionManager::getObjects(std::vector*>& objs) const +{ + objs.resize(AABB_arr.size()); + int i = 0; + for(auto it = AABB_arr.cbegin(), end = AABB_arr.cend(); it != end; ++it, ++i) + { + objs[i] = (*it)->obj; + } +} + +//============================================================================== +template +bool SaPCollisionManager::collide_(CollisionObject* obj, void* cdata, CollisionCallBack callback) const +{ + size_t axis = optimal_axis; + const AABB& obj_aabb = obj->getAABB(); + + S min_val = obj_aabb.min_[axis]; + // S max_val = obj_aabb.max_[axis]; + + EndPoint dummy; + SaPAABB dummy_aabb; + dummy_aabb.cached = obj_aabb; + dummy.minmax = 1; + dummy.aabb = &dummy_aabb; + + // compute stop_pos by binary search, this is cheaper than check it in while iteration linearly + const auto res_it = std::upper_bound(velist[axis].begin(), velist[axis].end(), &dummy, + std::bind(std::less(), + std::bind(static_cast(&EndPoint::getVal), std::placeholders::_1, axis), + std::bind(static_cast(&EndPoint::getVal), std::placeholders::_2, axis))); + + EndPoint* end_pos = nullptr; + if(res_it != velist[axis].end()) + end_pos = *res_it; + + EndPoint* pos = elist[axis]; + + while(pos != end_pos) + { + if(pos->aabb->obj != obj) + { + if((pos->minmax == 0) && (pos->aabb->hi->getVal(axis) >= min_val)) + { + if(pos->aabb->cached.overlap(obj->getAABB())) + if(callback(obj, pos->aabb->obj, cdata)) + return true; + } + } + pos = pos->next[axis]; + } + + return false; +} + +//============================================================================== +template +void SaPCollisionManager::collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const +{ + if(size() == 0) return; + + collide_(obj, cdata, callback); +} + +//============================================================================== +template +bool SaPCollisionManager::distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, S& min_dist) const +{ + Vector3 delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5; + AABB aabb = obj->getAABB(); + + if(min_dist < std::numeric_limits::max()) + { + Vector3 min_dist_delta(min_dist, min_dist, min_dist); + aabb.expand(min_dist_delta); + } + + size_t axis = optimal_axis; + + int status = 1; + S old_min_distance; + + EndPoint* start_pos = elist[axis]; + + while(1) + { + old_min_distance = min_dist; + S min_val = aabb.min_[axis]; + // S max_val = aabb.max_[axis]; + + EndPoint dummy; + SaPAABB dummy_aabb; + dummy_aabb.cached = aabb; + dummy.minmax = 1; + dummy.aabb = &dummy_aabb; + + + const auto res_it = std::upper_bound(velist[axis].begin(), velist[axis].end(), &dummy, + std::bind(std::less(), + std::bind(static_cast(&EndPoint::getVal), std::placeholders::_1, axis), + std::bind(static_cast(&EndPoint::getVal), std::placeholders::_2, axis))); + + EndPoint* end_pos = nullptr; + if(res_it != velist[axis].end()) + end_pos = *res_it; + + EndPoint* pos = start_pos; + + while(pos != end_pos) + { + // can change to pos->aabb->hi->getVal(axis) >= min_val - min_dist, and then update start_pos to end_pos. + // but this seems slower. + if((pos->minmax == 0) && (pos->aabb->hi->getVal(axis) >= min_val)) + { + CollisionObject* curr_obj = pos->aabb->obj; + if(curr_obj != obj) + { + if(!this->enable_tested_set_) + { + if(pos->aabb->cached.distance(obj->getAABB()) < min_dist) + { + if(callback(curr_obj, obj, cdata, min_dist)) + return true; + } + } + else + { + if(!this->inTestedSet(curr_obj, obj)) + { + if(pos->aabb->cached.distance(obj->getAABB()) < min_dist) + { + if(callback(curr_obj, obj, cdata, min_dist)) + return true; + } + + this->insertTestedSet(curr_obj, obj); + } + } + } + } + + pos = pos->next[axis]; + } + + if(status == 1) + { + if(old_min_distance < std::numeric_limits::max()) + break; + else + { + if(min_dist < old_min_distance) + { + Vector3 min_dist_delta(min_dist, min_dist, min_dist); + aabb = AABB(obj->getAABB(), min_dist_delta); + status = 0; + } + else + { + if(aabb.equal(obj->getAABB())) + aabb.expand(delta); + else + aabb.expand(obj->getAABB(), 2.0); + } + } + } + else if(status == 0) + break; + } + + return false; +} + +//============================================================================== +template +void SaPCollisionManager::distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const +{ + if(size() == 0) return; + + S min_dist = std::numeric_limits::max(); + + distance_(obj, cdata, callback, min_dist); +} + +//============================================================================== +template +void SaPCollisionManager::collide(void* cdata, CollisionCallBack callback) const +{ + if(size() == 0) return; + + for(auto it = overlap_pairs.cbegin(), end = overlap_pairs.cend(); it != end; ++it) + { + CollisionObject* obj1 = it->obj1; + CollisionObject* obj2 = it->obj2; + + if(callback(obj1, obj2, cdata)) + return; + } +} + +//============================================================================== +template +void SaPCollisionManager::distance(void* cdata, DistanceCallBack callback) const +{ + if(size() == 0) return; + + this->enable_tested_set_ = true; + this->tested_set.clear(); + + S min_dist = std::numeric_limits::max(); + + for(auto it = AABB_arr.cbegin(), end = AABB_arr.cend(); it != end; ++it) + { + if(distance_((*it)->obj, cdata, callback, min_dist)) + break; + } + + this->enable_tested_set_ = false; + this->tested_set.clear(); +} + +//============================================================================== +template +void SaPCollisionManager::collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const +{ + SaPCollisionManager* other_manager = static_cast(other_manager_); + + if((size() == 0) || (other_manager->size() == 0)) return; + + if(this == other_manager) + { + collide(cdata, callback); + return; + } + + if(this->size() < other_manager->size()) + { + for(auto it = AABB_arr.cbegin(); it != AABB_arr.cend(); ++it) + { + if(other_manager->collide_((*it)->obj, cdata, callback)) + return; + } + } + else + { + for(auto it = other_manager->AABB_arr.cbegin(), end = other_manager->AABB_arr.cend(); it != end; ++it) + { + if(collide_((*it)->obj, cdata, callback)) + return; + } + } +} + +//============================================================================== +template +void SaPCollisionManager::distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const +{ + SaPCollisionManager* other_manager = static_cast(other_manager_); + + if((size() == 0) || (other_manager->size() == 0)) return; + + if(this == other_manager) + { + distance(cdata, callback); + return; + } + + S min_dist = std::numeric_limits::max(); + + if(this->size() < other_manager->size()) + { + for(auto it = AABB_arr.cbegin(), end = AABB_arr.cend(); it != end; ++it) + { + if(other_manager->distance_((*it)->obj, cdata, callback, min_dist)) + return; + } + } + else + { + for(auto it = other_manager->AABB_arr.cbegin(), end = other_manager->AABB_arr.cend(); it != end; ++it) + { + if(distance_((*it)->obj, cdata, callback, min_dist)) + return; + } + } +} + +//============================================================================== +template +bool SaPCollisionManager::empty() const +{ + return AABB_arr.size(); } +} // namespace fcl #endif diff --git a/include/fcl/broadphase/broadphase_bruteforce.h b/include/fcl/broadphase/broadphase_bruteforce.h index 25d6d44fb..2330aedc0 100644 --- a/include/fcl/broadphase/broadphase_bruteforce.h +++ b/include/fcl/broadphase/broadphase_bruteforce.h @@ -46,19 +46,20 @@ namespace fcl { /// @brief Brute force N-body collision manager -class NaiveCollisionManager : public BroadPhaseCollisionManager +template +class NaiveCollisionManager : public BroadPhaseCollisionManager { public: NaiveCollisionManager() {} /// @brief add objects to the manager - void registerObjects(const std::vector& other_objs); + void registerObjects(const std::vector*>& other_objs); /// @brief add one object to the manager - void registerObject(CollisionObject* obj); + void registerObject(CollisionObject* obj); /// @brief remove one object from the manager - void unregisterObject(CollisionObject* obj); + void unregisterObject(CollisionObject* obj); /// @brief initialize the manager, related with the specific type of manager void setup(); @@ -70,25 +71,25 @@ class NaiveCollisionManager : public BroadPhaseCollisionManager void clear(); /// @brief return the objects managed by the manager - void getObjects(std::vector& objs) const; + void getObjects(std::vector*>& objs) const; /// @brief perform collision test between one object and all the objects belonging to the manager - void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const; + void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const; /// @brief perform distance computation between one object and all the objects belonging to the manager - void distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const; + void distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const; /// @brief perform collision test for the objects belonging to the manager (i.e., N^2 self collision) - void collide(void* cdata, CollisionCallBack callback) const; + void collide(void* cdata, CollisionCallBack callback) const; /// @brief perform distance test for the objects belonging to the manager (i.e., N^2 self distance) - void distance(void* cdata, DistanceCallBack callback) const; + void distance(void* cdata, DistanceCallBack callback) const; /// @brief perform collision test with objects belonging to another manager - void collide(BroadPhaseCollisionManager* other_manager, void* cdata, CollisionCallBack callback) const; + void collide(BroadPhaseCollisionManager* other_manager, void* cdata, CollisionCallBack callback) const; /// @brief perform distance test with objects belonging to another manager - void distance(BroadPhaseCollisionManager* other_manager, void* cdata, DistanceCallBack callback) const; + void distance(BroadPhaseCollisionManager* other_manager, void* cdata, DistanceCallBack callback) const; /// @brief whether the manager is empty bool empty() const; @@ -99,10 +100,202 @@ class NaiveCollisionManager : public BroadPhaseCollisionManager protected: /// @brief objects belonging to the manager are stored in a list structure - std::list objs; + std::list*> objs; }; +using NaiveCollisionManagerf = NaiveCollisionManager; +using NaiveCollisionManagerd = NaiveCollisionManager; +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +void NaiveCollisionManager::registerObjects(const std::vector*>& other_objs) +{ + std::copy(other_objs.begin(), other_objs.end(), std::back_inserter(objs)); +} + +//============================================================================== +template +void NaiveCollisionManager::unregisterObject(CollisionObject* obj) +{ + objs.remove(obj); +} + +//============================================================================== +template +void NaiveCollisionManager::registerObject(CollisionObject* obj) +{ + objs.push_back(obj); +} + +//============================================================================== +template +void NaiveCollisionManager::setup() +{ + +} + +//============================================================================== +template +void NaiveCollisionManager::update() +{ + +} + +//============================================================================== +template +void NaiveCollisionManager::clear() +{ + objs.clear(); +} + +//============================================================================== +template +void NaiveCollisionManager::getObjects(std::vector*>& objs_) const +{ + objs_.resize(objs.size()); + std::copy(objs.begin(), objs.end(), objs_.begin()); +} + +//============================================================================== +template +void NaiveCollisionManager::collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const +{ + if(size() == 0) return; + + for(auto* obj2 : objs) + { + if(callback(obj, obj2, cdata)) + return; + } +} + +//============================================================================== +template +void NaiveCollisionManager::distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const +{ + if(size() == 0) return; + + S min_dist = std::numeric_limits::max(); + for(auto* obj2 : objs) + { + if(obj->getAABB().distance(obj2->getAABB()) < min_dist) + { + if(callback(obj, obj2, cdata, min_dist)) + return; + } + } +} + +//============================================================================== +template +void NaiveCollisionManager::collide(void* cdata, CollisionCallBack callback) const +{ + if(size() == 0) return; + + for(typename std::list*>::const_iterator it1 = objs.begin(), end = objs.end(); + it1 != end; ++it1) + { + typename std::list*>::const_iterator it2 = it1; it2++; + for(; it2 != end; ++it2) + { + if((*it1)->getAABB().overlap((*it2)->getAABB())) + { + if(callback(*it1, *it2, cdata)) + return; + } + } + } } +//============================================================================== +template +void NaiveCollisionManager::distance(void* cdata, DistanceCallBack callback) const +{ + if(size() == 0) return; + + S min_dist = std::numeric_limits::max(); + for(typename std::list*>::const_iterator it1 = objs.begin(), end = objs.end(); it1 != end; ++it1) + { + typename std::list*>::const_iterator it2 = it1; it2++; + for(; it2 != end; ++it2) + { + if((*it1)->getAABB().distance((*it2)->getAABB()) < min_dist) + { + if(callback(*it1, *it2, cdata, min_dist)) + return; + } + } + } +} + +//============================================================================== +template +void NaiveCollisionManager::collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const +{ + NaiveCollisionManager* other_manager = static_cast(other_manager_); + + if((size() == 0) || (other_manager->size() == 0)) return; + + if(this == other_manager) + { + collide(cdata, callback); + return; + } + + for(auto* obj1 : objs) + { + for(auto* obj2 : other_manager->objs) + { + if(obj1->getAABB().overlap(obj2->getAABB())) + { + if(callback(obj1, obj2, cdata)) + return; + } + } + } +} + +//============================================================================== +template +void NaiveCollisionManager::distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const +{ + NaiveCollisionManager* other_manager = static_cast(other_manager_); + + if((size() == 0) || (other_manager->size() == 0)) return; + + if(this == other_manager) + { + distance(cdata, callback); + return; + } + + S min_dist = std::numeric_limits::max(); + for(auto* obj1 : objs) + { + for(auto* obj2 : other_manager->objs) + { + if(obj1->getAABB().distance(obj2->getAABB()) < min_dist) + { + if(callback(obj1, obj2, cdata, min_dist)) + return; + } + } + } +} + +//============================================================================== +template +bool NaiveCollisionManager::empty() const +{ + return objs.empty(); +} + +} // namespace + #endif diff --git a/include/fcl/broadphase/broadphase_dynamic_AABB_tree.h b/include/fcl/broadphase/broadphase_dynamic_AABB_tree.h index 99e3ed7c6..755375aaa 100644 --- a/include/fcl/broadphase/broadphase_dynamic_AABB_tree.h +++ b/include/fcl/broadphase/broadphase_dynamic_AABB_tree.h @@ -39,23 +39,28 @@ #ifndef FCL_BROAD_PHASE_DYNAMIC_AABB_TREE_H #define FCL_BROAD_PHASE_DYNAMIC_AABB_TREE_H -#include "fcl/broadphase/broadphase.h" -#include "fcl/broadphase/hierarchy_tree.h" -#include "fcl/BV/BV.h" -#include "fcl/shape/geometric_shapes_utility.h" #include #include #include - +#include "fcl/shape/box.h" +#include "fcl/shape/construct_box.h" +#include "fcl/broadphase/broadphase.h" +#include "fcl/broadphase/hierarchy_tree.h" +#include "fcl/BV/convert_bv.h" +#if FCL_HAVE_OCTOMAP +#include "fcl/octree.h" +#endif namespace fcl { -class DynamicAABBTreeCollisionManager : public BroadPhaseCollisionManager +template +class DynamicAABBTreeCollisionManager : public BroadPhaseCollisionManager { public: - typedef NodeBase DynamicAABBNode; - typedef std::unordered_map DynamicAABBTable; + + using DynamicAABBNode = NodeBase>; + using DynamicAABBTable = std::unordered_map*, DynamicAABBNode*> ; int max_tree_nonbalanced_level; int tree_incremental_balance_pass; @@ -83,13 +88,13 @@ class DynamicAABBTreeCollisionManager : public BroadPhaseCollisionManager } /// @brief add objects to the manager - void registerObjects(const std::vector& other_objs); + void registerObjects(const std::vector*>& other_objs); /// @brief add one object to the manager - void registerObject(CollisionObject* obj); + void registerObject(CollisionObject* obj); /// @brief remove one object from the manager - void unregisterObject(CollisionObject* obj); + void unregisterObject(CollisionObject* obj); /// @brief initialize the manager, related with the specific type of manager void setup(); @@ -98,10 +103,10 @@ class DynamicAABBTreeCollisionManager : public BroadPhaseCollisionManager void update(); /// @brief update the manager by explicitly given the object updated - void update(CollisionObject* updated_obj); + void update(CollisionObject* updated_obj); /// @brief update the manager by explicitly given the set of objects update - void update(const std::vector& updated_objs); + void update(const std::vector*>& updated_objs); /// @brief clear the manager void clear() @@ -111,29 +116,29 @@ class DynamicAABBTreeCollisionManager : public BroadPhaseCollisionManager } /// @brief return the objects managed by the manager - void getObjects(std::vector& objs) const + void getObjects(std::vector*>& objs) const { objs.resize(this->size()); std::transform(table.begin(), table.end(), objs.begin(), std::bind(&DynamicAABBTable::value_type::first, std::placeholders::_1)); } /// @brief perform collision test between one object and all the objects belonging to the manager - void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const; + void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const; /// @brief perform distance computation between one object and all the objects belonging to the manager - void distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const; + void distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const; /// @brief perform collision test for the objects belonging to the manager (i.e., N^2 self collision) - void collide(void* cdata, CollisionCallBack callback) const; + void collide(void* cdata, CollisionCallBack callback) const; /// @brief perform distance test for the objects belonging to the manager (i.e., N^2 self distance) - void distance(void* cdata, DistanceCallBack callback) const; + void distance(void* cdata, DistanceCallBack callback) const; /// @brief perform collision test with objects belonging to another manager - void collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const; + void collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const; /// @brief perform distance test with objects belonging to another manager - void distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const; + void distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const; /// @brief whether the manager is empty bool empty() const @@ -147,19 +152,895 @@ class DynamicAABBTreeCollisionManager : public BroadPhaseCollisionManager return dtree.size(); } - const HierarchyTree& getTree() const { return dtree; } + const HierarchyTree>& getTree() const { return dtree; } private: - HierarchyTree dtree; - std::unordered_map table; + HierarchyTree> dtree; + std::unordered_map*, DynamicAABBNode*> table; bool setup_; - void update_(CollisionObject* updated_obj); + void update_(CollisionObject* updated_obj); }; +using DynamicAABBTreeCollisionManagerf = DynamicAABBTreeCollisionManager; +using DynamicAABBTreeCollisionManagerd = DynamicAABBTreeCollisionManager; + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +namespace details +{ + +namespace dynamic_AABB_tree +{ + +#if FCL_HAVE_OCTOMAP +//============================================================================== +template +bool collisionRecurse_( + typename DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, + const OcTree* tree2, + const typename OcTree::OcTreeNode* root2, + const AABB& root2_bv, + const Transform3& tf2, + void* cdata, + CollisionCallBack callback) +{ + if(!root2) + { + if(root1->isLeaf()) + { + CollisionObject* obj1 = static_cast*>(root1->data); + + if(!obj1->isFree()) + { + OBB obb1, obb2; + convertBV(root1->bv, Transform3::Identity(), obb1); + convertBV(root2_bv, tf2, obb2); + + if(obb1.overlap(obb2)) + { + Box* box = new Box(); + Transform3 box_tf; + constructBox(root2_bv, tf2, *box, box_tf); + + box->cost_density = tree2->getDefaultOccupancy(); + + CollisionObject obj2(std::shared_ptr>(box), box_tf); + return callback(obj1, &obj2, cdata); + } + } + } + else + { + if(collisionRecurse_(root1->children[0], tree2, nullptr, root2_bv, tf2, cdata, callback)) + return true; + if(collisionRecurse_(root1->children[1], tree2, nullptr, root2_bv, tf2, cdata, callback)) + return true; + } + + return false; + } + else if(root1->isLeaf() && !tree2->nodeHasChildren(root2)) + { + CollisionObject* obj1 = static_cast*>(root1->data); + + if(!tree2->isNodeFree(root2) && !obj1->isFree()) + { + OBB obb1, obb2; + convertBV(root1->bv, Transform3::Identity(), obb1); + convertBV(root2_bv, tf2, obb2); + + if(obb1.overlap(obb2)) + { + Box* box = new Box(); + Transform3 box_tf; + constructBox(root2_bv, tf2, *box, box_tf); + + box->cost_density = root2->getOccupancy(); + box->threshold_occupied = tree2->getOccupancyThres(); + + CollisionObject obj2(std::shared_ptr>(box), box_tf); + return callback(obj1, &obj2, cdata); + } + else return false; + } + else return false; + } + + OBB obb1, obb2; + convertBV(root1->bv, Transform3::Identity(), obb1); + convertBV(root2_bv, tf2, obb2); + + if(tree2->isNodeFree(root2) || !obb1.overlap(obb2)) return false; + + if(!tree2->nodeHasChildren(root2) || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) + { + if(collisionRecurse_(root1->children[0], tree2, root2, root2_bv, tf2, cdata, callback)) + return true; + if(collisionRecurse_(root1->children[1], tree2, root2, root2_bv, tf2, cdata, callback)) + return true; + } + else + { + for(unsigned int i = 0; i < 8; ++i) + { + if(tree2->nodeChildExists(root2, i)) + { + const typename OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); + AABB child_bv; + computeChildBV(root2_bv, i, child_bv); + + if(collisionRecurse_(root1, tree2, child, child_bv, tf2, cdata, callback)) + return true; + } + else + { + AABB child_bv; + computeChildBV(root2_bv, i, child_bv); + if(collisionRecurse_(root1, tree2, nullptr, child_bv, tf2, cdata, callback)) + return true; + } + } + } + return false; +} + +//============================================================================== +template +bool collisionRecurse_( + typename DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, + const OcTree* tree2, + const typename OcTree::OcTreeNode* root2, + const AABB& root2_bv, + const Eigen::MatrixBase& translation2, + void* cdata, + CollisionCallBack callback) +{ + if(!root2) + { + if(root1->isLeaf()) + { + CollisionObject* obj1 = static_cast*>(root1->data); + + if(!obj1->isFree()) + { + const AABB& root2_bv_t = translate(root2_bv, translation2); + if(root1->bv.overlap(root2_bv_t)) + { + Box* box = new Box(); + Transform3 box_tf; + Transform3 tf2 = Transform3::Identity(); + tf2.translation() = translation2; + constructBox(root2_bv, tf2, *box, box_tf); + + box->cost_density = tree2->getOccupancyThres(); // thresholds are 0, 1, so uncertain + + CollisionObject obj2(std::shared_ptr>(box), box_tf); + return callback(obj1, &obj2, cdata); + } + } + } + else + { + if(collisionRecurse_(root1->children[0], tree2, nullptr, root2_bv, translation2, cdata, callback)) + return true; + if(collisionRecurse_(root1->children[1], tree2, nullptr, root2_bv, translation2, cdata, callback)) + return true; + } + + return false; + } + else if(root1->isLeaf() && !tree2->nodeHasChildren(root2)) + { + CollisionObject* obj1 = static_cast*>(root1->data); + + if(!tree2->isNodeFree(root2) && !obj1->isFree()) + { + const AABB& root2_bv_t = translate(root2_bv, translation2); + if(root1->bv.overlap(root2_bv_t)) + { + Box* box = new Box(); + Transform3 box_tf; + Transform3 tf2 = Transform3::Identity(); + tf2.translation() = translation2; + constructBox(root2_bv, tf2, *box, box_tf); + + box->cost_density = root2->getOccupancy(); + box->threshold_occupied = tree2->getOccupancyThres(); + + CollisionObject obj2(std::shared_ptr>(box), box_tf); + return callback(obj1, &obj2, cdata); + } + else return false; + } + else return false; + } + + const AABB& root2_bv_t = translate(root2_bv, translation2); + if(tree2->isNodeFree(root2) || !root1->bv.overlap(root2_bv_t)) return false; + + if(!tree2->nodeHasChildren(root2) || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) + { + if(collisionRecurse_(root1->children[0], tree2, root2, root2_bv, translation2, cdata, callback)) + return true; + if(collisionRecurse_(root1->children[1], tree2, root2, root2_bv, translation2, cdata, callback)) + return true; + } + else + { + for(unsigned int i = 0; i < 8; ++i) + { + if(tree2->nodeChildExists(root2, i)) + { + const typename OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); + AABB child_bv; + computeChildBV(root2_bv, i, child_bv); + + if(collisionRecurse_(root1, tree2, child, child_bv, translation2, cdata, callback)) + return true; + } + else + { + AABB child_bv; + computeChildBV(root2_bv, i, child_bv); + if(collisionRecurse_(root1, tree2, nullptr, child_bv, translation2, cdata, callback)) + return true; + } + } + } + return false; +} + +//============================================================================== +template +bool distanceRecurse_( + typename DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, + const OcTree* tree2, + const typename OcTree::OcTreeNode* root2, + const AABB& root2_bv, + const Transform3& tf2, + void* cdata, + DistanceCallBack callback, + S& min_dist) +{ + if(root1->isLeaf() && !tree2->nodeHasChildren(root2)) + { + if(tree2->isNodeOccupied(root2)) + { + Box* box = new Box(); + Transform3 box_tf; + constructBox(root2_bv, tf2, *box, box_tf); + CollisionObject obj(std::shared_ptr>(box), box_tf); + return callback(static_cast*>(root1->data), &obj, cdata, min_dist); + } + else return false; + } + + if(!tree2->isNodeOccupied(root2)) return false; + + if(!tree2->nodeHasChildren(root2) || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) + { + AABB aabb2; + convertBV(root2_bv, tf2, aabb2); + + S d1 = aabb2.distance(root1->children[0]->bv); + S d2 = aabb2.distance(root1->children[1]->bv); + + if(d2 < d1) + { + if(d2 < min_dist) + { + if(distanceRecurse_(root1->children[1], tree2, root2, root2_bv, tf2, cdata, callback, min_dist)) + return true; + } + + if(d1 < min_dist) + { + if(distanceRecurse_(root1->children[0], tree2, root2, root2_bv, tf2, cdata, callback, min_dist)) + return true; + } + } + else + { + if(d1 < min_dist) + { + if(distanceRecurse_(root1->children[0], tree2, root2, root2_bv, tf2, cdata, callback, min_dist)) + return true; + } + + if(d2 < min_dist) + { + if(distanceRecurse_(root1->children[1], tree2, root2, root2_bv, tf2, cdata, callback, min_dist)) + return true; + } + } + } + else + { + for(unsigned int i = 0; i < 8; ++i) + { + if(tree2->nodeChildExists(root2, i)) + { + const typename OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); + AABB child_bv; + computeChildBV(root2_bv, i, child_bv); + + AABB aabb2; + convertBV(child_bv, tf2, aabb2); + S d = root1->bv.distance(aabb2); + + if(d < min_dist) + { + if(distanceRecurse_(root1, tree2, child, child_bv, tf2, cdata, callback, min_dist)) + return true; + } + } + } + } + + return false; +} + +//============================================================================== +template +bool collisionRecurse( + typename DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, + const OcTree* tree2, + const typename OcTree::OcTreeNode* root2, + const AABB& root2_bv, + const Transform3& tf2, + void* cdata, + CollisionCallBack callback) +{ + if(tf2.linear().isIdentity()) + return collisionRecurse_(root1, tree2, root2, root2_bv, tf2.translation(), cdata, callback); + else // has rotation + return collisionRecurse_(root1, tree2, root2, root2_bv, tf2, cdata, callback); +} + +//============================================================================== +template +bool distanceRecurse_( + typename DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, + const OcTree* tree2, + const typename OcTree::OcTreeNode* root2, + const AABB& root2_bv, + const Eigen::MatrixBase& translation2, + void* cdata, + DistanceCallBack callback, + S& min_dist) +{ + if(root1->isLeaf() && !tree2->nodeHasChildren(root2)) + { + if(tree2->isNodeOccupied(root2)) + { + Box* box = new Box(); + Transform3 box_tf; + Transform3 tf2 = Transform3::Identity(); + tf2.translation() = translation2; + constructBox(root2_bv, tf2, *box, box_tf); + CollisionObject obj(std::shared_ptr>(box), box_tf); + return callback(static_cast*>(root1->data), &obj, cdata, min_dist); + } + else return false; + } + + if(!tree2->isNodeOccupied(root2)) return false; + + if(!tree2->nodeHasChildren(root2) || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) + { + const AABB& aabb2 = translate(root2_bv, translation2); + S d1 = aabb2.distance(root1->children[0]->bv); + S d2 = aabb2.distance(root1->children[1]->bv); + + if(d2 < d1) + { + if(d2 < min_dist) + { + if(distanceRecurse_(root1->children[1], tree2, root2, root2_bv, translation2, cdata, callback, min_dist)) + return true; + } + + if(d1 < min_dist) + { + if(distanceRecurse_(root1->children[0], tree2, root2, root2_bv, translation2, cdata, callback, min_dist)) + return true; + } + } + else + { + if(d1 < min_dist) + { + if(distanceRecurse_(root1->children[0], tree2, root2, root2_bv, translation2, cdata, callback, min_dist)) + return true; + } + + if(d2 < min_dist) + { + if(distanceRecurse_(root1->children[1], tree2, root2, root2_bv, translation2, cdata, callback, min_dist)) + return true; + } + } + } + else + { + for(unsigned int i = 0; i < 8; ++i) + { + if(tree2->nodeChildExists(root2, i)) + { + const typename OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); + AABB child_bv; + computeChildBV(root2_bv, i, child_bv); + const AABB& aabb2 = translate(child_bv, translation2); + + S d = root1->bv.distance(aabb2); + + if(d < min_dist) + { + if(distanceRecurse_(root1, tree2, child, child_bv, translation2, cdata, callback, min_dist)) + return true; + } + } + } + } + + return false; +} + +//============================================================================== +template +bool distanceRecurse(typename DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const OcTree* tree2, const typename OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3& tf2, void* cdata, DistanceCallBack callback, S& min_dist) +{ + if(tf2.linear().isIdentity()) + return distanceRecurse_(root1, tree2, root2, root2_bv, tf2.translation(), cdata, callback, min_dist); + else + return distanceRecurse_(root1, tree2, root2, root2_bv, tf2, cdata, callback, min_dist); +} + +#endif + +//============================================================================== +template +bool collisionRecurse( + typename DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, + typename DynamicAABBTreeCollisionManager::DynamicAABBNode* root2, + void* cdata, + CollisionCallBack callback) +{ + if(root1->isLeaf() && root2->isLeaf()) + { + if(!root1->bv.overlap(root2->bv)) return false; + return callback(static_cast*>(root1->data), static_cast*>(root2->data), cdata); + } + + if(!root1->bv.overlap(root2->bv)) return false; + + if(root2->isLeaf() || (!root1->isLeaf() && (root1->bv.size() > root2->bv.size()))) + { + if(collisionRecurse(root1->children[0], root2, cdata, callback)) + return true; + if(collisionRecurse(root1->children[1], root2, cdata, callback)) + return true; + } + else + { + if(collisionRecurse(root1, root2->children[0], cdata, callback)) + return true; + if(collisionRecurse(root1, root2->children[1], cdata, callback)) + return true; + } + return false; +} + +//============================================================================== +template +bool collisionRecurse(typename DynamicAABBTreeCollisionManager::DynamicAABBNode* root, CollisionObject* query, void* cdata, CollisionCallBack callback) +{ + if(root->isLeaf()) + { + if(!root->bv.overlap(query->getAABB())) return false; + return callback(static_cast*>(root->data), query, cdata); + } + + if(!root->bv.overlap(query->getAABB())) return false; + + int select_res = select(query->getAABB(), *(root->children[0]), *(root->children[1])); + if(collisionRecurse(root->children[select_res], query, cdata, callback)) + return true; + + if(collisionRecurse(root->children[1-select_res], query, cdata, callback)) + return true; + + return false; +} + +//============================================================================== +template +bool selfCollisionRecurse(typename DynamicAABBTreeCollisionManager::DynamicAABBNode* root, void* cdata, CollisionCallBack callback) +{ + if(root->isLeaf()) return false; + + if(selfCollisionRecurse(root->children[0], cdata, callback)) + return true; + + if(selfCollisionRecurse(root->children[1], cdata, callback)) + return true; + + if(collisionRecurse(root->children[0], root->children[1], cdata, callback)) + return true; + + return false; +} + +//============================================================================== +template +bool distanceRecurse( + typename DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, + typename DynamicAABBTreeCollisionManager::DynamicAABBNode* root2, + void* cdata, + DistanceCallBack callback, + S& min_dist) +{ + if(root1->isLeaf() && root2->isLeaf()) + { + CollisionObject* root1_obj = static_cast*>(root1->data); + CollisionObject* root2_obj = static_cast*>(root2->data); + return callback(root1_obj, root2_obj, cdata, min_dist); + } + + if(root2->isLeaf() || (!root1->isLeaf() && (root1->bv.size() > root2->bv.size()))) + { + S d1 = root2->bv.distance(root1->children[0]->bv); + S d2 = root2->bv.distance(root1->children[1]->bv); + + if(d2 < d1) + { + if(d2 < min_dist) + { + if(distanceRecurse(root1->children[1], root2, cdata, callback, min_dist)) + return true; + } + + if(d1 < min_dist) + { + if(distanceRecurse(root1->children[0], root2, cdata, callback, min_dist)) + return true; + } + } + else + { + if(d1 < min_dist) + { + if(distanceRecurse(root1->children[0], root2, cdata, callback, min_dist)) + return true; + } + + if(d2 < min_dist) + { + if(distanceRecurse(root1->children[1], root2, cdata, callback, min_dist)) + return true; + } + } + } + else + { + S d1 = root1->bv.distance(root2->children[0]->bv); + S d2 = root1->bv.distance(root2->children[1]->bv); + + if(d2 < d1) + { + if(d2 < min_dist) + { + if(distanceRecurse(root1, root2->children[1], cdata, callback, min_dist)) + return true; + } + + if(d1 < min_dist) + { + if(distanceRecurse(root1, root2->children[0], cdata, callback, min_dist)) + return true; + } + } + else + { + if(d1 < min_dist) + { + if(distanceRecurse(root1, root2->children[0], cdata, callback, min_dist)) + return true; + } + + if(d2 < min_dist) + { + if(distanceRecurse(root1, root2->children[1], cdata, callback, min_dist)) + return true; + } + } + } + + return false; +} + +//============================================================================== +template +bool distanceRecurse(typename DynamicAABBTreeCollisionManager::DynamicAABBNode* root, CollisionObject* query, void* cdata, DistanceCallBack callback, S& min_dist) +{ + if(root->isLeaf()) + { + CollisionObject* root_obj = static_cast*>(root->data); + return callback(root_obj, query, cdata, min_dist); + } + + S d1 = query->getAABB().distance(root->children[0]->bv); + S d2 = query->getAABB().distance(root->children[1]->bv); + + if(d2 < d1) + { + if(d2 < min_dist) + { + if(distanceRecurse(root->children[1], query, cdata, callback, min_dist)) + return true; + } + + if(d1 < min_dist) + { + if(distanceRecurse(root->children[0], query, cdata, callback, min_dist)) + return true; + } + } + else + { + if(d1 < min_dist) + { + if(distanceRecurse(root->children[0], query, cdata, callback, min_dist)) + return true; + } + + if(d2 < min_dist) + { + if(distanceRecurse(root->children[1], query, cdata, callback, min_dist)) + return true; + } + } + + return false; +} + +//============================================================================== +template +bool selfDistanceRecurse(typename DynamicAABBTreeCollisionManager::DynamicAABBNode* root, void* cdata, DistanceCallBack callback, S& min_dist) +{ + if(root->isLeaf()) return false; + + if(selfDistanceRecurse(root->children[0], cdata, callback, min_dist)) + return true; + + if(selfDistanceRecurse(root->children[1], cdata, callback, min_dist)) + return true; + + if(distanceRecurse(root->children[0], root->children[1], cdata, callback, min_dist)) + return true; + + return false; +} + +} // dynamic_AABB_tree + +} // details + +//============================================================================== +template +void DynamicAABBTreeCollisionManager::registerObjects(const std::vector*>& other_objs) +{ + if(other_objs.empty()) return; + + if(size() > 0) + { + BroadPhaseCollisionManager::registerObjects(other_objs); + } + else + { + std::vector leaves(other_objs.size()); + table.rehash(other_objs.size()); + for(size_t i = 0, size = other_objs.size(); i < size; ++i) + { + DynamicAABBNode* node = new DynamicAABBNode; // node will be managed by the dtree + node->bv = other_objs[i]->getAABB(); + node->parent = nullptr; + node->children[1] = nullptr; + node->data = other_objs[i]; + table[other_objs[i]] = node; + leaves[i] = node; + } + + dtree.init(leaves, tree_init_level); + + setup_ = true; + } +} + +//============================================================================== +template +void DynamicAABBTreeCollisionManager::registerObject(CollisionObject* obj) +{ + DynamicAABBNode* node = dtree.insert(obj->getAABB(), obj); + table[obj] = node; +} + +//============================================================================== +template +void DynamicAABBTreeCollisionManager::unregisterObject(CollisionObject* obj) +{ + DynamicAABBNode* node = table[obj]; + table.erase(obj); + dtree.remove(node); +} + +//============================================================================== +template +void DynamicAABBTreeCollisionManager::setup() +{ + if(!setup_) + { + int num = dtree.size(); + if(num == 0) + { + setup_ = true; + return; + } + + int height = dtree.getMaxHeight(); + + + if(height - std::log((S)num) / std::log(2.0) < max_tree_nonbalanced_level) + dtree.balanceIncremental(tree_incremental_balance_pass); + else + dtree.balanceTopdown(); + + setup_ = true; + } +} + +//============================================================================== +template +void DynamicAABBTreeCollisionManager::update() +{ + for(auto it = table.cbegin(); it != table.cend(); ++it) + { + CollisionObject* obj = it->first; + DynamicAABBNode* node = it->second; + node->bv = obj->getAABB(); + } + + dtree.refit(); + setup_ = false; + + setup(); +} + +//============================================================================== +template +void DynamicAABBTreeCollisionManager::update_(CollisionObject* updated_obj) +{ + const auto it = table.find(updated_obj); + if(it != table.end()) + { + DynamicAABBNode* node = it->second; + if(!node->bv.equal(updated_obj->getAABB())) + dtree.update(node, updated_obj->getAABB()); + } + setup_ = false; +} + +//============================================================================== +template +void DynamicAABBTreeCollisionManager::update(CollisionObject* updated_obj) +{ + update_(updated_obj); + setup(); +} + +//============================================================================== +template +void DynamicAABBTreeCollisionManager::update(const std::vector*>& updated_objs) +{ + for(size_t i = 0, size = updated_objs.size(); i < size; ++i) + update_(updated_objs[i]); + setup(); +} + +//============================================================================== +template +void DynamicAABBTreeCollisionManager::collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const +{ + if(size() == 0) return; + switch(obj->collisionGeometry()->getNodeType()) + { +#if FCL_HAVE_OCTOMAP + case GEOM_OCTREE: + { + if(!octree_as_geometry_collide) + { + const OcTree* octree = static_cast*>(obj->collisionGeometry().get()); + details::dynamic_AABB_tree::collisionRecurse(dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(), obj->getTransform(), cdata, callback); + } + else + details::dynamic_AABB_tree::collisionRecurse(dtree.getRoot(), obj, cdata, callback); + } + break; +#endif + default: + details::dynamic_AABB_tree::collisionRecurse(dtree.getRoot(), obj, cdata, callback); + } +} + +//============================================================================== +template +void DynamicAABBTreeCollisionManager::distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const +{ + if(size() == 0) return; + S min_dist = std::numeric_limits::max(); + switch(obj->collisionGeometry()->getNodeType()) + { +#if FCL_HAVE_OCTOMAP + case GEOM_OCTREE: + { + if(!octree_as_geometry_distance) + { + const OcTree* octree = static_cast*>(obj->collisionGeometry().get()); + details::dynamic_AABB_tree::distanceRecurse(dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(), obj->getTransform(), cdata, callback, min_dist); + } + else + details::dynamic_AABB_tree::distanceRecurse(dtree.getRoot(), obj, cdata, callback, min_dist); + } + break; +#endif + default: + details::dynamic_AABB_tree::distanceRecurse(dtree.getRoot(), obj, cdata, callback, min_dist); + } +} + +//============================================================================== +template +void DynamicAABBTreeCollisionManager::collide(void* cdata, CollisionCallBack callback) const +{ + if(size() == 0) return; + details::dynamic_AABB_tree::selfCollisionRecurse(dtree.getRoot(), cdata, callback); +} + +//============================================================================== +template +void DynamicAABBTreeCollisionManager::distance(void* cdata, DistanceCallBack callback) const +{ + if(size() == 0) return; + S min_dist = std::numeric_limits::max(); + details::dynamic_AABB_tree::selfDistanceRecurse(dtree.getRoot(), cdata, callback, min_dist); +} + +//============================================================================== +template +void DynamicAABBTreeCollisionManager::collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const +{ + DynamicAABBTreeCollisionManager* other_manager = static_cast(other_manager_); + if((size() == 0) || (other_manager->size() == 0)) return; + details::dynamic_AABB_tree::collisionRecurse(dtree.getRoot(), other_manager->dtree.getRoot(), cdata, callback); +} + +//============================================================================== +template +void DynamicAABBTreeCollisionManager::distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const +{ + DynamicAABBTreeCollisionManager* other_manager = static_cast(other_manager_); + if((size() == 0) || (other_manager->size() == 0)) return; + S min_dist = std::numeric_limits::max(); + details::dynamic_AABB_tree::distanceRecurse(dtree.getRoot(), other_manager->dtree.getRoot(), cdata, callback, min_dist); +} } diff --git a/include/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h b/include/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h index 94fbe4db0..9a1ac19d1 100644 --- a/include/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h +++ b/include/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h @@ -38,23 +38,28 @@ #ifndef FCL_BROAD_PHASE_DYNAMIC_AABB_TREE_ARRAY_H #define FCL_BROAD_PHASE_DYNAMIC_AABB_TREE_ARRAY_H -#include "fcl/broadphase/broadphase.h" -#include "fcl/broadphase/hierarchy_tree.h" -#include "fcl/BV/BV.h" -#include "fcl/shape/geometric_shapes_utility.h" #include #include #include - +#include "fcl/shape/box.h" +#include "fcl/shape/construct_box.h" +#include "fcl/broadphase/broadphase.h" +#include "fcl/broadphase/hierarchy_tree.h" +#include "fcl/BV/convert_bv.h" +#if FCL_HAVE_OCTOMAP +#include "fcl/octree.h" +#endif namespace fcl { -class DynamicAABBTreeCollisionManager_Array : public BroadPhaseCollisionManager +template +class DynamicAABBTreeCollisionManager_Array : public BroadPhaseCollisionManager { public: - typedef implementation_array::NodeBase DynamicAABBNode; - typedef std::unordered_map DynamicAABBTable; + + using DynamicAABBNode = implementation_array::NodeBase>; + using DynamicAABBTable = std::unordered_map*, size_t>; int max_tree_nonbalanced_level; int tree_incremental_balance_pass; @@ -81,13 +86,13 @@ class DynamicAABBTreeCollisionManager_Array : public BroadPhaseCollisionManager } /// @brief add objects to the manager - void registerObjects(const std::vector& other_objs); + void registerObjects(const std::vector*>& other_objs); /// @brief add one object to the manager - void registerObject(CollisionObject* obj); + void registerObject(CollisionObject* obj); /// @brief remove one object from the manager - void unregisterObject(CollisionObject* obj); + void unregisterObject(CollisionObject* obj); /// @brief initialize the manager, related with the specific type of manager void setup(); @@ -96,10 +101,10 @@ class DynamicAABBTreeCollisionManager_Array : public BroadPhaseCollisionManager void update(); /// @brief update the manager by explicitly given the object updated - void update(CollisionObject* updated_obj); + void update(CollisionObject* updated_obj); /// @brief update the manager by explicitly given the set of objects update - void update(const std::vector& updated_objs); + void update(const std::vector*>& updated_objs); /// @brief clear the manager void clear() @@ -109,29 +114,29 @@ class DynamicAABBTreeCollisionManager_Array : public BroadPhaseCollisionManager } /// @brief return the objects managed by the manager - void getObjects(std::vector& objs) const + void getObjects(std::vector*>& objs) const { objs.resize(this->size()); std::transform(table.begin(), table.end(), objs.begin(), std::bind(&DynamicAABBTable::value_type::first, std::placeholders::_1)); } /// @brief perform collision test between one object and all the objects belonging to the manager - void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const; + void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const; /// @brief perform distance computation between one object and all the objects belonging to the manager - void distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const; + void distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const; /// @brief perform collision test for the objects belonging to the manager (i.e., N^2 self collision) - void collide(void* cdata, CollisionCallBack callback) const; + void collide(void* cdata, CollisionCallBack callback) const; /// @brief perform distance test for the objects belonging to the manager (i.e., N^2 self distance) - void distance(void* cdata, DistanceCallBack callback) const; + void distance(void* cdata, DistanceCallBack callback) const; /// @brief perform collision test with objects belonging to another manager - void collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const; + void collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const; /// @brief perform distance test with objects belonging to another manager - void distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const; + void distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const; /// @brief whether the manager is empty bool empty() const @@ -146,18 +151,898 @@ class DynamicAABBTreeCollisionManager_Array : public BroadPhaseCollisionManager } - const implementation_array::HierarchyTree& getTree() const { return dtree; } + const implementation_array::HierarchyTree>& getTree() const { return dtree; } private: - implementation_array::HierarchyTree dtree; - std::unordered_map table; + implementation_array::HierarchyTree> dtree; + std::unordered_map*, size_t> table; bool setup_; - void update_(CollisionObject* updated_obj); + void update_(CollisionObject* updated_obj); }; +using DynamicAABBTreeCollisionManager_Arrayf = DynamicAABBTreeCollisionManager_Array; +using DynamicAABBTreeCollisionManager_Arrayd = DynamicAABBTreeCollisionManager_Array; + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +namespace details +{ + +namespace dynamic_AABB_tree_array +{ + +#if FCL_HAVE_OCTOMAP + +//============================================================================== +template +bool collisionRecurse_( + typename DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes1, + size_t root1_id, + const OcTree* tree2, + const typename OcTree::OcTreeNode* root2, + const AABB& root2_bv, + const Transform3& tf2, + void* cdata, + CollisionCallBack callback) +{ + typename DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* root1 = nodes1 + root1_id; + if(!root2) + { + if(root1->isLeaf()) + { + CollisionObject* obj1 = static_cast*>(root1->data); + + if(!obj1->isFree()) + { + OBB obb1, obb2; + convertBV(root1->bv, Transform3::Identity(), obb1); + convertBV(root2_bv, tf2, obb2); + + if(obb1.overlap(obb2)) + { + Box* box = new Box(); + Transform3 box_tf; + constructBox(root2_bv, tf2, *box, box_tf); + + box->cost_density = tree2->getDefaultOccupancy(); + + CollisionObject obj2(std::shared_ptr>(box), box_tf); + return callback(obj1, &obj2, cdata); + } + } + } + else + { + if(collisionRecurse_(nodes1, root1->children[0], tree2, nullptr, root2_bv, tf2, cdata, callback)) + return true; + if(collisionRecurse_(nodes1, root1->children[1], tree2, nullptr, root2_bv, tf2, cdata, callback)) + return true; + } + + return false; + } + else if(root1->isLeaf() && !tree2->nodeHasChildren(root2)) + { + CollisionObject* obj1 = static_cast*>(root1->data); + if(!tree2->isNodeFree(root2) && !obj1->isFree()) + { + OBB obb1, obb2; + convertBV(root1->bv, Transform3::Identity(), obb1); + convertBV(root2_bv, tf2, obb2); + + if(obb1.overlap(obb2)) + { + Box* box = new Box(); + Transform3 box_tf; + constructBox(root2_bv, tf2, *box, box_tf); + + box->cost_density = root2->getOccupancy(); + box->threshold_occupied = tree2->getOccupancyThres(); + + CollisionObject obj2(std::shared_ptr>(box), box_tf); + return callback(obj1, &obj2, cdata); + } + else return false; + } + else return false; + } + + OBB obb1, obb2; + convertBV(root1->bv, Transform3::Identity(), obb1); + convertBV(root2_bv, tf2, obb2); + + if(tree2->isNodeFree(root2) || !obb1.overlap(obb2)) return false; + + if(!tree2->nodeHasChildren(root2) || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) + { + if(collisionRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv, tf2, cdata, callback)) + return true; + if(collisionRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv, tf2, cdata, callback)) + return true; + } + else + { + for(unsigned int i = 0; i < 8; ++i) + { + if(tree2->nodeChildExists(root2, i)) + { + const typename OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); + AABB child_bv; + computeChildBV(root2_bv, i, child_bv); + + if(collisionRecurse_(nodes1, root1_id, tree2, child, child_bv, tf2, cdata, callback)) + return true; + } + else + { + AABB child_bv; + computeChildBV(root2_bv, i, child_bv); + if(collisionRecurse_(nodes1, root1_id, tree2, nullptr, child_bv, tf2, cdata, callback)) + return true; + } + } + } + + return false; +} + +//============================================================================== +template +bool collisionRecurse_( + typename DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes1, + size_t root1_id, + const OcTree* tree2, + const typename OcTree::OcTreeNode* root2, + const AABB& root2_bv, + const Eigen::MatrixBase& translation2, + void* cdata, + CollisionCallBack callback) +{ + typename DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* root1 = nodes1 + root1_id; + if(!root2) + { + if(root1->isLeaf()) + { + CollisionObject* obj1 = static_cast*>(root1->data); + + if(!obj1->isFree()) + { + const AABB& root_bv_t = translate(root2_bv, translation2); + if(root1->bv.overlap(root_bv_t)) + { + Box* box = new Box(); + Transform3 box_tf; + Transform3 tf2 = Transform3::Identity(); + tf2.translation() = translation2; + constructBox(root2_bv, tf2, *box, box_tf); + + box->cost_density = tree2->getDefaultOccupancy(); + + CollisionObject obj2(std::shared_ptr>(box), box_tf); + return callback(obj1, &obj2, cdata); + } + } + } + else + { + if(collisionRecurse_(nodes1, root1->children[0], tree2, nullptr, root2_bv, translation2, cdata, callback)) + return true; + if(collisionRecurse_(nodes1, root1->children[1], tree2, nullptr, root2_bv, translation2, cdata, callback)) + return true; + } + + return false; + } + else if(root1->isLeaf() && !tree2->nodeHasChildren(root2)) + { + CollisionObject* obj1 = static_cast*>(root1->data); + if(!tree2->isNodeFree(root2) && !obj1->isFree()) + { + const AABB& root_bv_t = translate(root2_bv, translation2); + if(root1->bv.overlap(root_bv_t)) + { + Box* box = new Box(); + Transform3 box_tf; + Transform3 tf2 = Transform3::Identity(); + tf2.translation() = translation2; + constructBox(root2_bv, tf2, *box, box_tf); + + box->cost_density = root2->getOccupancy(); + box->threshold_occupied = tree2->getOccupancyThres(); + + CollisionObject obj2(std::shared_ptr>(box), box_tf); + return callback(obj1, &obj2, cdata); + } + else return false; + } + else return false; + } + + const AABB& root_bv_t = translate(root2_bv, translation2); + if(tree2->isNodeFree(root2) || !root1->bv.overlap(root_bv_t)) return false; + + if(!tree2->nodeHasChildren(root2) || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) + { + if(collisionRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv, translation2, cdata, callback)) + return true; + if(collisionRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv, translation2, cdata, callback)) + return true; + } + else + { + for(unsigned int i = 0; i < 8; ++i) + { + if(tree2->nodeChildExists(root2, i)) + { + const typename OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); + AABB child_bv; + computeChildBV(root2_bv, i, child_bv); + + if(collisionRecurse_(nodes1, root1_id, tree2, child, child_bv, translation2, cdata, callback)) + return true; + } + else + { + AABB child_bv; + computeChildBV(root2_bv, i, child_bv); + if(collisionRecurse_(nodes1, root1_id, tree2, nullptr, child_bv, translation2, cdata, callback)) + return true; + } + } + } + + return false; +} + +//============================================================================== +template +bool distanceRecurse_(typename DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const typename OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3& tf2, void* cdata, DistanceCallBack callback, S& min_dist) +{ + typename DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* root1 = nodes1 + root1_id; + if(root1->isLeaf() && !tree2->nodeHasChildren(root2)) + { + if(tree2->isNodeOccupied(root2)) + { + Box* box = new Box(); + Transform3 box_tf; + constructBox(root2_bv, tf2, *box, box_tf); + CollisionObject obj(std::shared_ptr>(box), box_tf); + return callback(static_cast*>(root1->data), &obj, cdata, min_dist); + } + else return false; + } + + if(!tree2->isNodeOccupied(root2)) return false; + + if(!tree2->nodeHasChildren(root2) || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) + { + AABB aabb2; + convertBV(root2_bv, tf2, aabb2); + + S d1 = aabb2.distance((nodes1 + root1->children[0])->bv); + S d2 = aabb2.distance((nodes1 + root1->children[1])->bv); + + if(d2 < d1) + { + if(d2 < min_dist) + { + if(distanceRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv, tf2, cdata, callback, min_dist)) + return true; + } + + if(d1 < min_dist) + { + if(distanceRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv, tf2, cdata, callback, min_dist)) + return true; + } + } + else + { + if(d1 < min_dist) + { + if(distanceRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv, tf2, cdata, callback, min_dist)) + return true; + } + + if(d2 < min_dist) + { + if(distanceRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv, tf2, cdata, callback, min_dist)) + return true; + } + } + } + else + { + for(unsigned int i = 0; i < 8; ++i) + { + if(tree2->nodeChildExists(root2, i)) + { + const typename OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); + AABB child_bv; + computeChildBV(root2_bv, i, child_bv); + + AABB aabb2; + convertBV(child_bv, tf2, aabb2); + S d = root1->bv.distance(aabb2); + + if(d < min_dist) + { + if(distanceRecurse_(nodes1, root1_id, tree2, child, child_bv, tf2, cdata, callback, min_dist)) + return true; + } + } + } + } + + return false; +} + +//============================================================================== +template +bool distanceRecurse_( + typename DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes1, + size_t root1_id, + const OcTree* tree2, + const typename OcTree::OcTreeNode* root2, + const AABB& root2_bv, + const Eigen::MatrixBase& translation2, + void* cdata, + DistanceCallBack callback, + S& min_dist) +{ + typename DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* root1 = nodes1 + root1_id; + if(root1->isLeaf() && !tree2->nodeHasChildren(root2)) + { + if(tree2->isNodeOccupied(root2)) + { + Box* box = new Box(); + Transform3 box_tf; + Transform3 tf2 = Transform3::Identity(); + tf2.translation() = translation2; + constructBox(root2_bv, tf2, *box, box_tf); + CollisionObject obj(std::shared_ptr>(box), box_tf); + return callback(static_cast*>(root1->data), &obj, cdata, min_dist); + } + else return false; + } + + if(!tree2->isNodeOccupied(root2)) return false; + + if(!tree2->nodeHasChildren(root2) || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) + { + const AABB& aabb2 = translate(root2_bv, translation2); + + S d1 = aabb2.distance((nodes1 + root1->children[0])->bv); + S d2 = aabb2.distance((nodes1 + root1->children[1])->bv); + + if(d2 < d1) + { + if(d2 < min_dist) + { + if(distanceRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv, translation2, cdata, callback, min_dist)) + return true; + } + + if(d1 < min_dist) + { + if(distanceRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv, translation2, cdata, callback, min_dist)) + return true; + } + } + else + { + if(d1 < min_dist) + { + if(distanceRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv, translation2, cdata, callback, min_dist)) + return true; + } + + if(d2 < min_dist) + { + if(distanceRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv, translation2, cdata, callback, min_dist)) + return true; + } + } + } + else + { + for(unsigned int i = 0; i < 8; ++i) + { + if(tree2->nodeChildExists(root2, i)) + { + const typename OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); + AABB child_bv; + computeChildBV(root2_bv, i, child_bv); + + const AABB& aabb2 = translate(child_bv, translation2); + S d = root1->bv.distance(aabb2); + + if(d < min_dist) + { + if(distanceRecurse_(nodes1, root1_id, tree2, child, child_bv, translation2, cdata, callback, min_dist)) + return true; + } + } + } + } + + return false; +} + + +#endif + +//============================================================================== +template +bool collisionRecurse(typename DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes1, size_t root1_id, + typename DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes2, size_t root2_id, + void* cdata, CollisionCallBack callback) +{ + typename DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* root1 = nodes1 + root1_id; + typename DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* root2 = nodes2 + root2_id; + if(root1->isLeaf() && root2->isLeaf()) + { + if(!root1->bv.overlap(root2->bv)) return false; + return callback(static_cast*>(root1->data), static_cast*>(root2->data), cdata); + } + + if(!root1->bv.overlap(root2->bv)) return false; + + if(root2->isLeaf() || (!root1->isLeaf() && (root1->bv.size() > root2->bv.size()))) + { + if(collisionRecurse(nodes1, root1->children[0], nodes2, root2_id, cdata, callback)) + return true; + if(collisionRecurse(nodes1, root1->children[1], nodes2, root2_id, cdata, callback)) + return true; + } + else + { + if(collisionRecurse(nodes1, root1_id, nodes2, root2->children[0], cdata, callback)) + return true; + if(collisionRecurse(nodes1, root1_id, nodes2, root2->children[1], cdata, callback)) + return true; + } + return false; +} + +//============================================================================== +template +bool collisionRecurse(typename DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes, size_t root_id, CollisionObject* query, void* cdata, CollisionCallBack callback) +{ + typename DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* root = nodes + root_id; + if(root->isLeaf()) + { + if(!root->bv.overlap(query->getAABB())) return false; + return callback(static_cast*>(root->data), query, cdata); + } + + if(!root->bv.overlap(query->getAABB())) return false; + + int select_res = implementation_array::select(query->getAABB(), root->children[0], root->children[1], nodes); + + if(collisionRecurse(nodes, root->children[select_res], query, cdata, callback)) + return true; + + if(collisionRecurse(nodes, root->children[1-select_res], query, cdata, callback)) + return true; + + return false; +} + +//============================================================================== +template +bool selfCollisionRecurse(typename DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes, size_t root_id, void* cdata, CollisionCallBack callback) +{ + typename DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* root = nodes + root_id; + if(root->isLeaf()) return false; + + if(selfCollisionRecurse(nodes, root->children[0], cdata, callback)) + return true; + if(selfCollisionRecurse(nodes, root->children[1], cdata, callback)) + return true; + + if(collisionRecurse(nodes, root->children[0], nodes, root->children[1], cdata, callback)) + return true; + + return false; } +//============================================================================== +template +bool distanceRecurse(typename DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes1, size_t root1_id, + typename DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes2, size_t root2_id, + void* cdata, DistanceCallBack callback, S& min_dist) +{ + typename DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* root1 = nodes1 + root1_id; + typename DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* root2 = nodes2 + root2_id; + if(root1->isLeaf() && root2->isLeaf()) + { + CollisionObject* root1_obj = static_cast*>(root1->data); + CollisionObject* root2_obj = static_cast*>(root2->data); + return callback(root1_obj, root2_obj, cdata, min_dist); + } + + if(root2->isLeaf() || (!root1->isLeaf() && (root1->bv.size() > root2->bv.size()))) + { + S d1 = root2->bv.distance((nodes1 + root1->children[0])->bv); + S d2 = root2->bv.distance((nodes1 + root1->children[1])->bv); + + if(d2 < d1) + { + if(d2 < min_dist) + { + if(distanceRecurse(nodes1, root1->children[1], nodes2, root2_id, cdata, callback, min_dist)) + return true; + } + + if(d1 < min_dist) + { + if(distanceRecurse(nodes1, root1->children[0], nodes2, root2_id, cdata, callback, min_dist)) + return true; + } + } + else + { + if(d1 < min_dist) + { + if(distanceRecurse(nodes1, root1->children[0], nodes2, root2_id, cdata, callback, min_dist)) + return true; + } + + if(d2 < min_dist) + { + if(distanceRecurse(nodes1, root1->children[1], nodes2, root2_id, cdata, callback, min_dist)) + return true; + } + } + } + else + { + S d1 = root1->bv.distance((nodes2 + root2->children[0])->bv); + S d2 = root1->bv.distance((nodes2 + root2->children[1])->bv); + + if(d2 < d1) + { + if(d2 < min_dist) + { + if(distanceRecurse(nodes1, root1_id, nodes2, root2->children[1], cdata, callback, min_dist)) + return true; + } + + if(d1 < min_dist) + { + if(distanceRecurse(nodes1, root1_id, nodes2, root2->children[0], cdata, callback, min_dist)) + return true; + } + } + else + { + if(d1 < min_dist) + { + if(distanceRecurse(nodes1, root1_id, nodes2, root2->children[0], cdata, callback, min_dist)) + return true; + } + + if(d2 < min_dist) + { + if(distanceRecurse(nodes1, root1_id, nodes2, root2->children[1], cdata, callback, min_dist)) + return true; + } + } + } + + return false; +} + +//============================================================================== +template +bool distanceRecurse(typename DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes, size_t root_id, CollisionObject* query, void* cdata, DistanceCallBack callback, S& min_dist) +{ + typename DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* root = nodes + root_id; + if(root->isLeaf()) + { + CollisionObject* root_obj = static_cast*>(root->data); + return callback(root_obj, query, cdata, min_dist); + } + + S d1 = query->getAABB().distance((nodes + root->children[0])->bv); + S d2 = query->getAABB().distance((nodes + root->children[1])->bv); + + if(d2 < d1) + { + if(d2 < min_dist) + { + if(distanceRecurse(nodes, root->children[1], query, cdata, callback, min_dist)) + return true; + } + + if(d1 < min_dist) + { + if(distanceRecurse(nodes, root->children[0], query, cdata, callback, min_dist)) + return true; + } + } + else + { + if(d1 < min_dist) + { + if(distanceRecurse(nodes, root->children[0], query, cdata, callback, min_dist)) + return true; + } + + if(d2 < min_dist) + { + if(distanceRecurse(nodes, root->children[1], query, cdata, callback, min_dist)) + return true; + } + } + + return false; +} + +//============================================================================== +template +bool selfDistanceRecurse(typename DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes, size_t root_id, void* cdata, DistanceCallBack callback, S& min_dist) +{ + typename DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* root = nodes + root_id; + if(root->isLeaf()) return false; + + if(selfDistanceRecurse(nodes, root->children[0], cdata, callback, min_dist)) + return true; + + if(selfDistanceRecurse(nodes, root->children[1], cdata, callback, min_dist)) + return true; + + if(distanceRecurse(nodes, root->children[0], nodes, root->children[1], cdata, callback, min_dist)) + return true; + + return false; +} + + +#if FCL_HAVE_OCTOMAP + +//============================================================================== +template +bool collisionRecurse(typename DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const typename OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3& tf2, void* cdata, CollisionCallBack callback) +{ + if(tf2.linear().isIdentity()) + return collisionRecurse_(nodes1, root1_id, tree2, root2, root2_bv, tf2.translation(), cdata, callback); + else + return collisionRecurse_(nodes1, root1_id, tree2, root2, root2_bv, tf2, cdata, callback); +} + +//============================================================================== +template +bool distanceRecurse(typename DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const typename OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3& tf2, void* cdata, DistanceCallBack callback, S& min_dist) +{ + if(tf2.linear().isIdentity()) + return distanceRecurse_(nodes1, root1_id, tree2, root2, root2_bv, tf2.translation(), cdata, callback, min_dist); + else + return distanceRecurse_(nodes1, root1_id, tree2, root2, root2_bv, tf2, cdata, callback, min_dist); +} + +#endif + +} // dynamic_AABB_tree_array + +} // details + +//============================================================================== +template +void DynamicAABBTreeCollisionManager_Array::registerObjects(const std::vector*>& other_objs) +{ + if(other_objs.empty()) return; + + if(size() > 0) + { + BroadPhaseCollisionManager::registerObjects(other_objs); + } + else + { + DynamicAABBNode* leaves = new DynamicAABBNode[other_objs.size()]; + table.rehash(other_objs.size()); + for(size_t i = 0, size = other_objs.size(); i < size; ++i) + { + leaves[i].bv = other_objs[i]->getAABB(); + leaves[i].parent = dtree.NULL_NODE; + leaves[i].children[1] = dtree.NULL_NODE; + leaves[i].data = other_objs[i]; + table[other_objs[i]] = i; + } + + int n_leaves = other_objs.size(); + + dtree.init(leaves, n_leaves, tree_init_level); + + setup_ = true; + } +} + +//============================================================================== +template +void DynamicAABBTreeCollisionManager_Array::registerObject(CollisionObject* obj) +{ + size_t node = dtree.insert(obj->getAABB(), obj); + table[obj] = node; +} + +//============================================================================== +template +void DynamicAABBTreeCollisionManager_Array::unregisterObject(CollisionObject* obj) +{ + size_t node = table[obj]; + table.erase(obj); + dtree.remove(node); +} + +//============================================================================== +template +void DynamicAABBTreeCollisionManager_Array::setup() +{ + if(!setup_) + { + int num = dtree.size(); + if(num == 0) + { + setup_ = true; + return; + } + + int height = dtree.getMaxHeight(); + + + if(height - std::log((S)num) / std::log(2.0) < max_tree_nonbalanced_level) + dtree.balanceIncremental(tree_incremental_balance_pass); + else + dtree.balanceTopdown(); + + setup_ = true; + } +} + +//============================================================================== +template +void DynamicAABBTreeCollisionManager_Array::update() +{ + for(auto it = table.cbegin(), end = table.cend(); it != end; ++it) + { + const CollisionObject* obj = it->first; + size_t node = it->second; + dtree.getNodes()[node].bv = obj->getAABB(); + } + + dtree.refit(); + setup_ = false; + + setup(); +} + +//============================================================================== +template +void DynamicAABBTreeCollisionManager_Array::update_(CollisionObject* updated_obj) +{ + const auto it = table.find(updated_obj); + if(it != table.end()) + { + size_t node = it->second; + if(!dtree.getNodes()[node].bv.equal(updated_obj->getAABB())) + dtree.update(node, updated_obj->getAABB()); + } + setup_ = false; +} + +//============================================================================== +template +void DynamicAABBTreeCollisionManager_Array::update(CollisionObject* updated_obj) +{ + update_(updated_obj); + setup(); +} + +//============================================================================== +template +void DynamicAABBTreeCollisionManager_Array::update(const std::vector*>& updated_objs) +{ + for(size_t i = 0, size = updated_objs.size(); i < size; ++i) + update_(updated_objs[i]); + setup(); +} + +//============================================================================== +template +void DynamicAABBTreeCollisionManager_Array::collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const +{ + if(size() == 0) return; + switch(obj->collisionGeometry()->getNodeType()) + { +#if FCL_HAVE_OCTOMAP + case GEOM_OCTREE: + { + if(!octree_as_geometry_collide) + { + const OcTree* octree = static_cast*>(obj->collisionGeometry().get()); + details::dynamic_AABB_tree_array::collisionRecurse(dtree.getNodes(), dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(), obj->getTransform(), cdata, callback); + } + else + details::dynamic_AABB_tree_array::collisionRecurse(dtree.getNodes(), dtree.getRoot(), obj, cdata, callback); + } + break; +#endif + default: + details::dynamic_AABB_tree_array::collisionRecurse(dtree.getNodes(), dtree.getRoot(), obj, cdata, callback); + } +} + +//============================================================================== +template +void DynamicAABBTreeCollisionManager_Array::distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const +{ + if(size() == 0) return; + S min_dist = std::numeric_limits::max(); + switch(obj->collisionGeometry()->getNodeType()) + { +#if FCL_HAVE_OCTOMAP + case GEOM_OCTREE: + { + if(!octree_as_geometry_distance) + { + const OcTree* octree = static_cast*>(obj->collisionGeometry().get()); + details::dynamic_AABB_tree_array::distanceRecurse(dtree.getNodes(), dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(), obj->getTransform(), cdata, callback, min_dist); + } + else + details::dynamic_AABB_tree_array::distanceRecurse(dtree.getNodes(), dtree.getRoot(), obj, cdata, callback, min_dist); + } + break; +#endif + default: + details::dynamic_AABB_tree_array::distanceRecurse(dtree.getNodes(), dtree.getRoot(), obj, cdata, callback, min_dist); + } +} + +//============================================================================== +template +void DynamicAABBTreeCollisionManager_Array::collide(void* cdata, CollisionCallBack callback) const +{ + if(size() == 0) return; + details::dynamic_AABB_tree_array::selfCollisionRecurse(dtree.getNodes(), dtree.getRoot(), cdata, callback); +} + +//============================================================================== +template +void DynamicAABBTreeCollisionManager_Array::distance(void* cdata, DistanceCallBack callback) const +{ + if(size() == 0) return; + S min_dist = std::numeric_limits::max(); + details::dynamic_AABB_tree_array::selfDistanceRecurse(dtree.getNodes(), dtree.getRoot(), cdata, callback, min_dist); +} + +//============================================================================== +template +void DynamicAABBTreeCollisionManager_Array::collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const +{ + DynamicAABBTreeCollisionManager_Array* other_manager = static_cast(other_manager_); + if((size() == 0) || (other_manager->size() == 0)) return; + details::dynamic_AABB_tree_array::collisionRecurse(dtree.getNodes(), dtree.getRoot(), other_manager->dtree.getNodes(), other_manager->dtree.getRoot(), cdata, callback); +} + +//============================================================================== +template +void DynamicAABBTreeCollisionManager_Array::distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const +{ + DynamicAABBTreeCollisionManager_Array* other_manager = static_cast(other_manager_); + if((size() == 0) || (other_manager->size() == 0)) return; + S min_dist = std::numeric_limits::max(); + details::dynamic_AABB_tree_array::distanceRecurse(dtree.getNodes(), dtree.getRoot(), other_manager->dtree.getNodes(), other_manager->dtree.getRoot(), cdata, callback, min_dist); +} + +} // namespace fcl + #endif diff --git a/include/fcl/broadphase/broadphase_interval_tree.h b/include/fcl/broadphase/broadphase_interval_tree.h index f0179ca13..96468d94c 100644 --- a/include/fcl/broadphase/broadphase_interval_tree.h +++ b/include/fcl/broadphase/broadphase_interval_tree.h @@ -47,13 +47,14 @@ namespace fcl { /// @brief Collision manager based on interval tree -class IntervalTreeCollisionManager : public BroadPhaseCollisionManager +template +class IntervalTreeCollisionManager : public BroadPhaseCollisionManager { public: IntervalTreeCollisionManager() : setup_(false) { for(int i = 0; i < 3; ++i) - interval_trees[i] = NULL; + interval_trees[i] = nullptr; } ~IntervalTreeCollisionManager() @@ -62,10 +63,10 @@ class IntervalTreeCollisionManager : public BroadPhaseCollisionManager } /// @brief remove one object from the manager - void registerObject(CollisionObject* obj); + void registerObject(CollisionObject* obj); /// @brief add one object to the manager - void unregisterObject(CollisionObject* obj); + void unregisterObject(CollisionObject* obj); /// @brief initialize the manager, related with the specific type of manager void setup(); @@ -74,34 +75,34 @@ class IntervalTreeCollisionManager : public BroadPhaseCollisionManager void update(); /// @brief update the manager by explicitly given the object updated - void update(CollisionObject* updated_obj); + void update(CollisionObject* updated_obj); /// @brief update the manager by explicitly given the set of objects update - void update(const std::vector& updated_objs); + void update(const std::vector*>& updated_objs); /// @brief clear the manager void clear(); /// @brief return the objects managed by the manager - void getObjects(std::vector& objs) const; + void getObjects(std::vector*>& objs) const; /// @brief perform collision test between one object and all the objects belonging to the manager - void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const; + void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const; /// @brief perform distance computation between one object and all the objects belonging to the manager - void distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const; + void distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const; /// @brief perform collision test for the objects belonging to the manager (i.e., N^2 self collision) - void collide(void* cdata, CollisionCallBack callback) const; + void collide(void* cdata, CollisionCallBack callback) const; /// @brief perform distance test for the objects belonging to the manager (i.e., N^2 self distance) - void distance(void* cdata, DistanceCallBack callback) const; + void distance(void* cdata, DistanceCallBack callback) const; /// @brief perform collision test with objects belonging to another manager - void collide(BroadPhaseCollisionManager* other_manager, void* cdata, CollisionCallBack callback) const; + void collide(BroadPhaseCollisionManager* other_manager, void* cdata, CollisionCallBack callback) const; /// @brief perform distance test with objects belonging to another manager - void distance(BroadPhaseCollisionManager* other_manager, void* cdata, DistanceCallBack callback) const; + void distance(BroadPhaseCollisionManager* other_manager, void* cdata, DistanceCallBack callback) const; /// @brief whether the manager is empty bool empty() const; @@ -109,18 +110,16 @@ class IntervalTreeCollisionManager : public BroadPhaseCollisionManager /// @brief the number of objects managed by the manager inline size_t size() const { return endpoints[0].size() / 2; } - - protected: /// @brief SAP end point struct EndPoint { /// @brief object related with the end point - CollisionObject* obj; + CollisionObject* obj; /// @brief end point value - FCL_REAL value; + S value; /// @brief tag for whether it is a lower bound or higher bound of an interval, 0 for lo, and 1 for hi char minmax; @@ -134,8 +133,8 @@ class IntervalTreeCollisionManager : public BroadPhaseCollisionManager /// @brief Extention interval tree's interval to SAP interval, adding more information struct SAPInterval : public SimpleInterval { - CollisionObject* obj; - SAPInterval(double low_, double high_, CollisionObject* obj_) : SimpleInterval() + CollisionObject* obj; + SAPInterval(S low_, S high_, CollisionObject* obj_) : SimpleInterval() { low = low_; high = high_; @@ -144,13 +143,13 @@ class IntervalTreeCollisionManager : public BroadPhaseCollisionManager }; - bool checkColl(std::deque::const_iterator pos_start, std::deque::const_iterator pos_end, CollisionObject* obj, void* cdata, CollisionCallBack callback) const; + bool checkColl(std::deque::const_iterator pos_start, std::deque::const_iterator pos_end, CollisionObject* obj, void* cdata, CollisionCallBack callback) const; - bool checkDist(std::deque::const_iterator pos_start, std::deque::const_iterator pos_end, CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const; + bool checkDist(std::deque::const_iterator pos_start, std::deque::const_iterator pos_end, CollisionObject* obj, void* cdata, DistanceCallBack callback, S& min_dist) const; - bool collide_(CollisionObject* obj, void* cdata, CollisionCallBack callback) const; + bool collide_(CollisionObject* obj, void* cdata, CollisionCallBack callback) const; - bool distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const; + bool distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, S& min_dist) const; /// @brief vector stores all the end points std::vector endpoints[3]; @@ -158,13 +157,667 @@ class IntervalTreeCollisionManager : public BroadPhaseCollisionManager /// @brief interval tree manages the intervals IntervalTree* interval_trees[3]; - std::map obj_interval_maps[3]; + std::map*, SAPInterval*> obj_interval_maps[3]; /// @brief tag for whether the interval tree is maintained suitably bool setup_; }; +using IntervalTreeCollisionManagerf = IntervalTreeCollisionManager; +using IntervalTreeCollisionManagerd = IntervalTreeCollisionManager; + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +void IntervalTreeCollisionManager::unregisterObject(CollisionObject* obj) +{ + // must sorted before + setup(); + + EndPoint p; + p.value = obj->getAABB().min_[0]; + auto start1 = std::lower_bound(endpoints[0].begin(), endpoints[0].end(), p); + p.value = obj->getAABB().max_[0]; + auto end1 = std::upper_bound(start1, endpoints[0].end(), p); + + if(start1 < end1) + { + unsigned int start_id = start1 - endpoints[0].begin(); + unsigned int end_id = end1 - endpoints[0].begin(); + unsigned int cur_id = start_id; + for(unsigned int i = start_id; i < end_id; ++i) + { + if(endpoints[0][i].obj != obj) + { + if(i == cur_id) cur_id++; + else + { + endpoints[0][cur_id] = endpoints[0][i]; + cur_id++; + } + } + } + if(cur_id < end_id) + endpoints[0].resize(endpoints[0].size() - 2); + } + + p.value = obj->getAABB().min_[1]; + auto start2 = std::lower_bound(endpoints[1].begin(), endpoints[1].end(), p); + p.value = obj->getAABB().max_[1]; + auto end2 = std::upper_bound(start2, endpoints[1].end(), p); + + if(start2 < end2) + { + unsigned int start_id = start2 - endpoints[1].begin(); + unsigned int end_id = end2 - endpoints[1].begin(); + unsigned int cur_id = start_id; + for(unsigned int i = start_id; i < end_id; ++i) + { + if(endpoints[1][i].obj != obj) + { + if(i == cur_id) cur_id++; + else + { + endpoints[1][cur_id] = endpoints[1][i]; + cur_id++; + } + } + } + if(cur_id < end_id) + endpoints[1].resize(endpoints[1].size() - 2); + } + + + p.value = obj->getAABB().min_[2]; + auto start3 = std::lower_bound(endpoints[2].begin(), endpoints[2].end(), p); + p.value = obj->getAABB().max_[2]; + auto end3 = std::upper_bound(start3, endpoints[2].end(), p); + + if(start3 < end3) + { + unsigned int start_id = start3 - endpoints[2].begin(); + unsigned int end_id = end3 - endpoints[2].begin(); + unsigned int cur_id = start_id; + for(unsigned int i = start_id; i < end_id; ++i) + { + if(endpoints[2][i].obj != obj) + { + if(i == cur_id) cur_id++; + else + { + endpoints[2][cur_id] = endpoints[2][i]; + cur_id++; + } + } + } + if(cur_id < end_id) + endpoints[2].resize(endpoints[2].size() - 2); + } + + // update the interval tree + if(obj_interval_maps[0].find(obj) != obj_interval_maps[0].end()) + { + SAPInterval* ivl1 = obj_interval_maps[0][obj]; + SAPInterval* ivl2 = obj_interval_maps[1][obj]; + SAPInterval* ivl3 = obj_interval_maps[2][obj]; + + interval_trees[0]->deleteNode(ivl1); + interval_trees[1]->deleteNode(ivl2); + interval_trees[2]->deleteNode(ivl3); + + delete ivl1; + delete ivl2; + delete ivl3; + + obj_interval_maps[0].erase(obj); + obj_interval_maps[1].erase(obj); + obj_interval_maps[2].erase(obj); + } +} + +//============================================================================== +template +void IntervalTreeCollisionManager::registerObject(CollisionObject* obj) +{ + EndPoint p, q; + + p.obj = obj; + q.obj = obj; + p.minmax = 0; + q.minmax = 1; + p.value = obj->getAABB().min_[0]; + q.value = obj->getAABB().max_[0]; + endpoints[0].push_back(p); + endpoints[0].push_back(q); + + p.value = obj->getAABB().min_[1]; + q.value = obj->getAABB().max_[1]; + endpoints[1].push_back(p); + endpoints[1].push_back(q); + + p.value = obj->getAABB().min_[2]; + q.value = obj->getAABB().max_[2]; + endpoints[2].push_back(p); + endpoints[2].push_back(q); + setup_ = false; +} + +//============================================================================== +template +void IntervalTreeCollisionManager::setup() +{ + if(!setup_) + { + std::sort(endpoints[0].begin(), endpoints[0].end()); + std::sort(endpoints[1].begin(), endpoints[1].end()); + std::sort(endpoints[2].begin(), endpoints[2].end()); + + for(int i = 0; i < 3; ++i) + delete interval_trees[i]; + + for(int i = 0; i < 3; ++i) + interval_trees[i] = new IntervalTree; + + for(unsigned int i = 0, size = endpoints[0].size(); i < size; ++i) + { + EndPoint p = endpoints[0][i]; + CollisionObject* obj = p.obj; + if(p.minmax == 0) + { + SAPInterval* ivl1 = new SAPInterval(obj->getAABB().min_[0], obj->getAABB().max_[0], obj); + SAPInterval* ivl2 = new SAPInterval(obj->getAABB().min_[1], obj->getAABB().max_[1], obj); + SAPInterval* ivl3 = new SAPInterval(obj->getAABB().min_[2], obj->getAABB().max_[2], obj); + + interval_trees[0]->insert(ivl1); + interval_trees[1]->insert(ivl2); + interval_trees[2]->insert(ivl3); + + obj_interval_maps[0][obj] = ivl1; + obj_interval_maps[1][obj] = ivl2; + obj_interval_maps[2][obj] = ivl3; + } + } + + setup_ = true; + } +} + +//============================================================================== +template +void IntervalTreeCollisionManager::update() +{ + setup_ = false; + + for(unsigned int i = 0, size = endpoints[0].size(); i < size; ++i) + { + if(endpoints[0][i].minmax == 0) + endpoints[0][i].value = endpoints[0][i].obj->getAABB().min_[0]; + else + endpoints[0][i].value = endpoints[0][i].obj->getAABB().max_[0]; + } + + for(unsigned int i = 0, size = endpoints[1].size(); i < size; ++i) + { + if(endpoints[1][i].minmax == 0) + endpoints[1][i].value = endpoints[1][i].obj->getAABB().min_[1]; + else + endpoints[1][i].value = endpoints[1][i].obj->getAABB().max_[1]; + } + + for(unsigned int i = 0, size = endpoints[2].size(); i < size; ++i) + { + if(endpoints[2][i].minmax == 0) + endpoints[2][i].value = endpoints[2][i].obj->getAABB().min_[2]; + else + endpoints[2][i].value = endpoints[2][i].obj->getAABB().max_[2]; + } + + setup(); + +} + +//============================================================================== +template +void IntervalTreeCollisionManager::update(CollisionObject* updated_obj) +{ + AABB old_aabb; + const AABB& new_aabb = updated_obj->getAABB(); + for(int i = 0; i < 3; ++i) + { + const auto it = obj_interval_maps[i].find(updated_obj); + interval_trees[i]->deleteNode(it->second); + old_aabb.min_[i] = it->second->low; + old_aabb.max_[i] = it->second->high; + it->second->low = new_aabb.min_[i]; + it->second->high = new_aabb.max_[i]; + interval_trees[i]->insert(it->second); + } + + EndPoint dummy; + typename std::vector::iterator it; + for(int i = 0; i < 3; ++i) + { + dummy.value = old_aabb.min_[i]; + it = std::lower_bound(endpoints[i].begin(), endpoints[i].end(), dummy); + for(; it != endpoints[i].end(); ++it) + { + if(it->obj == updated_obj && it->minmax == 0) + { + it->value = new_aabb.min_[i]; + break; + } + } + + dummy.value = old_aabb.max_[i]; + it = std::lower_bound(endpoints[i].begin(), endpoints[i].end(), dummy); + for(; it != endpoints[i].end(); ++it) + { + if(it->obj == updated_obj && it->minmax == 0) + { + it->value = new_aabb.max_[i]; + break; + } + } + std::sort(endpoints[i].begin(), endpoints[i].end()); + } +} + +//============================================================================== +template +void IntervalTreeCollisionManager::update(const std::vector*>& updated_objs) +{ + for(size_t i = 0; i < updated_objs.size(); ++i) + update(updated_objs[i]); +} + +//============================================================================== +template +void IntervalTreeCollisionManager::clear() +{ + endpoints[0].clear(); + endpoints[1].clear(); + endpoints[2].clear(); + + delete interval_trees[0]; interval_trees[0] = nullptr; + delete interval_trees[1]; interval_trees[1] = nullptr; + delete interval_trees[2]; interval_trees[2] = nullptr; + + for(int i = 0; i < 3; ++i) + { + for(auto it = obj_interval_maps[i].cbegin(), end = obj_interval_maps[i].cend(); + it != end; ++it) + { + delete it->second; + } + } + + for(int i = 0; i < 3; ++i) + obj_interval_maps[i].clear(); + + setup_ = false; +} + +//============================================================================== +template +void IntervalTreeCollisionManager::getObjects(std::vector*>& objs) const +{ + objs.resize(endpoints[0].size() / 2); + unsigned int j = 0; + for(unsigned int i = 0, size = endpoints[0].size(); i < size; ++i) + { + if(endpoints[0][i].minmax == 0) + { + objs[j] = endpoints[0][i].obj; j++; + } + } } +//============================================================================== +template +void IntervalTreeCollisionManager::collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const +{ + if(size() == 0) return; + collide_(obj, cdata, callback); +} + +//============================================================================== +template +bool IntervalTreeCollisionManager::collide_(CollisionObject* obj, void* cdata, CollisionCallBack callback) const +{ + static const unsigned int CUTOFF = 100; + + std::deque results0, results1, results2; + + results0 = interval_trees[0]->query(obj->getAABB().min_[0], obj->getAABB().max_[0]); + if(results0.size() > CUTOFF) + { + results1 = interval_trees[1]->query(obj->getAABB().min_[1], obj->getAABB().max_[1]); + if(results1.size() > CUTOFF) + { + results2 = interval_trees[2]->query(obj->getAABB().min_[2], obj->getAABB().max_[2]); + if(results2.size() > CUTOFF) + { + int d1 = results0.size(); + int d2 = results1.size(); + int d3 = results2.size(); + + if(d1 >= d2 && d1 >= d3) + return checkColl(results0.begin(), results0.end(), obj, cdata, callback); + else if(d2 >= d1 && d2 >= d3) + return checkColl(results1.begin(), results1.end(), obj, cdata, callback); + else + return checkColl(results2.begin(), results2.end(), obj, cdata, callback); + } + else + return checkColl(results2.begin(), results2.end(), obj, cdata, callback); + } + else + return checkColl(results1.begin(), results1.end(), obj, cdata, callback); + } + else + return checkColl(results0.begin(), results0.end(), obj, cdata, callback); +} + +//============================================================================== +template +void IntervalTreeCollisionManager::distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const +{ + if(size() == 0) return; + S min_dist = std::numeric_limits::max(); + distance_(obj, cdata, callback, min_dist); +} + +//============================================================================== +template +bool IntervalTreeCollisionManager::distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, S& min_dist) const +{ + static const unsigned int CUTOFF = 100; + + Vector3 delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5; + AABB aabb = obj->getAABB(); + if(min_dist < std::numeric_limits::max()) + { + Vector3 min_dist_delta(min_dist, min_dist, min_dist); + aabb.expand(min_dist_delta); + } + + int status = 1; + S old_min_distance; + + while(1) + { + bool dist_res = false; + + old_min_distance = min_dist; + + std::deque results0, results1, results2; + + results0 = interval_trees[0]->query(aabb.min_[0], aabb.max_[0]); + if(results0.size() > CUTOFF) + { + results1 = interval_trees[1]->query(aabb.min_[1], aabb.max_[1]); + if(results1.size() > CUTOFF) + { + results2 = interval_trees[2]->query(aabb.min_[2], aabb.max_[2]); + if(results2.size() > CUTOFF) + { + int d1 = results0.size(); + int d2 = results1.size(); + int d3 = results2.size(); + + if(d1 >= d2 && d1 >= d3) + dist_res = checkDist(results0.begin(), results0.end(), obj, cdata, callback, min_dist); + else if(d2 >= d1 && d2 >= d3) + dist_res = checkDist(results1.begin(), results1.end(), obj, cdata, callback, min_dist); + else + dist_res = checkDist(results2.begin(), results2.end(), obj, cdata, callback, min_dist); + } + else + dist_res = checkDist(results2.begin(), results2.end(), obj, cdata, callback, min_dist); + } + else + dist_res = checkDist(results1.begin(), results1.end(), obj, cdata, callback, min_dist); + } + else + dist_res = checkDist(results0.begin(), results0.end(), obj, cdata, callback, min_dist); + + if(dist_res) return true; + + results0.clear(); + results1.clear(); + results2.clear(); + + if(status == 1) + { + if(old_min_distance < std::numeric_limits::max()) + break; + else + { + if(min_dist < old_min_distance) + { + Vector3 min_dist_delta(min_dist, min_dist, min_dist); + aabb = AABB(obj->getAABB(), min_dist_delta); + status = 0; + } + else + { + if(aabb.equal(obj->getAABB())) + aabb.expand(delta); + else + aabb.expand(obj->getAABB(), 2.0); + } + } + } + else if(status == 0) + break; + } + + return false; +} + +//============================================================================== +template +void IntervalTreeCollisionManager::collide(void* cdata, CollisionCallBack callback) const +{ + if(size() == 0) return; + + std::set*> active; + std::set*, CollisionObject*> > overlap; + unsigned int n = endpoints[0].size(); + S diff_x = endpoints[0][0].value - endpoints[0][n-1].value; + S diff_y = endpoints[1][0].value - endpoints[1][n-1].value; + S diff_z = endpoints[2][0].value - endpoints[2][n-1].value; + + int axis = 0; + if(diff_y > diff_x && diff_y > diff_z) + axis = 1; + else if(diff_z > diff_y && diff_z > diff_x) + axis = 2; + + for(unsigned int i = 0; i < n; ++i) + { + const EndPoint& endpoint = endpoints[axis][i]; + CollisionObject* index = endpoint.obj; + if(endpoint.minmax == 0) + { + auto iter = active.begin(); + auto end = active.end(); + for(; iter != end; ++iter) + { + CollisionObject* active_index = *iter; + const AABB& b0 = active_index->getAABB(); + const AABB& b1 = index->getAABB(); + + int axis2 = (axis + 1) % 3; + int axis3 = (axis + 2) % 3; + + if(b0.axisOverlap(b1, axis2) && b0.axisOverlap(b1, axis3)) + { + std::pair*, CollisionObject*> >::iterator, bool> insert_res; + if(active_index < index) + insert_res = overlap.insert(std::make_pair(active_index, index)); + else + insert_res = overlap.insert(std::make_pair(index, active_index)); + + if(insert_res.second) + { + if(callback(active_index, index, cdata)) + return; + } + } + } + active.insert(index); + } + else + active.erase(index); + } + +} + +//============================================================================== +template +void IntervalTreeCollisionManager::distance(void* cdata, DistanceCallBack callback) const +{ + if(size() == 0) return; + + this->enable_tested_set_ = true; + this->tested_set.clear(); + S min_dist = std::numeric_limits::max(); + + for(size_t i = 0; i < endpoints[0].size(); ++i) + if(distance_(endpoints[0][i].obj, cdata, callback, min_dist)) break; + + this->enable_tested_set_ = false; + this->tested_set.clear(); +} + +//============================================================================== +template +void IntervalTreeCollisionManager::collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const +{ + IntervalTreeCollisionManager* other_manager = static_cast(other_manager_); + + if((size() == 0) || (other_manager->size() == 0)) return; + + if(this == other_manager) + { + collide(cdata, callback); + return; + } + + if(this->size() < other_manager->size()) + { + for(size_t i = 0, size = endpoints[0].size(); i < size; ++i) + if(other_manager->collide_(endpoints[0][i].obj, cdata, callback)) return; + } + else + { + for(size_t i = 0, size = other_manager->endpoints[0].size(); i < size; ++i) + if(collide_(other_manager->endpoints[0][i].obj, cdata, callback)) return; + } +} + +//============================================================================== +template +void IntervalTreeCollisionManager::distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const +{ + IntervalTreeCollisionManager* other_manager = static_cast(other_manager_); + + if((size() == 0) || (other_manager->size() == 0)) return; + + if(this == other_manager) + { + distance(cdata, callback); + return; + } + + S min_dist = std::numeric_limits::max(); + + if(this->size() < other_manager->size()) + { + for(size_t i = 0, size = endpoints[0].size(); i < size; ++i) + if(other_manager->distance_(endpoints[0][i].obj, cdata, callback, min_dist)) return; + } + else + { + for(size_t i = 0, size = other_manager->endpoints[0].size(); i < size; ++i) + if(distance_(other_manager->endpoints[0][i].obj, cdata, callback, min_dist)) return; + } +} + +//============================================================================== +template +bool IntervalTreeCollisionManager::empty() const +{ + return endpoints[0].empty(); +} + +//============================================================================== +template +bool IntervalTreeCollisionManager::checkColl(std::deque::const_iterator pos_start, std::deque::const_iterator pos_end, CollisionObject* obj, void* cdata, CollisionCallBack callback) const +{ + while(pos_start < pos_end) + { + SAPInterval* ivl = static_cast(*pos_start); + if(ivl->obj != obj) + { + if(ivl->obj->getAABB().overlap(obj->getAABB())) + { + if(callback(ivl->obj, obj, cdata)) + return true; + } + } + + pos_start++; + } + + return false; +} + +//============================================================================== +template +bool IntervalTreeCollisionManager::checkDist(std::deque::const_iterator pos_start, std::deque::const_iterator pos_end, CollisionObject* obj, void* cdata, DistanceCallBack callback, S& min_dist) const +{ + while(pos_start < pos_end) + { + SAPInterval* ivl = static_cast(*pos_start); + if(ivl->obj != obj) + { + if(!this->enable_tested_set_) + { + if(ivl->obj->getAABB().distance(obj->getAABB()) < min_dist) + { + if(callback(ivl->obj, obj, cdata, min_dist)) + return true; + } + } + else + { + if(!this->inTestedSet(ivl->obj, obj)) + { + if(ivl->obj->getAABB().distance(obj->getAABB()) < min_dist) + { + if(callback(ivl->obj, obj, cdata, min_dist)) + return true; + } + + this->insertTestedSet(ivl->obj, obj); + } + } + } + + pos_start++; + } + + return false; +} + +} // namespace fcl + #endif diff --git a/include/fcl/broadphase/broadphase_spatialhash.h b/include/fcl/broadphase/broadphase_spatialhash.h index 8562299d8..07e240490 100644 --- a/include/fcl/broadphase/broadphase_spatialhash.h +++ b/include/fcl/broadphase/broadphase_spatialhash.h @@ -35,81 +35,39 @@ /** \author Jia Pan */ -#ifndef FCL_BROAD_PHASE_SPATIAL_HASH_H -#define FCL_BROAD_PHASE_SPATIAL_HASH_H +#ifndef FCL_BROADPHASE_BROADPAHSESPATIALHASH_H +#define FCL_BROADPHASE_BROADPAHSESPATIALHASH_H -#include "fcl/broadphase/broadphase.h" -#include "fcl/broadphase/hash.h" -#include "fcl/BV/AABB.h" #include #include +#include "fcl/BV/AABB.h" +#include "fcl/broadphase/broadphase.h" +#include "fcl/broadphase/simple_hash_table.h" +#include "fcl/broadphase/spatial_hash.h" namespace fcl { -/// @brief Spatial hash function: hash an AABB to a set of integer values -struct SpatialHash -{ - SpatialHash(const AABB& scene_limit_, FCL_REAL cell_size_) : cell_size(cell_size_), - scene_limit(scene_limit_) - { - width[0] = std::ceil(scene_limit.width() / cell_size); - width[1] = std::ceil(scene_limit.height() / cell_size); - width[2] = std::ceil(scene_limit.depth() / cell_size); - } - - std::vector operator() (const AABB& aabb) const - { - int min_x = std::floor((aabb.min_[0] - scene_limit.min_[0]) / cell_size); - int max_x = std::ceil((aabb.max_[0] - scene_limit.min_[0]) / cell_size); - int min_y = std::floor((aabb.min_[1] - scene_limit.min_[1]) / cell_size); - int max_y = std::ceil((aabb.max_[1] - scene_limit.min_[1]) / cell_size); - int min_z = std::floor((aabb.min_[2] - scene_limit.min_[2]) / cell_size); - int max_z = std::ceil((aabb.max_[2] - scene_limit.min_[2]) / cell_size); - - std::vector keys((max_x - min_x) * (max_y - min_y) * (max_z - min_z)); - int id = 0; - for(int x = min_x; x < max_x; ++x) - { - for(int y = min_y; y < max_y; ++y) - { - for(int z = min_z; z < max_z; ++z) - { - keys[id++] = x + y * width[0] + z * width[0] * width[1]; - } - } - } - return keys; - } - -private: - - FCL_REAL cell_size; - AABB scene_limit; - unsigned int width[3]; -}; - /// @brief spatial hashing collision mananger -template > -class SpatialHashingCollisionManager : public BroadPhaseCollisionManager +template, CollisionObject*, SpatialHash> > +class SpatialHashingCollisionManager : public BroadPhaseCollisionManager { public: - SpatialHashingCollisionManager(FCL_REAL cell_size, const Vector3d& scene_min, const Vector3d& scene_max, unsigned int default_table_size = 1000) : scene_limit(AABB(scene_min, scene_max)), - hash_table(new HashTable(SpatialHash(scene_limit, cell_size))) - { - hash_table->init(default_table_size); - } + SpatialHashingCollisionManager( + S cell_size, + const Vector3& scene_min, + const Vector3& scene_max, + unsigned int default_table_size = 1000); - ~SpatialHashingCollisionManager() - { - delete hash_table; - } + ~SpatialHashingCollisionManager(); /// @brief add one object to the manager - void registerObject(CollisionObject* obj); + void registerObject(CollisionObject* obj); /// @brief remove one object from the manager - void unregisterObject(CollisionObject* obj); + void unregisterObject(CollisionObject* obj); /// @brief initialize the manager, related with the specific type of manager void setup(); @@ -118,34 +76,34 @@ class SpatialHashingCollisionManager : public BroadPhaseCollisionManager void update(); /// @brief update the manager by explicitly given the object updated - void update(CollisionObject* updated_obj); + void update(CollisionObject* updated_obj); /// @brief update the manager by explicitly given the set of objects update - void update(const std::vector& updated_objs); + void update(const std::vector*>& updated_objs); /// @brief clear the manager void clear(); /// @brief return the objects managed by the manager - void getObjects(std::vector& objs) const; + void getObjects(std::vector*>& objs) const; /// @brief perform collision test between one object and all the objects belonging to the manager - void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const; + void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const; /// @brief perform distance computation between one object and all the objects belonging ot the manager - void distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const; + void distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const; /// @brief perform collision test for the objects belonging to the manager (i.e, N^2 self collision) - void collide(void* cdata, CollisionCallBack callback) const; + void collide(void* cdata, CollisionCallBack callback) const; /// @brief perform distance test for the objects belonging to the manager (i.e., N^2 self distance) - void distance(void* cdata, DistanceCallBack callback) const; + void distance(void* cdata, DistanceCallBack callback) const; /// @brief perform collision test with objects belonging to another manager - void collide(BroadPhaseCollisionManager* other_manager, void* cdata, CollisionCallBack callback) const; + void collide(BroadPhaseCollisionManager* other_manager, void* cdata, CollisionCallBack callback) const; /// @brief perform distance test with objects belonging to another manager - void distance(BroadPhaseCollisionManager* other_manager, void* cdata, DistanceCallBack callback) const; + void distance(BroadPhaseCollisionManager* other_manager, void* cdata, DistanceCallBack callback) const; /// @brief whether the manager is empty bool empty() const; @@ -154,46 +112,684 @@ class SpatialHashingCollisionManager : public BroadPhaseCollisionManager size_t size() const; /// @brief compute the bound for the environent - static void computeBound(std::vector& objs, Vector3d& l, Vector3d& u) - { - AABB bound; - for(unsigned int i = 0; i < objs.size(); ++i) - bound += objs[i]->getAABB(); - - l = bound.min_; - u = bound.max_; - } + static void computeBound(std::vector*>& objs, Vector3& l, Vector3& u); protected: /// @brief perform collision test between one object and all the objects belonging to the manager - bool collide_(CollisionObject* obj, void* cdata, CollisionCallBack callback) const; + bool collide_(CollisionObject* obj, void* cdata, CollisionCallBack callback) const; /// @brief perform distance computation between one object and all the objects belonging ot the manager - bool distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const; - + bool distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, S& min_dist) const; /// @brief all objects in the scene - std::list objs; + std::list*> objs; + + /// @brief objects partially penetrating (not totally inside nor outside) the + /// scene limit are in another list + std::list*> objs_partially_penetrating_scene_limit; /// @brief objects outside the scene limit are in another list - std::list objs_outside_scene_limit; + std::list*> objs_outside_scene_limit; /// @brief the size of the scene - AABB scene_limit; + AABB scene_limit; /// @brief store the map between objects and their aabbs. will make update more convenient - std::map obj_aabb_map; + std::map*, AABB> obj_aabb_map; /// @brief objects in the scene limit (given by scene_min and scene_max) are in the spatial hash table HashTable* hash_table; +private: + + enum ObjectStatus + { + Inside, + PartiallyPenetrating, + Outside + }; + + template + bool distanceObjectToObjects( + CollisionObject* obj, + const Container& objs, + void* cdata, + DistanceCallBack callback, + S& min_dist) const; + }; +template, CollisionObject*, SpatialHash>> +using SpatialHashingCollisionManagerf = SpatialHashingCollisionManager; + +template, CollisionObject*, SpatialHash>> +using SpatialHashingCollisionManagerd = SpatialHashingCollisionManager; + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +SpatialHashingCollisionManager::SpatialHashingCollisionManager( + S cell_size, + const Vector3& scene_min, + const Vector3& scene_max, + unsigned int default_table_size) + : scene_limit(AABB(scene_min, scene_max)), + hash_table(new HashTable(SpatialHash(scene_limit, cell_size))) +{ + hash_table->init(default_table_size); +} + +//============================================================================== +template +SpatialHashingCollisionManager::~SpatialHashingCollisionManager() +{ + delete hash_table; +} + +//============================================================================== +template +void SpatialHashingCollisionManager::registerObject( + CollisionObject* obj) +{ + objs.push_back(obj); + + const AABB& obj_aabb = obj->getAABB(); + AABB overlap_aabb; + + if(scene_limit.overlap(obj_aabb, overlap_aabb)) + { + if(!scene_limit.contain(obj_aabb)) + objs_partially_penetrating_scene_limit.push_back(obj); + + hash_table->insert(overlap_aabb, obj); + } + else + { + objs_outside_scene_limit.push_back(obj); + } + + obj_aabb_map[obj] = obj_aabb; +} + +//============================================================================== +template +void SpatialHashingCollisionManager::unregisterObject(CollisionObject* obj) +{ + objs.remove(obj); + + const AABB& obj_aabb = obj->getAABB(); + AABB overlap_aabb; + + if(scene_limit.overlap(obj_aabb, overlap_aabb)) + { + if(!scene_limit.contain(obj_aabb)) + objs_partially_penetrating_scene_limit.remove(obj); + + hash_table->remove(overlap_aabb, obj); + } + else + { + objs_outside_scene_limit.remove(obj); + } + + obj_aabb_map.erase(obj); +} + +//============================================================================== +template +void SpatialHashingCollisionManager::setup() +{ + // Do nothing +} + +//============================================================================== +template +void SpatialHashingCollisionManager::update() +{ + hash_table->clear(); + objs_partially_penetrating_scene_limit.clear(); + objs_outside_scene_limit.clear(); + + for(auto it = objs.cbegin(), end = objs.cend(); it != end; ++it) + { + CollisionObject* obj = *it; + const AABB& obj_aabb = obj->getAABB(); + AABB overlap_aabb; + + if(scene_limit.overlap(obj_aabb, overlap_aabb)) + { + if(!scene_limit.contain(obj_aabb)) + objs_partially_penetrating_scene_limit.push_back(obj); + + hash_table->insert(overlap_aabb, obj); + } + else + { + objs_outside_scene_limit.push_back(obj); + } + + obj_aabb_map[obj] = obj_aabb; + } +} + +//============================================================================== +template +void SpatialHashingCollisionManager::update(CollisionObject* updated_obj) +{ + const AABB& new_aabb = updated_obj->getAABB(); + const AABB& old_aabb = obj_aabb_map[updated_obj]; + + AABB old_overlap_aabb; + const auto is_old_aabb_overlapping + = scene_limit.overlap(old_aabb, old_overlap_aabb); + if(is_old_aabb_overlapping) + hash_table->remove(old_overlap_aabb, updated_obj); + + AABB new_overlap_aabb; + const auto is_new_aabb_overlapping + = scene_limit.overlap(new_aabb, new_overlap_aabb); + if(is_new_aabb_overlapping) + hash_table->insert(new_overlap_aabb, updated_obj); + + ObjectStatus old_status; + if(is_old_aabb_overlapping) + { + if(scene_limit.contain(old_aabb)) + old_status = Inside; + else + old_status = PartiallyPenetrating; + } + else + { + old_status = Outside; + } + + if(is_new_aabb_overlapping) + { + if(scene_limit.contain(new_aabb)) + { + if (old_status == PartiallyPenetrating) + { + // Status change: PartiallyPenetrating --> Inside + // Required action(s): + // - remove object from "objs_partially_penetrating_scene_limit" + + auto find_it = std::find(objs_partially_penetrating_scene_limit.begin(), + objs_partially_penetrating_scene_limit.end(), + updated_obj); + objs_partially_penetrating_scene_limit.erase(find_it); + } + else if (old_status == Outside) + { + // Status change: Outside --> Inside + // Required action(s): + // - remove object from "objs_outside_scene_limit" + + auto find_it = std::find(objs_outside_scene_limit.begin(), + objs_outside_scene_limit.end(), + updated_obj); + objs_outside_scene_limit.erase(find_it); + } + } + else + { + if (old_status == Inside) + { + // Status change: Inside --> PartiallyPenetrating + // Required action(s): + // - add object to "objs_partially_penetrating_scene_limit" + + objs_partially_penetrating_scene_limit.push_back(updated_obj); + } + else if (old_status == Outside) + { + // Status change: Outside --> PartiallyPenetrating + // Required action(s): + // - remove object from "objs_outside_scene_limit" + // - add object to "objs_partially_penetrating_scene_limit" + + auto find_it = std::find(objs_outside_scene_limit.begin(), + objs_outside_scene_limit.end(), + updated_obj); + objs_outside_scene_limit.erase(find_it); + + objs_partially_penetrating_scene_limit.push_back(updated_obj); + } + } + } + else + { + if (old_status == Inside) + { + // Status change: Inside --> Outside + // Required action(s): + // - add object to "objs_outside_scene_limit" + + objs_outside_scene_limit.push_back(updated_obj); + } + else if (old_status == PartiallyPenetrating) + { + // Status change: PartiallyPenetrating --> Outside + // Required action(s): + // - remove object from "objs_partially_penetrating_scene_limit" + // - add object to "objs_outside_scene_limit" + + auto find_it = std::find(objs_partially_penetrating_scene_limit.begin(), + objs_partially_penetrating_scene_limit.end(), + updated_obj); + objs_partially_penetrating_scene_limit.erase(find_it); + + objs_outside_scene_limit.push_back(updated_obj); + } + } + + obj_aabb_map[updated_obj] = new_aabb; +} + +//============================================================================== +template +void SpatialHashingCollisionManager::update(const std::vector*>& updated_objs) +{ + for(size_t i = 0; i < updated_objs.size(); ++i) + update(updated_objs[i]); +} + +//============================================================================== +template +void SpatialHashingCollisionManager::clear() +{ + objs.clear(); + hash_table->clear(); + objs_outside_scene_limit.clear(); + obj_aabb_map.clear(); +} + +//============================================================================== +template +void SpatialHashingCollisionManager::getObjects(std::vector*>& objs_) const +{ + objs_.resize(objs.size()); + std::copy(objs.begin(), objs.end(), objs_.begin()); +} + +//============================================================================== +template +void SpatialHashingCollisionManager::collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const +{ + if(size() == 0) return; + collide_(obj, cdata, callback); +} + +//============================================================================== +template +void SpatialHashingCollisionManager::distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const +{ + if(size() == 0) return; + S min_dist = std::numeric_limits::max(); + distance_(obj, cdata, callback, min_dist); +} + +//============================================================================== +template +bool SpatialHashingCollisionManager::collide_( + CollisionObject* obj, void* cdata, CollisionCallBack callback) const +{ + const auto& obj_aabb = obj->getAABB(); + AABB overlap_aabb; + + if(scene_limit.overlap(obj_aabb, overlap_aabb)) + { + const auto query_result = hash_table->query(overlap_aabb); + for(const auto& obj2 : query_result) + { + if(obj == obj2) + continue; + + if(callback(obj, obj2, cdata)) + return true; + } + + if(!scene_limit.contain(obj_aabb)) + { + for(const auto& obj2 : objs_outside_scene_limit) + { + if(obj == obj2) + continue; + + if(callback(obj, obj2, cdata)) + return true; + } + } + } + else + { + for(const auto& obj2 : objs_partially_penetrating_scene_limit) + { + if(obj == obj2) + continue; + + if(callback(obj, obj2, cdata)) + return true; + } + + for(const auto& obj2 : objs_outside_scene_limit) + { + if(obj == obj2) + continue; + + if(callback(obj, obj2, cdata)) + return true; + } + } + + return false; +} + +//============================================================================== +template +bool SpatialHashingCollisionManager::distance_( + CollisionObject* obj, void* cdata, DistanceCallBack callback, S& min_dist) const +{ + auto delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5; + auto aabb = obj->getAABB(); + if(min_dist < std::numeric_limits::max()) + { + Vector3 min_dist_delta(min_dist, min_dist, min_dist); + aabb.expand(min_dist_delta); + } + + AABB overlap_aabb; + + auto status = 1; + S old_min_distance; + + while(1) + { + old_min_distance = min_dist; + + if(scene_limit.overlap(aabb, overlap_aabb)) + { + if (distanceObjectToObjects( + obj, hash_table->query(overlap_aabb), cdata, callback, min_dist)) + { + return true; + } + + if(!scene_limit.contain(aabb)) + { + if (distanceObjectToObjects( + obj, objs_outside_scene_limit, cdata, callback, min_dist)) + { + return true; + } + } + } + else + { + if (distanceObjectToObjects( + obj, objs_partially_penetrating_scene_limit, cdata, callback, min_dist)) + { + return true; + } + + if (distanceObjectToObjects( + obj, objs_outside_scene_limit, cdata, callback, min_dist)) + { + return true; + } + } + + if(status == 1) + { + if(old_min_distance < std::numeric_limits::max()) + { + break; + } + else + { + if(min_dist < old_min_distance) + { + Vector3 min_dist_delta(min_dist, min_dist, min_dist); + aabb = AABB(obj->getAABB(), min_dist_delta); + status = 0; + } + else + { + if(aabb.equal(obj->getAABB())) + aabb.expand(delta); + else + aabb.expand(obj->getAABB(), 2.0); + } + } + } + else if(status == 0) + { + break; + } + } + + return false; +} + +//============================================================================== +template +void SpatialHashingCollisionManager::collide( + void* cdata, CollisionCallBack callback) const +{ + if(size() == 0) + return; + + for(const auto& obj1 : objs) + { + const auto& obj_aabb = obj1->getAABB(); + AABB overlap_aabb; + + if(scene_limit.overlap(obj_aabb, overlap_aabb)) + { + auto query_result = hash_table->query(overlap_aabb); + for(const auto& obj2 : query_result) + { + if(obj1 < obj2) + { + if(callback(obj1, obj2, cdata)) + return; + } + } + + if(!scene_limit.contain(obj_aabb)) + { + for(const auto& obj2 : objs_outside_scene_limit) + { + if(obj1 < obj2) + { + if(callback(obj1, obj2, cdata)) + return; + } + } + } + } + else + { + for(const auto& obj2 : objs_partially_penetrating_scene_limit) + { + if(obj1 < obj2) + { + if(callback(obj1, obj2, cdata)) + return; + } + } + + for(const auto& obj2 : objs_outside_scene_limit) + { + if(obj1 < obj2) + { + if(callback(obj1, obj2, cdata)) + return; + } + } + } + } +} + +//============================================================================== +template +void SpatialHashingCollisionManager::distance( + void* cdata, DistanceCallBack callback) const +{ + if(size() == 0) + return; + + this->enable_tested_set_ = true; + this->tested_set.clear(); + + S min_dist = std::numeric_limits::max(); + + for(const auto& obj : objs) + { + if(distance_(obj, cdata, callback, min_dist)) + break; + } + + this->enable_tested_set_ = false; + this->tested_set.clear(); +} + +//============================================================================== +template +void SpatialHashingCollisionManager::collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const +{ + auto* other_manager = static_cast* >(other_manager_); + + if((size() == 0) || (other_manager->size() == 0)) + return; + + if(this == other_manager) + { + collide(cdata, callback); + return; + } + if(this->size() < other_manager->size()) + { + for(const auto& obj : objs) + { + if(other_manager->collide_(obj, cdata, callback)) + return; + } + } + else + { + for(const auto& obj : other_manager->objs) + { + if(collide_(obj, cdata, callback)) + return; + } + } } -#include "fcl/broadphase/broadphase_spatialhash.hxx" +//============================================================================== +template +void SpatialHashingCollisionManager::distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const +{ + auto* other_manager = static_cast* >(other_manager_); + + if((size() == 0) || (other_manager->size() == 0)) + return; + + if(this == other_manager) + { + distance(cdata, callback); + return; + } + + S min_dist = std::numeric_limits::max(); + + if(this->size() < other_manager->size()) + { + for(const auto& obj : objs) + if(other_manager->distance_(obj, cdata, callback, min_dist)) return; + } + else + { + for(const auto& obj : other_manager->objs) + if(distance_(obj, cdata, callback, min_dist)) return; + } +} + +//============================================================================== +template +bool SpatialHashingCollisionManager::empty() const +{ + return objs.empty(); +} + +//============================================================================== +template +size_t SpatialHashingCollisionManager::size() const +{ + return objs.size(); +} + +//============================================================================== +template +void SpatialHashingCollisionManager::computeBound( + std::vector*>& objs, Vector3& l, Vector3& u) +{ + AABB bound; + for(unsigned int i = 0; i < objs.size(); ++i) + bound += objs[i]->getAABB(); + + l = bound.min_; + u = bound.max_; +} + +//============================================================================== +template +template +bool SpatialHashingCollisionManager::distanceObjectToObjects( + CollisionObject* obj, + const Container& objs, + void* cdata, + DistanceCallBack callback, + S& min_dist) const +{ + for(auto& obj2 : objs) + { + if(obj == obj2) + continue; + + if(!this->enable_tested_set_) + { + if(obj->getAABB().distance(obj2->getAABB()) < min_dist) + { + if(callback(obj, obj2, cdata, min_dist)) + return true; + } + } + else + { + if(!this->inTestedSet(obj, obj2)) + { + if(obj->getAABB().distance(obj2->getAABB()) < min_dist) + { + if(callback(obj, obj2, cdata, min_dist)) + return true; + } + + this->insertTestedSet(obj, obj2); + } + } + } + + return false; +} +} // namespace fcl #endif diff --git a/include/fcl/broadphase/broadphase_spatialhash.hxx b/include/fcl/broadphase/broadphase_spatialhash.hxx deleted file mode 100644 index 0a5613403..000000000 --- a/include/fcl/broadphase/broadphase_spatialhash.hxx +++ /dev/null @@ -1,459 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -namespace fcl -{ - -template -void SpatialHashingCollisionManager::registerObject(CollisionObject* obj) -{ - objs.push_back(obj); - - const AABB& obj_aabb = obj->getAABB(); - AABB overlap_aabb; - - if(scene_limit.overlap(obj_aabb, overlap_aabb)) - { - if(!scene_limit.contain(obj_aabb)) - objs_outside_scene_limit.push_back(obj); - - hash_table->insert(overlap_aabb, obj); - } - else - objs_outside_scene_limit.push_back(obj); - - obj_aabb_map[obj] = obj_aabb; -} - -template -void SpatialHashingCollisionManager::unregisterObject(CollisionObject* obj) -{ - objs.remove(obj); - - const AABB& obj_aabb = obj->getAABB(); - AABB overlap_aabb; - - if(scene_limit.overlap(obj_aabb, overlap_aabb)) - { - if(!scene_limit.contain(obj_aabb)) - objs_outside_scene_limit.remove(obj); - - hash_table->remove(overlap_aabb, obj); - } - else - objs_outside_scene_limit.remove(obj); - - obj_aabb_map.erase(obj); -} - -template -void SpatialHashingCollisionManager::setup() -{} - -template -void SpatialHashingCollisionManager::update() -{ - hash_table->clear(); - objs_outside_scene_limit.clear(); - - for(std::list::const_iterator it = objs.begin(), end = objs.end(); - it != end; ++it) - { - CollisionObject* obj = *it; - const AABB& obj_aabb = obj->getAABB(); - AABB overlap_aabb; - - if(scene_limit.overlap(obj_aabb, overlap_aabb)) - { - if(!scene_limit.contain(obj_aabb)) - objs_outside_scene_limit.push_back(obj); - - hash_table->insert(overlap_aabb, obj); - } - else - objs_outside_scene_limit.push_back(obj); - - obj_aabb_map[obj] = obj_aabb; - } -} - -template -void SpatialHashingCollisionManager::update(CollisionObject* updated_obj) -{ - const AABB& new_aabb = updated_obj->getAABB(); - const AABB& old_aabb = obj_aabb_map[updated_obj]; - - if(!scene_limit.contain(old_aabb)) // previously not completely in scene limit - { - if(scene_limit.contain(new_aabb)) - { - std::list::iterator find_it = std::find(objs_outside_scene_limit.begin(), - objs_outside_scene_limit.end(), - updated_obj); - - objs_outside_scene_limit.erase(find_it); - } - } - else if(!scene_limit.contain(new_aabb)) // previous completely in scenelimit, now not - objs_outside_scene_limit.push_back(updated_obj); - - AABB old_overlap_aabb; - if(scene_limit.overlap(old_aabb, old_overlap_aabb)) - hash_table->remove(old_overlap_aabb, updated_obj); - - AABB new_overlap_aabb; - if(scene_limit.overlap(new_aabb, new_overlap_aabb)) - hash_table->insert(new_overlap_aabb, updated_obj); - - obj_aabb_map[updated_obj] = new_aabb; -} - -template -void SpatialHashingCollisionManager::update(const std::vector& updated_objs) -{ - for(size_t i = 0; i < updated_objs.size(); ++i) - update(updated_objs[i]); -} - - -template -void SpatialHashingCollisionManager::clear() -{ - objs.clear(); - hash_table->clear(); - objs_outside_scene_limit.clear(); - obj_aabb_map.clear(); -} - -template -void SpatialHashingCollisionManager::getObjects(std::vector& objs_) const -{ - objs_.resize(objs.size()); - std::copy(objs.begin(), objs.end(), objs_.begin()); -} - -template -void SpatialHashingCollisionManager::collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const -{ - if(size() == 0) return; - collide_(obj, cdata, callback); -} - -template -void SpatialHashingCollisionManager::distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const -{ - if(size() == 0) return; - FCL_REAL min_dist = std::numeric_limits::max(); - distance_(obj, cdata, callback, min_dist); -} - -template -bool SpatialHashingCollisionManager::collide_(CollisionObject* obj, void* cdata, CollisionCallBack callback) const -{ - const AABB& obj_aabb = obj->getAABB(); - AABB overlap_aabb; - - if(scene_limit.overlap(obj_aabb, overlap_aabb)) - { - if(!scene_limit.contain(obj_aabb)) - { - for(std::list::const_iterator it = objs_outside_scene_limit.begin(), end = objs_outside_scene_limit.end(); - it != end; ++it) - { - if(obj == *it) continue; - if(callback(obj, *it, cdata)) return true; - } - } - - std::vector query_result = hash_table->query(overlap_aabb); - for(unsigned int i = 0; i < query_result.size(); ++i) - { - if(obj == query_result[i]) continue; - if(callback(obj, query_result[i], cdata)) return true; - } - } - else - { - ; - for(std::list::const_iterator it = objs_outside_scene_limit.begin(), end = objs_outside_scene_limit.end(); - it != end; ++it) - { - if(obj == *it) continue; - if(callback(obj, *it, cdata)) return true; - } - } - - return false; -} - -template -bool SpatialHashingCollisionManager::distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const -{ - Vector3d delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5; - AABB aabb = obj->getAABB(); - if(min_dist < std::numeric_limits::max()) - { - Vector3d min_dist_delta(min_dist, min_dist, min_dist); - aabb.expand(min_dist_delta); - } - - AABB overlap_aabb; - - int status = 1; - FCL_REAL old_min_distance; - - while(1) - { - old_min_distance = min_dist; - - if(scene_limit.overlap(aabb, overlap_aabb)) - { - if(!scene_limit.contain(aabb)) - { - for(std::list::const_iterator it = objs_outside_scene_limit.begin(), end = objs_outside_scene_limit.end(); - it != end; ++it) - { - if(obj == *it) continue; - if(!enable_tested_set_) - { - if(obj->getAABB().distance((*it)->getAABB()) < min_dist) - if(callback(obj, *it, cdata, min_dist)) return true; - } - else - { - if(!inTestedSet(obj, *it)) - { - if(obj->getAABB().distance((*it)->getAABB()) < min_dist) - if(callback(obj, *it, cdata, min_dist)) return true; - insertTestedSet(obj, *it); - } - } - } - } - - std::vector query_result = hash_table->query(overlap_aabb); - for(unsigned int i = 0; i < query_result.size(); ++i) - { - if(obj == query_result[i]) continue; - if(!enable_tested_set_) - { - if(obj->getAABB().distance(query_result[i]->getAABB()) < min_dist) - if(callback(obj, query_result[i], cdata, min_dist)) return true; - } - else - { - if(!inTestedSet(obj, query_result[i])) - { - if(obj->getAABB().distance(query_result[i]->getAABB()) < min_dist) - if(callback(obj, query_result[i], cdata, min_dist)) return true; - insertTestedSet(obj, query_result[i]); - } - } - } - } - else - { - for(std::list::const_iterator it = objs_outside_scene_limit.begin(), end = objs_outside_scene_limit.end(); - it != end; ++it) - { - if(obj == *it) continue; - if(!enable_tested_set_) - { - if(obj->getAABB().distance((*it)->getAABB()) < min_dist) - if(callback(obj, *it, cdata, min_dist)) return true; - } - else - { - if(!inTestedSet(obj, *it)) - { - if(obj->getAABB().distance((*it)->getAABB()) < min_dist) - if(callback(obj, *it, cdata, min_dist)) return true; - insertTestedSet(obj, *it); - } - } - } - } - - if(status == 1) - { - if(old_min_distance < std::numeric_limits::max()) - break; - else - { - if(min_dist < old_min_distance) - { - Vector3d min_dist_delta(min_dist, min_dist, min_dist); - aabb = AABB(obj->getAABB(), min_dist_delta); - status = 0; - } - else - { - if(aabb.equal(obj->getAABB())) - aabb.expand(delta); - else - aabb.expand(obj->getAABB(), 2.0); - } - } - } - else if(status == 0) - break; - } - - return false; -} - -template -void SpatialHashingCollisionManager::collide(void* cdata, CollisionCallBack callback) const -{ - if(size() == 0) return; - - for(std::list::const_iterator it1 = objs.begin(), end1 = objs.end(); - it1 != end1; ++it1) - { - const AABB& obj_aabb = (*it1)->getAABB(); - AABB overlap_aabb; - - if(scene_limit.overlap(obj_aabb, overlap_aabb)) - { - if(!scene_limit.contain(obj_aabb)) - { - for(std::list::const_iterator it2 = objs_outside_scene_limit.begin(), end2 = objs_outside_scene_limit.end(); - it2 != end2; ++it2) - { - if(*it1 < *it2) { if(callback(*it1, *it2, cdata)) return; } - } - } - - std::vector query_result = hash_table->query(overlap_aabb); - for(unsigned int i = 0; i < query_result.size(); ++i) - { - if(*it1 < query_result[i]) { if(callback(*it1, query_result[i], cdata)) return; } - } - } - else - { - for(std::list::const_iterator it2 = objs_outside_scene_limit.begin(), end2 = objs_outside_scene_limit.end(); - it2 != end2; ++it2) - { - if(*it1 < *it2) { if(callback(*it1, *it2, cdata)) return; } - } - } - } -} - -template -void SpatialHashingCollisionManager::distance(void* cdata, DistanceCallBack callback) const -{ - if(size() == 0) return; - - enable_tested_set_ = true; - tested_set.clear(); - - FCL_REAL min_dist = std::numeric_limits::max(); - - for(std::list::const_iterator it = objs.begin(), end = objs.end(); it != end; ++it) - if(distance_(*it, cdata, callback, min_dist)) break; - - enable_tested_set_ = false; - tested_set.clear(); -} - -template -void SpatialHashingCollisionManager::collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const -{ - SpatialHashingCollisionManager* other_manager = static_cast* >(other_manager_); - - if((size() == 0) || (other_manager->size() == 0)) return; - - if(this == other_manager) - { - collide(cdata, callback); - return; - } - - if(this->size() < other_manager->size()) - { - for(std::list::const_iterator it = objs.begin(), end = objs.end(); it != end; ++it) - if(other_manager->collide_(*it, cdata, callback)) return; - } - else - { - for(std::list::const_iterator it = other_manager->objs.begin(), end = other_manager->objs.end(); it != end; ++it) - if(collide_(*it, cdata, callback)) return; - } -} - -template -void SpatialHashingCollisionManager::distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const -{ - SpatialHashingCollisionManager* other_manager = static_cast* >(other_manager_); - - if((size() == 0) || (other_manager->size() == 0)) return; - - if(this == other_manager) - { - distance(cdata, callback); - return; - } - - FCL_REAL min_dist = std::numeric_limits::max(); - - if(this->size() < other_manager->size()) - { - for(std::list::const_iterator it = objs.begin(), end = objs.end(); it != end; ++it) - if(other_manager->distance_(*it, cdata, callback, min_dist)) return; - } - else - { - for(std::list::const_iterator it = other_manager->objs.begin(), end = other_manager->objs.end(); it != end; ++it) - if(distance_(*it, cdata, callback, min_dist)) return; - } -} - -template -bool SpatialHashingCollisionManager::empty() const -{ - return objs.empty(); -} - -template -size_t SpatialHashingCollisionManager::size() const -{ - return objs.size(); -} - -} diff --git a/include/fcl/broadphase/hash.h b/include/fcl/broadphase/hash.h deleted file mode 100644 index 35027a024..000000000 --- a/include/fcl/broadphase/hash.h +++ /dev/null @@ -1,189 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#ifndef FCL_HASH_H -#define FCL_HASH_H - -#include -#include -#include -#include -#include - -namespace fcl -{ - -/// @brief A simple hash table implemented as multiple buckets. HashFnc is any extended hash function: HashFnc(key) = {index1, index2, ..., } -template -class SimpleHashTable -{ -protected: - typedef std::list Bin; - - std::vector table_; - - HashFnc h_; - - size_t table_size_; - -public: - SimpleHashTable(const HashFnc& h) : h_(h) - { - } - - ///@ brief Init the number of bins in the hash table - void init(size_t size) - { - if(size == 0) - { - throw std::logic_error("SimpleHashTable must have non-zero size."); - } - - table_.resize(size); - table_size_ = size; - } - - //// @brief Insert a key-value pair into the table - void insert(Key key, Data value) - { - std::vector indices = h_(key); - size_t range = table_.size(); - for(size_t i = 0; i < indices.size(); ++i) - table_[indices[i] % range].push_back(value); - } - - /// @brief Find the elements in the hash table whose key is the same as query key. - std::vector query(Key key) const - { - size_t range = table_.size(); - std::vector indices = h_(key); - std::set result; - for(size_t i = 0; i < indices.size(); ++i) - { - unsigned int index = indices[i] % range; - std::copy(table_[index].begin(), table_[index].end(), std::inserter(result, result.end())); - } - - return std::vector(result.begin(), result.end()); - } - - /// @brief remove the key-value pair from the table - void remove(Key key, Data value) - { - size_t range = table_.size(); - std::vector indices = h_(key); - for(size_t i = 0; i < indices.size(); ++i) - { - unsigned int index = indices[i] % range; - table_[index].remove(value); - } - } - - /// @brief clear the hash table - void clear() - { - table_.clear(); - table_.resize(table_size_); - } -}; - - -template -class unordered_map_hash_table : public std::unordered_map {}; - -/// @brief A hash table implemented using unordered_map -template class TableT = unordered_map_hash_table> -class SparseHashTable -{ -protected: - HashFnc h_; - typedef std::list Bin; - typedef TableT Table; - - Table table_; -public: - SparseHashTable(const HashFnc& h) : h_(h) {} - - /// @brief Init the hash table. The bucket size is dynamically decided. - void init(size_t) { table_.clear(); } - - /// @brief insert one key-value pair into the hash table - void insert(Key key, Data value) - { - std::vector indices = h_(key); - for(size_t i = 0; i < indices.size(); ++i) - table_[indices[i]].push_back(value); - } - - /// @brief find the elements whose key is the same as the query - std::vector query(Key key) const - { - std::vector indices = h_(key); - std::set result; - for(size_t i = 0; i < indices.size(); ++i) - { - unsigned int index = indices[i]; - typename Table::const_iterator p = table_.find(index); - if(p != table_.end()) - std::copy((*p).second.begin(), (*p).second.end(), std::inserter(result, result.end())); - } - - return std::vector(result.begin(), result.end()); - } - - /// @brief remove one key-value pair from the hash table - void remove(Key key, Data value) - { - std::vector indices = h_(key); - for(size_t i = 0; i < indices.size(); ++i) - { - unsigned int index = indices[i]; - table_[index].remove(value); - } - } - - /// @brief clear the hash table - void clear() - { - table_.clear(); - } -}; - - -} - -#endif diff --git a/include/fcl/broadphase/hierarchy_tree.h b/include/fcl/broadphase/hierarchy_tree.h index d79407efa..4501017f1 100644 --- a/include/fcl/broadphase/hierarchy_tree.h +++ b/include/fcl/broadphase/hierarchy_tree.h @@ -41,13 +41,15 @@ #include #include #include +#include +#include "fcl/common/warning.h" #include "fcl/BV/AABB.h" #include "fcl/broadphase/morton.h" namespace fcl { -/// @brief dynamic AABB tree node +/// @brief dynamic AABB tree node template struct NodeBase { @@ -58,7 +60,7 @@ struct NodeBase NodeBase* parent; /// @brief whether is a leaf - bool isLeaf() const { return (children[1] == NULL); } + bool isLeaf() const { return (children[1] == nullptr); } /// @brief whether is internal node bool isInternal() const { return !isLeaf(); } @@ -75,9 +77,9 @@ struct NodeBase NodeBase() { - parent = NULL; - children[0] = NULL; - children[1] = NULL; + parent = nullptr; + children[0] = nullptr; + children[1] = nullptr; } }; @@ -90,44 +92,56 @@ bool nodeBaseLess(NodeBase* a, NodeBase* b, int d) return false; } +//============================================================================== +template +struct SelectImpl +{ + static std::size_t run( + const NodeBase& /*query*/, + const NodeBase& /*node1*/, + const NodeBase& /*node2*/) + { + return 0; + } + + static std::size_t run( + const BV& /*query*/, + const NodeBase& /*node1*/, + const NodeBase& /*node2*/) + { + return 0; + } +}; + /// @brief select from node1 and node2 which is close to a given query. 0 for node1 and 1 for node2 template -size_t select(const NodeBase& query, const NodeBase& node1, const NodeBase& node2) +size_t select( + const NodeBase& query, + const NodeBase& node1, + const NodeBase& node2) { - return 0; + return SelectImpl::run(query, node1, node2); } -template<> -size_t select(const NodeBase& node, const NodeBase& node1, const NodeBase& node2); - /// @brief select from node1 and node2 which is close to a given query bounding volume. 0 for node1 and 1 for node2 template -size_t select(const BV& query, const NodeBase& node1, const NodeBase& node2) +size_t select( + const BV& query, + const NodeBase& node1, + const NodeBase& node2) { - return 0; + return SelectImpl::run(query, node1, node2); } -template<> -size_t select(const AABB& query, const NodeBase& node1, const NodeBase& node2); - - /// @brief Class for hierarchy tree structure template class HierarchyTree { - typedef NodeBase NodeType; - typedef typename std::vector* >::iterator NodeVecIterator; - typedef typename std::vector* >::const_iterator NodeVecConstIterator; +public: - struct SortByMorton - { - bool operator() (const NodeType* a, const NodeType* b) const - { - return a->code < b->code; - } - }; + using S = typename BV::S; -public: + typedef NodeBase NodeType; /// @brief Create hierarchy tree with suitable setting. /// bu_threshold decides the height of tree node to start bottom-up construction / optimization; @@ -159,10 +173,10 @@ class HierarchyTree bool update(NodeType* leaf, const BV& bv); /// @brief update one leaf's bounding volume, with prediction - bool update(NodeType* leaf, const BV& bv, const Vector3d& vel, FCL_REAL margin); + bool update(NodeType* leaf, const BV& bv, const Vector3& vel, S margin); /// @brief update one leaf's bounding volume, with prediction - bool update(NodeType* leaf, const BV& bv, const Vector3d& vel); + bool update(NodeType* leaf, const BV& bv, const Vector3& vel); /// @brief get the max height of the tree size_t getMaxHeight() const; @@ -198,6 +212,17 @@ class HierarchyTree private: + typedef typename std::vector* >::iterator NodeVecIterator; + typedef typename std::vector* >::const_iterator NodeVecConstIterator; + + struct SortByMorton + { + bool operator() (const NodeType* a, const NodeType* b) const + { + return a->code < b->code; + } + }; + /// @brief construct a tree for a set of leaves from bottom -- very heavy way void bottomup(const NodeVecIterator lbeg, const NodeVecIterator lend); @@ -211,7 +236,7 @@ class HierarchyTree void getMaxDepth(NodeType* node, size_t depth, size_t& max_depth) const; /// @brief construct a tree from a list of nodes stored in [lbeg, lend) in a topdown manner. - /// During construction, first compute the best split axis as the axis along with the longest AABB edge. + /// During construction, first compute the best split axis as the axis along with the longest AABB edge. /// Then compute the median of all nodes' center projection onto the axis and using it as the split threshold. NodeType* topdown_0(const NodeVecIterator lbeg, const NodeVecIterator lend); @@ -302,14 +327,6 @@ class HierarchyTree int bu_threshold; }; - -template<> -bool HierarchyTree::update(NodeBase* leaf, const AABB& bv_, const Vector3d& vel, FCL_REAL margin); - -template<> -bool HierarchyTree::update(NodeBase* leaf, const AABB& bv_, const Vector3d& vel); - - namespace implementation_array { @@ -362,31 +379,40 @@ struct nodeBaseLess size_t d; }; +//============================================================================== +template +struct SelectImpl +{ + static bool run(size_t query, size_t node1, size_t node2, NodeBase* nodes) + { + return 0; + } + + static bool run(const BV& query, size_t node1, size_t node2, NodeBase* nodes) + { + return 0; + } +}; /// @brief select the node from node1 and node2 which is close to the query-th node in the nodes. 0 for node1 and 1 for node2. template size_t select(size_t query, size_t node1, size_t node2, NodeBase* nodes) { - return 0; + return SelectImpl::run(query, node1, node2, nodes); } -template<> -size_t select(size_t query, size_t node1, size_t node2, NodeBase* nodes); - -/// @brief select the node from node1 and node2 which is close to the query AABB. 0 for node1 and 1 for node2. +/// @brief select the node from node1 and node2 which is close to the query AABB. 0 for node1 and 1 for node2. template size_t select(const BV& query, size_t node1, size_t node2, NodeBase* nodes) { - return 0; + return SelectImpl::run(query, node1, node2, nodes); } -template<> -size_t select(const AABB& query, size_t node1, size_t node2, NodeBase* nodes); - /// @brief Class for hierarchy tree structure template class HierarchyTree { + using S = typename BV::S; typedef NodeBase NodeType; struct SortByMorton @@ -438,10 +464,10 @@ class HierarchyTree bool update(size_t leaf, const BV& bv); /// @brief update one leaf's bounding volume, with prediction - bool update(size_t leaf, const BV& bv, const Vector3d& vel, FCL_REAL margin); + bool update(size_t leaf, const BV& bv, const Vector3& vel, S margin); /// @brief update one leaf's bounding volume, with prediction - bool update(size_t leaf, const BV& bv, const Vector3d& vel); + bool update(size_t leaf, const BV& bv, const Vector3& vel); /// @brief get the max height of the tree size_t getMaxHeight() const; @@ -491,7 +517,7 @@ class HierarchyTree void getMaxDepth(size_t node, size_t depth, size_t& max_depth) const; /// @brief construct a tree from a list of nodes stored in [lbeg, lend) in a topdown manner. - /// During construction, first compute the best split axis as the axis along with the longest AABB edge. + /// During construction, first compute the best split axis as the axis along with the longest AABB edge. /// Then compute the median of all nodes' center projection onto the axis and using it as the split threshold. size_t topdown_0(size_t* lbeg, size_t* lend); @@ -583,10 +609,1991 @@ const size_t HierarchyTree::NULL_NODE; } +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +HierarchyTree::HierarchyTree(int bu_threshold_, int topdown_level_) +{ + root_node = nullptr; + n_leaves = 0; + free_node = nullptr; + max_lookahead_level = -1; + opath = 0; + bu_threshold = bu_threshold_; + topdown_level = topdown_level_; +} + +template +HierarchyTree::~HierarchyTree() +{ + clear(); +} + +template +void HierarchyTree::init(std::vector& leaves, int level) +{ + switch(level) + { + case 0: + init_0(leaves); + break; + case 1: + init_1(leaves); + break; + case 2: + init_2(leaves); + break; + case 3: + init_3(leaves); + break; + default: + init_0(leaves); + } +} + +template +typename HierarchyTree::NodeType* HierarchyTree::insert(const BV& bv, void* data) +{ + NodeType* leaf = createNode(nullptr, bv, data); + insertLeaf(root_node, leaf); + ++n_leaves; + return leaf; +} + +template +void HierarchyTree::remove(NodeType* leaf) +{ + removeLeaf(leaf); + deleteNode(leaf); + --n_leaves; +} + +template +void HierarchyTree::clear() +{ + if(root_node) + recurseDeleteNode(root_node); + n_leaves = 0; + delete free_node; + free_node = nullptr; + max_lookahead_level = -1; + opath = 0; +} + +template +bool HierarchyTree::empty() const +{ + return (nullptr == root_node); +} + +template +void HierarchyTree::update(NodeType* leaf, int lookahead_level) +{ + typename HierarchyTree::NodeType* root = removeLeaf(leaf); + if(root) + { + if(lookahead_level > 0) + { + for(int i = 0; (i < lookahead_level) && root->parent; ++i) + root = root->parent; + } + else + root = root_node; + } + insertLeaf(root, leaf); +} + +template +bool HierarchyTree::update(NodeType* leaf, const BV& bv) +{ + if(leaf->bv.contain(bv)) return false; + update_(leaf, bv); + return true; +} + +//============================================================================== +template +struct UpdateImpl +{ + static bool run( + const HierarchyTree& tree, + typename HierarchyTree::NodeType* leaf, + const BV& bv, + const Vector3& /*vel*/, + S /*margin*/) + { + if(leaf->bv.contain(bv)) return false; + tree.update_(leaf, bv); + return true; + } + + static bool run( + const HierarchyTree& tree, + typename HierarchyTree::NodeType* leaf, + const BV& bv, + const Vector3& /*vel*/) + { + if(leaf->bv.contain(bv)) return false; + tree.update_(leaf, bv); + return true; + } +}; + +template +bool HierarchyTree::update(NodeType* leaf, const BV& bv, const Vector3& vel, S margin) +{ + return UpdateImpl::run(*this, leaf, bv, vel, margin); +} + +template +bool HierarchyTree::update(NodeType* leaf, const BV& bv, const Vector3& vel) +{ + return UpdateImpl::run(*this, leaf, bv, vel); +} + +template +size_t HierarchyTree::getMaxHeight() const +{ + if(!root_node) + return 0; + return getMaxHeight(root_node); +} + +template +size_t HierarchyTree::getMaxDepth() const +{ + if(!root_node) return 0; + + size_t max_depth; + getMaxDepth(root_node, 0, max_depth); + return max_depth; +} + +template +void HierarchyTree::balanceBottomup() +{ + if(root_node) + { + std::vector leaves; + leaves.reserve(n_leaves); + fetchLeaves(root_node, leaves); + bottomup(leaves.begin(), leaves.end()); + root_node = leaves[0]; + } +} + +template +void HierarchyTree::balanceTopdown() +{ + if(root_node) + { + std::vector leaves; + leaves.reserve(n_leaves); + fetchLeaves(root_node, leaves); + root_node = topdown(leaves.begin(), leaves.end()); + } +} + +template +void HierarchyTree::balanceIncremental(int iterations) +{ + if(iterations < 0) iterations = n_leaves; + if(root_node && (iterations > 0)) + { + for(int i = 0; i < iterations; ++i) + { + NodeType* node = root_node; + unsigned int bit = 0; + while(!node->isLeaf()) + { + node = sort(node, root_node)->children[(opath>>bit)&1]; + bit = (bit+1)&(sizeof(unsigned int) * 8-1); + } + update(node); + ++opath; + } + } +} + +template +void HierarchyTree::refit() +{ + if(root_node) + recurseRefit(root_node); +} + +template +void HierarchyTree::extractLeaves(const NodeType* root, std::vector& leaves) const +{ + if(!root->isLeaf()) + { + extractLeaves(root->children[0], leaves); + extractLeaves(root->children[1], leaves); + } + else + leaves.push_back(root); +} + +template +size_t HierarchyTree::size() const +{ + return n_leaves; +} + +template +typename HierarchyTree::NodeType* HierarchyTree::getRoot() const +{ + return root_node; +} + +template +typename HierarchyTree::NodeType*& HierarchyTree::getRoot() +{ + return root_node; +} + +template +void HierarchyTree::print(NodeType* root, int depth) +{ + for(int i = 0; i < depth; ++i) + std::cout << " "; + std::cout << " (" << root->bv.min_[0] << ", " << root->bv.min_[1] << ", " << root->bv.min_[2] << "; " << root->bv.max_[0] << ", " << root->bv.max_[1] << ", " << root->bv.max_[2] << ")" << std::endl; + if(root->isLeaf()) + { + } + else + { + print(root->children[0], depth+1); + print(root->children[1], depth+1); + } +} + + + +template +void HierarchyTree::bottomup(const NodeVecIterator lbeg, const NodeVecIterator lend) +{ + NodeVecIterator lcur_end = lend; + while(lbeg < lcur_end - 1) + { + NodeVecIterator min_it1, min_it2; + S min_size = std::numeric_limits::max(); + for(NodeVecIterator it1 = lbeg; it1 < lcur_end; ++it1) + { + for(NodeVecIterator it2 = it1 + 1; it2 < lcur_end; ++it2) + { + S cur_size = ((*it1)->bv + (*it2)->bv).size(); + if(cur_size < min_size) + { + min_size = cur_size; + min_it1 = it1; + min_it2 = it2; + } + } + } + + NodeType* n[2] = {*min_it1, *min_it2}; + NodeType* p = createNode(nullptr, n[0]->bv, n[1]->bv, nullptr); + p->children[0] = n[0]; + p->children[1] = n[1]; + n[0]->parent = p; + n[1]->parent = p; + *min_it1 = p; + NodeType* tmp = *min_it2; + lcur_end--; + *min_it2 = *lcur_end; + *lcur_end = tmp; + } +} + +template +typename HierarchyTree::NodeType* HierarchyTree::topdown(const NodeVecIterator lbeg, const NodeVecIterator lend) +{ + switch(topdown_level) + { + case 0: + return topdown_0(lbeg, lend); + break; + case 1: + return topdown_1(lbeg, lend); + break; + default: + return topdown_0(lbeg, lend); + } +} + +template +size_t HierarchyTree::getMaxHeight(NodeType* node) const +{ + if(!node->isLeaf()) + { + size_t height1 = getMaxHeight(node->children[0]); + size_t height2 = getMaxHeight(node->children[1]); + return std::max(height1, height2) + 1; + } + else + return 0; +} + +template +void HierarchyTree::getMaxDepth(NodeType* node, size_t depth, size_t& max_depth) const +{ + if(!node->isLeaf()) + { + getMaxDepth(node->children[0], depth+1, max_depth); + getMaxDepth(node->children[1], depth+1, max_depth); + } + else + max_depth = std::max(max_depth, depth); +} + +template +typename HierarchyTree::NodeType* HierarchyTree::topdown_0(const NodeVecIterator lbeg, const NodeVecIterator lend) +{ + int num_leaves = lend - lbeg; + if(num_leaves > 1) + { + if(num_leaves > bu_threshold) + { + BV vol = (*lbeg)->bv; + for(NodeVecIterator it = lbeg + 1; it < lend; ++it) + vol += (*it)->bv; + + int best_axis = 0; + S extent[3] = {vol.width(), vol.height(), vol.depth()}; + if(extent[1] > extent[0]) best_axis = 1; + if(extent[2] > extent[best_axis]) best_axis = 2; + + // compute median + NodeVecIterator lcenter = lbeg + num_leaves / 2; + std::nth_element(lbeg, lcenter, lend, std::bind(&nodeBaseLess, std::placeholders::_1, std::placeholders::_2, std::ref(best_axis))); + + NodeType* node = createNode(nullptr, vol, nullptr); + node->children[0] = topdown_0(lbeg, lcenter); + node->children[1] = topdown_0(lcenter, lend); + node->children[0]->parent = node; + node->children[1]->parent = node; + return node; + } + else + { + bottomup(lbeg, lend); + return *lbeg; + } + } + return *lbeg; +} + +template +typename HierarchyTree::NodeType* HierarchyTree::topdown_1(const NodeVecIterator lbeg, const NodeVecIterator lend) +{ + int num_leaves = lend - lbeg; + if(num_leaves > 1) + { + if(num_leaves > bu_threshold) + { + Vector3 split_p = (*lbeg)->bv.center(); + BV vol = (*lbeg)->bv; + NodeVecIterator it; + for(it = lbeg + 1; it < lend; ++it) + { + split_p += (*it)->bv.center(); + vol += (*it)->bv; + } + split_p /= (S)(num_leaves); + int best_axis = -1; + int bestmidp = num_leaves; + int splitcount[3][2] = {{0,0}, {0,0}, {0,0}}; + for(it = lbeg; it < lend; ++it) + { + Vector3 x = (*it)->bv.center() - split_p; + for(size_t j = 0; j < 3; ++j) + ++splitcount[j][x[j] > 0 ? 1 : 0]; + } + + for(size_t i = 0; i < 3; ++i) + { + if((splitcount[i][0] > 0) && (splitcount[i][1] > 0)) + { + int midp = std::abs(splitcount[i][0] - splitcount[i][1]); + if(midp < bestmidp) + { + best_axis = i; + bestmidp = midp; + } + } + } + + if(best_axis < 0) best_axis = 0; + + S split_value = split_p[best_axis]; + NodeVecIterator lcenter = lbeg; + for(it = lbeg; it < lend; ++it) + { + if((*it)->bv.center()[best_axis] < split_value) + { + NodeType* temp = *it; + *it = *lcenter; + *lcenter = temp; + ++lcenter; + } + } + + NodeType* node = createNode(nullptr, vol, nullptr); + node->children[0] = topdown_1(lbeg, lcenter); + node->children[1] = topdown_1(lcenter, lend); + node->children[0]->parent = node; + node->children[1]->parent = node; + return node; + } + else + { + bottomup(lbeg, lend); + return *lbeg; + } + } + return *lbeg; +} + +template +void HierarchyTree::init_0(std::vector& leaves) +{ + clear(); + root_node = topdown(leaves.begin(), leaves.end()); + n_leaves = leaves.size(); + max_lookahead_level = -1; + opath = 0; +} + +template +void HierarchyTree::init_1(std::vector& leaves) +{ + clear(); + + BV bound_bv; + if(leaves.size() > 0) + bound_bv = leaves[0]->bv; + for(size_t i = 1; i < leaves.size(); ++i) + bound_bv += leaves[i]->bv; + + morton_functor coder(bound_bv); + for(size_t i = 0; i < leaves.size(); ++i) + leaves[i]->code = coder(leaves[i]->bv.center()); + + std::sort(leaves.begin(), leaves.end(), SortByMorton()); + + root_node = mortonRecurse_0(leaves.begin(), leaves.end(), (1 << (coder.bits()-1)), coder.bits()-1); + + refit(); + n_leaves = leaves.size(); + max_lookahead_level = -1; + opath = 0; +} + +template +void HierarchyTree::init_2(std::vector& leaves) +{ + clear(); + + BV bound_bv; + if(leaves.size() > 0) + bound_bv = leaves[0]->bv; + for(size_t i = 1; i < leaves.size(); ++i) + bound_bv += leaves[i]->bv; + + morton_functor coder(bound_bv); + for(size_t i = 0; i < leaves.size(); ++i) + leaves[i]->code = coder(leaves[i]->bv.center()); + + std::sort(leaves.begin(), leaves.end(), SortByMorton()); + + root_node = mortonRecurse_1(leaves.begin(), leaves.end(), (1 << (coder.bits()-1)), coder.bits()-1); + + refit(); + n_leaves = leaves.size(); + max_lookahead_level = -1; + opath = 0; +} + +template +void HierarchyTree::init_3(std::vector& leaves) +{ + clear(); + + BV bound_bv; + if(leaves.size() > 0) + bound_bv = leaves[0]->bv; + for(size_t i = 1; i < leaves.size(); ++i) + bound_bv += leaves[i]->bv; + + morton_functor coder(bound_bv); + for(size_t i = 0; i < leaves.size(); ++i) + leaves[i]->code = coder(leaves[i]->bv.center()); + + std::sort(leaves.begin(), leaves.end(), SortByMorton()); + + root_node = mortonRecurse_2(leaves.begin(), leaves.end()); + + refit(); + n_leaves = leaves.size(); + max_lookahead_level = -1; + opath = 0; +} + +template +typename HierarchyTree::NodeType* HierarchyTree::mortonRecurse_0(const NodeVecIterator lbeg, const NodeVecIterator lend, const FCL_UINT32& split, int bits) +{ + int num_leaves = lend - lbeg; + if(num_leaves > 1) + { + if(bits > 0) + { + NodeType dummy; + dummy.code = split; + NodeVecIterator lcenter = std::lower_bound(lbeg, lend, &dummy, SortByMorton()); + + if(lcenter == lbeg) + { + FCL_UINT32 split2 = split | (1 << (bits - 1)); + return mortonRecurse_0(lbeg, lend, split2, bits - 1); + } + else if(lcenter == lend) + { + FCL_UINT32 split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); + return mortonRecurse_0(lbeg, lend, split1, bits - 1); + } + else + { + FCL_UINT32 split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); + FCL_UINT32 split2 = split | (1 << (bits - 1)); + + NodeType* child1 = mortonRecurse_0(lbeg, lcenter, split1, bits - 1); + NodeType* child2 = mortonRecurse_0(lcenter, lend, split2, bits - 1); + NodeType* node = createNode(nullptr, nullptr); + node->children[0] = child1; + node->children[1] = child2; + child1->parent = node; + child2->parent = node; + return node; + } + } + else + { + NodeType* node = topdown(lbeg, lend); + return node; + } + } + else + return *lbeg; } +template +typename HierarchyTree::NodeType* HierarchyTree::mortonRecurse_1(const NodeVecIterator lbeg, const NodeVecIterator lend, const FCL_UINT32& split, int bits) +{ + int num_leaves = lend - lbeg; + if(num_leaves > 1) + { + if(bits > 0) + { + NodeType dummy; + dummy.code = split; + NodeVecIterator lcenter = std::lower_bound(lbeg, lend, &dummy, SortByMorton()); + + if(lcenter == lbeg) + { + FCL_UINT32 split2 = split | (1 << (bits - 1)); + return mortonRecurse_1(lbeg, lend, split2, bits - 1); + } + else if(lcenter == lend) + { + FCL_UINT32 split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); + return mortonRecurse_1(lbeg, lend, split1, bits - 1); + } + else + { + FCL_UINT32 split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); + FCL_UINT32 split2 = split | (1 << (bits - 1)); + + NodeType* child1 = mortonRecurse_1(lbeg, lcenter, split1, bits - 1); + NodeType* child2 = mortonRecurse_1(lcenter, lend, split2, bits - 1); + NodeType* node = createNode(nullptr, nullptr); + node->children[0] = child1; + node->children[1] = child2; + child1->parent = node; + child2->parent = node; + return node; + } + } + else + { + NodeType* child1 = mortonRecurse_1(lbeg, lbeg + num_leaves / 2, 0, bits - 1); + NodeType* child2 = mortonRecurse_1(lbeg + num_leaves / 2, lend, 0, bits - 1); + NodeType* node = createNode(nullptr, nullptr); + node->children[0] = child1; + node->children[1] = child2; + child1->parent = node; + child2->parent = node; + return node; + } + } + else + return *lbeg; +} + +template +typename HierarchyTree::NodeType* HierarchyTree::mortonRecurse_2(const NodeVecIterator lbeg, const NodeVecIterator lend) +{ + int num_leaves = lend - lbeg; + if(num_leaves > 1) + { + NodeType* child1 = mortonRecurse_2(lbeg, lbeg + num_leaves / 2); + NodeType* child2 = mortonRecurse_2(lbeg + num_leaves / 2, lend); + NodeType* node = createNode(nullptr, nullptr); + node->children[0] = child1; + node->children[1] = child2; + child1->parent = node; + child2->parent = node; + return node; + } + else + return *lbeg; +} + + +template +void HierarchyTree::update_(NodeType* leaf, const BV& bv) +{ + NodeType* root = removeLeaf(leaf); + if(root) + { + if(max_lookahead_level >= 0) + { + for(int i = 0; (i < max_lookahead_level) && root->parent; ++i) + root = root->parent; + } + else + root = root_node; + } + + leaf->bv = bv; + insertLeaf(root, leaf); +} + +template +typename HierarchyTree::NodeType* HierarchyTree::sort(NodeType* n, NodeType*& r) +{ + NodeType* p = n->parent; + if(p > n) + { + int i = indexOf(n); + int j = 1 - i; + NodeType* s = p->children[j]; + NodeType* q = p->parent; + if(q) q->children[indexOf(p)] = n; else r = n; + s->parent = n; + p->parent = n; + n->parent = q; + p->children[0] = n->children[0]; + p->children[1] = n->children[1]; + n->children[0]->parent = p; + n->children[1]->parent = p; + n->children[i] = p; + n->children[j] = s; + std::swap(p->bv, n->bv); + return p; + } + return n; +} + +template +void HierarchyTree::insertLeaf(NodeType* root, NodeType* leaf) +{ + if(!root_node) + { + root_node = leaf; + leaf->parent = nullptr; + } + else + { + if(!root->isLeaf()) + { + do + { + root = root->children[select(*leaf, *(root->children[0]), *(root->children[1]))]; + } + while(!root->isLeaf()); + } + + NodeType* prev = root->parent; + NodeType* node = createNode(prev, leaf->bv, root->bv, nullptr); + if(prev) + { + prev->children[indexOf(root)] = node; + node->children[0] = root; root->parent = node; + node->children[1] = leaf; leaf->parent = node; + do + { + if(!prev->bv.contain(node->bv)) + prev->bv = prev->children[0]->bv + prev->children[1]->bv; + else + break; + node = prev; + } while (nullptr != (prev = node->parent)); + } + else + { + node->children[0] = root; root->parent = node; + node->children[1] = leaf; leaf->parent = node; + root_node = node; + } + } +} + +template +typename HierarchyTree::NodeType* HierarchyTree::removeLeaf(NodeType* leaf) +{ + if(leaf == root_node) + { + root_node = nullptr; + return nullptr; + } + else + { + NodeType* parent = leaf->parent; + NodeType* prev = parent->parent; + NodeType* sibling = parent->children[1-indexOf(leaf)]; + if(prev) + { + prev->children[indexOf(parent)] = sibling; + sibling->parent = prev; + deleteNode(parent); + while(prev) + { + BV new_bv = prev->children[0]->bv + prev->children[1]->bv; + if(!new_bv.equal(prev->bv)) + { + prev->bv = new_bv; + prev = prev->parent; + } + else break; + } + + return prev ? prev : root_node; + } + else + { + root_node = sibling; + sibling->parent = nullptr; + deleteNode(parent); + return root_node; + } + } +} + +template +void HierarchyTree::fetchLeaves(NodeType* root, std::vector& leaves, int depth) +{ + if((!root->isLeaf()) && depth) + { + fetchLeaves(root->children[0], leaves, depth-1); + fetchLeaves(root->children[1], leaves, depth-1); + deleteNode(root); + } + else + { + leaves.push_back(root); + } +} + + +template +size_t HierarchyTree::indexOf(NodeType* node) +{ + // node cannot be nullptr + return (node->parent->children[1] == node); +} + +template +typename HierarchyTree::NodeType* HierarchyTree::createNode(NodeType* parent, + const BV& bv, + void* data) +{ + NodeType* node = createNode(parent, data); + node->bv = bv; + return node; +} + +template +typename HierarchyTree::NodeType* HierarchyTree::createNode(NodeType* parent, + const BV& bv1, + const BV& bv2, + void* data) +{ + NodeType* node = createNode(parent, data); + node->bv = bv1 + bv2; + return node; +} + + +template +typename HierarchyTree::NodeType* HierarchyTree::createNode(NodeType* parent, void* data) +{ + NodeType* node = nullptr; + if(free_node) + { + node = free_node; + free_node = nullptr; + } + else + node = new NodeType(); + node->parent = parent; + node->data = data; + node->children[1] = 0; + return node; +} + +template +void HierarchyTree::deleteNode(NodeType* node) +{ + if(free_node != node) + { + delete free_node; + free_node = node; + } +} + +template +void HierarchyTree::recurseDeleteNode(NodeType* node) +{ + if(!node->isLeaf()) + { + recurseDeleteNode(node->children[0]); + recurseDeleteNode(node->children[1]); + } + + if(node == root_node) root_node = nullptr; + deleteNode(node); +} + +template +void HierarchyTree::recurseRefit(NodeType* node) +{ + if(!node->isLeaf()) + { + recurseRefit(node->children[0]); + recurseRefit(node->children[1]); + node->bv = node->children[0]->bv + node->children[1]->bv; + } + else + return; +} + +template +BV HierarchyTree::bounds(const std::vector& leaves) +{ + if(leaves.size() == 0) return BV(); + BV bv = leaves[0]->bv; + for(size_t i = 1; i < leaves.size(); ++i) + { + bv += leaves[i]->bv; + } + + return bv; +} + +template +BV HierarchyTree::bounds(const NodeVecIterator lbeg, const NodeVecIterator lend) +{ + if(lbeg == lend) return BV(); + BV bv = *lbeg; + for(NodeVecIterator it = lbeg + 1; it < lend; ++it) + { + bv += (*it)->bv; + } + + return bv; +} + +} + + +namespace fcl +{ +namespace implementation_array +{ + +template +HierarchyTree::HierarchyTree(int bu_threshold_, int topdown_level_) +{ + root_node = NULL_NODE; + n_nodes = 0; + n_nodes_alloc = 16; + nodes = new NodeType[n_nodes_alloc]; + for(size_t i = 0; i < n_nodes_alloc - 1; ++i) + nodes[i].next = i + 1; + nodes[n_nodes_alloc - 1].next = NULL_NODE; + n_leaves = 0; + freelist = 0; + opath = 0; + max_lookahead_level = -1; + bu_threshold = bu_threshold_; + topdown_level = topdown_level_; +} + +template +HierarchyTree::~HierarchyTree() +{ + delete [] nodes; +} + +template +void HierarchyTree::init(NodeType* leaves, int n_leaves_, int level) +{ + switch(level) + { + case 0: + init_0(leaves, n_leaves_); + break; + case 1: + init_1(leaves, n_leaves_); + break; + case 2: + init_2(leaves, n_leaves_); + break; + case 3: + init_3(leaves, n_leaves_); + break; + default: + init_0(leaves, n_leaves_); + } +} + +template +void HierarchyTree::init_0(NodeType* leaves, int n_leaves_) +{ + clear(); + + n_leaves = n_leaves_; + root_node = NULL_NODE; + nodes = new NodeType[n_leaves * 2]; + memcpy(nodes, leaves, sizeof(NodeType) * n_leaves); + freelist = n_leaves; + n_nodes = n_leaves; + n_nodes_alloc = 2 * n_leaves; + for(size_t i = n_leaves; i < n_nodes_alloc; ++i) + nodes[i].next = i + 1; + nodes[n_nodes_alloc - 1].next = NULL_NODE; + + size_t* ids = new size_t[n_leaves]; + for(size_t i = 0; i < n_leaves; ++i) + ids[i] = i; + + root_node = topdown(ids, ids + n_leaves); + delete [] ids; + + opath = 0; + max_lookahead_level = -1; +} + +template +void HierarchyTree::init_1(NodeType* leaves, int n_leaves_) +{ + clear(); + + n_leaves = n_leaves_; + root_node = NULL_NODE; + nodes = new NodeType[n_leaves * 2]; + memcpy(nodes, leaves, sizeof(NodeType) * n_leaves); + freelist = n_leaves; + n_nodes = n_leaves; + n_nodes_alloc = 2 * n_leaves; + for(size_t i = n_leaves; i < n_nodes_alloc; ++i) + nodes[i].next = i + 1; + nodes[n_nodes_alloc - 1].next = NULL_NODE; + + + BV bound_bv; + if(n_leaves > 0) + bound_bv = nodes[0].bv; + for(size_t i = 1; i < n_leaves; ++i) + bound_bv += nodes[i].bv; + + morton_functor coder(bound_bv); + for(size_t i = 0; i < n_leaves; ++i) + nodes[i].code = coder(nodes[i].bv.center()); + + + size_t* ids = new size_t[n_leaves]; + for(size_t i = 0; i < n_leaves; ++i) + ids[i] = i; + + FCL_SUPPRESS_MAYBE_UNINITIALIZED_BEGIN + SortByMorton comp; + FCL_SUPPRESS_MAYBE_UNINITIALIZED_END + comp.nodes = nodes; + std::sort(ids, ids + n_leaves, comp); + root_node = mortonRecurse_0(ids, ids + n_leaves, (1 << (coder.bits()-1)), coder.bits()-1); + delete [] ids; + + refit(); + + opath = 0; + max_lookahead_level = -1; +} + +template +void HierarchyTree::init_2(NodeType* leaves, int n_leaves_) +{ + clear(); + + n_leaves = n_leaves_; + root_node = NULL_NODE; + nodes = new NodeType[n_leaves * 2]; + memcpy(nodes, leaves, sizeof(NodeType) * n_leaves); + freelist = n_leaves; + n_nodes = n_leaves; + n_nodes_alloc = 2 * n_leaves; + for(size_t i = n_leaves; i < n_nodes_alloc; ++i) + nodes[i].next = i + 1; + nodes[n_nodes_alloc - 1].next = NULL_NODE; + + + BV bound_bv; + if(n_leaves > 0) + bound_bv = nodes[0].bv; + for(size_t i = 1; i < n_leaves; ++i) + bound_bv += nodes[i].bv; + + morton_functor coder(bound_bv); + for(size_t i = 0; i < n_leaves; ++i) + nodes[i].code = coder(nodes[i].bv.center()); + + + size_t* ids = new size_t[n_leaves]; + for(size_t i = 0; i < n_leaves; ++i) + ids[i] = i; + + FCL_SUPPRESS_MAYBE_UNINITIALIZED_BEGIN + SortByMorton comp; + FCL_SUPPRESS_MAYBE_UNINITIALIZED_END + comp.nodes = nodes; + std::sort(ids, ids + n_leaves, comp); + root_node = mortonRecurse_1(ids, ids + n_leaves, (1 << (coder.bits()-1)), coder.bits()-1); + delete [] ids; + + refit(); + + opath = 0; + max_lookahead_level = -1; +} + +template +void HierarchyTree::init_3(NodeType* leaves, int n_leaves_) +{ + clear(); + + n_leaves = n_leaves_; + root_node = NULL_NODE; + nodes = new NodeType[n_leaves * 2]; + memcpy(nodes, leaves, sizeof(NodeType) * n_leaves); + freelist = n_leaves; + n_nodes = n_leaves; + n_nodes_alloc = 2 * n_leaves; + for(size_t i = n_leaves; i < n_nodes_alloc; ++i) + nodes[i].next = i + 1; + nodes[n_nodes_alloc - 1].next = NULL_NODE; + + + BV bound_bv; + if(n_leaves > 0) + bound_bv = nodes[0].bv; + for(size_t i = 1; i < n_leaves; ++i) + bound_bv += nodes[i].bv; + + morton_functor coder(bound_bv); + for(size_t i = 0; i < n_leaves; ++i) + nodes[i].code = coder(nodes[i].bv.center()); + + + size_t* ids = new size_t[n_leaves]; + for(size_t i = 0; i < n_leaves; ++i) + ids[i] = i; + + FCL_SUPPRESS_MAYBE_UNINITIALIZED_BEGIN + SortByMorton comp; + FCL_SUPPRESS_MAYBE_UNINITIALIZED_END + comp.nodes = nodes; + std::sort(ids, ids + n_leaves, comp); + root_node = mortonRecurse_2(ids, ids + n_leaves); + delete [] ids; + + refit(); + + opath = 0; + max_lookahead_level = -1; +} + +template +size_t HierarchyTree::insert(const BV& bv, void* data) +{ + size_t node = createNode(NULL_NODE, bv, data); + insertLeaf(root_node, node); + ++n_leaves; + return node; +} + +template +void HierarchyTree::remove(size_t leaf) +{ + removeLeaf(leaf); + deleteNode(leaf); + --n_leaves; +} + +template +void HierarchyTree::clear() +{ + delete [] nodes; + root_node = NULL_NODE; + n_nodes = 0; + n_nodes_alloc = 16; + nodes = new NodeType[n_nodes_alloc]; + for(size_t i = 0; i < n_nodes_alloc; ++i) + nodes[i].next = i + 1; + nodes[n_nodes_alloc - 1].next = NULL_NODE; + n_leaves = 0; + freelist = 0; + opath = 0; + max_lookahead_level = -1; +} + +template +bool HierarchyTree::empty() const +{ + return (n_nodes == 0); +} + +template +void HierarchyTree::update(size_t leaf, int lookahead_level) +{ + size_t root = removeLeaf(leaf); + if(root != NULL_NODE) + { + if(lookahead_level > 0) + { + for(int i = 0; (i < lookahead_level) && (nodes[root].parent != NULL_NODE); ++i) + root = nodes[root].parent; + } + else + root = root_node; + } + insertLeaf(root, leaf); +} + +template +bool HierarchyTree::update(size_t leaf, const BV& bv) +{ + if(nodes[leaf].bv.contain(bv)) return false; + update_(leaf, bv); + return true; +} + +template +bool HierarchyTree::update(size_t leaf, const BV& bv, const Vector3& vel, S margin) +{ + if(nodes[leaf].bv.contain(bv)) return false; + update_(leaf, bv); + return true; +} + +template +bool HierarchyTree::update(size_t leaf, const BV& bv, const Vector3& vel) +{ + if(nodes[leaf].bv.contain(bv)) return false; + update_(leaf, bv); + return true; +} + +template +size_t HierarchyTree::getMaxHeight() const +{ + if(root_node == NULL_NODE) return 0; + + return getMaxHeight(root_node); +} + +template +size_t HierarchyTree::getMaxDepth() const +{ + if(root_node == NULL_NODE) return 0; + + size_t max_depth; + getMaxDepth(root_node, 0, max_depth); + return max_depth; +} + +template +void HierarchyTree::balanceBottomup() +{ + if(root_node != NULL_NODE) + { + NodeType* leaves = new NodeType[n_leaves]; + NodeType* leaves_ = leaves; + extractLeaves(root_node, leaves_); + root_node = NULL_NODE; + memcpy(nodes, leaves, sizeof(NodeType) * n_leaves); + freelist = n_leaves; + n_nodes = n_leaves; + for(size_t i = n_leaves; i < n_nodes_alloc; ++i) + nodes[i].next = i + 1; + nodes[n_nodes_alloc - 1].next = NULL_NODE; + + + size_t* ids = new size_t[n_leaves]; + for(size_t i = 0; i < n_leaves; ++i) + ids[i] = i; + + bottomup(ids, ids + n_leaves); + root_node = *ids; + + delete [] ids; + } +} + +template +void HierarchyTree::balanceTopdown() +{ + if(root_node != NULL_NODE) + { + NodeType* leaves = new NodeType[n_leaves]; + NodeType* leaves_ = leaves; + extractLeaves(root_node, leaves_); + root_node = NULL_NODE; + memcpy(nodes, leaves, sizeof(NodeType) * n_leaves); + freelist = n_leaves; + n_nodes = n_leaves; + for(size_t i = n_leaves; i < n_nodes_alloc; ++i) + nodes[i].next = i + 1; + nodes[n_nodes_alloc - 1].next = NULL_NODE; + + size_t* ids = new size_t[n_leaves]; + for(size_t i = 0; i < n_leaves; ++i) + ids[i] = i; + + root_node = topdown(ids, ids + n_leaves); + delete [] ids; + } +} + +template +void HierarchyTree::balanceIncremental(int iterations) +{ + if(iterations < 0) iterations = n_leaves; + if((root_node != NULL_NODE) && (iterations > 0)) + { + for(int i = 0; i < iterations; ++i) + { + size_t node = root_node; + unsigned int bit = 0; + while(!nodes[node].isLeaf()) + { + node = nodes[node].children[(opath>>bit)&1]; + bit = (bit+1)&(sizeof(unsigned int) * 8-1); + } + update(node); + ++opath; + } + } +} + +template +void HierarchyTree::refit() +{ + if(root_node != NULL_NODE) + recurseRefit(root_node); +} + +template +void HierarchyTree::extractLeaves(size_t root, NodeType*& leaves) const +{ + if(!nodes[root].isLeaf()) + { + extractLeaves(nodes[root].children[0], leaves); + extractLeaves(nodes[root].children[1], leaves); + } + else + { + *leaves = nodes[root]; + leaves++; + } +} + +template +size_t HierarchyTree::size() const +{ + return n_leaves; +} + +template +size_t HierarchyTree::getRoot() const +{ + return root_node; +} + +template +typename HierarchyTree::NodeType* HierarchyTree::getNodes() const +{ + return nodes; +} + +template +void HierarchyTree::print(size_t root, int depth) +{ + for(int i = 0; i < depth; ++i) + std::cout << " "; + NodeType* n = nodes + root; + std::cout << " (" << n->bv.min_[0] << ", " << n->bv.min_[1] << ", " << n->bv.min_[2] << "; " << n->bv.max_[0] << ", " << n->bv.max_[1] << ", " << n->bv.max_[2] << ")" << std::endl; + if(n->isLeaf()) + { + } + else + { + print(n->children[0], depth+1); + print(n->children[1], depth+1); + } +} + +template +size_t HierarchyTree::getMaxHeight(size_t node) const +{ + if(!nodes[node].isLeaf()) + { + size_t height1 = getMaxHeight(nodes[node].children[0]); + size_t height2 = getMaxHeight(nodes[node].children[1]); + return std::max(height1, height2) + 1; + } + else + return 0; +} + +template +void HierarchyTree::getMaxDepth(size_t node, size_t depth, size_t& max_depth) const +{ + if(!nodes[node].isLeaf()) + { + getMaxDepth(nodes[node].children[0], depth+1, max_depth); + getmaxDepth(nodes[node].children[1], depth+1, max_depth); + } + else + max_depth = std::max(max_depth, depth); +} + +template +void HierarchyTree::bottomup(size_t* lbeg, size_t* lend) +{ + size_t* lcur_end = lend; + while(lbeg < lcur_end - 1) + { + size_t* min_it1 = nullptr, *min_it2 = nullptr; + S min_size = std::numeric_limits::max(); + for(size_t* it1 = lbeg; it1 < lcur_end; ++it1) + { + for(size_t* it2 = it1 + 1; it2 < lcur_end; ++it2) + { + S cur_size = (nodes[*it1].bv + nodes[*it2].bv).size(); + if(cur_size < min_size) + { + min_size = cur_size; + min_it1 = it1; + min_it2 = it2; + } + } + } + + size_t p = createNode(NULL_NODE, nodes[*min_it1].bv, nodes[*min_it2].bv, nullptr); + nodes[p].children[0] = *min_it1; + nodes[p].children[1] = *min_it2; + nodes[*min_it1].parent = p; + nodes[*min_it2].parent = p; + *min_it1 = p; + size_t tmp = *min_it2; + lcur_end--; + *min_it2 = *lcur_end; + *lcur_end = tmp; + } +} + +template +size_t HierarchyTree::topdown(size_t* lbeg, size_t* lend) +{ + switch(topdown_level) + { + case 0: + return topdown_0(lbeg, lend); + break; + case 1: + return topdown_1(lbeg, lend); + break; + default: + return topdown_0(lbeg, lend); + } +} + +template +size_t HierarchyTree::topdown_0(size_t* lbeg, size_t* lend) +{ + int num_leaves = lend - lbeg; + if(num_leaves > 1) + { + if(num_leaves > bu_threshold) + { + BV vol = nodes[*lbeg].bv; + for(size_t* i = lbeg + 1; i < lend; ++i) + vol += nodes[*i].bv; + + int best_axis = 0; + S extent[3] = {vol.width(), vol.height(), vol.depth()}; + if(extent[1] > extent[0]) best_axis = 1; + if(extent[2] > extent[best_axis]) best_axis = 2; + + nodeBaseLess comp(nodes, best_axis); + size_t* lcenter = lbeg + num_leaves / 2; + std::nth_element(lbeg, lcenter, lend, comp); + + size_t node = createNode(NULL_NODE, vol, nullptr); + nodes[node].children[0] = topdown_0(lbeg, lcenter); + nodes[node].children[1] = topdown_0(lcenter, lend); + nodes[nodes[node].children[0]].parent = node; + nodes[nodes[node].children[1]].parent = node; + return node; + } + else + { + bottomup(lbeg, lend); + return *lbeg; + } + } + return *lbeg; +} + +template +size_t HierarchyTree::topdown_1(size_t* lbeg, size_t* lend) +{ + int num_leaves = lend - lbeg; + if(num_leaves > 1) + { + if(num_leaves > bu_threshold) + { + Vector3 split_p = nodes[*lbeg].bv.center(); + BV vol = nodes[*lbeg].bv; + for(size_t* i = lbeg + 1; i < lend; ++i) + { + split_p += nodes[*i].bv.center(); + vol += nodes[*i].bv; + } + split_p /= (S)(num_leaves); + int best_axis = -1; + int bestmidp = num_leaves; + int splitcount[3][2] = {{0,0}, {0,0}, {0,0}}; + for(size_t* i = lbeg; i < lend; ++i) + { + Vector3 x = nodes[*i].bv.center() - split_p; + for(size_t j = 0; j < 3; ++j) + ++splitcount[j][x[j] > 0 ? 1 : 0]; + } + + for(size_t i = 0; i < 3; ++i) + { + if((splitcount[i][0] > 0) && (splitcount[i][1] > 0)) + { + int midp = std::abs(splitcount[i][0] - splitcount[i][1]); + if(midp < bestmidp) + { + best_axis = i; + bestmidp = midp; + } + } + } + + if(best_axis < 0) best_axis = 0; + + S split_value = split_p[best_axis]; + size_t* lcenter = lbeg; + for(size_t* i = lbeg; i < lend; ++i) + { + if(nodes[*i].bv.center()[best_axis] < split_value) + { + size_t temp = *i; + *i = *lcenter; + *lcenter = temp; + ++lcenter; + } + } + + size_t node = createNode(NULL_NODE, vol, nullptr); + nodes[node].children[0] = topdown_1(lbeg, lcenter); + nodes[node].children[1] = topdown_1(lcenter, lend); + nodes[nodes[node].children[0]].parent = node; + nodes[nodes[node].children[1]].parent = node; + return node; + } + else + { + bottomup(lbeg, lend); + return *lbeg; + } + } + return *lbeg; +} + +template +size_t HierarchyTree::mortonRecurse_0(size_t* lbeg, size_t* lend, const FCL_UINT32& split, int bits) +{ + int num_leaves = lend - lbeg; + if(num_leaves > 1) + { + if(bits > 0) + { + SortByMorton comp; + comp.nodes = nodes; + comp.split = split; + size_t* lcenter = std::lower_bound(lbeg, lend, NULL_NODE, comp); + + if(lcenter == lbeg) + { + FCL_UINT32 split2 = split | (1 << (bits - 1)); + return mortonRecurse_0(lbeg, lend, split2, bits - 1); + } + else if(lcenter == lend) + { + FCL_UINT32 split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); + return mortonRecurse_0(lbeg, lend, split1, bits - 1); + } + else + { + FCL_UINT32 split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); + FCL_UINT32 split2 = split | (1 << (bits - 1)); + + size_t child1 = mortonRecurse_0(lbeg, lcenter, split1, bits - 1); + size_t child2 = mortonRecurse_0(lcenter, lend, split2, bits - 1); + size_t node = createNode(NULL_NODE, nullptr); + nodes[node].children[0] = child1; + nodes[node].children[1] = child2; + nodes[child1].parent = node; + nodes[child2].parent = node; + return node; + } + } + else + { + size_t node = topdown(lbeg, lend); + return node; + } + } + else + return *lbeg; +} + +template +size_t HierarchyTree::mortonRecurse_1(size_t* lbeg, size_t* lend, const FCL_UINT32& split, int bits) +{ + int num_leaves = lend - lbeg; + if(num_leaves > 1) + { + if(bits > 0) + { + SortByMorton comp; + comp.nodes = nodes; + comp.split = split; + size_t* lcenter = std::lower_bound(lbeg, lend, NULL_NODE, comp); + + if(lcenter == lbeg) + { + FCL_UINT32 split2 = split | (1 << (bits - 1)); + return mortonRecurse_1(lbeg, lend, split2, bits - 1); + } + else if(lcenter == lend) + { + FCL_UINT32 split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); + return mortonRecurse_1(lbeg, lend, split1, bits - 1); + } + else + { + FCL_UINT32 split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); + FCL_UINT32 split2 = split | (1 << (bits - 1)); + + size_t child1 = mortonRecurse_1(lbeg, lcenter, split1, bits - 1); + size_t child2 = mortonRecurse_1(lcenter, lend, split2, bits - 1); + size_t node = createNode(NULL_NODE, nullptr); + nodes[node].children[0] = child1; + nodes[node].children[1] = child2; + nodes[child1].parent = node; + nodes[child2].parent = node; + return node; + } + } + else + { + size_t child1 = mortonRecurse_1(lbeg, lbeg + num_leaves / 2, 0, bits - 1); + size_t child2 = mortonRecurse_1(lbeg + num_leaves / 2, lend, 0, bits - 1); + size_t node = createNode(NULL_NODE, nullptr); + nodes[node].children[0] = child1; + nodes[node].children[1] = child2; + nodes[child1].parent = node; + nodes[child2].parent = node; + return node; + } + } + else + return *lbeg; +} + +template +size_t HierarchyTree::mortonRecurse_2(size_t* lbeg, size_t* lend) +{ + int num_leaves = lend - lbeg; + if(num_leaves > 1) + { + size_t child1 = mortonRecurse_2(lbeg, lbeg + num_leaves / 2); + size_t child2 = mortonRecurse_2(lbeg + num_leaves / 2, lend); + size_t node = createNode(NULL_NODE, nullptr); + nodes[node].children[0] = child1; + nodes[node].children[1] = child2; + nodes[child1].parent = node; + nodes[child2].parent = node; + return node; + } + else + return *lbeg; +} + +template +void HierarchyTree::insertLeaf(size_t root, size_t leaf) +{ + if(root_node == NULL_NODE) + { + root_node = leaf; + nodes[leaf].parent = NULL_NODE; + } + else + { + if(!nodes[root].isLeaf()) + { + do + { + root = nodes[root].children[select(leaf, nodes[root].children[0], nodes[root].children[1], nodes)]; + } + while(!nodes[root].isLeaf()); + } + + size_t prev = nodes[root].parent; + size_t node = createNode(prev, nodes[leaf].bv, nodes[root].bv, nullptr); + if(prev != NULL_NODE) + { + nodes[prev].children[indexOf(root)] = node; + nodes[node].children[0] = root; nodes[root].parent = node; + nodes[node].children[1] = leaf; nodes[leaf].parent = node; + do + { + if(!nodes[prev].bv.contain(nodes[node].bv)) + nodes[prev].bv = nodes[nodes[prev].children[0]].bv + nodes[nodes[prev].children[1]].bv; + else + break; + node = prev; + } while (NULL_NODE != (prev = nodes[node].parent)); + } + else + { + nodes[node].children[0] = root; nodes[root].parent = node; + nodes[node].children[1] = leaf; nodes[leaf].parent = node; + root_node = node; + } + } +} + +template +size_t HierarchyTree::removeLeaf(size_t leaf) +{ + if(leaf == root_node) + { + root_node = NULL_NODE; + return NULL_NODE; + } + else + { + size_t parent = nodes[leaf].parent; + size_t prev = nodes[parent].parent; + size_t sibling = nodes[parent].children[1-indexOf(leaf)]; + + if(prev != NULL_NODE) + { + nodes[prev].children[indexOf(parent)] = sibling; + nodes[sibling].parent = prev; + deleteNode(parent); + while(prev != NULL_NODE) + { + BV new_bv = nodes[nodes[prev].children[0]].bv + nodes[nodes[prev].children[1]].bv; + if(!new_bv.equal(nodes[prev].bv)) + { + nodes[prev].bv = new_bv; + prev = nodes[prev].parent; + } + else break; + } + + return (prev != NULL_NODE) ? prev : root_node; + } + else + { + root_node = sibling; + nodes[sibling].parent = NULL_NODE; + deleteNode(parent); + return root_node; + } + } +} + +template +void HierarchyTree::update_(size_t leaf, const BV& bv) +{ + size_t root = removeLeaf(leaf); + if(root != NULL_NODE) + { + if(max_lookahead_level >= 0) + { + for(int i = 0; (i < max_lookahead_level) && (nodes[root].parent != NULL_NODE); ++i) + root = nodes[root].parent; + } + + nodes[leaf].bv = bv; + insertLeaf(root, leaf); + } +} + +template +size_t HierarchyTree::indexOf(size_t node) +{ + return (nodes[nodes[node].parent].children[1] == node); +} + +template +size_t HierarchyTree::allocateNode() +{ + if(freelist == NULL_NODE) + { + NodeType* old_nodes = nodes; + n_nodes_alloc *= 2; + nodes = new NodeType[n_nodes_alloc]; + memcpy(nodes, old_nodes, n_nodes * sizeof(NodeType)); + delete [] old_nodes; + + for(size_t i = n_nodes; i < n_nodes_alloc - 1; ++i) + nodes[i].next = i + 1; + nodes[n_nodes_alloc - 1].next = NULL_NODE; + freelist = n_nodes; + } + + size_t node_id = freelist; + freelist = nodes[node_id].next; + nodes[node_id].parent = NULL_NODE; + nodes[node_id].children[0] = NULL_NODE; + nodes[node_id].children[1] = NULL_NODE; + ++n_nodes; + return node_id; +} + +template +size_t HierarchyTree::createNode(size_t parent, + const BV& bv1, + const BV& bv2, + void* data) +{ + size_t node = allocateNode(); + nodes[node].parent = parent; + nodes[node].data = data; + nodes[node].bv = bv1 + bv2; + return node; +} + +template +size_t HierarchyTree::createNode(size_t parent, + const BV& bv, + void* data) +{ + size_t node = allocateNode(); + nodes[node].parent = parent; + nodes[node].data = data; + nodes[node].bv = bv; + return node; +} + +template +size_t HierarchyTree::createNode(size_t parent, + void* data) +{ + size_t node = allocateNode(); + nodes[node].parent = parent; + nodes[node].data = data; + return node; +} + +template +void HierarchyTree::deleteNode(size_t node) +{ + nodes[node].next = freelist; + freelist = node; + --n_nodes; +} + +template +void HierarchyTree::recurseRefit(size_t node) +{ + if(!nodes[node].isLeaf()) + { + recurseRefit(nodes[node].children[0]); + recurseRefit(nodes[node].children[1]); + nodes[node].bv = nodes[nodes[node].children[0]].bv + nodes[nodes[node].children[1]].bv; + } + else + return; +} + +template +void HierarchyTree::fetchLeaves(size_t root, NodeType*& leaves, int depth) +{ + if((!nodes[root].isLeaf()) && depth) + { + fetchLeaves(nodes[root].children[0], leaves, depth-1); + fetchLeaves(nodes[root].children[1], leaves, depth-1); + deleteNode(root); + } + else + { + *leaves = nodes[root]; + leaves++; + } +} + +} + +//============================================================================== +template +struct SelectImpl> +{ + static std::size_t run( + const NodeBase>& node, + const NodeBase>& node1, + const NodeBase>& node2) + { + const AABB& bv = node.bv; + const AABB& bv1 = node1.bv; + const AABB& bv2 = node2.bv; + Vector3 v = bv.min_ + bv.max_; + Vector3 v1 = v - (bv1.min_ + bv1.max_); + Vector3 v2 = v - (bv2.min_ + bv2.max_); + S d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]); + S d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]); + return (d1 < d2) ? 0 : 1; + } + + static std::size_t run( + const AABB& query, + const NodeBase>& node1, + const NodeBase>& node2) + { + const AABB& bv = query; + const AABB& bv1 = node1.bv; + const AABB& bv2 = node2.bv; + Vector3 v = bv.min_ + bv.max_; + Vector3 v1 = v - (bv1.min_ + bv1.max_); + Vector3 v2 = v - (bv2.min_ + bv2.max_); + S d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]); + S d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]); + return (d1 < d2) ? 0 : 1; + } + + static bool run( + const HierarchyTree>& tree, + typename HierarchyTree>::NodeType* leaf, + const AABB& bv_, + const Vector3& vel, + S margin) + { + AABB bv(bv_); + if(leaf->bv.contain(bv)) return false; + Vector3 marginv(margin); + bv.min_ -= marginv; + bv.max_ += marginv; + if(vel[0] > 0) bv.max_[0] += vel[0]; + else bv.min_[0] += vel[0]; + if(vel[1] > 0) bv.max_[1] += vel[1]; + else bv.min_[1] += vel[1]; + if(vel[2] > 0) bv.max_[2] += vel[2]; + else bv.max_[2] += vel[2]; + tree.update(leaf, bv); + return true; + } + + static bool run( + const HierarchyTree>& tree, + typename HierarchyTree>::NodeType* leaf, + const AABB& bv_, + const Vector3& vel) + { + AABB bv(bv_); + if(leaf->bv.contain(bv)) return false; + if(vel[0] > 0) bv.max_[0] += vel[0]; + else bv.min_[0] += vel[0]; + if(vel[1] > 0) bv.max_[1] += vel[1]; + else bv.min_[1] += vel[1]; + if(vel[2] > 0) bv.max_[2] += vel[2]; + else bv.max_[2] += vel[2]; + tree.update(leaf, bv); + return true; + } +}; + +namespace implementation_array +{ + +//============================================================================== +template +struct SelectImpl> +{ + static bool run(size_t query, size_t node1, size_t node2, NodeBase>* nodes) + { + const AABB& bv = nodes[query].bv; + const AABB& bv1 = nodes[node1].bv; + const AABB& bv2 = nodes[node2].bv; + Vector3 v = bv.min_ + bv.max_; + Vector3 v1 = v - (bv1.min_ + bv1.max_); + Vector3 v2 = v - (bv2.min_ + bv2.max_); + S d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]); + S d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]); + return (d1 < d2) ? 0 : 1; + } + + static bool run(const AABB& query, size_t node1, size_t node2, NodeBase>* nodes) + { + const AABB& bv = query; + const AABB& bv1 = nodes[node1].bv; + const AABB& bv2 = nodes[node2].bv; + Vector3 v = bv.min_ + bv.max_; + Vector3 v1 = v - (bv1.min_ + bv1.max_); + Vector3 v2 = v - (bv2.min_ + bv2.max_); + S d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]); + S d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]); + return (d1 < d2) ? 0 : 1; + } +}; -#include "fcl/broadphase/hierarchy_tree.hxx" +} // namespace implementation_array +} // namespace fcl #endif diff --git a/include/fcl/broadphase/hierarchy_tree.hxx b/include/fcl/broadphase/hierarchy_tree.hxx deleted file mode 100644 index e6c164bd4..000000000 --- a/include/fcl/broadphase/hierarchy_tree.hxx +++ /dev/null @@ -1,1879 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#include - -namespace fcl -{ - -template -HierarchyTree::HierarchyTree(int bu_threshold_, int topdown_level_) -{ - root_node = NULL; - n_leaves = 0; - free_node = NULL; - max_lookahead_level = -1; - opath = 0; - bu_threshold = bu_threshold_; - topdown_level = topdown_level_; -} - -template -HierarchyTree::~HierarchyTree() -{ - clear(); -} - -template -void HierarchyTree::init(std::vector& leaves, int level) -{ - switch(level) - { - case 0: - init_0(leaves); - break; - case 1: - init_1(leaves); - break; - case 2: - init_2(leaves); - break; - case 3: - init_3(leaves); - break; - default: - init_0(leaves); - } -} - -template -typename HierarchyTree::NodeType* HierarchyTree::insert(const BV& bv, void* data) -{ - NodeType* leaf = createNode(NULL, bv, data); - insertLeaf(root_node, leaf); - ++n_leaves; - return leaf; -} - -template -void HierarchyTree::remove(NodeType* leaf) -{ - removeLeaf(leaf); - deleteNode(leaf); - --n_leaves; -} - -template -void HierarchyTree::clear() -{ - if(root_node) - recurseDeleteNode(root_node); - n_leaves = 0; - delete free_node; - free_node = NULL; - max_lookahead_level = -1; - opath = 0; -} - -template -bool HierarchyTree::empty() const -{ - return (NULL == root_node); -} - -template -void HierarchyTree::update(NodeType* leaf, int lookahead_level) -{ - NodeType* root = removeLeaf(leaf); - if(root) - { - if(lookahead_level > 0) - { - for(int i = 0; (i < lookahead_level) && root->parent; ++i) - root = root->parent; - } - else - root = root_node; - } - insertLeaf(root, leaf); -} - -template -bool HierarchyTree::update(NodeType* leaf, const BV& bv) -{ - if(leaf->bv.contain(bv)) return false; - update_(leaf, bv); - return true; -} - -template -bool HierarchyTree::update(NodeType* leaf, const BV& bv, const Vector3d& vel, FCL_REAL margin) -{ - if(leaf->bv.contain(bv)) return false; - update_(leaf, bv); - return true; -} - -template -bool HierarchyTree::update(NodeType* leaf, const BV& bv, const Vector3d& vel) -{ - if(leaf->bv.contain(bv)) return false; - update_(leaf, bv); - return true; -} - -template -size_t HierarchyTree::getMaxHeight() const -{ - if(!root_node) - return 0; - return getMaxHeight(root_node); -} - -template -size_t HierarchyTree::getMaxDepth() const -{ - if(!root_node) return 0; - - size_t max_depth; - getMaxDepth(root_node, 0, max_depth); - return max_depth; -} - -template -void HierarchyTree::balanceBottomup() -{ - if(root_node) - { - std::vector leaves; - leaves.reserve(n_leaves); - fetchLeaves(root_node, leaves); - bottomup(leaves.begin(), leaves.end()); - root_node = leaves[0]; - } -} - -template -void HierarchyTree::balanceTopdown() -{ - if(root_node) - { - std::vector leaves; - leaves.reserve(n_leaves); - fetchLeaves(root_node, leaves); - root_node = topdown(leaves.begin(), leaves.end()); - } -} - -template -void HierarchyTree::balanceIncremental(int iterations) -{ - if(iterations < 0) iterations = n_leaves; - if(root_node && (iterations > 0)) - { - for(int i = 0; i < iterations; ++i) - { - NodeType* node = root_node; - unsigned int bit = 0; - while(!node->isLeaf()) - { - node = sort(node, root_node)->children[(opath>>bit)&1]; - bit = (bit+1)&(sizeof(unsigned int) * 8-1); - } - update(node); - ++opath; - } - } -} - -template -void HierarchyTree::refit() -{ - if(root_node) - recurseRefit(root_node); -} - -template -void HierarchyTree::extractLeaves(const NodeType* root, std::vector& leaves) const -{ - if(!root->isLeaf()) - { - extractLeaves(root->children[0], leaves); - extractLeaves(root->children[1], leaves); - } - else - leaves.push_back(root); -} - -template -size_t HierarchyTree::size() const -{ - return n_leaves; -} - -template -typename HierarchyTree::NodeType* HierarchyTree::getRoot() const -{ - return root_node; -} - -template -typename HierarchyTree::NodeType*& HierarchyTree::getRoot() -{ - return root_node; -} - -template -void HierarchyTree::print(NodeType* root, int depth) -{ - for(int i = 0; i < depth; ++i) - std::cout << " "; - std::cout << " (" << root->bv.min_[0] << ", " << root->bv.min_[1] << ", " << root->bv.min_[2] << "; " << root->bv.max_[0] << ", " << root->bv.max_[1] << ", " << root->bv.max_[2] << ")" << std::endl; - if(root->isLeaf()) - { - } - else - { - print(root->children[0], depth+1); - print(root->children[1], depth+1); - } -} - - - -template -void HierarchyTree::bottomup(const NodeVecIterator lbeg, const NodeVecIterator lend) -{ - NodeVecIterator lcur_end = lend; - while(lbeg < lcur_end - 1) - { - NodeVecIterator min_it1, min_it2; - FCL_REAL min_size = std::numeric_limits::max(); - for(NodeVecIterator it1 = lbeg; it1 < lcur_end; ++it1) - { - for(NodeVecIterator it2 = it1 + 1; it2 < lcur_end; ++it2) - { - FCL_REAL cur_size = ((*it1)->bv + (*it2)->bv).size(); - if(cur_size < min_size) - { - min_size = cur_size; - min_it1 = it1; - min_it2 = it2; - } - } - } - - NodeType* n[2] = {*min_it1, *min_it2}; - NodeType* p = createNode(NULL, n[0]->bv, n[1]->bv, NULL); - p->children[0] = n[0]; - p->children[1] = n[1]; - n[0]->parent = p; - n[1]->parent = p; - *min_it1 = p; - NodeType* tmp = *min_it2; - lcur_end--; - *min_it2 = *lcur_end; - *lcur_end = tmp; - } -} - -template -typename HierarchyTree::NodeType* HierarchyTree::topdown(const NodeVecIterator lbeg, const NodeVecIterator lend) -{ - switch(topdown_level) - { - case 0: - return topdown_0(lbeg, lend); - break; - case 1: - return topdown_1(lbeg, lend); - break; - default: - return topdown_0(lbeg, lend); - } -} - -template -size_t HierarchyTree::getMaxHeight(NodeType* node) const -{ - if(!node->isLeaf()) - { - size_t height1 = getMaxHeight(node->children[0]); - size_t height2 = getMaxHeight(node->children[1]); - return std::max(height1, height2) + 1; - } - else - return 0; -} - -template -void HierarchyTree::getMaxDepth(NodeType* node, size_t depth, size_t& max_depth) const -{ - if(!node->isLeaf()) - { - getMaxDepth(node->children[0], depth+1, max_depth); - getMaxDepth(node->children[1], depth+1, max_depth); - } - else - max_depth = std::max(max_depth, depth); -} - -template -typename HierarchyTree::NodeType* HierarchyTree::topdown_0(const NodeVecIterator lbeg, const NodeVecIterator lend) -{ - int num_leaves = lend - lbeg; - if(num_leaves > 1) - { - if(num_leaves > bu_threshold) - { - BV vol = (*lbeg)->bv; - for(NodeVecIterator it = lbeg + 1; it < lend; ++it) - vol += (*it)->bv; - - int best_axis = 0; - FCL_REAL extent[3] = {vol.width(), vol.height(), vol.depth()}; - if(extent[1] > extent[0]) best_axis = 1; - if(extent[2] > extent[best_axis]) best_axis = 2; - - // compute median - NodeVecIterator lcenter = lbeg + num_leaves / 2; - std::nth_element(lbeg, lcenter, lend, std::bind(&nodeBaseLess, std::placeholders::_1, std::placeholders::_2, std::ref(best_axis))); - - NodeType* node = createNode(NULL, vol, NULL); - node->children[0] = topdown_0(lbeg, lcenter); - node->children[1] = topdown_0(lcenter, lend); - node->children[0]->parent = node; - node->children[1]->parent = node; - return node; - } - else - { - bottomup(lbeg, lend); - return *lbeg; - } - } - return *lbeg; -} - -template -typename HierarchyTree::NodeType* HierarchyTree::topdown_1(const NodeVecIterator lbeg, const NodeVecIterator lend) -{ - int num_leaves = lend - lbeg; - if(num_leaves > 1) - { - if(num_leaves > bu_threshold) - { - Vector3d split_p = (*lbeg)->bv.center(); - BV vol = (*lbeg)->bv; - NodeVecIterator it; - for(it = lbeg + 1; it < lend; ++it) - { - split_p += (*it)->bv.center(); - vol += (*it)->bv; - } - split_p /= (FCL_REAL)(num_leaves); - int best_axis = -1; - int bestmidp = num_leaves; - int splitcount[3][2] = {{0,0}, {0,0}, {0,0}}; - for(it = lbeg; it < lend; ++it) - { - Vector3d x = (*it)->bv.center() - split_p; - for(size_t j = 0; j < 3; ++j) - ++splitcount[j][x[j] > 0 ? 1 : 0]; - } - - for(size_t i = 0; i < 3; ++i) - { - if((splitcount[i][0] > 0) && (splitcount[i][1] > 0)) - { - int midp = std::abs(splitcount[i][0] - splitcount[i][1]); - if(midp < bestmidp) - { - best_axis = i; - bestmidp = midp; - } - } - } - - if(best_axis < 0) best_axis = 0; - - FCL_REAL split_value = split_p[best_axis]; - NodeVecIterator lcenter = lbeg; - for(it = lbeg; it < lend; ++it) - { - if((*it)->bv.center()[best_axis] < split_value) - { - NodeType* temp = *it; - *it = *lcenter; - *lcenter = temp; - ++lcenter; - } - } - - NodeType* node = createNode(NULL, vol, NULL); - node->children[0] = topdown_1(lbeg, lcenter); - node->children[1] = topdown_1(lcenter, lend); - node->children[0]->parent = node; - node->children[1]->parent = node; - return node; - } - else - { - bottomup(lbeg, lend); - return *lbeg; - } - } - return *lbeg; -} - -template -void HierarchyTree::init_0(std::vector& leaves) -{ - clear(); - root_node = topdown(leaves.begin(), leaves.end()); - n_leaves = leaves.size(); - max_lookahead_level = -1; - opath = 0; -} - -template -void HierarchyTree::init_1(std::vector& leaves) -{ - clear(); - - BV bound_bv; - if(leaves.size() > 0) - bound_bv = leaves[0]->bv; - for(size_t i = 1; i < leaves.size(); ++i) - bound_bv += leaves[i]->bv; - - morton_functor coder(bound_bv); - for(size_t i = 0; i < leaves.size(); ++i) - leaves[i]->code = coder(leaves[i]->bv.center()); - - std::sort(leaves.begin(), leaves.end(), SortByMorton()); - - root_node = mortonRecurse_0(leaves.begin(), leaves.end(), (1 << (coder.bits()-1)), coder.bits()-1); - - refit(); - n_leaves = leaves.size(); - max_lookahead_level = -1; - opath = 0; -} - -template -void HierarchyTree::init_2(std::vector& leaves) -{ - clear(); - - BV bound_bv; - if(leaves.size() > 0) - bound_bv = leaves[0]->bv; - for(size_t i = 1; i < leaves.size(); ++i) - bound_bv += leaves[i]->bv; - - morton_functor coder(bound_bv); - for(size_t i = 0; i < leaves.size(); ++i) - leaves[i]->code = coder(leaves[i]->bv.center()); - - std::sort(leaves.begin(), leaves.end(), SortByMorton()); - - root_node = mortonRecurse_1(leaves.begin(), leaves.end(), (1 << (coder.bits()-1)), coder.bits()-1); - - refit(); - n_leaves = leaves.size(); - max_lookahead_level = -1; - opath = 0; -} - -template -void HierarchyTree::init_3(std::vector& leaves) -{ - clear(); - - BV bound_bv; - if(leaves.size() > 0) - bound_bv = leaves[0]->bv; - for(size_t i = 1; i < leaves.size(); ++i) - bound_bv += leaves[i]->bv; - - morton_functor coder(bound_bv); - for(size_t i = 0; i < leaves.size(); ++i) - leaves[i]->code = coder(leaves[i]->bv.center()); - - std::sort(leaves.begin(), leaves.end(), SortByMorton()); - - root_node = mortonRecurse_2(leaves.begin(), leaves.end()); - - refit(); - n_leaves = leaves.size(); - max_lookahead_level = -1; - opath = 0; -} - -template -typename HierarchyTree::NodeType* HierarchyTree::mortonRecurse_0(const NodeVecIterator lbeg, const NodeVecIterator lend, const FCL_UINT32& split, int bits) -{ - int num_leaves = lend - lbeg; - if(num_leaves > 1) - { - if(bits > 0) - { - NodeType dummy; - dummy.code = split; - NodeVecIterator lcenter = std::lower_bound(lbeg, lend, &dummy, SortByMorton()); - - if(lcenter == lbeg) - { - FCL_UINT32 split2 = split | (1 << (bits - 1)); - return mortonRecurse_0(lbeg, lend, split2, bits - 1); - } - else if(lcenter == lend) - { - FCL_UINT32 split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); - return mortonRecurse_0(lbeg, lend, split1, bits - 1); - } - else - { - FCL_UINT32 split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); - FCL_UINT32 split2 = split | (1 << (bits - 1)); - - NodeType* child1 = mortonRecurse_0(lbeg, lcenter, split1, bits - 1); - NodeType* child2 = mortonRecurse_0(lcenter, lend, split2, bits - 1); - NodeType* node = createNode(NULL, NULL); - node->children[0] = child1; - node->children[1] = child2; - child1->parent = node; - child2->parent = node; - return node; - } - } - else - { - NodeType* node = topdown(lbeg, lend); - return node; - } - } - else - return *lbeg; -} - -template -typename HierarchyTree::NodeType* HierarchyTree::mortonRecurse_1(const NodeVecIterator lbeg, const NodeVecIterator lend, const FCL_UINT32& split, int bits) -{ - int num_leaves = lend - lbeg; - if(num_leaves > 1) - { - if(bits > 0) - { - NodeType dummy; - dummy.code = split; - NodeVecIterator lcenter = std::lower_bound(lbeg, lend, &dummy, SortByMorton()); - - if(lcenter == lbeg) - { - FCL_UINT32 split2 = split | (1 << (bits - 1)); - return mortonRecurse_1(lbeg, lend, split2, bits - 1); - } - else if(lcenter == lend) - { - FCL_UINT32 split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); - return mortonRecurse_1(lbeg, lend, split1, bits - 1); - } - else - { - FCL_UINT32 split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); - FCL_UINT32 split2 = split | (1 << (bits - 1)); - - NodeType* child1 = mortonRecurse_1(lbeg, lcenter, split1, bits - 1); - NodeType* child2 = mortonRecurse_1(lcenter, lend, split2, bits - 1); - NodeType* node = createNode(NULL, NULL); - node->children[0] = child1; - node->children[1] = child2; - child1->parent = node; - child2->parent = node; - return node; - } - } - else - { - NodeType* child1 = mortonRecurse_1(lbeg, lbeg + num_leaves / 2, 0, bits - 1); - NodeType* child2 = mortonRecurse_1(lbeg + num_leaves / 2, lend, 0, bits - 1); - NodeType* node = createNode(NULL, NULL); - node->children[0] = child1; - node->children[1] = child2; - child1->parent = node; - child2->parent = node; - return node; - } - } - else - return *lbeg; -} - -template -typename HierarchyTree::NodeType* HierarchyTree::mortonRecurse_2(const NodeVecIterator lbeg, const NodeVecIterator lend) -{ - int num_leaves = lend - lbeg; - if(num_leaves > 1) - { - NodeType* child1 = mortonRecurse_2(lbeg, lbeg + num_leaves / 2); - NodeType* child2 = mortonRecurse_2(lbeg + num_leaves / 2, lend); - NodeType* node = createNode(NULL, NULL); - node->children[0] = child1; - node->children[1] = child2; - child1->parent = node; - child2->parent = node; - return node; - } - else - return *lbeg; -} - - -template -void HierarchyTree::update_(NodeType* leaf, const BV& bv) -{ - NodeType* root = removeLeaf(leaf); - if(root) - { - if(max_lookahead_level >= 0) - { - for(int i = 0; (i < max_lookahead_level) && root->parent; ++i) - root = root->parent; - } - else - root = root_node; - } - - leaf->bv = bv; - insertLeaf(root, leaf); -} - -template -typename HierarchyTree::NodeType* HierarchyTree::sort(NodeType* n, NodeType*& r) -{ - NodeType* p = n->parent; - if(p > n) - { - int i = indexOf(n); - int j = 1 - i; - NodeType* s = p->children[j]; - NodeType* q = p->parent; - if(q) q->children[indexOf(p)] = n; else r = n; - s->parent = n; - p->parent = n; - n->parent = q; - p->children[0] = n->children[0]; - p->children[1] = n->children[1]; - n->children[0]->parent = p; - n->children[1]->parent = p; - n->children[i] = p; - n->children[j] = s; - std::swap(p->bv, n->bv); - return p; - } - return n; -} - -template -void HierarchyTree::insertLeaf(NodeType* root, NodeType* leaf) -{ - if(!root_node) - { - root_node = leaf; - leaf->parent = NULL; - } - else - { - if(!root->isLeaf()) - { - do - { - root = root->children[select(*leaf, *(root->children[0]), *(root->children[1]))]; - } - while(!root->isLeaf()); - } - - NodeType* prev = root->parent; - NodeType* node = createNode(prev, leaf->bv, root->bv, NULL); - if(prev) - { - prev->children[indexOf(root)] = node; - node->children[0] = root; root->parent = node; - node->children[1] = leaf; leaf->parent = node; - do - { - if(!prev->bv.contain(node->bv)) - prev->bv = prev->children[0]->bv + prev->children[1]->bv; - else - break; - node = prev; - } while (NULL != (prev = node->parent)); - } - else - { - node->children[0] = root; root->parent = node; - node->children[1] = leaf; leaf->parent = node; - root_node = node; - } - } -} - -template -typename HierarchyTree::NodeType* HierarchyTree::removeLeaf(NodeType* leaf) -{ - if(leaf == root_node) - { - root_node = NULL; - return NULL; - } - else - { - NodeType* parent = leaf->parent; - NodeType* prev = parent->parent; - NodeType* sibling = parent->children[1-indexOf(leaf)]; - if(prev) - { - prev->children[indexOf(parent)] = sibling; - sibling->parent = prev; - deleteNode(parent); - while(prev) - { - BV new_bv = prev->children[0]->bv + prev->children[1]->bv; - if(!new_bv.equal(prev->bv)) - { - prev->bv = new_bv; - prev = prev->parent; - } - else break; - } - - return prev ? prev : root_node; - } - else - { - root_node = sibling; - sibling->parent = NULL; - deleteNode(parent); - return root_node; - } - } -} - -template -void HierarchyTree::fetchLeaves(NodeType* root, std::vector& leaves, int depth) -{ - if((!root->isLeaf()) && depth) - { - fetchLeaves(root->children[0], leaves, depth-1); - fetchLeaves(root->children[1], leaves, depth-1); - deleteNode(root); - } - else - { - leaves.push_back(root); - } -} - - -template -size_t HierarchyTree::indexOf(NodeType* node) -{ - // node cannot be NULL - return (node->parent->children[1] == node); -} - -template -typename HierarchyTree::NodeType* HierarchyTree::createNode(NodeType* parent, - const BV& bv, - void* data) -{ - NodeType* node = createNode(parent, data); - node->bv = bv; - return node; -} - -template -typename HierarchyTree::NodeType* HierarchyTree::createNode(NodeType* parent, - const BV& bv1, - const BV& bv2, - void* data) -{ - NodeType* node = createNode(parent, data); - node->bv = bv1 + bv2; - return node; -} - - -template -typename HierarchyTree::NodeType* HierarchyTree::createNode(NodeType* parent, void* data) -{ - NodeType* node = NULL; - if(free_node) - { - node = free_node; - free_node = NULL; - } - else - node = new NodeType(); - node->parent = parent; - node->data = data; - node->children[1] = 0; - return node; -} - -template -void HierarchyTree::deleteNode(NodeType* node) -{ - if(free_node != node) - { - delete free_node; - free_node = node; - } -} - -template -void HierarchyTree::recurseDeleteNode(NodeType* node) -{ - if(!node->isLeaf()) - { - recurseDeleteNode(node->children[0]); - recurseDeleteNode(node->children[1]); - } - - if(node == root_node) root_node = NULL; - deleteNode(node); -} - -template -void HierarchyTree::recurseRefit(NodeType* node) -{ - if(!node->isLeaf()) - { - recurseRefit(node->children[0]); - recurseRefit(node->children[1]); - node->bv = node->children[0]->bv + node->children[1]->bv; - } - else - return; -} - -template -BV HierarchyTree::bounds(const std::vector& leaves) -{ - if(leaves.size() == 0) return BV(); - BV bv = leaves[0]->bv; - for(size_t i = 1; i < leaves.size(); ++i) - { - bv += leaves[i]->bv; - } - - return bv; -} - -template -BV HierarchyTree::bounds(const NodeVecIterator lbeg, const NodeVecIterator lend) -{ - if(lbeg == lend) return BV(); - BV bv = *lbeg; - for(NodeVecIterator it = lbeg + 1; it < lend; ++it) - { - bv += (*it)->bv; - } - - return bv; -} - -} - - -namespace fcl -{ -namespace implementation_array -{ - -template -HierarchyTree::HierarchyTree(int bu_threshold_, int topdown_level_) -{ - root_node = NULL_NODE; - n_nodes = 0; - n_nodes_alloc = 16; - nodes = new NodeType[n_nodes_alloc]; - for(size_t i = 0; i < n_nodes_alloc - 1; ++i) - nodes[i].next = i + 1; - nodes[n_nodes_alloc - 1].next = NULL_NODE; - n_leaves = 0; - freelist = 0; - opath = 0; - max_lookahead_level = -1; - bu_threshold = bu_threshold_; - topdown_level = topdown_level_; -} - -template -HierarchyTree::~HierarchyTree() -{ - delete [] nodes; -} - -template -void HierarchyTree::init(NodeType* leaves, int n_leaves_, int level) -{ - switch(level) - { - case 0: - init_0(leaves, n_leaves_); - break; - case 1: - init_1(leaves, n_leaves_); - break; - case 2: - init_2(leaves, n_leaves_); - break; - case 3: - init_3(leaves, n_leaves_); - break; - default: - init_0(leaves, n_leaves_); - } -} - -template -void HierarchyTree::init_0(NodeType* leaves, int n_leaves_) -{ - clear(); - - n_leaves = n_leaves_; - root_node = NULL_NODE; - nodes = new NodeType[n_leaves * 2]; - memcpy(nodes, leaves, sizeof(NodeType) * n_leaves); - freelist = n_leaves; - n_nodes = n_leaves; - n_nodes_alloc = 2 * n_leaves; - for(size_t i = n_leaves; i < n_nodes_alloc; ++i) - nodes[i].next = i + 1; - nodes[n_nodes_alloc - 1].next = NULL_NODE; - - size_t* ids = new size_t[n_leaves]; - for(size_t i = 0; i < n_leaves; ++i) - ids[i] = i; - - root_node = topdown(ids, ids + n_leaves); - delete [] ids; - - opath = 0; - max_lookahead_level = -1; -} - -template -void HierarchyTree::init_1(NodeType* leaves, int n_leaves_) -{ - clear(); - - n_leaves = n_leaves_; - root_node = NULL_NODE; - nodes = new NodeType[n_leaves * 2]; - memcpy(nodes, leaves, sizeof(NodeType) * n_leaves); - freelist = n_leaves; - n_nodes = n_leaves; - n_nodes_alloc = 2 * n_leaves; - for(size_t i = n_leaves; i < n_nodes_alloc; ++i) - nodes[i].next = i + 1; - nodes[n_nodes_alloc - 1].next = NULL_NODE; - - - BV bound_bv; - if(n_leaves > 0) - bound_bv = nodes[0].bv; - for(size_t i = 1; i < n_leaves; ++i) - bound_bv += nodes[i].bv; - - morton_functor coder(bound_bv); - for(size_t i = 0; i < n_leaves; ++i) - nodes[i].code = coder(nodes[i].bv.center()); - - - size_t* ids = new size_t[n_leaves]; - for(size_t i = 0; i < n_leaves; ++i) - ids[i] = i; - - SortByMorton comp; - comp.nodes = nodes; - std::sort(ids, ids + n_leaves, comp); - root_node = mortonRecurse_0(ids, ids + n_leaves, (1 << (coder.bits()-1)), coder.bits()-1); - delete [] ids; - - refit(); - - opath = 0; - max_lookahead_level = -1; -} - -template -void HierarchyTree::init_2(NodeType* leaves, int n_leaves_) -{ - clear(); - - n_leaves = n_leaves_; - root_node = NULL_NODE; - nodes = new NodeType[n_leaves * 2]; - memcpy(nodes, leaves, sizeof(NodeType) * n_leaves); - freelist = n_leaves; - n_nodes = n_leaves; - n_nodes_alloc = 2 * n_leaves; - for(size_t i = n_leaves; i < n_nodes_alloc; ++i) - nodes[i].next = i + 1; - nodes[n_nodes_alloc - 1].next = NULL_NODE; - - - BV bound_bv; - if(n_leaves > 0) - bound_bv = nodes[0].bv; - for(size_t i = 1; i < n_leaves; ++i) - bound_bv += nodes[i].bv; - - morton_functor coder(bound_bv); - for(size_t i = 0; i < n_leaves; ++i) - nodes[i].code = coder(nodes[i].bv.center()); - - - size_t* ids = new size_t[n_leaves]; - for(size_t i = 0; i < n_leaves; ++i) - ids[i] = i; - - SortByMorton comp; - comp.nodes = nodes; - std::sort(ids, ids + n_leaves, comp); - root_node = mortonRecurse_1(ids, ids + n_leaves, (1 << (coder.bits()-1)), coder.bits()-1); - delete [] ids; - - refit(); - - opath = 0; - max_lookahead_level = -1; -} - -template -void HierarchyTree::init_3(NodeType* leaves, int n_leaves_) -{ - clear(); - - n_leaves = n_leaves_; - root_node = NULL_NODE; - nodes = new NodeType[n_leaves * 2]; - memcpy(nodes, leaves, sizeof(NodeType) * n_leaves); - freelist = n_leaves; - n_nodes = n_leaves; - n_nodes_alloc = 2 * n_leaves; - for(size_t i = n_leaves; i < n_nodes_alloc; ++i) - nodes[i].next = i + 1; - nodes[n_nodes_alloc - 1].next = NULL_NODE; - - - BV bound_bv; - if(n_leaves > 0) - bound_bv = nodes[0].bv; - for(size_t i = 1; i < n_leaves; ++i) - bound_bv += nodes[i].bv; - - morton_functor coder(bound_bv); - for(size_t i = 0; i < n_leaves; ++i) - nodes[i].code = coder(nodes[i].bv.center()); - - - size_t* ids = new size_t[n_leaves]; - for(size_t i = 0; i < n_leaves; ++i) - ids[i] = i; - - SortByMorton comp; - comp.nodes = nodes; - std::sort(ids, ids + n_leaves, comp); - root_node = mortonRecurse_2(ids, ids + n_leaves); - delete [] ids; - - refit(); - - opath = 0; - max_lookahead_level = -1; -} - -template -size_t HierarchyTree::insert(const BV& bv, void* data) -{ - size_t node = createNode(NULL_NODE, bv, data); - insertLeaf(root_node, node); - ++n_leaves; - return node; -} - -template -void HierarchyTree::remove(size_t leaf) -{ - removeLeaf(leaf); - deleteNode(leaf); - --n_leaves; -} - -template -void HierarchyTree::clear() -{ - delete [] nodes; - root_node = NULL_NODE; - n_nodes = 0; - n_nodes_alloc = 16; - nodes = new NodeType[n_nodes_alloc]; - for(size_t i = 0; i < n_nodes_alloc; ++i) - nodes[i].next = i + 1; - nodes[n_nodes_alloc - 1].next = NULL_NODE; - n_leaves = 0; - freelist = 0; - opath = 0; - max_lookahead_level = -1; -} - -template -bool HierarchyTree::empty() const -{ - return (n_nodes == 0); -} - -template -void HierarchyTree::update(size_t leaf, int lookahead_level) -{ - size_t root = removeLeaf(leaf); - if(root != NULL_NODE) - { - if(lookahead_level > 0) - { - for(int i = 0; (i < lookahead_level) && (nodes[root].parent != NULL_NODE); ++i) - root = nodes[root].parent; - } - else - root = root_node; - } - insertLeaf(root, leaf); -} - -template -bool HierarchyTree::update(size_t leaf, const BV& bv) -{ - if(nodes[leaf].bv.contain(bv)) return false; - update_(leaf, bv); - return true; -} - -template -bool HierarchyTree::update(size_t leaf, const BV& bv, const Vector3d& vel, FCL_REAL margin) -{ - if(nodes[leaf].bv.contain(bv)) return false; - update_(leaf, bv); - return true; -} - -template -bool HierarchyTree::update(size_t leaf, const BV& bv, const Vector3d& vel) -{ - if(nodes[leaf].bv.contain(bv)) return false; - update_(leaf, bv); - return true; -} - -template -size_t HierarchyTree::getMaxHeight() const -{ - if(root_node == NULL_NODE) return 0; - - return getMaxHeight(root_node); -} - -template -size_t HierarchyTree::getMaxDepth() const -{ - if(root_node == NULL_NODE) return 0; - - size_t max_depth; - getMaxDepth(root_node, 0, max_depth); - return max_depth; -} - -template -void HierarchyTree::balanceBottomup() -{ - if(root_node != NULL_NODE) - { - NodeType* leaves = new NodeType[n_leaves]; - NodeType* leaves_ = leaves; - extractLeaves(root_node, leaves_); - root_node = NULL_NODE; - memcpy(nodes, leaves, sizeof(NodeType) * n_leaves); - freelist = n_leaves; - n_nodes = n_leaves; - for(size_t i = n_leaves; i < n_nodes_alloc; ++i) - nodes[i].next = i + 1; - nodes[n_nodes_alloc - 1].next = NULL_NODE; - - - size_t* ids = new size_t[n_leaves]; - for(size_t i = 0; i < n_leaves; ++i) - ids[i] = i; - - bottomup(ids, ids + n_leaves); - root_node = *ids; - - delete [] ids; - } -} - -template -void HierarchyTree::balanceTopdown() -{ - if(root_node != NULL_NODE) - { - NodeType* leaves = new NodeType[n_leaves]; - NodeType* leaves_ = leaves; - extractLeaves(root_node, leaves_); - root_node = NULL_NODE; - memcpy(nodes, leaves, sizeof(NodeType) * n_leaves); - freelist = n_leaves; - n_nodes = n_leaves; - for(size_t i = n_leaves; i < n_nodes_alloc; ++i) - nodes[i].next = i + 1; - nodes[n_nodes_alloc - 1].next = NULL_NODE; - - size_t* ids = new size_t[n_leaves]; - for(size_t i = 0; i < n_leaves; ++i) - ids[i] = i; - - root_node = topdown(ids, ids + n_leaves); - delete [] ids; - } -} - -template -void HierarchyTree::balanceIncremental(int iterations) -{ - if(iterations < 0) iterations = n_leaves; - if((root_node != NULL_NODE) && (iterations > 0)) - { - for(int i = 0; i < iterations; ++i) - { - size_t node = root_node; - unsigned int bit = 0; - while(!nodes[node].isLeaf()) - { - node = nodes[node].children[(opath>>bit)&1]; - bit = (bit+1)&(sizeof(unsigned int) * 8-1); - } - update(node); - ++opath; - } - } -} - -template -void HierarchyTree::refit() -{ - if(root_node != NULL_NODE) - recurseRefit(root_node); -} - -template -void HierarchyTree::extractLeaves(size_t root, NodeType*& leaves) const -{ - if(!nodes[root].isLeaf()) - { - extractLeaves(nodes[root].children[0], leaves); - extractLeaves(nodes[root].children[1], leaves); - } - else - { - *leaves = nodes[root]; - leaves++; - } -} - -template -size_t HierarchyTree::size() const -{ - return n_leaves; -} - -template -size_t HierarchyTree::getRoot() const -{ - return root_node; -} - -template -typename HierarchyTree::NodeType* HierarchyTree::getNodes() const -{ - return nodes; -} - -template -void HierarchyTree::print(size_t root, int depth) -{ - for(int i = 0; i < depth; ++i) - std::cout << " "; - NodeType* n = nodes + root; - std::cout << " (" << n->bv.min_[0] << ", " << n->bv.min_[1] << ", " << n->bv.min_[2] << "; " << n->bv.max_[0] << ", " << n->bv.max_[1] << ", " << n->bv.max_[2] << ")" << std::endl; - if(n->isLeaf()) - { - } - else - { - print(n->children[0], depth+1); - print(n->children[1], depth+1); - } -} - -template -size_t HierarchyTree::getMaxHeight(size_t node) const -{ - if(!nodes[node].isLeaf()) - { - size_t height1 = getMaxHeight(nodes[node].children[0]); - size_t height2 = getMaxHeight(nodes[node].children[1]); - return std::max(height1, height2) + 1; - } - else - return 0; -} - -template -void HierarchyTree::getMaxDepth(size_t node, size_t depth, size_t& max_depth) const -{ - if(!nodes[node].isLeaf()) - { - getMaxDepth(nodes[node].children[0], depth+1, max_depth); - getmaxDepth(nodes[node].children[1], depth+1, max_depth); - } - else - max_depth = std::max(max_depth, depth); -} - -template -void HierarchyTree::bottomup(size_t* lbeg, size_t* lend) -{ - size_t* lcur_end = lend; - while(lbeg < lcur_end - 1) - { - size_t* min_it1 = NULL, *min_it2 = NULL; - FCL_REAL min_size = std::numeric_limits::max(); - for(size_t* it1 = lbeg; it1 < lcur_end; ++it1) - { - for(size_t* it2 = it1 + 1; it2 < lcur_end; ++it2) - { - FCL_REAL cur_size = (nodes[*it1].bv + nodes[*it2].bv).size(); - if(cur_size < min_size) - { - min_size = cur_size; - min_it1 = it1; - min_it2 = it2; - } - } - } - - size_t p = createNode(NULL_NODE, nodes[*min_it1].bv, nodes[*min_it2].bv, NULL); - nodes[p].children[0] = *min_it1; - nodes[p].children[1] = *min_it2; - nodes[*min_it1].parent = p; - nodes[*min_it2].parent = p; - *min_it1 = p; - size_t tmp = *min_it2; - lcur_end--; - *min_it2 = *lcur_end; - *lcur_end = tmp; - } -} - -template -size_t HierarchyTree::topdown(size_t* lbeg, size_t* lend) -{ - switch(topdown_level) - { - case 0: - return topdown_0(lbeg, lend); - break; - case 1: - return topdown_1(lbeg, lend); - break; - default: - return topdown_0(lbeg, lend); - } -} - -template -size_t HierarchyTree::topdown_0(size_t* lbeg, size_t* lend) -{ - int num_leaves = lend - lbeg; - if(num_leaves > 1) - { - if(num_leaves > bu_threshold) - { - BV vol = nodes[*lbeg].bv; - for(size_t* i = lbeg + 1; i < lend; ++i) - vol += nodes[*i].bv; - - int best_axis = 0; - FCL_REAL extent[3] = {vol.width(), vol.height(), vol.depth()}; - if(extent[1] > extent[0]) best_axis = 1; - if(extent[2] > extent[best_axis]) best_axis = 2; - - nodeBaseLess comp(nodes, best_axis); - size_t* lcenter = lbeg + num_leaves / 2; - std::nth_element(lbeg, lcenter, lend, comp); - - size_t node = createNode(NULL_NODE, vol, NULL); - nodes[node].children[0] = topdown_0(lbeg, lcenter); - nodes[node].children[1] = topdown_0(lcenter, lend); - nodes[nodes[node].children[0]].parent = node; - nodes[nodes[node].children[1]].parent = node; - return node; - } - else - { - bottomup(lbeg, lend); - return *lbeg; - } - } - return *lbeg; -} - -template -size_t HierarchyTree::topdown_1(size_t* lbeg, size_t* lend) -{ - int num_leaves = lend - lbeg; - if(num_leaves > 1) - { - if(num_leaves > bu_threshold) - { - Vector3d split_p = nodes[*lbeg].bv.center(); - BV vol = nodes[*lbeg].bv; - for(size_t* i = lbeg + 1; i < lend; ++i) - { - split_p += nodes[*i].bv.center(); - vol += nodes[*i].bv; - } - split_p /= (FCL_REAL)(num_leaves); - int best_axis = -1; - int bestmidp = num_leaves; - int splitcount[3][2] = {{0,0}, {0,0}, {0,0}}; - for(size_t* i = lbeg; i < lend; ++i) - { - Vector3d x = nodes[*i].bv.center() - split_p; - for(size_t j = 0; j < 3; ++j) - ++splitcount[j][x[j] > 0 ? 1 : 0]; - } - - for(size_t i = 0; i < 3; ++i) - { - if((splitcount[i][0] > 0) && (splitcount[i][1] > 0)) - { - int midp = std::abs(splitcount[i][0] - splitcount[i][1]); - if(midp < bestmidp) - { - best_axis = i; - bestmidp = midp; - } - } - } - - if(best_axis < 0) best_axis = 0; - - FCL_REAL split_value = split_p[best_axis]; - size_t* lcenter = lbeg; - for(size_t* i = lbeg; i < lend; ++i) - { - if(nodes[*i].bv.center()[best_axis] < split_value) - { - size_t temp = *i; - *i = *lcenter; - *lcenter = temp; - ++lcenter; - } - } - - size_t node = createNode(NULL_NODE, vol, NULL); - nodes[node].children[0] = topdown_1(lbeg, lcenter); - nodes[node].children[1] = topdown_1(lcenter, lend); - nodes[nodes[node].children[0]].parent = node; - nodes[nodes[node].children[1]].parent = node; - return node; - } - else - { - bottomup(lbeg, lend); - return *lbeg; - } - } - return *lbeg; -} - -template -size_t HierarchyTree::mortonRecurse_0(size_t* lbeg, size_t* lend, const FCL_UINT32& split, int bits) -{ - int num_leaves = lend - lbeg; - if(num_leaves > 1) - { - if(bits > 0) - { - SortByMorton comp; - comp.nodes = nodes; - comp.split = split; - size_t* lcenter = std::lower_bound(lbeg, lend, NULL_NODE, comp); - - if(lcenter == lbeg) - { - FCL_UINT32 split2 = split | (1 << (bits - 1)); - return mortonRecurse_0(lbeg, lend, split2, bits - 1); - } - else if(lcenter == lend) - { - FCL_UINT32 split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); - return mortonRecurse_0(lbeg, lend, split1, bits - 1); - } - else - { - FCL_UINT32 split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); - FCL_UINT32 split2 = split | (1 << (bits - 1)); - - size_t child1 = mortonRecurse_0(lbeg, lcenter, split1, bits - 1); - size_t child2 = mortonRecurse_0(lcenter, lend, split2, bits - 1); - size_t node = createNode(NULL_NODE, NULL); - nodes[node].children[0] = child1; - nodes[node].children[1] = child2; - nodes[child1].parent = node; - nodes[child2].parent = node; - return node; - } - } - else - { - size_t node = topdown(lbeg, lend); - return node; - } - } - else - return *lbeg; -} - -template -size_t HierarchyTree::mortonRecurse_1(size_t* lbeg, size_t* lend, const FCL_UINT32& split, int bits) -{ - int num_leaves = lend - lbeg; - if(num_leaves > 1) - { - if(bits > 0) - { - SortByMorton comp; - comp.nodes = nodes; - comp.split = split; - size_t* lcenter = std::lower_bound(lbeg, lend, NULL_NODE, comp); - - if(lcenter == lbeg) - { - FCL_UINT32 split2 = split | (1 << (bits - 1)); - return mortonRecurse_1(lbeg, lend, split2, bits - 1); - } - else if(lcenter == lend) - { - FCL_UINT32 split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); - return mortonRecurse_1(lbeg, lend, split1, bits - 1); - } - else - { - FCL_UINT32 split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); - FCL_UINT32 split2 = split | (1 << (bits - 1)); - - size_t child1 = mortonRecurse_1(lbeg, lcenter, split1, bits - 1); - size_t child2 = mortonRecurse_1(lcenter, lend, split2, bits - 1); - size_t node = createNode(NULL_NODE, NULL); - nodes[node].children[0] = child1; - nodes[node].children[1] = child2; - nodes[child1].parent = node; - nodes[child2].parent = node; - return node; - } - } - else - { - size_t child1 = mortonRecurse_1(lbeg, lbeg + num_leaves / 2, 0, bits - 1); - size_t child2 = mortonRecurse_1(lbeg + num_leaves / 2, lend, 0, bits - 1); - size_t node = createNode(NULL_NODE, NULL); - nodes[node].children[0] = child1; - nodes[node].children[1] = child2; - nodes[child1].parent = node; - nodes[child2].parent = node; - return node; - } - } - else - return *lbeg; -} - -template -size_t HierarchyTree::mortonRecurse_2(size_t* lbeg, size_t* lend) -{ - int num_leaves = lend - lbeg; - if(num_leaves > 1) - { - size_t child1 = mortonRecurse_2(lbeg, lbeg + num_leaves / 2); - size_t child2 = mortonRecurse_2(lbeg + num_leaves / 2, lend); - size_t node = createNode(NULL_NODE, NULL); - nodes[node].children[0] = child1; - nodes[node].children[1] = child2; - nodes[child1].parent = node; - nodes[child2].parent = node; - return node; - } - else - return *lbeg; -} - -template -void HierarchyTree::insertLeaf(size_t root, size_t leaf) -{ - if(root_node == NULL_NODE) - { - root_node = leaf; - nodes[leaf].parent = NULL_NODE; - } - else - { - if(!nodes[root].isLeaf()) - { - do - { - root = nodes[root].children[select(leaf, nodes[root].children[0], nodes[root].children[1], nodes)]; - } - while(!nodes[root].isLeaf()); - } - - size_t prev = nodes[root].parent; - size_t node = createNode(prev, nodes[leaf].bv, nodes[root].bv, NULL); - if(prev != NULL_NODE) - { - nodes[prev].children[indexOf(root)] = node; - nodes[node].children[0] = root; nodes[root].parent = node; - nodes[node].children[1] = leaf; nodes[leaf].parent = node; - do - { - if(!nodes[prev].bv.contain(nodes[node].bv)) - nodes[prev].bv = nodes[nodes[prev].children[0]].bv + nodes[nodes[prev].children[1]].bv; - else - break; - node = prev; - } while (NULL_NODE != (prev = nodes[node].parent)); - } - else - { - nodes[node].children[0] = root; nodes[root].parent = node; - nodes[node].children[1] = leaf; nodes[leaf].parent = node; - root_node = node; - } - } -} - -template -size_t HierarchyTree::removeLeaf(size_t leaf) -{ - if(leaf == root_node) - { - root_node = NULL_NODE; - return NULL_NODE; - } - else - { - size_t parent = nodes[leaf].parent; - size_t prev = nodes[parent].parent; - size_t sibling = nodes[parent].children[1-indexOf(leaf)]; - - if(prev != NULL_NODE) - { - nodes[prev].children[indexOf(parent)] = sibling; - nodes[sibling].parent = prev; - deleteNode(parent); - while(prev != NULL_NODE) - { - BV new_bv = nodes[nodes[prev].children[0]].bv + nodes[nodes[prev].children[1]].bv; - if(!new_bv.equal(nodes[prev].bv)) - { - nodes[prev].bv = new_bv; - prev = nodes[prev].parent; - } - else break; - } - - return (prev != NULL_NODE) ? prev : root_node; - } - else - { - root_node = sibling; - nodes[sibling].parent = NULL_NODE; - deleteNode(parent); - return root_node; - } - } -} - -template -void HierarchyTree::update_(size_t leaf, const BV& bv) -{ - size_t root = removeLeaf(leaf); - if(root != NULL_NODE) - { - if(max_lookahead_level >= 0) - { - for(int i = 0; (i < max_lookahead_level) && (nodes[root].parent != NULL_NODE); ++i) - root = nodes[root].parent; - } - - nodes[leaf].bv = bv; - insertLeaf(root, leaf); - } -} - -template -size_t HierarchyTree::indexOf(size_t node) -{ - return (nodes[nodes[node].parent].children[1] == node); -} - -template -size_t HierarchyTree::allocateNode() -{ - if(freelist == NULL_NODE) - { - NodeType* old_nodes = nodes; - n_nodes_alloc *= 2; - nodes = new NodeType[n_nodes_alloc]; - memcpy(nodes, old_nodes, n_nodes * sizeof(NodeType)); - delete [] old_nodes; - - for(size_t i = n_nodes; i < n_nodes_alloc - 1; ++i) - nodes[i].next = i + 1; - nodes[n_nodes_alloc - 1].next = NULL_NODE; - freelist = n_nodes; - } - - size_t node_id = freelist; - freelist = nodes[node_id].next; - nodes[node_id].parent = NULL_NODE; - nodes[node_id].children[0] = NULL_NODE; - nodes[node_id].children[1] = NULL_NODE; - ++n_nodes; - return node_id; -} - -template -size_t HierarchyTree::createNode(size_t parent, - const BV& bv1, - const BV& bv2, - void* data) -{ - size_t node = allocateNode(); - nodes[node].parent = parent; - nodes[node].data = data; - nodes[node].bv = bv1 + bv2; - return node; -} - -template -size_t HierarchyTree::createNode(size_t parent, - const BV& bv, - void* data) -{ - size_t node = allocateNode(); - nodes[node].parent = parent; - nodes[node].data = data; - nodes[node].bv = bv; - return node; -} - -template -size_t HierarchyTree::createNode(size_t parent, - void* data) -{ - size_t node = allocateNode(); - nodes[node].parent = parent; - nodes[node].data = data; - return node; -} - -template -void HierarchyTree::deleteNode(size_t node) -{ - nodes[node].next = freelist; - freelist = node; - --n_nodes; -} - -template -void HierarchyTree::recurseRefit(size_t node) -{ - if(!nodes[node].isLeaf()) - { - recurseRefit(nodes[node].children[0]); - recurseRefit(nodes[node].children[1]); - nodes[node].bv = nodes[nodes[node].children[0]].bv + nodes[nodes[node].children[1]].bv; - } - else - return; -} - -template -void HierarchyTree::fetchLeaves(size_t root, NodeType*& leaves, int depth) -{ - if((!nodes[root].isLeaf()) && depth) - { - fetchLeaves(nodes[root].children[0], leaves, depth-1); - fetchLeaves(nodes[root].children[1], leaves, depth-1); - deleteNode(root); - } - else - { - *leaves = nodes[root]; - leaves++; - } -} - - - -} -} diff --git a/include/fcl/broadphase/interval_tree.h b/include/fcl/broadphase/interval_tree.h index d76e75b90..1cc49f6fd 100644 --- a/include/fcl/broadphase/interval_tree.h +++ b/include/fcl/broadphase/interval_tree.h @@ -41,6 +41,8 @@ #include #include +#include +#include namespace fcl { @@ -159,8 +161,534 @@ class IntervalTree unsigned int recursion_node_stack_top; }; +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +IntervalTreeNode::IntervalTreeNode(){} + +IntervalTreeNode::IntervalTreeNode(SimpleInterval* new_interval) : + stored_interval (new_interval), + key(new_interval->low), + high(new_interval->high), + max_high(high) {} + +IntervalTreeNode::~IntervalTreeNode() {} + + +/// @brief Class describes the information needed when we take the +/// right branch in searching for intervals but possibly come back +/// and check the left branch as well. +struct it_recursion_node +{ +public: + IntervalTreeNode* start_node; + + unsigned int parent_index; + + bool try_right_branch; +}; + + +IntervalTree::IntervalTree() +{ + nil = new IntervalTreeNode; + nil->left = nil->right = nil->parent = nil; + nil->red = false; + nil->key = nil->high = nil->max_high = -std::numeric_limits::max(); + nil->stored_interval = nullptr; + + root = new IntervalTreeNode; + root->parent = root->left = root->right = nil; + root->key = root->high = root->max_high = std::numeric_limits::max(); + root->red = false; + root->stored_interval = nullptr; + + /// the following are used for the query function + recursion_node_stack_size = 128; + recursion_node_stack = (it_recursion_node*)malloc(recursion_node_stack_size*sizeof(it_recursion_node)); + recursion_node_stack_top = 1; + recursion_node_stack[0].start_node = nullptr; } -#endif +IntervalTree::~IntervalTree() +{ + IntervalTreeNode* x = root->left; + std::deque nodes_to_free; + + if(x != nil) + { + if(x->left != nil) + { + nodes_to_free.push_back(x->left); + } + if(x->right != nil) + { + nodes_to_free.push_back(x->right); + } + + delete x; + while( nodes_to_free.size() > 0) + { + x = nodes_to_free.back(); + nodes_to_free.pop_back(); + if(x->left != nil) + { + nodes_to_free.push_back(x->left); + } + if(x->right != nil) + { + nodes_to_free.push_back(x->right); + } + delete x; + } + } + delete nil; + delete root; + free(recursion_node_stack); +} + + +void IntervalTree::leftRotate(IntervalTreeNode* x) +{ + IntervalTreeNode* y; + + y = x->right; + x->right = y->left; + + if(y->left != nil) y->left->parent = x; + + y->parent = x->parent; + + if(x == x->parent->left) + x->parent->left = y; + else + x->parent->right = y; + + y->left = x; + x->parent = y; + + x->max_high = std::max(x->left->max_high, std::max(x->right->max_high, x->high)); + y->max_high = std::max(x->max_high, std::max(y->right->max_high, y->high)); +} + + +void IntervalTree::rightRotate(IntervalTreeNode* y) +{ + IntervalTreeNode* x; + + x = y->left; + y->left = x->right; + + if(nil != x->right) x->right->parent = y; + + x->parent = y->parent; + if(y == y->parent->left) + y->parent->left = x; + else + y->parent->right = x; + + x->right = y; + y->parent = x; + + y->max_high = std::max(y->left->max_high, std::max(y->right->max_high, y->high)); + x->max_high = std::max(x->left->max_high, std::max(y->max_high, x->high)); +} + + + +/// @brief Inserts z into the tree as if it were a regular binary tree +void IntervalTree::recursiveInsert(IntervalTreeNode* z) +{ + IntervalTreeNode* x; + IntervalTreeNode* y; + + z->left = z->right = nil; + y = root; + x = root->left; + while(x != nil) + { + y = x; + if(x->key > z->key) + x = x->left; + else + x = x->right; + } + z->parent = y; + if((y == root) || (y->key > z->key)) + y->left = z; + else + y->right = z; +} + + +void IntervalTree::fixupMaxHigh(IntervalTreeNode* x) +{ + while(x != root) + { + x->max_high = std::max(x->high, std::max(x->left->max_high, x->right->max_high)); + x = x->parent; + } +} + +IntervalTreeNode* IntervalTree::insert(SimpleInterval* new_interval) +{ + IntervalTreeNode* y; + IntervalTreeNode* x; + IntervalTreeNode* new_node; + + x = new IntervalTreeNode(new_interval); + recursiveInsert(x); + fixupMaxHigh(x->parent); + new_node = x; + x->red = true; + while(x->parent->red) + { + /// use sentinel instead of checking for root + if(x->parent == x->parent->parent->left) + { + y = x->parent->parent->right; + if(y->red) + { + x->parent->red = true; + y->red = true; + x->parent->parent->red = true; + x = x->parent->parent; + } + else + { + if(x == x->parent->right) + { + x = x->parent; + leftRotate(x); + } + x->parent->red = false; + x->parent->parent->red = true; + rightRotate(x->parent->parent); + } + } + else + { + y = x->parent->parent->left; + if(y->red) + { + x->parent->red = false; + y->red = false; + x->parent->parent->red = true; + x = x->parent->parent; + } + else + { + if(x == x->parent->left) + { + x = x->parent; + rightRotate(x); + } + x->parent->red = false; + x->parent->parent->red = true; + leftRotate(x->parent->parent); + } + } + } + root->left->red = false; + return new_node; +} + +IntervalTreeNode* IntervalTree::getSuccessor(IntervalTreeNode* x) const +{ + IntervalTreeNode* y; + + if(nil != (y = x->right)) + { + while(y->left != nil) + y = y->left; + return y; + } + else + { + y = x->parent; + while(x == y->right) + { + x = y; + y = y->parent; + } + if(y == root) return nil; + return y; + } +} + + +IntervalTreeNode* IntervalTree::getPredecessor(IntervalTreeNode* x) const +{ + IntervalTreeNode* y; + + if(nil != (y = x->left)) + { + while(y->right != nil) + y = y->right; + return y; + } + else + { + y = x->parent; + while(x == y->left) + { + if(y == root) return nil; + x = y; + y = y->parent; + } + return y; + } +} + +void IntervalTreeNode::print(IntervalTreeNode* nil, IntervalTreeNode* root) const +{ + stored_interval->print(); + std::cout << ", k = " << key << ", h = " << high << ", mH = " << max_high; + std::cout << " l->key = "; + if(left == nil) std::cout << "nullptr"; else std::cout << left->key; + std::cout << " r->key = "; + if(right == nil) std::cout << "nullptr"; else std::cout << right->key; + std::cout << " p->key = "; + if(parent == root) std::cout << "nullptr"; else std::cout << parent->key; + std::cout << " red = " << (int)red << std::endl; +} + +void IntervalTree::recursivePrint(IntervalTreeNode* x) const +{ + if(x != nil) + { + recursivePrint(x->left); + x->print(nil,root); + recursivePrint(x->right); + } +} + + +void IntervalTree::print() const +{ + recursivePrint(root->left); +} + +void IntervalTree::deleteFixup(IntervalTreeNode* x) +{ + IntervalTreeNode* w; + IntervalTreeNode* root_left_node = root->left; + + while((!x->red) && (root_left_node != x)) + { + if(x == x->parent->left) + { + w = x->parent->right; + if(w->red) + { + w->red = false; + x->parent->red = true; + leftRotate(x->parent); + w = x->parent->right; + } + if((!w->right->red) && (!w->left->red)) + { + w->red = true; + x = x->parent; + } + else + { + if(!w->right->red) + { + w->left->red = false; + w->red = true; + rightRotate(w); + w = x->parent->right; + } + w->red = x->parent->red; + x->parent->red = false; + w->right->red = false; + leftRotate(x->parent); + x = root_left_node; + } + } + else + { + w = x->parent->left; + if(w->red) + { + w->red = false; + x->parent->red = true; + rightRotate(x->parent); + w = x->parent->left; + } + if((!w->right->red) && (!w->left->red)) + { + w->red = true; + x = x->parent; + } + else + { + if(!w->left->red) + { + w->right->red = false; + w->red = true; + leftRotate(w); + w = x->parent->left; + } + w->red = x->parent->red; + x->parent->red = false; + w->left->red = false; + rightRotate(x->parent); + x = root_left_node; + } + } + } + x->red = false; +} +void IntervalTree::deleteNode(SimpleInterval* ivl) +{ + IntervalTreeNode* node = recursiveSearch(root, ivl); + if(node) + deleteNode(node); +} +IntervalTreeNode* IntervalTree::recursiveSearch(IntervalTreeNode* node, SimpleInterval* ivl) const +{ + if(node != nil) + { + if(node->stored_interval == ivl) + return node; + + IntervalTreeNode* left = recursiveSearch(node->left, ivl); + if(left != nil) return left; + IntervalTreeNode* right = recursiveSearch(node->right, ivl); + if(right != nil) return right; + } + + return nil; +} + +SimpleInterval* IntervalTree::deleteNode(IntervalTreeNode* z) +{ + IntervalTreeNode* y; + IntervalTreeNode* x; + SimpleInterval* node_to_delete = z->stored_interval; + + y= ((z->left == nil) || (z->right == nil)) ? z : getSuccessor(z); + x= (y->left == nil) ? y->right : y->left; + if(root == (x->parent = y->parent)) + { + root->left = x; + } + else + { + if(y == y->parent->left) + { + y->parent->left = x; + } + else + { + y->parent->right = x; + } + } + + /// @brief y should not be nil in this case + /// y is the node to splice out and x is its child + if(y != z) + { + y->max_high = -std::numeric_limits::max(); + y->left = z->left; + y->right = z->right; + y->parent = z->parent; + z->left->parent = z->right->parent = y; + if(z == z->parent->left) + z->parent->left = y; + else + z->parent->right = y; + + fixupMaxHigh(x->parent); + if(!(y->red)) + { + y->red = z->red; + deleteFixup(x); + } + else + y->red = z->red; + delete z; + } + else + { + fixupMaxHigh(x->parent); + if(!(y->red)) deleteFixup(x); + delete y; + } + + return node_to_delete; +} + +/// @brief returns 1 if the intervals overlap, and 0 otherwise +bool overlap(double a1, double a2, double b1, double b2) +{ + if(a1 <= b1) + { + return (b1 <= a2); + } + else + { + return (a1 <= b2); + } +} + +std::deque IntervalTree::query(double low, double high) +{ + std::deque result_stack; + IntervalTreeNode* x = root->left; + bool run = (x != nil); + + current_parent = 0; + + while(run) + { + if(overlap(low,high,x->key,x->high)) + { + result_stack.push_back(x->stored_interval); + recursion_node_stack[current_parent].try_right_branch = true; + } + if(x->left->max_high >= low) + { + if(recursion_node_stack_top == recursion_node_stack_size) + { + recursion_node_stack_size *= 2; + recursion_node_stack = (it_recursion_node *)realloc(recursion_node_stack, recursion_node_stack_size * sizeof(it_recursion_node)); + if(recursion_node_stack == nullptr) + exit(1); + } + recursion_node_stack[recursion_node_stack_top].start_node = x; + recursion_node_stack[recursion_node_stack_top].try_right_branch = false; + recursion_node_stack[recursion_node_stack_top].parent_index = current_parent; + current_parent = recursion_node_stack_top++; + x = x->left; + } + else + x = x->right; + + run = (x != nil); + while((!run) && (recursion_node_stack_top > 1)) + { + if(recursion_node_stack[--recursion_node_stack_top].try_right_branch) + { + x=recursion_node_stack[recursion_node_stack_top].start_node->right; + current_parent=recursion_node_stack[recursion_node_stack_top].parent_index; + recursion_node_stack[current_parent].try_right_branch = true; + run = (x != nil); + } + } + } + return result_stack; +} + + +} // namespace fcl + +#endif diff --git a/include/fcl/broadphase/morton.h b/include/fcl/broadphase/morton.h index fe3821903..38fea748d 100644 --- a/include/fcl/broadphase/morton.h +++ b/include/fcl/broadphase/morton.h @@ -51,9 +51,10 @@ namespace fcl namespace details { -static inline FCL_UINT32 quantize(FCL_REAL x, FCL_UINT32 n) +template +FCL_UINT32 quantize(S x, FCL_UINT32 n) { - return std::max(std::min((FCL_UINT32)(x * (FCL_REAL)n), FCL_UINT32(n-1)), FCL_UINT32(0)); + return std::max(std::min((FCL_UINT32)(x * (S)n), FCL_UINT32(n-1)), FCL_UINT32(0)); } /// @brief compute 30 bit morton code @@ -94,25 +95,25 @@ static inline FCL_UINT64 morton_code60(FCL_UINT32 x, FCL_UINT32 y, FCL_UINT32 z) /// @endcond -/// @brief Functor to compute the morton code for a given AABB +/// @brief Functor to compute the morton code for a given AABB /// This is specialized for 32- and 64-bit unsigned integers giving /// a 30- or 60-bit code, respectively, and for `std::bitset` where /// N is the length of the code and must be a multiple of 3. -template +template struct morton_functor {}; -/// @brief Functor to compute 30 bit morton code for a given AABB -template<> -struct morton_functor +/// @brief Functor to compute 30 bit morton code for a given AABB +template +struct morton_functor { - morton_functor(const AABB& bbox) : base(bbox.min_), + morton_functor(const AABB& bbox) : base(bbox.min_), inv(1.0 / (bbox.max_[0] - bbox.min_[0]), 1.0 / (bbox.max_[1] - bbox.min_[1]), 1.0 / (bbox.max_[2] - bbox.min_[2])) {} - FCL_UINT32 operator() (const Vector3d& point) const + FCL_UINT32 operator() (const Vector3& point) const { FCL_UINT32 x = details::quantize((point[0] - base[0]) * inv[0], 1024u); FCL_UINT32 y = details::quantize((point[1] - base[1]) * inv[1], 1024u); @@ -121,24 +122,24 @@ struct morton_functor return details::morton_code(x, y, z); } - const Vector3d base; - const Vector3d inv; + const Vector3 base; + const Vector3 inv; static constexpr size_t bits() { return 30; } }; -/// @brief Functor to compute 60 bit morton code for a given AABB -template<> -struct morton_functor +/// @brief Functor to compute 60 bit morton code for a given AABB +template +struct morton_functor { - morton_functor(const AABB& bbox) : base(bbox.min_), + morton_functor(const AABB& bbox) : base(bbox.min_), inv(1.0 / (bbox.max_[0] - bbox.min_[0]), 1.0 / (bbox.max_[1] - bbox.min_[1]), 1.0 / (bbox.max_[2] - bbox.min_[2])) {} - FCL_UINT64 operator() (const Vector3d& point) const + FCL_UINT64 operator() (const Vector3& point) const { FCL_UINT32 x = details::quantize((point[0] - base[0]) * inv[0], 1u << 20); FCL_UINT32 y = details::quantize((point[1] - base[1]) * inv[1], 1u << 20); @@ -147,31 +148,31 @@ struct morton_functor return details::morton_code60(x, y, z); } - const Vector3d base; - const Vector3d inv; + const Vector3 base; + const Vector3 inv; static constexpr size_t bits() { return 60; } }; -/// @brief Functor to compute N bit morton code for a given AABB +/// @brief Functor to compute N bit morton code for a given AABB /// N must be a multiple of 3. -template -struct morton_functor> +template +struct morton_functor> { static_assert(N%3==0, "Number of bits must be a multiple of 3"); - morton_functor(const AABB& bbox) : base(bbox.min_), + morton_functor(const AABB& bbox) : base(bbox.min_), inv(1.0 / (bbox.max_[0] - bbox.min_[0]), 1.0 / (bbox.max_[1] - bbox.min_[1]), 1.0 / (bbox.max_[2] - bbox.min_[2])) {} - std::bitset operator() (const Vector3d& point) const + std::bitset operator() (const Vector3& point) const { - FCL_REAL x = (point[0] - base[0]) * inv[0]; - FCL_REAL y = (point[1] - base[1]) * inv[1]; - FCL_REAL z = (point[2] - base[2]) * inv[2]; + S x = (point[0] - base[0]) * inv[0]; + S y = (point[1] - base[1]) * inv[1]; + S z = (point[2] - base[2]) * inv[2]; int start_bit = bits() - 1; std::bitset bset; @@ -192,8 +193,8 @@ struct morton_functor> return bset; } - const Vector3d base; - const Vector3d inv; + const Vector3 base; + const Vector3 inv; static constexpr size_t bits() { return N; } }; diff --git a/include/fcl/broadphase/simple_hash_table.h b/include/fcl/broadphase/simple_hash_table.h new file mode 100644 index 000000000..584df9b81 --- /dev/null +++ b/include/fcl/broadphase/simple_hash_table.h @@ -0,0 +1,160 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_BROADPHASE_SIMPLEHASHTABLE_H +#define FCL_BROADPHASE_SIMPLEHASHTABLE_H + +#include +#include +#include +#include + +namespace fcl +{ + +/// @brief A simple hash table implemented as multiple buckets. HashFnc is any +/// extended hash function: HashFnc(key) = {index1, index2, ..., } +template +class SimpleHashTable +{ +protected: + typedef std::list Bin; + + std::vector table_; + + HashFnc h_; + + size_t table_size_; + +public: + SimpleHashTable(const HashFnc& h); + + /// @brief Init the number of bins in the hash table + void init(size_t size); + + //// @brief Insert a key-value pair into the table + void insert(Key key, Data value); + + /// @brief Find the elements in the hash table whose key is the same as query + /// key. + std::vector query(Key key) const; + + /// @brief remove the key-value pair from the table + void remove(Key key, Data value); + + /// @brief clear the hash table + void clear(); +}; + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +SimpleHashTable::SimpleHashTable(const HashFnc& h) + : h_(h) +{ + // Do nothing +} + +//============================================================================== +template +void SimpleHashTable::init(size_t size) +{ + if(size == 0) + { + throw std::logic_error("SimpleHashTable must have non-zero size."); + } + + table_.resize(size); + table_size_ = size; +} + +//============================================================================== +template +void SimpleHashTable::insert(Key key, Data value) +{ + std::vector indices = h_(key); + size_t range = table_.size(); + for(size_t i = 0; i < indices.size(); ++i) + table_[indices[i] % range].push_back(value); +} + +//============================================================================== +template +std::vector SimpleHashTable::query(Key key) const +{ + size_t range = table_.size(); + std::vector indices = h_(key); + std::set result; + for(size_t i = 0; i < indices.size(); ++i) + { + unsigned int index = indices[i] % range; + std::copy(table_[index].begin(), table_[index].end(), + std::inserter(result, result.end())); + } + + return std::vector(result.begin(), result.end()); +} + +//============================================================================== +template +void SimpleHashTable::remove(Key key, Data value) +{ + size_t range = table_.size(); + std::vector indices = h_(key); + for(size_t i = 0; i < indices.size(); ++i) + { + unsigned int index = indices[i] % range; + table_[index].remove(value); + } +} + +//============================================================================== +template +void SimpleHashTable::clear() +{ + table_.clear(); + table_.resize(table_size_); +} + +} // namespace fcl + +#endif diff --git a/include/fcl/broadphase/sparse_hash_table.h b/include/fcl/broadphase/sparse_hash_table.h new file mode 100644 index 000000000..7a21e3198 --- /dev/null +++ b/include/fcl/broadphase/sparse_hash_table.h @@ -0,0 +1,160 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_BROADPHASE_SPARSEHASHTABLE_H +#define FCL_BROADPHASE_SPARSEHASHTABLE_H + +#include +#include +#include +#include +#include + +namespace fcl +{ + +template +class unordered_map_hash_table : public std::unordered_map {}; + +/// @brief A hash table implemented using unordered_map +template class TableT = unordered_map_hash_table> +class SparseHashTable +{ +protected: + HashFnc h_; + typedef std::list Bin; + typedef TableT Table; + + Table table_; +public: + SparseHashTable(const HashFnc& h); + + /// @brief Init the hash table. The bucket size is dynamically decided. + void init(size_t); + + /// @brief insert one key-value pair into the hash table + void insert(Key key, Data value); + + /// @brief find the elements whose key is the same as the query + std::vector query(Key key) const; + + /// @brief remove one key-value pair from the hash table + void remove(Key key, Data value); + + /// @brief clear the hash table + void clear(); +}; + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template class TableT> +SparseHashTable::SparseHashTable(const HashFnc& h) + : h_(h) +{ + // Do nothing +} + +//============================================================================== +template class TableT> +void SparseHashTable::init(size_t) +{ + table_.clear(); +} + +//============================================================================== +template class TableT> +void SparseHashTable::insert(Key key, Data value) +{ + std::vector indices = h_(key); + for(size_t i = 0; i < indices.size(); ++i) + table_[indices[i]].push_back(value); +} + +//============================================================================== +template class TableT> +std::vector SparseHashTable::query(Key key) const +{ + std::vector indices = h_(key); + std::set result; + for(size_t i = 0; i < indices.size(); ++i) + { + unsigned int index = indices[i]; + typename Table::const_iterator p = table_.find(index); + if(p != table_.end()) + { + std::copy((*p).second.begin(), (*p).second.end(), std + ::inserter(result, result.end())); + } + } + + return std::vector(result.begin(), result.end()); +} + +//============================================================================== +template class TableT> +void SparseHashTable::remove(Key key, Data value) +{ + std::vector indices = h_(key); + for(size_t i = 0; i < indices.size(); ++i) + { + unsigned int index = indices[i]; + table_[index].remove(value); + } +} + +//============================================================================== +template class TableT> +void SparseHashTable::clear() +{ + table_.clear(); +} + +} // namespace fcl + +#endif diff --git a/include/fcl/broadphase/spatial_hash.h b/include/fcl/broadphase/spatial_hash.h new file mode 100644 index 000000000..069971a13 --- /dev/null +++ b/include/fcl/broadphase/spatial_hash.h @@ -0,0 +1,110 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_BROADPHASE_SPATIALHASH_H +#define FCL_BROADPHASE_SPATIALHASH_H + +#include "fcl/BV/AABB.h" + +namespace fcl +{ + +/// @brief Spatial hash function: hash an AABB to a set of integer values +template +struct SpatialHash +{ + using S = S_; + + SpatialHash(const AABB& scene_limit_, S cell_size_); + + std::vector operator() (const AABB& aabb) const; + +private: + + S cell_size; + AABB scene_limit; + unsigned int width[3]; +}; + +using SpatialHashf = SpatialHash; +using SpatialHashd = SpatialHash; + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +SpatialHash::SpatialHash(const AABB& scene_limit_, S cell_size_) + : cell_size(cell_size_), scene_limit(scene_limit_) +{ + width[0] = std::ceil(scene_limit.width() / cell_size); + width[1] = std::ceil(scene_limit.height() / cell_size); + width[2] = std::ceil(scene_limit.depth() / cell_size); +} + +//============================================================================== +template +std::vector SpatialHash::operator()(const AABB& aabb) const +{ + int min_x = std::floor((aabb.min_[0] - scene_limit.min_[0]) / cell_size); + int max_x = std::ceil((aabb.max_[0] - scene_limit.min_[0]) / cell_size); + int min_y = std::floor((aabb.min_[1] - scene_limit.min_[1]) / cell_size); + int max_y = std::ceil((aabb.max_[1] - scene_limit.min_[1]) / cell_size); + int min_z = std::floor((aabb.min_[2] - scene_limit.min_[2]) / cell_size); + int max_z = std::ceil((aabb.max_[2] - scene_limit.min_[2]) / cell_size); + + std::vector keys((max_x - min_x) * (max_y - min_y) * (max_z - min_z)); + int id = 0; + for(int x = min_x; x < max_x; ++x) + { + for(int y = min_y; y < max_y; ++y) + { + for(int z = min_z; z < max_z; ++z) + { + keys[id++] = x + y * width[0] + z * width[0] * width[1]; + } + } + } + return keys; +} + +} // namespace fcl + +#endif diff --git a/include/fcl/ccd/conservative_advancement.h b/include/fcl/ccd/conservative_advancement.h index da16ee21b..b298fd9a5 100644 --- a/include/fcl/ccd/conservative_advancement.h +++ b/include/fcl/ccd/conservative_advancement.h @@ -38,11 +38,22 @@ #ifndef FCL_CONSERVATIVE_ADVANCEMENT_H #define FCL_CONSERVATIVE_ADVANCEMENT_H - #include "fcl/collision_object.h" #include "fcl/collision_data.h" #include "fcl/ccd/motion_base.h" - +#include "fcl/BVH/BVH_model.h" +#include "fcl/shape/box.h" +#include "fcl/shape/capsule.h" +#include "fcl/shape/cone.h" +#include "fcl/shape/convex.h" +#include "fcl/shape/cylinder.h" +#include "fcl/shape/ellipsoid.h" +#include "fcl/shape/halfspace.h" +#include "fcl/shape/plane.h" +#include "fcl/shape/sphere.h" +#include "fcl/shape/triangle_p.h" +#include "fcl/traversal/traversal_nodes.h" +#include "fcl/traversal/traversal_recurse.h" namespace fcl { @@ -50,19 +61,969 @@ namespace fcl template struct ConservativeAdvancementFunctionMatrix { - typedef FCL_REAL (*ConservativeAdvancementFunc)(const CollisionGeometry* o1, const MotionBase* motion1, const CollisionGeometry* o2, const MotionBase* motion2, const NarrowPhaseSolver* nsolver, const ContinuousCollisionRequest& request, ContinuousCollisionResult& result); + using S = typename NarrowPhaseSolver::S; + + typedef S (*ConservativeAdvancementFunc)(const CollisionGeometry* o1, const MotionBase* motion1, const CollisionGeometry* o2, const MotionBase* motion2, const NarrowPhaseSolver* nsolver, const ContinuousCollisionRequest& request, ContinuousCollisionResult& result); ConservativeAdvancementFunc conservative_advancement_matrix[NODE_COUNT][NODE_COUNT]; ConservativeAdvancementFunctionMatrix(); }; +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +bool conservativeAdvancement(const BVHModel& o1, + const MotionBase* motion1, + const BVHModel& o2, + const MotionBase* motion2, + const CollisionRequest& request, + CollisionResult& result, + typename BV::S& toc) +{ + using S = typename BV::S; + + Transform3 tf1, tf2; + motion1->getCurrentTransform(tf1); + motion2->getCurrentTransform(tf2); + + // whether the first start configuration is in collision + if(collide(&o1, tf1, &o2, tf2, request, result)) + { + toc = 0; + return true; + } + + + BVHModel* o1_tmp = new BVHModel(o1); + BVHModel* o2_tmp = new BVHModel(o2); + + + MeshConservativeAdvancementTraversalNode node; + + node.motion1 = motion1; + node.motion2 = motion2; + + do + { + // repeatedly update mesh to global coordinate, so time consuming + initialize(node, *o1_tmp, tf1, *o2_tmp, tf2); + + node.delta_t = 1; + node.min_distance = std::numeric_limits::max(); + + distanceRecurse(&node, 0, 0, nullptr); + if(node.delta_t <= node.t_err) + { + // std::cout << node.delta_t << " " << node.t_err << std::endl; + break; + } + node.toc += node.delta_t; + if(node.toc > 1) + { + node.toc = 1; + break; + } + + node.motion1->integrate(node.toc); + node.motion2->integrate(node.toc); + + motion1->getCurrentTransform(tf1); + motion2->getCurrentTransform(tf2); + } + while(1); + + delete o1_tmp; + delete o2_tmp; + + toc = node.toc; + + if(node.toc < 1) + return true; + + return false; } -#endif +namespace details +{ + +template +bool conservativeAdvancementMeshOriented(const BVHModel& o1, + const MotionBase* motion1, + const BVHModel& o2, + const MotionBase* motion2, + const CollisionRequest& request, + CollisionResult& result, + typename BV::S& toc) +{ + using S = typename BV::S; + + Transform3 tf1, tf2; + motion1->getCurrentTransform(tf1); + motion2->getCurrentTransform(tf2); + + // whether the first start configuration is in collision + if(collide(&o1, tf1, &o2, tf2, request, result)) + { + toc = 0; + return true; + } + + + ConservativeAdvancementOrientedNode node; + + initialize(node, o1, tf1, o2, tf2); + + node.motion1 = motion1; + node.motion2 = motion2; + + do + { + node.motion1->getCurrentTransform(tf1); + node.motion2->getCurrentTransform(tf2); + + // compute the transformation from 1 to 2 + Transform3 tf = tf1.inverse(Eigen::Isometry) * tf2; + node.R = tf.linear(); + node.T = tf.translation(); + + node.delta_t = 1; + node.min_distance = std::numeric_limits::max(); + + distanceRecurse(&node, 0, 0, nullptr); + + if(node.delta_t <= node.t_err) + { + // std::cout << node.delta_t << " " << node.t_err << std::endl; + break; + } + + node.toc += node.delta_t; + if(node.toc > 1) + { + node.toc = 1; + break; + } + + node.motion1->integrate(node.toc); + node.motion2->integrate(node.toc); + } + while(1); + + toc = node.toc; + + if(node.toc < 1) + return true; + + return false; +} + + +} + +template +bool conservativeAdvancement(const Shape1& o1, + const MotionBase* motion1, + const Shape2& o2, + const MotionBase* motion2, + const NarrowPhaseSolver* solver, + const CollisionRequest& request, + CollisionResult& result, + typename Shape1::S& toc) +{ + using S = typename Shape1::S; + + Transform3 tf1, tf2; + motion1->getCurrentTransform(tf1); + motion2->getCurrentTransform(tf2); + + // whether the first start configuration is in collision + if(collide(&o1, tf1, &o2, tf2, request, result)) + { + toc = 0; + return true; + } + + ShapeConservativeAdvancementTraversalNode node; + + initialize(node, o1, tf1, o2, tf2, solver); + + node.motion1 = motion1; + node.motion2 = motion2; + + do + { + motion1->getCurrentTransform(tf1); + motion2->getCurrentTransform(tf2); + node.tf1 = tf1; + node.tf2 = tf2; + + node.delta_t = 1; + node.min_distance = std::numeric_limits::max(); + + distanceRecurse(&node, 0, 0, nullptr); + + if(node.delta_t <= node.t_err) + { + // std::cout << node.delta_t << " " << node.t_err << std::endl; + break; + } + + node.toc += node.delta_t; + if(node.toc > 1) + { + node.toc = 1; + break; + } + + node.motion1->integrate(node.toc); + node.motion2->integrate(node.toc); + } + while(1); + + toc = node.toc; + + if(node.toc < 1) + return true; + + return false; +} + +template +bool conservativeAdvancement(const BVHModel& o1, + const MotionBase* motion1, + const Shape& o2, + const MotionBase* motion2, + const NarrowPhaseSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result, + typename BV::S& toc) +{ + using S = typename BV::S; + + Transform3 tf1, tf2; + motion1->getCurrentTransform(tf1); + motion2->getCurrentTransform(tf2); + + if(collide(&o1, tf1, &o2, tf2, request, result)) + { + toc = 0; + return true; + } + + BVHModel* o1_tmp = new BVHModel(o1); + + MeshShapeConservativeAdvancementTraversalNode node; + + node.motion1 = motion1; + node.motion2 = motion2; + + do + { + // initialize update mesh to global coordinate, so time consuming + initialize(node, *o1_tmp, tf1, o2, tf2, nsolver); + + node.delta_t = 1; + node.min_distance = std::numeric_limits::max(); + + distanceRecurse(&node, 0, 0, nullptr); + + if(node.delta_t <= node.t_err) + { + break; + } + + node.toc += node.delta_t; + if(node.toc > 1) + { + node.toc = 1; + break; + } + + node.motion1->integrate(node.toc); + node.motion2->integrate(node.toc); + + motion1->getCurrentTransform(tf1); + motion2->getCurrentTransform(tf2); + } + while(1); + + delete o1_tmp; + + toc = node.toc; + + if(node.toc < 1) + return true; + + return false; +} + +namespace details +{ + +template +bool conservativeAdvancementMeshShapeOriented(const BVHModel& o1, + const MotionBase* motion1, + const Shape& o2, + const MotionBase* motion2, + const NarrowPhaseSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result, + typename BV::S& toc) +{ + using S = typename BV::S; + + Transform3 tf1, tf2; + motion1->getCurrentTransform(tf1); + motion2->getCurrentTransform(tf2); + + if(collide(&o1, tf1, &o2, tf2, request, result)) + { + toc = 0; + return true; + } + + ConservativeAdvancementOrientedNode node; + + initialize(node, o1, tf1, o2, tf2, nsolver); + + node.motion1 = motion1; + node.motion2 = motion2; + + do + { + node.motion1->getCurrentTransform(tf1); + node.motion2->getCurrentTransform(tf2); + + node.tf1 = tf1; + node.tf2 = tf2; + + node.delta_t = 1; + node.min_distance = std::numeric_limits::max(); + + distanceRecurse(&node, 0, 0, nullptr); + + if(node.delta_t <= node.t_err) + { + break; + } + + node.toc += node.delta_t; + if(node.toc > 1) + { + node.toc = 1; + break; + } + + node.motion1->integrate(node.toc); + node.motion2->integrate(node.toc); + } + while(1); + + toc = node.toc; + + if(node.toc < 1) + return true; + + return false; +} + +} + + +template +bool conservativeAdvancement(const BVHModel>& o1, + const MotionBase* motion1, + const Shape& o2, + const MotionBase* motion2, + const NarrowPhaseSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result, + typename Shape::S& toc) +{ + using S = typename Shape::S; + + return details::conservativeAdvancementMeshShapeOriented, Shape, NarrowPhaseSolver, MeshShapeConservativeAdvancementTraversalNodeRSS >(o1, motion1, o2, motion2, nsolver, request, result, toc); +} + +template +bool conservativeAdvancement(const BVHModel>& o1, + const MotionBase* motion1, + const Shape& o2, + const MotionBase* motion2, + const NarrowPhaseSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result, + typename Shape::S& toc) +{ + using S = typename Shape::S; + + return details::conservativeAdvancementMeshShapeOriented, Shape, NarrowPhaseSolver, MeshShapeConservativeAdvancementTraversalNodeOBBRSS >(o1, motion1, o2, motion2, nsolver, request, result, toc); +} + +template +bool conservativeAdvancement(const Shape& o1, + const MotionBase* motion1, + const BVHModel& o2, + const MotionBase* motion2, + const NarrowPhaseSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result, + typename BV::S& toc) +{ + using S = typename BV::S; + + Transform3 tf1, tf2; + motion1->getCurrentTransform(tf1); + motion2->getCurrentTransform(tf2); + + if(collide(&o1, tf1, &o2, tf2, request, result)) + { + toc = 0; + return true; + } + BVHModel* o2_tmp = new BVHModel(o2); + ShapeMeshConservativeAdvancementTraversalNode node; + node.motion1 = motion1; + node.motion2 = motion2; + do + { + // initialize update mesh to global coordinate, so time consuming + initialize(node, o1, tf1, *o2_tmp, tf2, nsolver); + + node.delta_t = 1; + node.min_distance = std::numeric_limits::max(); + + distanceRecurse(&node, 0, 0, nullptr); + + if(node.delta_t <= node.t_err) + { + break; + } + + node.toc += node.delta_t; + if(node.toc > 1) + { + node.toc = 1; + break; + } + + node.motion1->integrate(node.toc); + node.motion2->integrate(node.toc); + + motion1->getCurrentTransform(tf1); + motion2->getCurrentTransform(tf2); + } + while(1); + + delete o2_tmp; + + toc = node.toc; + + if(node.toc < 1) + return true; + + return false; +} + +namespace details +{ + +template +bool conservativeAdvancementShapeMeshOriented(const Shape& o1, + const MotionBase* motion1, + const BVHModel& o2, + const MotionBase* motion2, + const NarrowPhaseSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result, + typename BV::S& toc) +{ + using S = typename BV::S; + Transform3 tf1, tf2; + motion1->getCurrentTransform(tf1); + motion2->getCurrentTransform(tf2); + + if(collide(&o1, tf1, &o2, tf2, request, result)) + { + toc = 0; + return true; + } + + ConservativeAdvancementOrientedNode node; + + initialize(node, o1, tf1, o2, tf2, nsolver); + + node.motion1 = motion1; + node.motion2 = motion2; + + do + { + node.motion1->getCurrentTransform(tf1); + node.motion2->getCurrentTransform(tf2); + + node.tf1 = tf1; + node.tf2 = tf2; + + node.delta_t = 1; + node.min_distance = std::numeric_limits::max(); + + distanceRecurse(&node, 0, 0, nullptr); + + if(node.delta_t <= node.t_err) + { + break; + } + + node.toc += node.delta_t; + if(node.toc > 1) + { + node.toc = 1; + break; + } + + node.motion1->integrate(node.toc); + node.motion2->integrate(node.toc); + } + while(1); + + toc = node.toc; + + if(node.toc < 1) + return true; + + return false; +} + +} + +//============================================================================== +template +struct ConservativeAdvancementImpl +{ + static bool run( + const Shape& o1, + const MotionBase* motion1, + const BVHModel>& o2, + const MotionBase* motion2, + const NarrowPhaseSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result, + S& toc) + { + return details::conservativeAdvancementShapeMeshOriented, NarrowPhaseSolver, ShapeMeshConservativeAdvancementTraversalNodeRSS >(o1, motion1, o2, motion2, nsolver, request, result, toc); + } + + static bool run( + const Shape& o1, + const MotionBase* motion1, + const BVHModel>& o2, + const MotionBase* motion2, + const NarrowPhaseSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result, + S& toc) + { + return details::conservativeAdvancementShapeMeshOriented, NarrowPhaseSolver, ShapeMeshConservativeAdvancementTraversalNodeOBBRSS >(o1, motion1, o2, motion2, nsolver, request, result, toc); + } +}; + +template +bool conservativeAdvancement(const Shape& o1, + const MotionBase* motion1, + const BVHModel>& o2, + const MotionBase* motion2, + const NarrowPhaseSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result, + typename Shape::S& toc) +{ + return ConservativeAdvancementImpl< + typename Shape::S, Shape, NarrowPhaseSolver>::run( + o1, motion1, o2, motion2, nsolver, request, result, toc); +} + +template +bool conservativeAdvancement(const Shape& o1, + const MotionBase* motion1, + const BVHModel>& o2, + const MotionBase* motion2, + const NarrowPhaseSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result, + typename Shape::S& toc) +{ + return ConservativeAdvancementImpl< + typename Shape::S, Shape, NarrowPhaseSolver>::run( + o1, motion1, o2, motion2, nsolver, request, result, toc); +} + +//============================================================================== +template +struct ConservativeAdvancementImpl>, NarrowPhaseSolver> +{ + static bool run( + const BVHModel>& o1, + const MotionBase* motion1, + const BVHModel>& o2, + const MotionBase* motion2, + const NarrowPhaseSolver* /*nsolver*/, + const CollisionRequest& request, + CollisionResult& result, + S& toc) + { + return details::conservativeAdvancementMeshOriented, MeshConservativeAdvancementTraversalNodeRSS>(o1, motion1, o2, motion2, request, result, toc); + } +}; + +//============================================================================== +template +struct ConservativeAdvancementImpl>, NarrowPhaseSolver> +{ + static bool run( + const BVHModel>& o1, + const MotionBase* motion1, + const BVHModel>& o2, + const MotionBase* motion2, + const NarrowPhaseSolver* /*nsolver*/, + const CollisionRequest& request, + CollisionResult& result, + S& toc) + { + return details::conservativeAdvancementMeshOriented, MeshConservativeAdvancementTraversalNodeOBBRSS>(o1, motion1, o2, motion2, request, result, toc); + } +}; + +template +typename BV::S BVHConservativeAdvancement(const CollisionGeometry* o1, const MotionBase* motion1, const CollisionGeometry* o2, const MotionBase* motion2, const NarrowPhaseSolver* nsolver, const ContinuousCollisionRequest& request, ContinuousCollisionResult& result) +{ + using S = typename BV::S; + + const BVHModel* obj1 = static_cast*>(o1); + const BVHModel* obj2 = static_cast*>(o2); + + CollisionRequest c_request; + CollisionResult c_result; + S toc; + bool is_collide = conservativeAdvancement(*obj1, motion1, *obj2, motion2, c_request, c_result, toc); + + result.is_collide = is_collide; + result.time_of_contact = toc; + + return toc; +} + +template +typename Shape1::S ShapeConservativeAdvancement(const CollisionGeometry* o1, const MotionBase* motion1, const CollisionGeometry* o2, const MotionBase* motion2, const NarrowPhaseSolver* nsolver, const ContinuousCollisionRequest& request, ContinuousCollisionResult& result) +{ + using S = typename Shape1::S; + + const Shape1* obj1 = static_cast(o1); + const Shape2* obj2 = static_cast(o2); + + CollisionRequest c_request; + CollisionResult c_result; + S toc; + bool is_collide = conservativeAdvancement(*obj1, motion1, *obj2, motion2, nsolver, c_request, c_result, toc); + + result.is_collide = is_collide; + result.time_of_contact = toc; + + return toc; +} + +template +typename BV::S ShapeBVHConservativeAdvancement(const CollisionGeometry* o1, const MotionBase* motion1, const CollisionGeometry* o2, const MotionBase* motion2, const NarrowPhaseSolver* nsolver, const ContinuousCollisionRequest& request, ContinuousCollisionResult& result) +{ + using S = typename BV::S; + + const Shape* obj1 = static_cast(o1); + const BVHModel* obj2 = static_cast*>(o2); + + CollisionRequest c_request; + CollisionResult c_result; + S toc; + + bool is_collide = conservativeAdvancement(*obj1, motion1, *obj2, motion2, nsolver, c_request, c_result, toc); + + result.is_collide = is_collide; + result.time_of_contact = toc; + + return toc; +} + +template +typename BV::S BVHShapeConservativeAdvancement(const CollisionGeometry* o1, const MotionBase* motion1, const CollisionGeometry* o2, const MotionBase* motion2, const NarrowPhaseSolver* nsolver, const ContinuousCollisionRequest& request, ContinuousCollisionResult& result) +{ + using S = typename BV::S; + + const BVHModel* obj1 = static_cast*>(o1); + const Shape* obj2 = static_cast(o2); + + CollisionRequest c_request; + CollisionResult c_result; + S toc; + + bool is_collide = conservativeAdvancement(*obj1, motion1, *obj2, motion2, nsolver, c_request, c_result, toc); + + result.is_collide = is_collide; + result.time_of_contact = toc; + + return toc; +} + +template +ConservativeAdvancementFunctionMatrix::ConservativeAdvancementFunctionMatrix() +{ + using S = typename NarrowPhaseSolver::S; + + for(int i = 0; i < NODE_COUNT; ++i) + { + for(int j = 0; j < NODE_COUNT; ++j) + conservative_advancement_matrix[i][j] = nullptr; + } + + + conservative_advancement_matrix[GEOM_BOX][GEOM_BOX] = &ShapeConservativeAdvancement, Box, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_BOX][GEOM_SPHERE] = &ShapeConservativeAdvancement, Sphere, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_BOX][GEOM_CAPSULE] = &ShapeConservativeAdvancement, Capsule, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_BOX][GEOM_CONE] = &ShapeConservativeAdvancement, Cone, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_BOX][GEOM_CYLINDER] = &ShapeConservativeAdvancement, Cylinder, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_BOX][GEOM_CONVEX] = &ShapeConservativeAdvancement, Convex, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_BOX][GEOM_PLANE] = &ShapeConservativeAdvancement, Plane, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_BOX][GEOM_HALFSPACE] = &ShapeConservativeAdvancement, Halfspace, NarrowPhaseSolver>; + + conservative_advancement_matrix[GEOM_SPHERE][GEOM_BOX] = &ShapeConservativeAdvancement, Box, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_SPHERE][GEOM_SPHERE] = &ShapeConservativeAdvancement, Sphere, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_SPHERE][GEOM_CAPSULE] = &ShapeConservativeAdvancement, Capsule, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_SPHERE][GEOM_CONE] = &ShapeConservativeAdvancement, Cone, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_SPHERE][GEOM_CYLINDER] = &ShapeConservativeAdvancement, Cylinder, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_SPHERE][GEOM_CONVEX] = &ShapeConservativeAdvancement, Convex, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_SPHERE][GEOM_PLANE] = &ShapeConservativeAdvancement, Plane, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_SPHERE][GEOM_HALFSPACE] = &ShapeConservativeAdvancement, Halfspace, NarrowPhaseSolver>; + + conservative_advancement_matrix[GEOM_CAPSULE][GEOM_BOX] = &ShapeConservativeAdvancement, Box, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_CAPSULE][GEOM_SPHERE] = &ShapeConservativeAdvancement, Sphere, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_CAPSULE][GEOM_CAPSULE] = &ShapeConservativeAdvancement, Capsule, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_CAPSULE][GEOM_CONE] = &ShapeConservativeAdvancement, Cone, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_CAPSULE][GEOM_CYLINDER] = &ShapeConservativeAdvancement, Cylinder, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_CAPSULE][GEOM_CONVEX] = &ShapeConservativeAdvancement, Convex, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_CAPSULE][GEOM_PLANE] = &ShapeConservativeAdvancement, Plane, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_CAPSULE][GEOM_HALFSPACE] = &ShapeConservativeAdvancement, Halfspace, NarrowPhaseSolver>; + + conservative_advancement_matrix[GEOM_CONE][GEOM_BOX] = &ShapeConservativeAdvancement, Box, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_CONE][GEOM_SPHERE] = &ShapeConservativeAdvancement, Sphere, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_CONE][GEOM_CAPSULE] = &ShapeConservativeAdvancement, Capsule, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_CONE][GEOM_CONE] = &ShapeConservativeAdvancement, Cone, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_CONE][GEOM_CYLINDER] = &ShapeConservativeAdvancement, Cylinder, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_CONE][GEOM_CONVEX] = &ShapeConservativeAdvancement, Convex, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_CONE][GEOM_PLANE] = &ShapeConservativeAdvancement, Plane, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_CONE][GEOM_HALFSPACE] = &ShapeConservativeAdvancement, Halfspace, NarrowPhaseSolver>; + + conservative_advancement_matrix[GEOM_CYLINDER][GEOM_BOX] = &ShapeConservativeAdvancement, Box, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_CYLINDER][GEOM_SPHERE] = &ShapeConservativeAdvancement, Sphere, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_CYLINDER][GEOM_CAPSULE] = &ShapeConservativeAdvancement, Capsule, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_CYLINDER][GEOM_CONE] = &ShapeConservativeAdvancement, Cone, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_CYLINDER][GEOM_CYLINDER] = &ShapeConservativeAdvancement, Cylinder, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_CYLINDER][GEOM_CONVEX] = &ShapeConservativeAdvancement, Convex, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_CYLINDER][GEOM_PLANE] = &ShapeConservativeAdvancement, Plane, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_CYLINDER][GEOM_HALFSPACE] = &ShapeConservativeAdvancement, Halfspace, NarrowPhaseSolver>; + + conservative_advancement_matrix[GEOM_CONVEX][GEOM_BOX] = &ShapeConservativeAdvancement, Box, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_CONVEX][GEOM_SPHERE] = &ShapeConservativeAdvancement, Sphere, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_CONVEX][GEOM_CAPSULE] = &ShapeConservativeAdvancement, Capsule, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_CONVEX][GEOM_CONE] = &ShapeConservativeAdvancement, Cone, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_CONVEX][GEOM_CYLINDER] = &ShapeConservativeAdvancement, Cylinder, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_CONVEX][GEOM_CONVEX] = &ShapeConservativeAdvancement, Convex, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_CONVEX][GEOM_PLANE] = &ShapeConservativeAdvancement, Plane, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_CONVEX][GEOM_HALFSPACE] = &ShapeConservativeAdvancement, Halfspace, NarrowPhaseSolver>; + + conservative_advancement_matrix[GEOM_PLANE][GEOM_BOX] = &ShapeConservativeAdvancement, Box, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_PLANE][GEOM_SPHERE] = &ShapeConservativeAdvancement, Sphere, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_PLANE][GEOM_CAPSULE] = &ShapeConservativeAdvancement, Capsule, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_PLANE][GEOM_CONE] = &ShapeConservativeAdvancement, Cone, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_PLANE][GEOM_CYLINDER] = &ShapeConservativeAdvancement, Cylinder, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_PLANE][GEOM_CONVEX] = &ShapeConservativeAdvancement, Convex, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_PLANE][GEOM_PLANE] = &ShapeConservativeAdvancement, Plane, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_PLANE][GEOM_HALFSPACE] = &ShapeConservativeAdvancement, Halfspace, NarrowPhaseSolver>; + + conservative_advancement_matrix[GEOM_HALFSPACE][GEOM_BOX] = &ShapeConservativeAdvancement, Box, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_HALFSPACE][GEOM_SPHERE] = &ShapeConservativeAdvancement, Sphere, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_HALFSPACE][GEOM_CAPSULE] = &ShapeConservativeAdvancement, Capsule, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_HALFSPACE][GEOM_CONE] = &ShapeConservativeAdvancement, Cone, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_HALFSPACE][GEOM_CYLINDER] = &ShapeConservativeAdvancement, Cylinder, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_HALFSPACE][GEOM_CONVEX] = &ShapeConservativeAdvancement, Convex, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_HALFSPACE][GEOM_PLANE] = &ShapeConservativeAdvancement, Plane, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_HALFSPACE][GEOM_HALFSPACE] = &ShapeConservativeAdvancement, Halfspace, NarrowPhaseSolver>; + + conservative_advancement_matrix[BV_AABB][GEOM_BOX] = &BVHShapeConservativeAdvancement, Box, NarrowPhaseSolver>; + conservative_advancement_matrix[BV_AABB][GEOM_SPHERE] = &BVHShapeConservativeAdvancement, Sphere, NarrowPhaseSolver>; + conservative_advancement_matrix[BV_AABB][GEOM_CAPSULE] = &BVHShapeConservativeAdvancement, Capsule, NarrowPhaseSolver>; + conservative_advancement_matrix[BV_AABB][GEOM_CONE] = &BVHShapeConservativeAdvancement, Cone, NarrowPhaseSolver>; + conservative_advancement_matrix[BV_AABB][GEOM_CYLINDER] = &BVHShapeConservativeAdvancement, Cylinder, NarrowPhaseSolver>; + conservative_advancement_matrix[BV_AABB][GEOM_CONVEX] = &BVHShapeConservativeAdvancement, Convex, NarrowPhaseSolver>; + conservative_advancement_matrix[BV_AABB][GEOM_PLANE] = &BVHShapeConservativeAdvancement, Plane, NarrowPhaseSolver>; + conservative_advancement_matrix[BV_AABB][GEOM_HALFSPACE] = &BVHShapeConservativeAdvancement, Halfspace, NarrowPhaseSolver>; + + conservative_advancement_matrix[BV_OBB][GEOM_BOX] = &BVHShapeConservativeAdvancement, Box, NarrowPhaseSolver>; + conservative_advancement_matrix[BV_OBB][GEOM_SPHERE] = &BVHShapeConservativeAdvancement, Sphere, NarrowPhaseSolver>; + conservative_advancement_matrix[BV_OBB][GEOM_CAPSULE] = &BVHShapeConservativeAdvancement, Capsule, NarrowPhaseSolver>; + conservative_advancement_matrix[BV_OBB][GEOM_CONE] = &BVHShapeConservativeAdvancement, Cone, NarrowPhaseSolver>; + conservative_advancement_matrix[BV_OBB][GEOM_CYLINDER] = &BVHShapeConservativeAdvancement, Cylinder, NarrowPhaseSolver>; + conservative_advancement_matrix[BV_OBB][GEOM_CONVEX] = &BVHShapeConservativeAdvancement, Convex, NarrowPhaseSolver>; + conservative_advancement_matrix[BV_OBB][GEOM_PLANE] = &BVHShapeConservativeAdvancement, Plane, NarrowPhaseSolver>; + conservative_advancement_matrix[BV_OBB][GEOM_HALFSPACE] = &BVHShapeConservativeAdvancement, Halfspace, NarrowPhaseSolver>; + + conservative_advancement_matrix[BV_OBBRSS][GEOM_BOX] = &BVHShapeConservativeAdvancement, Box, NarrowPhaseSolver>; + conservative_advancement_matrix[BV_OBBRSS][GEOM_SPHERE] = &BVHShapeConservativeAdvancement, Sphere, NarrowPhaseSolver>; + conservative_advancement_matrix[BV_OBBRSS][GEOM_CAPSULE] = &BVHShapeConservativeAdvancement, Capsule, NarrowPhaseSolver>; + conservative_advancement_matrix[BV_OBBRSS][GEOM_CONE] = &BVHShapeConservativeAdvancement, Cone, NarrowPhaseSolver>; + conservative_advancement_matrix[BV_OBBRSS][GEOM_CYLINDER] = &BVHShapeConservativeAdvancement, Cylinder, NarrowPhaseSolver>; + conservative_advancement_matrix[BV_OBBRSS][GEOM_CONVEX] = &BVHShapeConservativeAdvancement, Convex, NarrowPhaseSolver>; + conservative_advancement_matrix[BV_OBBRSS][GEOM_PLANE] = &BVHShapeConservativeAdvancement, Plane, NarrowPhaseSolver>; + conservative_advancement_matrix[BV_OBBRSS][GEOM_HALFSPACE] = &BVHShapeConservativeAdvancement, Halfspace, NarrowPhaseSolver>; + + conservative_advancement_matrix[BV_RSS][GEOM_BOX] = &BVHShapeConservativeAdvancement, Box, NarrowPhaseSolver>; + conservative_advancement_matrix[BV_RSS][GEOM_SPHERE] = &BVHShapeConservativeAdvancement, Sphere, NarrowPhaseSolver>; + conservative_advancement_matrix[BV_RSS][GEOM_CAPSULE] = &BVHShapeConservativeAdvancement, Capsule, NarrowPhaseSolver>; + conservative_advancement_matrix[BV_RSS][GEOM_CONE] = &BVHShapeConservativeAdvancement, Cone, NarrowPhaseSolver>; + conservative_advancement_matrix[BV_RSS][GEOM_CYLINDER] = &BVHShapeConservativeAdvancement, Cylinder, NarrowPhaseSolver>; + conservative_advancement_matrix[BV_RSS][GEOM_CONVEX] = &BVHShapeConservativeAdvancement, Convex, NarrowPhaseSolver>; + conservative_advancement_matrix[BV_RSS][GEOM_PLANE] = &BVHShapeConservativeAdvancement, Plane, NarrowPhaseSolver>; + conservative_advancement_matrix[BV_RSS][GEOM_HALFSPACE] = &BVHShapeConservativeAdvancement, Halfspace, NarrowPhaseSolver>; + + conservative_advancement_matrix[BV_KDOP16][GEOM_BOX] = &BVHShapeConservativeAdvancement, Box, NarrowPhaseSolver>; + conservative_advancement_matrix[BV_KDOP16][GEOM_SPHERE] = &BVHShapeConservativeAdvancement, Sphere, NarrowPhaseSolver>; + conservative_advancement_matrix[BV_KDOP16][GEOM_CAPSULE] = &BVHShapeConservativeAdvancement, Capsule, NarrowPhaseSolver>; + conservative_advancement_matrix[BV_KDOP16][GEOM_CONE] = &BVHShapeConservativeAdvancement, Cone, NarrowPhaseSolver>; + conservative_advancement_matrix[BV_KDOP16][GEOM_CYLINDER] = &BVHShapeConservativeAdvancement, Cylinder, NarrowPhaseSolver>; + conservative_advancement_matrix[BV_KDOP16][GEOM_CONVEX] = &BVHShapeConservativeAdvancement, Convex, NarrowPhaseSolver>; + conservative_advancement_matrix[BV_KDOP16][GEOM_PLANE] = &BVHShapeConservativeAdvancement, Plane, NarrowPhaseSolver>; + conservative_advancement_matrix[BV_KDOP16][GEOM_HALFSPACE] = &BVHShapeConservativeAdvancement, Halfspace, NarrowPhaseSolver>; + + conservative_advancement_matrix[BV_KDOP18][GEOM_BOX] = &BVHShapeConservativeAdvancement, Box, NarrowPhaseSolver>; + conservative_advancement_matrix[BV_KDOP18][GEOM_SPHERE] = &BVHShapeConservativeAdvancement, Sphere, NarrowPhaseSolver>; + conservative_advancement_matrix[BV_KDOP18][GEOM_CAPSULE] = &BVHShapeConservativeAdvancement, Capsule, NarrowPhaseSolver>; + conservative_advancement_matrix[BV_KDOP18][GEOM_CONE] = &BVHShapeConservativeAdvancement, Cone, NarrowPhaseSolver>; + conservative_advancement_matrix[BV_KDOP18][GEOM_CYLINDER] = &BVHShapeConservativeAdvancement, Cylinder, NarrowPhaseSolver>; + conservative_advancement_matrix[BV_KDOP18][GEOM_CONVEX] = &BVHShapeConservativeAdvancement, Convex, NarrowPhaseSolver>; + conservative_advancement_matrix[BV_KDOP18][GEOM_PLANE] = &BVHShapeConservativeAdvancement, Plane, NarrowPhaseSolver>; + conservative_advancement_matrix[BV_KDOP18][GEOM_HALFSPACE] = &BVHShapeConservativeAdvancement, Halfspace, NarrowPhaseSolver>; + + conservative_advancement_matrix[BV_KDOP24][GEOM_BOX] = &BVHShapeConservativeAdvancement, Box, NarrowPhaseSolver>; + conservative_advancement_matrix[BV_KDOP24][GEOM_SPHERE] = &BVHShapeConservativeAdvancement, Sphere, NarrowPhaseSolver>; + conservative_advancement_matrix[BV_KDOP24][GEOM_CAPSULE] = &BVHShapeConservativeAdvancement, Capsule, NarrowPhaseSolver>; + conservative_advancement_matrix[BV_KDOP24][GEOM_CONE] = &BVHShapeConservativeAdvancement, Cone, NarrowPhaseSolver>; + conservative_advancement_matrix[BV_KDOP24][GEOM_CYLINDER] = &BVHShapeConservativeAdvancement, Cylinder, NarrowPhaseSolver>; + conservative_advancement_matrix[BV_KDOP24][GEOM_CONVEX] = &BVHShapeConservativeAdvancement, Convex, NarrowPhaseSolver>; + conservative_advancement_matrix[BV_KDOP24][GEOM_PLANE] = &BVHShapeConservativeAdvancement, Plane, NarrowPhaseSolver>; + conservative_advancement_matrix[BV_KDOP24][GEOM_HALFSPACE] = &BVHShapeConservativeAdvancement, Halfspace, NarrowPhaseSolver>; + + conservative_advancement_matrix[BV_kIOS][GEOM_BOX] = &BVHShapeConservativeAdvancement, Box, NarrowPhaseSolver>; + conservative_advancement_matrix[BV_kIOS][GEOM_SPHERE] = &BVHShapeConservativeAdvancement, Sphere, NarrowPhaseSolver>; + conservative_advancement_matrix[BV_kIOS][GEOM_CAPSULE] = &BVHShapeConservativeAdvancement, Capsule, NarrowPhaseSolver>; + conservative_advancement_matrix[BV_kIOS][GEOM_CONE] = &BVHShapeConservativeAdvancement, Cone, NarrowPhaseSolver>; + conservative_advancement_matrix[BV_kIOS][GEOM_CYLINDER] = &BVHShapeConservativeAdvancement, Cylinder, NarrowPhaseSolver>; + conservative_advancement_matrix[BV_kIOS][GEOM_CONVEX] = &BVHShapeConservativeAdvancement, Convex, NarrowPhaseSolver>; + conservative_advancement_matrix[BV_kIOS][GEOM_PLANE] = &BVHShapeConservativeAdvancement, Plane, NarrowPhaseSolver>; + conservative_advancement_matrix[BV_kIOS][GEOM_HALFSPACE] = &BVHShapeConservativeAdvancement, Halfspace, NarrowPhaseSolver>; + + + conservative_advancement_matrix[GEOM_BOX][BV_AABB] = &ShapeBVHConservativeAdvancement, AABB, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_SPHERE][BV_AABB] = &ShapeBVHConservativeAdvancement, AABB, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_CAPSULE][BV_AABB] = &ShapeBVHConservativeAdvancement, AABB, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_CONE][BV_AABB] = &ShapeBVHConservativeAdvancement, AABB, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_CYLINDER][BV_AABB] = &ShapeBVHConservativeAdvancement, AABB, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_CONVEX][BV_AABB] = &ShapeBVHConservativeAdvancement, AABB, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_PLANE][BV_AABB] = &ShapeBVHConservativeAdvancement, AABB, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_HALFSPACE][BV_AABB] = &ShapeBVHConservativeAdvancement, AABB, NarrowPhaseSolver>; + + conservative_advancement_matrix[GEOM_BOX][BV_OBB] = &ShapeBVHConservativeAdvancement, OBB, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_SPHERE][BV_OBB] = &ShapeBVHConservativeAdvancement, OBB, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_CAPSULE][BV_OBB] = &ShapeBVHConservativeAdvancement, OBB, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_CONE][BV_OBB] = &ShapeBVHConservativeAdvancement, OBB, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_CYLINDER][BV_OBB] = &ShapeBVHConservativeAdvancement, OBB, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_CONVEX][BV_OBB] = &ShapeBVHConservativeAdvancement, OBB, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_PLANE][BV_OBB] = &ShapeBVHConservativeAdvancement, OBB, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_HALFSPACE][BV_OBB] = &ShapeBVHConservativeAdvancement, OBB, NarrowPhaseSolver>; + + conservative_advancement_matrix[GEOM_BOX][BV_RSS] = &ShapeBVHConservativeAdvancement, RSS, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_SPHERE][BV_RSS] = &ShapeBVHConservativeAdvancement, RSS, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_CAPSULE][BV_RSS] = &ShapeBVHConservativeAdvancement, RSS, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_CONE][BV_RSS] = &ShapeBVHConservativeAdvancement, RSS, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_CYLINDER][BV_RSS] = &ShapeBVHConservativeAdvancement, RSS, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_CONVEX][BV_RSS] = &ShapeBVHConservativeAdvancement, RSS, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_PLANE][BV_RSS] = &ShapeBVHConservativeAdvancement, RSS, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_HALFSPACE][BV_RSS] = &ShapeBVHConservativeAdvancement, RSS, NarrowPhaseSolver>; + + conservative_advancement_matrix[GEOM_BOX][BV_OBBRSS] = &ShapeBVHConservativeAdvancement, OBBRSS, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_SPHERE][BV_OBBRSS] = &ShapeBVHConservativeAdvancement, OBBRSS, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_CAPSULE][BV_OBBRSS] = &ShapeBVHConservativeAdvancement, OBBRSS, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_CONE][BV_OBBRSS] = &ShapeBVHConservativeAdvancement, OBBRSS, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_CYLINDER][BV_OBBRSS] = &ShapeBVHConservativeAdvancement, OBBRSS, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_CONVEX][BV_OBBRSS] = &ShapeBVHConservativeAdvancement, OBBRSS, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_PLANE][BV_OBBRSS] = &ShapeBVHConservativeAdvancement, OBBRSS, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_HALFSPACE][BV_OBBRSS] = &ShapeBVHConservativeAdvancement, OBBRSS, NarrowPhaseSolver>; + + conservative_advancement_matrix[GEOM_BOX][BV_KDOP16] = &ShapeBVHConservativeAdvancement, KDOP, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_SPHERE][BV_KDOP16] = &ShapeBVHConservativeAdvancement, KDOP, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_CAPSULE][BV_KDOP16] = &ShapeBVHConservativeAdvancement, KDOP, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_CONE][BV_KDOP16] = &ShapeBVHConservativeAdvancement, KDOP, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_CYLINDER][BV_KDOP16] = &ShapeBVHConservativeAdvancement, KDOP, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_CONVEX][BV_KDOP16] = &ShapeBVHConservativeAdvancement, KDOP, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_PLANE][BV_KDOP16] = &ShapeBVHConservativeAdvancement, KDOP, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_HALFSPACE][BV_KDOP16] = &ShapeBVHConservativeAdvancement, KDOP, NarrowPhaseSolver>; + + conservative_advancement_matrix[GEOM_BOX][BV_KDOP18] = &ShapeBVHConservativeAdvancement, KDOP, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_SPHERE][BV_KDOP18] = &ShapeBVHConservativeAdvancement, KDOP, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_CAPSULE][BV_KDOP18] = &ShapeBVHConservativeAdvancement, KDOP, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_CONE][BV_KDOP18] = &ShapeBVHConservativeAdvancement, KDOP, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_CYLINDER][BV_KDOP18] = &ShapeBVHConservativeAdvancement, KDOP, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_CONVEX][BV_KDOP18] = &ShapeBVHConservativeAdvancement, KDOP, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_PLANE][BV_KDOP18] = &ShapeBVHConservativeAdvancement, KDOP, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_HALFSPACE][BV_KDOP18] = &ShapeBVHConservativeAdvancement, KDOP, NarrowPhaseSolver>; + + conservative_advancement_matrix[GEOM_BOX][BV_KDOP24] = &ShapeBVHConservativeAdvancement, KDOP, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_SPHERE][BV_KDOP24] = &ShapeBVHConservativeAdvancement, KDOP, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_CAPSULE][BV_KDOP24] = &ShapeBVHConservativeAdvancement, KDOP, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_CONE][BV_KDOP24] = &ShapeBVHConservativeAdvancement, KDOP, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_CYLINDER][BV_KDOP24] = &ShapeBVHConservativeAdvancement, KDOP, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_CONVEX][BV_KDOP24] = &ShapeBVHConservativeAdvancement, KDOP, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_PLANE][BV_KDOP24] = &ShapeBVHConservativeAdvancement, KDOP, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_HALFSPACE][BV_KDOP24] = &ShapeBVHConservativeAdvancement, KDOP, NarrowPhaseSolver>; + + conservative_advancement_matrix[GEOM_BOX][BV_kIOS] = &ShapeBVHConservativeAdvancement, kIOS, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_SPHERE][BV_kIOS] = &ShapeBVHConservativeAdvancement, kIOS, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_CAPSULE][BV_kIOS] = &ShapeBVHConservativeAdvancement, kIOS, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_CONE][BV_kIOS] = &ShapeBVHConservativeAdvancement, kIOS, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_CYLINDER][BV_kIOS] = &ShapeBVHConservativeAdvancement, kIOS, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_CONVEX][BV_kIOS] = &ShapeBVHConservativeAdvancement, kIOS, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_PLANE][BV_kIOS] = &ShapeBVHConservativeAdvancement, kIOS, NarrowPhaseSolver>; + conservative_advancement_matrix[GEOM_HALFSPACE][BV_kIOS] = &ShapeBVHConservativeAdvancement, kIOS, NarrowPhaseSolver>; + + conservative_advancement_matrix[BV_AABB][BV_AABB] = &BVHConservativeAdvancement, NarrowPhaseSolver>; + conservative_advancement_matrix[BV_OBB][BV_OBB] = &BVHConservativeAdvancement, NarrowPhaseSolver>; + conservative_advancement_matrix[BV_RSS][BV_RSS] = &BVHConservativeAdvancement, NarrowPhaseSolver>; + conservative_advancement_matrix[BV_OBBRSS][BV_OBBRSS] = &BVHConservativeAdvancement, NarrowPhaseSolver>; + conservative_advancement_matrix[BV_KDOP16][BV_KDOP16] = &BVHConservativeAdvancement, NarrowPhaseSolver>; + conservative_advancement_matrix[BV_KDOP18][BV_KDOP18] = &BVHConservativeAdvancement, NarrowPhaseSolver>; + conservative_advancement_matrix[BV_KDOP24][BV_KDOP24] = &BVHConservativeAdvancement, NarrowPhaseSolver>; + conservative_advancement_matrix[BV_kIOS][BV_kIOS] = &BVHConservativeAdvancement, NarrowPhaseSolver>; + +} + + +} // namespace fcl + +#endif diff --git a/include/fcl/ccd/interval.h b/include/fcl/ccd/interval.h index aaf7c3df9..fbcbbce7d 100644 --- a/include/fcl/ccd/interval.h +++ b/include/fcl/ccd/interval.h @@ -36,194 +36,482 @@ // This code is based on code developed by Stephane Redon at UNC and Inria for the CATCH library: http://graphics.ewha.ac.kr/CATCH/ /** \author Jia Pan */ - #ifndef FCL_CCD_INTERVAL_H #define FCL_CCD_INTERVAL_H +#include #include "fcl/data_types.h" namespace fcl { /// @brief Interval class for [a, b] +template struct Interval { - FCL_REAL i_[2]; + S i_[2]; - Interval() { i_[0] = i_[1] = 0; } + Interval(); - explicit Interval(FCL_REAL v) - { - i_[0] = i_[1] = v; - } + explicit Interval(S v); /// @brief construct interval [left, right] - Interval(FCL_REAL left, FCL_REAL right) - { - i_[0] = left; i_[1] = right; - } + Interval(S left, S right); /// @brief construct interval [left, right] - inline void setValue(FCL_REAL a, FCL_REAL b) - { - i_[0] = a; i_[1] = b; - } + void setValue(S a, S b); /// @brief construct zero interval [x, x] - inline void setValue(FCL_REAL x) - { - i_[0] = i_[1] = x; - } + void setValue(S x); /// @brief access the interval endpoints: 0 for left, 1 for right end - inline FCL_REAL operator [] (size_t i) const - { - return i_[i]; - } + S operator [] (size_t i) const; /// @brief access the interval endpoints: 0 for left, 1 for right end - inline FCL_REAL& operator [] (size_t i) - { - return i_[i]; - } + S& operator [] (size_t i); /// @brief whether two intervals are the same - inline bool operator == (const Interval& other) const - { - if(i_[0] != other.i_[0]) return false; - if(i_[1] != other.i_[1]) return false; - return true; - } + bool operator == (const Interval& other) const; /// @brief add two intervals - inline Interval operator + (const Interval& other) const - { - return Interval(i_[0] + other.i_[0], i_[1] + other.i_[1]); - } + Interval operator + (const Interval& other) const; /// @brief minus another interval - inline Interval operator - (const Interval& other) const + Interval operator - (const Interval& other) const; + + Interval& operator += (const Interval& other); + + Interval& operator -= (const Interval& other); + + Interval operator * (const Interval& other) const; + + Interval& operator *= (const Interval& other); + + Interval operator * (S d) const; + + Interval& operator *= (S d); + + /// @brief other must not contain 0 + Interval operator / (const Interval& other) const; + + Interval& operator /= (const Interval& other); + + /// @brief determine whether the intersection between intervals is empty + bool overlap(const Interval& other) const; + + bool intersect(const Interval& other); + + Interval operator - () const; + + /// @brief Return the nearest distance for points within the interval to zero + S getAbsLower() const; + + /// @brief Return the farthest distance for points within the interval to zero + S getAbsUpper() const; + + bool contains(S v) const; + + /// @brief Compute the minimum interval contains v and original interval + Interval& bound(S v); + + /// @brief Compute the minimum interval contains other and original interval + Interval& bound(const Interval& other); + + void print() const; + + S center() const; + + S diameter() const; +}; + +template +Interval bound(const Interval& i, S v); + +template +Interval bound(const Interval& i, const Interval& other); + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +Interval::Interval() +{ + i_[0] = i_[1] = 0; +} + +//============================================================================== +template +Interval::Interval(S v) +{ + i_[0] = i_[1] = v; +} + +//============================================================================== +template +Interval::Interval(S left, S right) +{ + i_[0] = left; i_[1] = right; +} + +//============================================================================== +template +void Interval::setValue(S a, S b) +{ + i_[0] = a; i_[1] = b; +} + +//============================================================================== +template +void Interval::setValue(S x) +{ + i_[0] = i_[1] = x; +} + +//============================================================================== +template +S Interval::operator [] (size_t i) const +{ + return i_[i]; +} + +//============================================================================== +template +S& Interval::operator [] (size_t i) +{ + return i_[i]; +} + +//============================================================================== +template +bool Interval::operator == (const Interval& other) const +{ + if(i_[0] != other.i_[0]) return false; + if(i_[1] != other.i_[1]) return false; + return true; +} + +//============================================================================== +template +Interval Interval::operator + (const Interval& other) const +{ + return Interval(i_[0] + other.i_[0], i_[1] + other.i_[1]); +} + +//============================================================================== +template +Interval Interval::operator - (const Interval& other) const +{ + return Interval(i_[0] - other.i_[1], i_[1] - other.i_[0]); +} + +//============================================================================== +template +Interval& Interval::operator += (const Interval& other) +{ + i_[0] += other.i_[0]; + i_[1] += other.i_[1]; + return *this; +} + +//============================================================================== +template +Interval& Interval::operator -= (const Interval& other) +{ + i_[0] -= other.i_[1]; + i_[1] -= other.i_[0]; + return *this; +} + +//============================================================================== +template +Interval Interval::operator * (const Interval& other) const +{ + if(other.i_[0] >= 0) { - return Interval(i_[0] - other.i_[1], i_[1] - other.i_[0]); + if(i_[0] >= 0) return Interval(i_[0] * other.i_[0], i_[1] * other.i_[1]); + if(i_[1] <= 0) return Interval(i_[0] * other.i_[1], i_[1] * other.i_[0]); + return Interval(i_[0] * other.i_[1], i_[1] * other.i_[1]); } - - inline Interval& operator += (const Interval& other) + if(other.i_[1] <= 0) { - i_[0] += other.i_[0]; - i_[1] += other.i_[1]; - return *this; + if(i_[0] >= 0) return Interval(i_[1] * other.i_[0], i_[0] * other.i_[1]); + if(i_[1] <= 0) return Interval(i_[1] * other.i_[1], i_[0] * other.i_[0]); + return Interval(i_[1] * other.i_[0], i_[0] * other.i_[0]); } - inline Interval& operator -= (const Interval& other) + if(i_[0] >= 0) return Interval(i_[1] * other.i_[0], i_[1] * other.i_[1]); + if(i_[1] <= 0) return Interval(i_[0] * other.i_[1], i_[0] * other.i_[0]); + + S v00 = i_[0] * other.i_[0]; + S v11 = i_[1] * other.i_[1]; + if(v00 <= v11) { - i_[0] -= other.i_[1]; - i_[1] -= other.i_[0]; - return *this; + S v01 = i_[0] * other.i_[1]; + S v10 = i_[1] * other.i_[0]; + if(v01 < v10) return Interval(v01, v11); + return Interval(v10, v11); } - Interval operator * (const Interval& other) const; - - Interval& operator *= (const Interval& other); + S v01 = i_[0] * other.i_[1]; + S v10 = i_[1] * other.i_[0]; + if(v01 < v10) return Interval(v01, v00); + return Interval(v10, v00); +} - inline Interval operator * (FCL_REAL d) const +//============================================================================== +template +Interval& Interval::operator *= (const Interval& other) +{ + if(other.i_[0] >= 0) { - if(d >= 0) return Interval(i_[0] * d, i_[1] * d); - return Interval(i_[1] * d, i_[0] * d); + if(i_[0] >= 0) + { + i_[0] *= other.i_[0]; + i_[1] *= other.i_[1]; + } + else if(i_[1] <= 0) + { + i_[0] *= other.i_[1]; + i_[1] *= other.i_[0]; + } + else + { + i_[0] *= other.i_[1]; + i_[1] *= other.i_[1]; + } + return *this; } - inline Interval& operator *= (FCL_REAL d) + if(other.i_[1] <= 0) { - if(d >= 0) + if(i_[0] >= 0) { - i_[0] *= d; - i_[1] *= d; + S tmp = i_[0]; + i_[0] = i_[1] * other.i_[0]; + i_[1] = tmp * other.i_[1]; + } + else if(i_[1] <= 0) + { + S tmp = i_[0]; + i_[0] = i_[1] * other.i_[1]; + i_[1] = tmp * other.i_[0]; } else { - FCL_REAL tmp = i_[0]; - i_[0] = i_[1] * d; - i_[1] = tmp * d; + S tmp = i_[0]; + i_[0] = i_[1] * other.i_[0]; + i_[1] = tmp * other.i_[0]; } - return *this; } - /// @brief other must not contain 0 - Interval operator / (const Interval& other) const; - - Interval& operator /= (const Interval& other); - - /// @brief determine whether the intersection between intervals is empty - inline bool overlap(const Interval& other) const + if(i_[0] >= 0) { - if(i_[1] < other.i_[0]) return false; - if(i_[0] > other.i_[1]) return false; - return true; + i_[0] = i_[1] * other.i_[0]; + i_[1] *= other.i_[1]; + return *this; } - inline bool intersect(const Interval& other) + if(i_[1] <= 0) { - if(i_[1] < other.i_[0]) return false; - if(i_[0] > other.i_[1]) return false; - if(i_[1] > other.i_[1]) i_[1] = other.i_[1]; - if(i_[0] < other.i_[0]) i_[0] = other.i_[0]; - return true; + i_[1] = i_[0] * other.i_[0]; + i_[0] *= other.i_[1]; + return *this; } - inline Interval operator - () const + S v00 = i_[0] * other.i_[0]; + S v11 = i_[1] * other.i_[1]; + if(v00 <= v11) { - return Interval(-i_[1], -i_[0]); + S v01 = i_[0] * other.i_[1]; + S v10 = i_[1] * other.i_[0]; + if(v01 < v10) + { + i_[0] = v01; + i_[1] = v11; + } + else + { + i_[0] = v10; + i_[1] = v11; + } + return *this; } - /// @brief Return the nearest distance for points within the interval to zero - inline FCL_REAL getAbsLower() const + S v01 = i_[0] * other.i_[1]; + S v10 = i_[1] * other.i_[0]; + if(v01 < v10) { - if(i_[0] >= 0) return i_[0]; - if(i_[1] >= 0) return 0; - return -i_[1]; + i_[0] = v01; + i_[1] = v00; } - - /// @brief Return the farthest distance for points within the interval to zero - inline FCL_REAL getAbsUpper() const + else { - if(i_[0] + i_[1] >= 0) return i_[1]; - return i_[0]; + i_[0] = v10; + i_[1] = v00; } + return *this; +} + +//============================================================================== +template +Interval Interval::operator * (S d) const +{ + if(d >= 0) return Interval(i_[0] * d, i_[1] * d); + return Interval(i_[1] * d, i_[0] * d); +} - inline bool contains(FCL_REAL v) const +//============================================================================== +template +Interval& Interval::operator *= (S d) +{ + if(d >= 0) { - if(v < i_[0]) return false; - if(v > i_[1]) return false; - return true; + i_[0] *= d; + i_[1] *= d; } - - /// @brief Compute the minimum interval contains v and original interval - inline Interval& bound(FCL_REAL v) + else { - if(v < i_[0]) i_[0] = v; - if(v > i_[1]) i_[1] = v; - return *this; + S tmp = i_[0]; + i_[0] = i_[1] * d; + i_[1] = tmp * d; } + return *this; +} - /// @brief Compute the minimum interval contains other and original interval - inline Interval& bound(const Interval& other) - { - if(other.i_[0] < i_[0]) i_[0] = other.i_[0]; - if(other.i_[1] > i_[1]) i_[1] = other.i_[1]; - return *this; - } +//============================================================================== +template +Interval Interval::operator / (const Interval& other) const +{ + return *this * Interval(1.0 / other.i_[1], 1.0 / other.i_[0]); +} +//============================================================================== +template +Interval& Interval::operator /= (const Interval& other) +{ + *this *= Interval(1.0 / other.i_[1], 1.0 / other.i_[0]); + return *this; +} - void print() const; - inline FCL_REAL center() const { return 0.5 * (i_[0] + i_[1]); } - inline FCL_REAL diameter() const { return i_[1] -i_[0]; } -}; +//============================================================================== +template +bool Interval::overlap(const Interval& other) const +{ + if(i_[1] < other.i_[0]) return false; + if(i_[0] > other.i_[1]) return false; + return true; +} -Interval bound(const Interval& i, FCL_REAL v); +//============================================================================== +template +bool Interval::intersect(const Interval& other) +{ + if(i_[1] < other.i_[0]) return false; + if(i_[0] > other.i_[1]) return false; + if(i_[1] > other.i_[1]) i_[1] = other.i_[1]; + if(i_[0] < other.i_[0]) i_[0] = other.i_[0]; + return true; +} -Interval bound(const Interval& i, const Interval& other); +//============================================================================== +template +Interval Interval::operator -() const +{ + return Interval(-i_[1], -i_[0]); +} +//============================================================================== +template +S Interval::getAbsLower() const +{ + if(i_[0] >= 0) return i_[0]; + if(i_[1] >= 0) return 0; + return -i_[1]; } + +//============================================================================== +template +S Interval::getAbsUpper() const +{ + if(i_[0] + i_[1] >= 0) return i_[1]; + return i_[0]; +} + +//============================================================================== +template +bool Interval::contains(S v) const +{ + if(v < i_[0]) return false; + if(v > i_[1]) return false; + return true; +} + +//============================================================================== +template +Interval& Interval::bound(S v) +{ + if(v < i_[0]) i_[0] = v; + if(v > i_[1]) i_[1] = v; + return *this; +} + +//============================================================================== +template +Interval& Interval::bound(const Interval& other) +{ + if(other.i_[0] < i_[0]) i_[0] = other.i_[0]; + if(other.i_[1] > i_[1]) i_[1] = other.i_[1]; + return *this; +} + +//============================================================================== +template +void Interval::print() const +{ + std::cout << "[" << i_[0] << ", " << i_[1] << "]" << std::endl; +} + +//============================================================================== +template +S Interval::center() const +{ + return 0.5 * (i_[0] + i_[1]); +} + +//============================================================================== +template +S Interval::diameter() const +{ + return i_[1] -i_[0]; +} + +//============================================================================== +template +Interval bound(const Interval& i, S v) +{ + Interval res = i; + if(v < res.i_[0]) res.i_[0] = v; + if(v > res.i_[1]) res.i_[1] = v; + return res; +} + +//============================================================================== +template +Interval bound(const Interval& i, const Interval& other) +{ + Interval res = i; + if(other.i_[0] < res.i_[0]) res.i_[0] = other.i_[0]; + if(other.i_[1] > res.i_[1]) res.i_[1] = other.i_[1]; + return res; +} + +} // namespace fcl + #endif diff --git a/include/fcl/ccd/interval_matrix.h b/include/fcl/ccd/interval_matrix.h index 15c1ff6dc..21e2d8d59 100644 --- a/include/fcl/ccd/interval_matrix.h +++ b/include/fcl/ccd/interval_matrix.h @@ -45,38 +45,39 @@ namespace fcl { +template struct IMatrix3 { - IVector3 v_[3]; + IVector3 v_[3]; IMatrix3(); - IMatrix3(FCL_REAL v); - IMatrix3(const Matrix3d& m); - IMatrix3(FCL_REAL m[3][3][2]); - IMatrix3(FCL_REAL m[3][3]); - IMatrix3(Interval m[3][3]); - IMatrix3(const IVector3& v1, const IVector3& v2, const IVector3& v3); + IMatrix3(S v); + IMatrix3(const Matrix3& m); + IMatrix3(S m[3][3][2]); + IMatrix3(S m[3][3]); + IMatrix3(Interval m[3][3]); + IMatrix3(const IVector3& v1, const IVector3& v2, const IVector3& v3); void setIdentity(); - IVector3 getColumn(size_t i) const; - const IVector3& getRow(size_t i) const; + IVector3 getColumn(size_t i) const; + const IVector3& getRow(size_t i) const; - Vector3d getColumnLow(size_t i) const; - Vector3d getRowLow(size_t i) const; + Vector3 getColumnLow(size_t i) const; + Vector3 getRowLow(size_t i) const; - Vector3d getColumnHigh(size_t i) const; - Vector3d getRowHigh(size_t i) const; + Vector3 getColumnHigh(size_t i) const; + Vector3 getRowHigh(size_t i) const; - Matrix3d getLow() const; - Matrix3d getHigh() const; + Matrix3 getLow() const; + Matrix3 getHigh() const; - inline const Interval& operator () (size_t i, size_t j) const + inline const Interval& operator () (size_t i, size_t j) const { return v_[i][j]; } - inline Interval& operator () (size_t i, size_t j) + inline Interval& operator () (size_t i, size_t j) { return v_[i][j]; } @@ -87,21 +88,301 @@ struct IMatrix3 IMatrix3 operator - (const IMatrix3& m) const; IMatrix3& operator -= (const IMatrix3& m); - IVector3 operator * (const Vector3d& v) const; - IVector3 operator * (const IVector3& v) const; + IVector3 operator * (const Vector3& v) const; + IVector3 operator * (const IVector3& v) const; IMatrix3 operator * (const IMatrix3& m) const; - IMatrix3 operator * (const Matrix3d& m) const; + IMatrix3 operator * (const Matrix3& m) const; IMatrix3& operator *= (const IMatrix3& m); - IMatrix3& operator *= (const Matrix3d& m); + IMatrix3& operator *= (const Matrix3& m); IMatrix3& rotationConstrain(); void print() const; }; -IMatrix3 rotationConstrain(const IMatrix3& m); +template +IMatrix3 rotationConstrain(const IMatrix3& m); +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +IMatrix3::IMatrix3() {} + +//============================================================================== +template +IMatrix3::IMatrix3(S v) +{ + v_[0].setValue(v); + v_[1].setValue(v); + v_[2].setValue(v); +} + +//============================================================================== +template +IMatrix3::IMatrix3(const Matrix3& m) +{ + v_[0].setValue(m.row(0)[0], m.row(0)[1], m.row(0)[2]); + v_[1].setValue(m.row(1)[0], m.row(1)[1], m.row(1)[2]); + v_[2].setValue(m.row(2)[0], m.row(2)[1], m.row(2)[2]); +} + +//============================================================================== +template +IMatrix3::IMatrix3(S m[3][3][2]) +{ + v_[0].setValue(m[0]); + v_[1].setValue(m[1]); + v_[2].setValue(m[2]); +} + +//============================================================================== +template +IMatrix3::IMatrix3(S m[3][3]) +{ + v_[0].setValue(m[0]); + v_[1].setValue(m[1]); + v_[2].setValue(m[2]); +} + +//============================================================================== +template +IMatrix3::IMatrix3(Interval m[3][3]) +{ + v_[0].setValue(m[0]); + v_[1].setValue(m[1]); + v_[2].setValue(m[2]); +} + +//============================================================================== +template +IMatrix3::IMatrix3(const IVector3& v1, const IVector3& v2, const IVector3& v3) +{ + v_[0] = v1; + v_[1] = v2; + v_[2] = v3; +} + +//============================================================================== +template +void IMatrix3::setIdentity() +{ + v_[0].setValue(1, 0, 0); + v_[1].setValue(0, 1, 0); + v_[2].setValue(0, 0, 1); +} + +//============================================================================== +template +IVector3 IMatrix3::getColumn(size_t i) const +{ + return IVector3(v_[0][i], v_[1][i], v_[2][i]); +} + +//============================================================================== +template +const IVector3& IMatrix3::getRow(size_t i) const +{ + return v_[i]; +} + +//============================================================================== +template +Vector3 IMatrix3::getColumnLow(size_t i) const +{ + return Vector3(v_[0][i][0], v_[1][i][0], v_[2][i][0]); +} + +//============================================================================== +template +Vector3 IMatrix3::getRowLow(size_t i) const +{ + return Vector3(v_[i][0][0], v_[i][1][0], v_[i][2][0]); +} + +//============================================================================== +template +Vector3 IMatrix3::getColumnHigh(size_t i) const +{ + return Vector3(v_[0][i][1], v_[1][i][1], v_[2][i][1]); +} + +//============================================================================== +template +Vector3 IMatrix3::getRowHigh(size_t i) const +{ + return Vector3(v_[i][0][1], v_[i][1][1], v_[i][2][1]); +} + +//============================================================================== +template +Matrix3 IMatrix3::getLow() const +{ + Matrix3 m; + m << v_[0][0][1], v_[0][1][1], v_[0][2][1], + v_[1][0][1], v_[1][1][1], v_[1][2][1], + v_[2][0][1], v_[2][1][1], v_[2][2][1]; + return m; +} + +//============================================================================== +template +Matrix3 IMatrix3::getHigh() const +{ + Matrix3 m; + m << v_[0][0][1], v_[0][1][1], v_[0][2][1], + v_[1][0][1], v_[1][1][1], v_[1][2][1], + v_[2][0][1], v_[2][1][1], v_[2][2][1]; + return m; +} + +//============================================================================== +template +IMatrix3 IMatrix3::operator * (const Matrix3& m) const +{ + return IMatrix3(IVector3(v_[0].dot(m.col(0)), v_[0].dot(m.col(1)), v_[0].dot(m.col(2))), + IVector3(v_[1].dot(m.col(0)), v_[1].dot(m.col(1)), v_[1].dot(m.col(2))), + IVector3(v_[2].dot(m.col(0)), v_[2].dot(m.col(1)), v_[2].dot(m.col(2)))); } +//============================================================================== +template +IVector3 IMatrix3::operator * (const Vector3& v) const +{ + return IVector3(v_[0].dot(v), v_[1].dot(v), v_[2].dot(v)); +} + +//============================================================================== +template +IVector3 IMatrix3::operator * (const IVector3& v) const +{ + return IVector3(v_[0].dot(v), v_[1].dot(v), v_[2].dot(v)); +} + +//============================================================================== +template +IMatrix3 IMatrix3::operator * (const IMatrix3& m) const +{ + const IVector3& mc0 = m.getColumn(0); + const IVector3& mc1 = m.getColumn(1); + const IVector3& mc2 = m.getColumn(2); + + return IMatrix3(IVector3(v_[0].dot(mc0), v_[0].dot(mc1), v_[0].dot(mc2)), + IVector3(v_[1].dot(mc0), v_[1].dot(mc1), v_[1].dot(mc2)), + IVector3(v_[2].dot(mc0), v_[2].dot(mc1), v_[2].dot(mc2))); +} + +//============================================================================== +template +IMatrix3& IMatrix3::operator *= (const Matrix3& m) +{ + v_[0].setValue(v_[0].dot(m.col(0)), v_[0].dot(m.col(1)), v_[0].dot(m.col(2))); + v_[1].setValue(v_[1].dot(m.col(0)), v_[1].dot(m.col(1)), v_[1].dot(m.col(2))); + v_[2].setValue(v_[2].dot(m.col(0)), v_[2].dot(m.col(1)), v_[2].dot(m.col(2))); + return *this; +} + +//============================================================================== +template +IMatrix3& IMatrix3::operator *= (const IMatrix3& m) +{ + const IVector3& mc0 = m.getColumn(0); + const IVector3& mc1 = m.getColumn(1); + const IVector3& mc2 = m.getColumn(2); + + v_[0].setValue(v_[0].dot(mc0), v_[0].dot(mc1), v_[0].dot(mc2)); + v_[1].setValue(v_[1].dot(mc0), v_[1].dot(mc1), v_[1].dot(mc2)); + v_[2].setValue(v_[2].dot(mc0), v_[2].dot(mc1), v_[2].dot(mc2)); + return *this; +} + +//============================================================================== +template +IMatrix3 IMatrix3::operator + (const IMatrix3& m) const +{ + return IMatrix3(v_[0] + m.v_[0], v_[1] + m.v_[1], v_[2] + m.v_[2]); +} + +//============================================================================== +template +IMatrix3& IMatrix3::operator += (const IMatrix3& m) +{ + v_[0] += m.v_[0]; + v_[1] += m.v_[1]; + v_[2] += m.v_[2]; + return *this; +} + +//============================================================================== +template +IMatrix3 IMatrix3::operator - (const IMatrix3& m) const +{ + return IMatrix3(v_[0] - m.v_[0], v_[1] - m.v_[1], v_[2] - m.v_[2]); +} + +//============================================================================== +template +IMatrix3& IMatrix3::operator -= (const IMatrix3& m) +{ + v_[0] -= m.v_[0]; + v_[1] -= m.v_[1]; + v_[2] -= m.v_[2]; + return *this; +} + +//============================================================================== +template +IMatrix3& IMatrix3::rotationConstrain() +{ + for(std::size_t i = 0; i < 3; ++i) + { + for(std::size_t j = 0; j < 3; ++j) + { + if(v_[i][j][0] < -1) v_[i][j][0] = -1; + else if(v_[i][j][0] > 1) v_[i][j][0] = 1; + + if(v_[i][j][1] < -1) v_[i][j][1] = -1; + else if(v_[i][j][1] > 1) v_[i][j][1] = 1; + } + } + + return *this; +} + +//============================================================================== +template +void IMatrix3::print() const +{ + std::cout << "[" << v_[0][0][0] << "," << v_[0][0][1] << "]" << " [" << v_[0][1][0] << "," << v_[0][1][1] << "]" << " [" << v_[0][2][0] << "," << v_[0][2][1] << "]" << std::endl; + std::cout << "[" << v_[1][0][0] << "," << v_[1][0][1] << "]" << " [" << v_[1][1][0] << "," << v_[1][1][1] << "]" << " [" << v_[1][2][0] << "," << v_[1][2][1] << "]" << std::endl; + std::cout << "[" << v_[2][0][0] << "," << v_[2][0][1] << "]" << " [" << v_[2][1][0] << "," << v_[2][1][1] << "]" << " [" << v_[2][2][0] << "," << v_[2][2][1] << "]" << std::endl; +} + +//============================================================================== +template +IMatrix3 rotationConstrain(const IMatrix3& m) +{ + IMatrix3 res; + for(std::size_t i = 0; i < 3; ++i) + { + for(std::size_t j = 0; j < 3; ++j) + { + if(m(i, j)[0] < -1) res(i, j)[0] = -1; + else if(m(i, j)[0] > 1) res(i, j)[0] = 1; + + if(m(i, j)[1] < -1) res(i, j)[1] = -1; + else if(m(i, j)[1] > 1) res(i, j)[1] = 1; + } + } + + return res; +} + +} // namespace fcl + #endif diff --git a/include/fcl/ccd/interval_vector.h b/include/fcl/ccd/interval_vector.h index 19168a0bc..92a90d34b 100644 --- a/include/fcl/ccd/interval_vector.h +++ b/include/fcl/ccd/interval_vector.h @@ -44,74 +44,35 @@ namespace fcl { +template struct IVector3 { - Interval i_[3]; + Interval i_[3]; IVector3(); - IVector3(FCL_REAL v); - IVector3(FCL_REAL x, FCL_REAL y, FCL_REAL z); - IVector3(FCL_REAL xl, FCL_REAL xu, FCL_REAL yl, FCL_REAL yu, FCL_REAL zl, FCL_REAL zu); - IVector3(Interval v[3]); - IVector3(FCL_REAL v[3][2]); - IVector3(const Interval& v1, const Interval& v2, const Interval& v3); - IVector3(const Vector3d& v); - - inline void setValue(FCL_REAL v) - { - i_[0].setValue(v); - i_[1].setValue(v); - i_[2].setValue(v); - } - - inline void setValue(FCL_REAL x, FCL_REAL y, FCL_REAL z) - { - i_[0].setValue(x); - i_[1].setValue(y); - i_[2].setValue(z); - } - - inline void setValue(FCL_REAL xl, FCL_REAL xu, FCL_REAL yl, FCL_REAL yu, FCL_REAL zl, FCL_REAL zu) - { - i_[0].setValue(xl, xu); - i_[1].setValue(yl, yu); - i_[2].setValue(zl, zu); - } - - inline void setValue(FCL_REAL v[3][2]) - { - i_[0].setValue(v[0][0], v[0][1]); - i_[1].setValue(v[1][0], v[1][1]); - i_[2].setValue(v[2][0], v[2][1]); - } - - inline void setValue(Interval v[3]) - { - i_[0] = v[0]; - i_[1] = v[1]; - i_[2] = v[2]; - } - - inline void setValue(const Interval& v1, const Interval& v2, const Interval& v3) - { - i_[0] = v1; - i_[1] = v2; - i_[2] = v3; - } - - inline void setValue(const Vector3d& v) - { - i_[0].setValue(v[0]); - i_[1].setValue(v[1]); - i_[2].setValue(v[2]); - } - - inline void setValue(FCL_REAL v[3]) - { - i_[0].setValue(v[0]); - i_[1].setValue(v[1]); - i_[2].setValue(v[2]); - } + IVector3(S v); + IVector3(S x, S y, S z); + IVector3(S xl, S xu, S yl, S yu, S zl, S zu); + IVector3(Interval v[3]); + IVector3(S v[3][2]); + IVector3(const Interval& v1, const Interval& v2, const Interval& v3); + IVector3(const Vector3& v); + + void setValue(S v); + + void setValue(S x, S y, S z); + + void setValue(S xl, S xu, S yl, S yu, S zl, S zu); + + void setValue(S v[3][2]); + + void setValue(Interval v[3]); + + void setValue(const Interval& v1, const Interval& v2, const Interval& v3); + + void setValue(const Vector3& v); + + void setValue(S v[3]); IVector3 operator + (const IVector3& other) const; IVector3& operator += (const IVector3& other); @@ -119,48 +80,384 @@ struct IVector3 IVector3 operator - (const IVector3& other) const; IVector3& operator -= (const IVector3& other); - Interval dot(const IVector3& other) const; + Interval dot(const IVector3& other) const; IVector3 cross(const IVector3& other) const; - Interval dot(const Vector3d& other) const; - IVector3 cross(const Vector3d& other) const; + Interval dot(const Vector3& other) const; + IVector3 cross(const Vector3& other) const; - inline const Interval& operator [] (size_t i) const - { - return i_[i]; - } + const Interval& operator [] (size_t i) const; - inline Interval& operator [] (size_t i) - { - return i_[i]; - } + Interval& operator [] (size_t i); - inline Vector3d getLow() const - { - return Vector3d(i_[0][0], i_[1][0], i_[2][0]); - } + Vector3 getLow() const; - inline Vector3d getHigh() const - { - return Vector3d(i_[0][1], i_[1][1], i_[2][1]); - } + Vector3 getHigh() const; void print() const; - Vector3d center() const; - FCL_REAL volumn() const; + Vector3 center() const; + S volumn() const; void setZero(); - void bound(const Vector3d& v); + void bound(const Vector3& v); void bound(const IVector3& v); bool overlap(const IVector3& v) const; bool contain(const IVector3& v) const; }; -IVector3 bound(const IVector3& i, const Vector3d& v); +template +IVector3 bound(const IVector3& i, const Vector3& v); + +template +IVector3 bound(const IVector3& i, const IVector3& v); + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +IVector3::IVector3() +{ + // Do nothing +} + +//============================================================================== +template +IVector3::IVector3(S v) +{ + setValue(v); +} + +//============================================================================== +template +IVector3::IVector3(S x, S y, S z) +{ + setValue(x, y, z); +} + +//============================================================================== +template +IVector3::IVector3(S xl, S xu, S yl, S yu, S zl, S zu) +{ + setValue(xl, xu, yl, yu, zl, zu); +} + +//============================================================================== +template +IVector3::IVector3(S v[3][2]) +{ + setValue(v); +} + +//============================================================================== +template +IVector3::IVector3(Interval v[3]) +{ + setValue(v); +} + +//============================================================================== +template +IVector3::IVector3(const Interval& v1, const Interval& v2, const Interval& v3) +{ + setValue(v1, v2, v3); +} + +//============================================================================== +template +IVector3::IVector3(const Vector3& v) +{ + setValue(v); +} + +//============================================================================== +template +void IVector3::setValue(S v) +{ + i_[0].setValue(v); + i_[1].setValue(v); + i_[2].setValue(v); +} + +//============================================================================== +template +void IVector3::setValue(S x, S y, S z) +{ + i_[0].setValue(x); + i_[1].setValue(y); + i_[2].setValue(z); +} + +//============================================================================== +template +void IVector3::setValue(S xl, S xu, S yl, S yu, S zl, S zu) +{ + i_[0].setValue(xl, xu); + i_[1].setValue(yl, yu); + i_[2].setValue(zl, zu); +} + +//============================================================================== +template +void IVector3::setValue(S v[3][2]) +{ + i_[0].setValue(v[0][0], v[0][1]); + i_[1].setValue(v[1][0], v[1][1]); + i_[2].setValue(v[2][0], v[2][1]); +} + +//============================================================================== +template +void IVector3::setValue(Interval v[3]) +{ + i_[0] = v[0]; + i_[1] = v[1]; + i_[2] = v[2]; +} + +//============================================================================== +template +void IVector3::setValue(const Interval& v1, const Interval& v2, const Interval& v3) +{ + i_[0] = v1; + i_[1] = v2; + i_[2] = v3; +} + +//============================================================================== +template +void IVector3::setValue(const Vector3& v) +{ + i_[0].setValue(v[0]); + i_[1].setValue(v[1]); + i_[2].setValue(v[2]); +} + +//============================================================================== +template +void IVector3::setValue(S v[]) +{ + i_[0].setValue(v[0]); + i_[1].setValue(v[1]); + i_[2].setValue(v[2]); +} + +//============================================================================== +template +void IVector3::setZero() +{ + setValue((S)0.0); +} + +//============================================================================== +template +IVector3 IVector3::operator + (const IVector3& other) const +{ + return IVector3(i_[0] + other.i_[0], i_[1] + other.i_[1], i_[2] + other.i_[2]); +} + +//============================================================================== +template +IVector3& IVector3::operator += (const IVector3& other) +{ + i_[0] += other[0]; + i_[1] += other[1]; + i_[2] += other[2]; + return *this; +} + +//============================================================================== +template +IVector3 IVector3::operator - (const IVector3& other) const +{ + return IVector3(i_[0] - other.i_[0], i_[1] - other.i_[1], i_[2] - other.i_[2]); +} + +//============================================================================== +template +IVector3& IVector3::operator -= (const IVector3& other) +{ + i_[0] -= other[0]; + i_[1] -= other[1]; + i_[2] -= other[2]; + return *this; +} + +//============================================================================== +template +Interval IVector3::dot(const IVector3& other) const +{ + return i_[0] * other.i_[0] + i_[1] * other.i_[1] + i_[2] * other.i_[2]; +} + +//============================================================================== +template +IVector3 IVector3::cross(const IVector3& other) const +{ + return IVector3(i_[1] * other.i_[2] - i_[2] * other.i_[1], + i_[2] * other.i_[0] - i_[0] * other.i_[2], + i_[0] * other.i_[1] - i_[1] * other.i_[0]); +} + +//============================================================================== +template +Interval IVector3::dot(const Vector3& other) const +{ + return i_[0] * other[0] + i_[1] * other[1] + i_[2] * other[2]; +} + +//============================================================================== +template +const Interval&IVector3::operator [](size_t i) const +{ + return i_[i]; +} + +//============================================================================== +template +Interval&IVector3::operator [](size_t i) +{ + return i_[i]; +} + +//============================================================================== +template +Vector3 IVector3::getLow() const +{ + return Vector3(i_[0][0], i_[1][0], i_[2][0]); +} + +//============================================================================== +template +Vector3 IVector3::getHigh() const +{ + return Vector3(i_[0][1], i_[1][1], i_[2][1]); +} + +//============================================================================== +template +IVector3 IVector3::cross(const Vector3& other) const +{ + return IVector3(i_[1] * other[2] - i_[2] * other[1], + i_[2] * other[0] - i_[0] * other[2], + i_[0] * other[1] - i_[1] * other[0]); +} + +//============================================================================== +template +S IVector3::volumn() const +{ + return i_[0].diameter() * i_[1].diameter() * i_[2].diameter(); +} + +//============================================================================== +template +void IVector3::print() const +{ + std::cout << "[" << i_[0][0] << "," << i_[0][1] << "]" << std::endl; + std::cout << "[" << i_[1][0] << "," << i_[1][1] << "]" << std::endl; + std::cout << "[" << i_[2][0] << "," << i_[2][1] << "]" << std::endl; +} + +//============================================================================== +template +Vector3 IVector3::center() const +{ + return Vector3(i_[0].center(), i_[1].center(), i_[2].center()); +} + +//============================================================================== +template +void IVector3::bound(const IVector3& v) +{ + if(v[0][0] < i_[0][0]) i_[0][0] = v[0][0]; + if(v[1][0] < i_[1][0]) i_[1][0] = v[1][0]; + if(v[2][0] < i_[2][0]) i_[2][0] = v[2][0]; + + if(v[0][1] > i_[0][1]) i_[0][1] = v[0][1]; + if(v[1][1] > i_[1][1]) i_[1][1] = v[1][1]; + if(v[2][1] > i_[2][1]) i_[2][1] = v[2][1]; +} + +//============================================================================== +template +void IVector3::bound(const Vector3& v) +{ + if(v[0] < i_[0][0]) i_[0][0] = v[0]; + if(v[1] < i_[1][0]) i_[1][0] = v[1]; + if(v[2] < i_[2][0]) i_[2][0] = v[2]; + + if(v[0] > i_[0][1]) i_[0][1] = v[0]; + if(v[1] > i_[1][1]) i_[1][1] = v[1]; + if(v[2] > i_[2][1]) i_[2][1] = v[2]; +} + +//============================================================================== +template +IVector3 bound(const IVector3& i, const IVector3& v) +{ + IVector3 res(i); + if(v[0][0] < res.i_[0][0]) res.i_[0][0] = v[0][0]; + if(v[1][0] < res.i_[1][0]) res.i_[1][0] = v[1][0]; + if(v[2][0] < res.i_[2][0]) res.i_[2][0] = v[2][0]; -IVector3 bound(const IVector3& i, const IVector3& v); + if(v[0][1] > res.i_[0][1]) res.i_[0][1] = v[0][1]; + if(v[1][1] > res.i_[1][1]) res.i_[1][1] = v[1][1]; + if(v[2][1] > res.i_[2][1]) res.i_[2][1] = v[2][1]; + return res; } +//============================================================================== +template +IVector3 bound(const IVector3& i, const Vector3& v) +{ + IVector3 res(i); + if(v[0] < res.i_[0][0]) res.i_[0][0] = v[0]; + if(v[1] < res.i_[1][0]) res.i_[1][0] = v[1]; + if(v[2] < res.i_[2][0]) res.i_[2][0] = v[2]; + + if(v[0] > res.i_[0][1]) res.i_[0][1] = v[0]; + if(v[1] > res.i_[1][1]) res.i_[1][1] = v[1]; + if(v[2] > res.i_[2][1]) res.i_[2][1] = v[2]; + + return res; +} + +//============================================================================== +template +bool IVector3::overlap(const IVector3& v) const +{ + if(v[0][1] < i_[0][0]) return false; + if(v[1][1] < i_[1][0]) return false; + if(v[2][1] < i_[2][0]) return false; + + if(v[0][0] > i_[0][1]) return false; + if(v[1][0] > i_[1][1]) return false; + if(v[2][0] > i_[2][1]) return false; + + return true; +} + +//============================================================================== +template +bool IVector3::contain(const IVector3& v) const +{ + if(v[0][0] < i_[0][0]) return false; + if(v[1][0] < i_[1][0]) return false; + if(v[2][0] < i_[2][0]) return false; + + if(v[0][1] > i_[0][1]) return false; + if(v[1][1] > i_[1][1]) return false; + if(v[2][1] > i_[2][1]) return false; + + return true; +} + +} // namespace fcl + #endif diff --git a/include/fcl/ccd/motion.h b/include/fcl/ccd/motion.h index 1ec538e3b..72b10e807 100644 --- a/include/fcl/ccd/motion.h +++ b/include/fcl/ccd/motion.h @@ -48,12 +48,13 @@ namespace fcl { -class TranslationMotion : public MotionBase +template +class TranslationMotion : public MotionBase { public: /// @brief Construct motion from intial and goal transform - TranslationMotion(const Transform3d& tf1, - const Transform3d& tf2) : MotionBase(), + TranslationMotion(const Transform3& tf1, + const Transform3& tf2) : MotionBase(), rot(tf1.linear()), trans_start(tf1.translation()), trans_range(tf2.translation() - tf1.translation()), @@ -61,8 +62,8 @@ class TranslationMotion : public MotionBase { } - TranslationMotion(const Matrix3d& R, const Vector3d& T1, const Vector3d& T2) : MotionBase(), - tf(Transform3d::Identity()) + TranslationMotion(const Matrix3& R, const Vector3& T1, const Vector3& T2) : MotionBase(), + tf(Transform3::Identity()) { rot = R; trans_start = T1; @@ -71,7 +72,7 @@ class TranslationMotion : public MotionBase tf.translation() = trans_start; } - bool integrate(FCL_REAL dt) const + bool integrate(S dt) const override { if(dt > 1) dt = 1; @@ -82,54 +83,61 @@ class TranslationMotion : public MotionBase return true; } - FCL_REAL computeMotionBound(const BVMotionBoundVisitor& mb_visitor) const + S computeMotionBound(const BVMotionBoundVisitor& mb_visitor) const override { return mb_visitor.visit(*this); } - FCL_REAL computeMotionBound(const TriangleMotionBoundVisitor& mb_visitor) const + S computeMotionBound(const TriangleMotionBoundVisitor& mb_visitor) const override { return mb_visitor.visit(*this); } - void getCurrentTransform(Transform3d& tf_) const + void getCurrentTransform(Transform3& tf_) const override { tf_ = tf; } - void getTaylorModel(TMatrix3& tm, TVector3& tv) const + void getTaylorModel(TMatrix3& tm, TVector3& tv) const override { } - Vector3d getVelocity() const + Vector3 getVelocity() const { return trans_range; } private: /// @brief initial and goal transforms - Quaternion3d rot; - Vector3d trans_start, trans_range; + Quaternion rot; + Vector3 trans_start, trans_range; - mutable Transform3d tf; + mutable Transform3 tf; + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -class SplineMotion : public MotionBase +using TranslationMotionf = TranslationMotion; +using TranslationMotiond = TranslationMotion; + +template +class SplineMotion : public MotionBase { public: /// @brief Construct motion from 4 deBoor points - SplineMotion(const Vector3d& Td0, const Vector3d& Td1, const Vector3d& Td2, const Vector3d& Td3, - const Vector3d& Rd0, const Vector3d& Rd1, const Vector3d& Rd2, const Vector3d& Rd3); + SplineMotion(const Vector3& Td0, const Vector3& Td1, const Vector3& Td2, const Vector3& Td3, + const Vector3& Rd0, const Vector3& Rd1, const Vector3& Rd2, const Vector3& Rd3); // @brief Construct motion from initial and goal transform - SplineMotion(const Matrix3d& R1, const Vector3d& T1, - const Matrix3d& R2, const Vector3d& T2) : MotionBase() + SplineMotion(const Matrix3& R1, const Vector3& T1, + const Matrix3& R2, const Vector3& T2) : MotionBase() { // TODO } - SplineMotion(const Transform3d& tf1, - const Transform3d& tf2) : MotionBase() + SplineMotion(const Transform3& tf1, + const Transform3& tf2) : MotionBase() { // TODO } @@ -137,35 +145,35 @@ class SplineMotion : public MotionBase /// @brief Integrate the motion from 0 to dt /// We compute the current transformation from zero point instead of from last integrate time, for precision. - bool integrate(double dt) const; + bool integrate(S dt) const override; /// @brief Compute the motion bound for a bounding volume along a given direction n, which is defined in the visitor - FCL_REAL computeMotionBound(const BVMotionBoundVisitor& mb_visitor) const + S computeMotionBound(const BVMotionBoundVisitor& mb_visitor) const override { return mb_visitor.visit(*this); } /// @brief Compute the motion bound for a triangle along a given direction n, which is defined in the visitor - FCL_REAL computeMotionBound(const TriangleMotionBoundVisitor& mb_visitor) const + S computeMotionBound(const TriangleMotionBoundVisitor& mb_visitor) const override { return mb_visitor.visit(*this); } /// @brief Get the rotation and translation in current step - void getCurrentTransform(Transform3d& tf_) const + void getCurrentTransform(Transform3& tf_) const override { tf_ = tf; } - void getTaylorModel(TMatrix3& tm, TVector3& tv) const + void getTaylorModel(TMatrix3& tm, TVector3& tv) const override { // set tv - Vector3d c[4]; + Vector3 c[4]; c[0] = (Td[0] + Td[1] * 4 + Td[2] + Td[3]) * (1/6.0); c[1] = (-Td[0] + Td[2]) * (1/2.0); c[2] = (Td[0] - Td[1] * 2 + Td[2]) * (1/2.0); c[3] = (-Td[0] + Td[1] * 3 - Td[2] * 3 + Td[3]) * (1/6.0); - tv.setTimeInterval(getTimeInterval()); + tv.setTimeInterval(this->getTimeInterval()); for(std::size_t i = 0; i < 3; ++i) { for(std::size_t j = 0; j < 4; ++j) @@ -175,43 +183,43 @@ class SplineMotion : public MotionBase } // set tm - Matrix3d I = Matrix3d::Identity(); + Matrix3 I = Matrix3::Identity(); // R(t) = R(t0) + R'(t0) (t-t0) + 1/2 R''(t0)(t-t0)^2 + 1 / 6 R'''(t0) (t-t0)^3 + 1 / 24 R''''(l)(t-t0)^4; t0 = 0.5 /// 1. compute M(1/2) - Vector3d Rt0 = (Rd[0] + Rd[1] * 23 + Rd[2] * 23 + Rd[3]) * (1 / 48.0); - FCL_REAL Rt0_len = Rt0.norm(); - FCL_REAL inv_Rt0_len = 1.0 / Rt0_len; - FCL_REAL inv_Rt0_len_3 = inv_Rt0_len * inv_Rt0_len * inv_Rt0_len; - FCL_REAL inv_Rt0_len_5 = inv_Rt0_len_3 * inv_Rt0_len * inv_Rt0_len; - FCL_REAL theta0 = Rt0_len; - FCL_REAL costheta0 = cos(theta0); - FCL_REAL sintheta0 = sin(theta0); + Vector3 Rt0 = (Rd[0] + Rd[1] * 23 + Rd[2] * 23 + Rd[3]) * (1 / 48.0); + S Rt0_len = Rt0.norm(); + S inv_Rt0_len = 1.0 / Rt0_len; + S inv_Rt0_len_3 = inv_Rt0_len * inv_Rt0_len * inv_Rt0_len; + S inv_Rt0_len_5 = inv_Rt0_len_3 * inv_Rt0_len * inv_Rt0_len; + S theta0 = Rt0_len; + S costheta0 = cos(theta0); + S sintheta0 = sin(theta0); - Vector3d Wt0 = Rt0 * inv_Rt0_len; - Matrix3d hatWt0; + Vector3 Wt0 = Rt0 * inv_Rt0_len; + Matrix3 hatWt0; hat(hatWt0, Wt0); - Matrix3d hatWt0_sqr = hatWt0 * hatWt0; - Matrix3d Mt0 = I + hatWt0 * sintheta0 + hatWt0_sqr * (1 - costheta0); + Matrix3 hatWt0_sqr = hatWt0 * hatWt0; + Matrix3 Mt0 = I + hatWt0 * sintheta0 + hatWt0_sqr * (1 - costheta0); // TODO(JS): this could be improved by using exp(Wt0) /// 2. compute M'(1/2) - Vector3d dRt0 = (-Rd[0] - Rd[1] * 5 + Rd[2] * 5 + Rd[3]) * (1 / 8.0); - FCL_REAL Rt0_dot_dRt0 = Rt0.dot(dRt0); - FCL_REAL dtheta0 = Rt0_dot_dRt0 * inv_Rt0_len; - Vector3d dWt0 = dRt0 * inv_Rt0_len - Rt0 * (Rt0_dot_dRt0 * inv_Rt0_len_3); - Matrix3d hatdWt0; + Vector3 dRt0 = (-Rd[0] - Rd[1] * 5 + Rd[2] * 5 + Rd[3]) * (1 / 8.0); + S Rt0_dot_dRt0 = Rt0.dot(dRt0); + S dtheta0 = Rt0_dot_dRt0 * inv_Rt0_len; + Vector3 dWt0 = dRt0 * inv_Rt0_len - Rt0 * (Rt0_dot_dRt0 * inv_Rt0_len_3); + Matrix3 hatdWt0; hat(hatdWt0, dWt0); - Matrix3d dMt0 = hatdWt0 * sintheta0 + hatWt0 * (costheta0 * dtheta0) + hatWt0_sqr * (sintheta0 * dtheta0) + (hatWt0 * hatdWt0 + hatdWt0 * hatWt0) * (1 - costheta0); + Matrix3 dMt0 = hatdWt0 * sintheta0 + hatWt0 * (costheta0 * dtheta0) + hatWt0_sqr * (sintheta0 * dtheta0) + (hatWt0 * hatdWt0 + hatdWt0 * hatWt0) * (1 - costheta0); /// 3.1. compute M''(1/2) - Vector3d ddRt0 = (Rd[0] - Rd[1] - Rd[2] + Rd[3]) * 0.5; - FCL_REAL Rt0_dot_ddRt0 = Rt0.dot(ddRt0); - FCL_REAL dRt0_dot_dRt0 = dRt0.squaredNorm(); - FCL_REAL ddtheta0 = (Rt0_dot_ddRt0 + dRt0_dot_dRt0) * inv_Rt0_len - Rt0_dot_dRt0 * Rt0_dot_dRt0 * inv_Rt0_len_3; - Vector3d ddWt0 = ddRt0 * inv_Rt0_len - (dRt0 * (2 * Rt0_dot_dRt0) + Rt0 * (Rt0_dot_ddRt0 + dRt0_dot_dRt0)) * inv_Rt0_len_3 + (Rt0 * (3 * Rt0_dot_dRt0 * Rt0_dot_dRt0)) * inv_Rt0_len_5; - Matrix3d hatddWt0; + Vector3 ddRt0 = (Rd[0] - Rd[1] - Rd[2] + Rd[3]) * 0.5; + S Rt0_dot_ddRt0 = Rt0.dot(ddRt0); + S dRt0_dot_dRt0 = dRt0.squaredNorm(); + S ddtheta0 = (Rt0_dot_ddRt0 + dRt0_dot_dRt0) * inv_Rt0_len - Rt0_dot_dRt0 * Rt0_dot_dRt0 * inv_Rt0_len_3; + Vector3 ddWt0 = ddRt0 * inv_Rt0_len - (dRt0 * (2 * Rt0_dot_dRt0) + Rt0 * (Rt0_dot_ddRt0 + dRt0_dot_dRt0)) * inv_Rt0_len_3 + (Rt0 * (3 * Rt0_dot_dRt0 * Rt0_dot_dRt0)) * inv_Rt0_len_5; + Matrix3 hatddWt0; hat(hatddWt0, ddWt0); - Matrix3d ddMt0 = + Matrix3 ddMt0 = hatddWt0 * sintheta0 + hatWt0 * (costheta0 * dtheta0 - sintheta0 * dtheta0 * dtheta0 + costheta0 * ddtheta0) + hatdWt0 * (costheta0 * dtheta0) + @@ -221,7 +229,7 @@ class SplineMotion : public MotionBase (hatWt0 * hatddWt0 + hatddWt0 + hatWt0) * (1 - costheta0); - tm.setTimeInterval(getTimeInterval()); + tm.setTimeInterval(this->getTimeInterval()); for(std::size_t i = 0; i < 3; ++i) { for(std::size_t j = 0; j < 3; ++j) @@ -231,7 +239,7 @@ class SplineMotion : public MotionBase tm(i, j).coeff(2) = ddMt0(i, j) * 0.5; tm(i, j).coeff(3) = 0; - tm(i, j).remainder() = Interval(-1/48.0, 1/48.0); /// not correct, should fix + tm(i, j).remainder() = Interval(-1/48.0, 1/48.0); /// not correct, should fix } } } @@ -241,41 +249,43 @@ class SplineMotion : public MotionBase { } - FCL_REAL getWeight0(FCL_REAL t) const; - FCL_REAL getWeight1(FCL_REAL t) const; - FCL_REAL getWeight2(FCL_REAL t) const; - FCL_REAL getWeight3(FCL_REAL t) const; + S getWeight0(S t) const; + S getWeight1(S t) const; + S getWeight2(S t) const; + S getWeight3(S t) const; - Vector3d Td[4]; - Vector3d Rd[4]; + Vector3 Td[4]; + Vector3 Rd[4]; - Vector3d TA, TB, TC; - Vector3d RA, RB, RC; + Vector3 TA, TB, TC; + Vector3 RA, RB, RC; - FCL_REAL Rd0Rd0, Rd0Rd1, Rd0Rd2, Rd0Rd3, Rd1Rd1, Rd1Rd2, Rd1Rd3, Rd2Rd2, Rd2Rd3, Rd3Rd3; + S Rd0Rd0, Rd0Rd1, Rd0Rd2, Rd0Rd3, Rd1Rd1, Rd1Rd2, Rd1Rd3, Rd2Rd2, Rd2Rd3, Rd3Rd3; //// @brief The transformation at current time t - mutable Transform3d tf; + mutable Transform3 tf; /// @brief The time related with tf - mutable FCL_REAL tf_t; + mutable S tf_t; public: - FCL_REAL computeTBound(const Vector3d& n) const; + S computeTBound(const Vector3& n) const; - FCL_REAL computeDWMax() const; + S computeDWMax() const; - FCL_REAL getCurrentTime() const + S getCurrentTime() const { return tf_t; } + EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -class ScrewMotion : public MotionBase +template +class ScrewMotion : public MotionBase { public: /// @brief Default transformations are all identities - ScrewMotion() : MotionBase(), axis(Vector3d::UnitX()) + ScrewMotion() : MotionBase(), axis(Vector3::UnitX()) { // Default angular velocity is zero angular_vel = 0; @@ -287,10 +297,10 @@ class ScrewMotion : public MotionBase } /// @brief Construct motion from the initial rotation/translation and goal rotation/translation - ScrewMotion(const Matrix3d& R1, const Vector3d& T1, - const Matrix3d& R2, const Vector3d& T2) : MotionBase(), - tf1(Transform3d::Identity()), - tf2(Transform3d::Identity()) + ScrewMotion(const Matrix3& R1, const Vector3& T1, + const Matrix3& R2, const Vector3& T2) : MotionBase(), + tf1(Transform3::Identity()), + tf2(Transform3::Identity()) { tf1.linear() = R1; tf1.translation() = T1; @@ -304,8 +314,8 @@ class ScrewMotion : public MotionBase } /// @brief Construct motion from the initial transform and goal transform - ScrewMotion(const Transform3d& tf1_, - const Transform3d& tf2_) : tf1(tf1_), + ScrewMotion(const Transform3& tf1_, + const Transform3& tf2_) : tf1(tf1_), tf2(tf2_), tf(tf1) { @@ -320,51 +330,51 @@ class ScrewMotion : public MotionBase tf.linear() = absoluteRotation(dt).toRotationMatrix(); - Quaternion3d delta_rot = deltaRotation(dt); + Quaternion delta_rot = deltaRotation(dt); tf.translation() = p + axis * (dt * linear_vel) + delta_rot * (tf1.translation() - p); return true; } /// @brief Compute the motion bound for a bounding volume along a given direction n, which is defined in the visitor - FCL_REAL computeMotionBound(const BVMotionBoundVisitor& mb_visitor) const + S computeMotionBound(const BVMotionBoundVisitor& mb_visitor) const { return mb_visitor.visit(*this); } /// @brief Compute the motion bound for a triangle along a given direction n, which is defined in the visitor - FCL_REAL computeMotionBound(const TriangleMotionBoundVisitor& mb_visitor) const + S computeMotionBound(const TriangleMotionBoundVisitor& mb_visitor) const { return mb_visitor.visit(*this); } /// @brief Get the rotation and translation in current step - void getCurrentTransform(Transform3d& tf_) const + void getCurrentTransform(Transform3& tf_) const { tf_ = tf; } - void getTaylorModel(TMatrix3& tm, TVector3& tv) const + void getTaylorModel(TMatrix3& tm, TVector3& tv) const { - Matrix3d hat_axis; + Matrix3 hat_axis; hat(hat_axis, axis); - TaylorModel cos_model(getTimeInterval()); - generateTaylorModelForCosFunc(cos_model, angular_vel, 0); + TaylorModel cos_model(this->getTimeInterval()); + generateTaylorModelForCosFunc(cos_model, angular_vel, (S)0); - TaylorModel sin_model(getTimeInterval()); - generateTaylorModelForSinFunc(sin_model, angular_vel, 0); + TaylorModel sin_model(this->getTimeInterval()); + generateTaylorModelForSinFunc(sin_model, angular_vel, (S)0); - TMatrix3 delta_R = hat_axis * sin_model + TMatrix3 delta_R = hat_axis * sin_model - (hat_axis * hat_axis).eval() * (cos_model - 1) - + Matrix3d::Identity(); + + Matrix3::Identity(); - TaylorModel a(getTimeInterval()), b(getTimeInterval()), c(getTimeInterval()); - generateTaylorModelForLinearFunc(a, 0, linear_vel * axis[0]); - generateTaylorModelForLinearFunc(b, 0, linear_vel * axis[1]); - generateTaylorModelForLinearFunc(c, 0, linear_vel * axis[2]); - TVector3 delta_T = p - delta_R * p + TVector3(a, b, c); + TaylorModel a(this->getTimeInterval()), b(this->getTimeInterval()), c(this->getTimeInterval()); + generateTaylorModelForLinearFunc(a, (S)0, linear_vel * axis[0]); + generateTaylorModelForLinearFunc(b, (S)0, linear_vel * axis[1]); + generateTaylorModelForLinearFunc(c, (S)0, linear_vel * axis[2]); + TVector3 delta_T = p - delta_R * p + TVector3(a, b, c); tm = delta_R * tf1.linear().eval(); tv = delta_R * tf1.translation().eval() + delta_T; @@ -373,7 +383,7 @@ class ScrewMotion : public MotionBase protected: void computeScrewParameter() { - const Eigen::AngleAxisd aa(tf2.linear() * tf1.linear().transpose()); + const AngleAxis aa(tf2.linear() * tf1.linear().transpose()); axis = aa.axis(); angular_vel = aa.angle(); @@ -393,66 +403,68 @@ class ScrewMotion : public MotionBase } else { - Vector3d o = tf2.translation() - tf1.translation(); + Vector3 o = tf2.translation() - tf1.translation(); p = (tf1.translation() + tf2.translation() + axis.cross(o) * (1.0 / tan(angular_vel / 2.0))) * 0.5; linear_vel = o.dot(axis); } } - Quaternion3d deltaRotation(FCL_REAL dt) const + Quaternion deltaRotation(S dt) const { - return Quaternion3d(Eigen::AngleAxisd((FCL_REAL)(dt * angular_vel), axis)); + return Quaternion(AngleAxis((S)(dt * angular_vel), axis)); } - Quaternion3d absoluteRotation(FCL_REAL dt) const + Quaternion absoluteRotation(S dt) const { - Quaternion3d delta_t = deltaRotation(dt); + Quaternion delta_t = deltaRotation(dt); - return delta_t * Quaternion3d(tf1.linear()); + return delta_t * Quaternion(tf1.linear()); } /// @brief The transformation at time 0 - Transform3d tf1; + Transform3 tf1; /// @brief The transformation at time 1 - Transform3d tf2; + Transform3 tf2; /// @brief The transformation at current time t - mutable Transform3d tf; + mutable Transform3 tf; /// @brief screw axis - Vector3d axis; + Vector3 axis; - /// @brief A point on the axis S - Vector3d p; + /// @brief A point on the axis + Vector3 p; /// @brief linear velocity along the axis - FCL_REAL linear_vel; + S linear_vel; /// @brief angular velocity - FCL_REAL angular_vel; + S angular_vel; public: - inline FCL_REAL getLinearVelocity() const + inline S getLinearVelocity() const { return linear_vel; } - inline FCL_REAL getAngularVelocity() const + inline S getAngularVelocity() const { return angular_vel; } - inline const Vector3d& getAxis() const + inline const Vector3& getAxis() const { return axis; } - inline const Vector3d& getAxisOrigin() const + inline const Vector3& getAxisOrigin() const { return p; } + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; @@ -463,66 +475,67 @@ class ScrewMotion : public MotionBase /// Therefore, R(0) = R0, R(1) = R1 /// T(0) = T0 + R0 p_ref - p_ref /// T(1) = T1 + R1 p_ref - p_ref -class InterpMotion : public MotionBase +template +class InterpMotion : public MotionBase { public: /// @brief Default transformations are all identities InterpMotion(); /// @brief Construct motion from the initial rotation/translation and goal rotation/translation - InterpMotion(const Matrix3d& R1, const Vector3d& T1, - const Matrix3d& R2, const Vector3d& T2); + InterpMotion(const Matrix3& R1, const Vector3& T1, + const Matrix3& R2, const Vector3& T2); - InterpMotion(const Transform3d& tf1_, const Transform3d& tf2_); + InterpMotion(const Transform3& tf1_, const Transform3& tf2_); /// @brief Construct motion from the initial rotation/translation and goal rotation/translation related to some rotation center - InterpMotion(const Matrix3d& R1, const Vector3d& T1, - const Matrix3d& R2, const Vector3d& T2, - const Vector3d& O); + InterpMotion(const Matrix3& R1, const Vector3& T1, + const Matrix3& R2, const Vector3& T2, + const Vector3& O); - InterpMotion(const Transform3d& tf1_, const Transform3d& tf2_, const Vector3d& O); + InterpMotion(const Transform3& tf1_, const Transform3& tf2_, const Vector3& O); /// @brief Integrate the motion from 0 to dt /// We compute the current transformation from zero point instead of from last integrate time, for precision. bool integrate(double dt) const; /// @brief Compute the motion bound for a bounding volume along a given direction n, which is defined in the visitor - FCL_REAL computeMotionBound(const BVMotionBoundVisitor& mb_visitor) const + S computeMotionBound(const BVMotionBoundVisitor& mb_visitor) const { return mb_visitor.visit(*this); } /// @brief Compute the motion bound for a triangle along a given direction n, which is defined in the visitor - FCL_REAL computeMotionBound(const TriangleMotionBoundVisitor& mb_visitor) const + S computeMotionBound(const TriangleMotionBoundVisitor& mb_visitor) const { return mb_visitor.visit(*this); } /// @brief Get the rotation and translation in current step - void getCurrentTransform(Transform3d& tf_) const + void getCurrentTransform(Transform3& tf_) const { tf_ = tf; } - void getTaylorModel(TMatrix3& tm, TVector3& tv) const + void getTaylorModel(TMatrix3& tm, TVector3& tv) const { - Matrix3d hat_angular_axis; + Matrix3 hat_angular_axis; hat(hat_angular_axis, angular_axis); - TaylorModel cos_model(getTimeInterval()); - generateTaylorModelForCosFunc(cos_model, angular_vel, 0); - TaylorModel sin_model(getTimeInterval()); - generateTaylorModelForSinFunc(sin_model, angular_vel, 0); + TaylorModel cos_model(this->getTimeInterval()); + generateTaylorModelForCosFunc(cos_model, angular_vel, (S)0); + TaylorModel sin_model(this->getTimeInterval()); + generateTaylorModelForSinFunc(sin_model, angular_vel, (S)0); - TMatrix3 delta_R = hat_angular_axis * sin_model + TMatrix3 delta_R = hat_angular_axis * sin_model - (hat_angular_axis * hat_angular_axis).eval() * (cos_model - 1) - + Matrix3d::Identity(); + + Matrix3::Identity(); - TaylorModel a(getTimeInterval()), b(getTimeInterval()), c(getTimeInterval()); - generateTaylorModelForLinearFunc(a, 0, linear_vel[0]); - generateTaylorModelForLinearFunc(b, 0, linear_vel[1]); - generateTaylorModelForLinearFunc(c, 0, linear_vel[2]); - TVector3 delta_T(a, b, c); + TaylorModel a(this->getTimeInterval()), b(this->getTimeInterval()), c(this->getTimeInterval()); + generateTaylorModelForLinearFunc(a, (S)0, linear_vel[0]); + generateTaylorModelForLinearFunc(b, (S)0, linear_vel[1]); + generateTaylorModelForLinearFunc(c, (S)0, linear_vel[2]); + TVector3 delta_T(a, b, c); tm = delta_R * tf1.linear().eval(); tv = tf1 * reference_p @@ -534,55 +547,385 @@ class InterpMotion : public MotionBase void computeVelocity(); - Quaternion3d deltaRotation(FCL_REAL dt) const; + Quaternion deltaRotation(S dt) const; - Quaternion3d absoluteRotation(FCL_REAL dt) const; + Quaternion absoluteRotation(S dt) const; /// @brief The transformation at time 0 - Transform3d tf1; + Transform3 tf1; /// @brief The transformation at time 1 - Transform3d tf2; + Transform3 tf2; /// @brief The transformation at current time t - mutable Transform3d tf; + mutable Transform3 tf; /// @brief Linear velocity - Vector3d linear_vel; + Vector3 linear_vel; /// @brief Angular speed - FCL_REAL angular_vel; + S angular_vel; /// @brief Angular velocity axis - Vector3d angular_axis; + Vector3 angular_axis; /// @brief Reference point for the motion (in the object's local frame) - Vector3d reference_p; + Vector3 reference_p; public: - const Vector3d& getReferencePoint() const + const Vector3& getReferencePoint() const { return reference_p; } - const Vector3d& getAngularAxis() const + const Vector3& getAngularAxis() const { return angular_axis; } - FCL_REAL getAngularVelocity() const + S getAngularVelocity() const { return angular_vel; } - const Vector3d& getLinearVelocity() const + const Vector3& getLinearVelocity() const { return linear_vel; } + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +SplineMotion::SplineMotion(const Vector3& Td0, const Vector3& Td1, const Vector3& Td2, const Vector3& Td3, + const Vector3& Rd0, const Vector3& Rd1, const Vector3& Rd2, const Vector3& Rd3) : MotionBase() +{ + Td[0] = Td0; + Td[1] = Td1; + Td[2] = Td2; + Td[3] = Td3; + + Rd[0] = Rd0; + Rd[1] = Rd1; + Rd[2] = Rd2; + Rd[3] = Rd3; + + Rd0Rd0 = Rd[0].dot(Rd[0]); + Rd0Rd1 = Rd[0].dot(Rd[1]); + Rd0Rd2 = Rd[0].dot(Rd[2]); + Rd0Rd3 = Rd[0].dot(Rd[3]); + Rd1Rd1 = Rd[1].dot(Rd[1]); + Rd1Rd2 = Rd[1].dot(Rd[2]); + Rd1Rd3 = Rd[1].dot(Rd[3]); + Rd2Rd2 = Rd[2].dot(Rd[2]); + Rd2Rd3 = Rd[2].dot(Rd[3]); + Rd3Rd3 = Rd[3].dot(Rd[3]); + + TA = Td[1] * 3 - Td[2] * 3 + Td[3] - Td[0]; + TB = (Td[0] - Td[1] * 2 + Td[2]) * 3; + TC = (Td[2] - Td[0]) * 3; + + RA = Rd[1] * 3 - Rd[2] * 3 + Rd[3] - Rd[0]; + RB = (Rd[0] - Rd[1] * 2 + Rd[2]) * 3; + RC = (Rd[2] - Rd[0]) * 3; + + integrate(0.0); +} + +//============================================================================== +template +bool SplineMotion::integrate(S dt) const +{ + if(dt > 1) dt = 1; + + Vector3 cur_T = Td[0] * getWeight0(dt) + Td[1] * getWeight1(dt) + Td[2] * getWeight2(dt) + Td[3] * getWeight3(dt); + Vector3 cur_w = Rd[0] * getWeight0(dt) + Rd[1] * getWeight1(dt) + Rd[2] * getWeight2(dt) + Rd[3] * getWeight3(dt); + S cur_angle = cur_w.norm(); + cur_w.normalize(); + + tf.linear() = AngleAxis(cur_angle, cur_w).toRotationMatrix(); + tf.translation() = cur_T; + tf_t = dt; + return true; +} + +//============================================================================== +template +S SplineMotion::computeTBound(const Vector3& n) const +{ + S Ta = TA.dot(n); + S Tb = TB.dot(n); + S Tc = TC.dot(n); + + std::vector T_potential; + T_potential.push_back(tf_t); + T_potential.push_back(1); + if(Tb * Tb - 3 * Ta * Tc >= 0) + { + if(Ta == 0) + { + if(Tb != 0) + { + S tmp = -Tc / (2 * Tb); + if(tmp < 1 && tmp > tf_t) + T_potential.push_back(tmp); + } + } + else + { + S tmp_delta = sqrt(Tb * Tb - 3 * Ta * Tc); + S tmp1 = (-Tb + tmp_delta) / (3 * Ta); + S tmp2 = (-Tb - tmp_delta) / (3 * Ta); + if(tmp1 < 1 && tmp1 > tf_t) + T_potential.push_back(tmp1); + if(tmp2 < 1 && tmp2 > tf_t) + T_potential.push_back(tmp2); + } + } + + S T_bound = Ta * T_potential[0] * T_potential[0] * T_potential[0] + Tb * T_potential[0] * T_potential[0] + Tc * T_potential[0]; + for(unsigned int i = 1; i < T_potential.size(); ++i) + { + S T_bound_tmp = Ta * T_potential[i] * T_potential[i] * T_potential[i] + Tb * T_potential[i] * T_potential[i] + Tc * T_potential[i]; + if(T_bound_tmp > T_bound) T_bound = T_bound_tmp; + } + + + S cur_delta = Ta * tf_t * tf_t * tf_t + Tb * tf_t * tf_t + Tc * tf_t; + + T_bound -= cur_delta; + T_bound /= 6.0; + + return T_bound; +} + +//============================================================================== +template +S SplineMotion::computeDWMax() const +{ + // first compute ||w'|| + int a00[5] = {1,-4,6,-4,1}; + int a01[5] = {-3,10,-11,4,0}; + int a02[5] = {3,-8,6,0,-1}; + int a03[5] = {-1,2,-1,0,0}; + int a11[5] = {9,-24,16,0,0}; + int a12[5] = {-9,18,-5,-4,0}; + int a13[5] = {3,-4,0,0,0}; + int a22[5] = {9,-12,-2,4,1}; + int a23[5] = {-3,2,1,0,0}; + int a33[5] = {1,0,0,0,0}; + + S a[5]; + + for(int i = 0; i < 5; ++i) + { + a[i] = Rd0Rd0 * a00[i] + Rd0Rd1 * a01[i] + Rd0Rd2 * a02[i] + Rd0Rd3 * a03[i] + + Rd0Rd1 * a01[i] + Rd1Rd1 * a11[i] + Rd1Rd2 * a12[i] + Rd1Rd3 * a13[i] + + Rd0Rd2 * a02[i] + Rd1Rd2 * a12[i] + Rd2Rd2 * a22[i] + Rd2Rd3 * a23[i] + + Rd0Rd3 * a03[i] + Rd1Rd3 * a13[i] + Rd2Rd3 * a23[i] + Rd3Rd3 * a33[i]; + a[i] /= 4.0; + } + + // compute polynomial for ||w'||' + int da00[4] = {4,-12,12,-4}; + int da01[4] = {-12,30,-22,4}; + int da02[4] = {12,-24,12,0}; + int da03[4] = {-4,6,-2,0}; + int da11[4] = {36,-72,32,0}; + int da12[4] = {-36,54,-10,-4}; + int da13[4] = {12,-12,0,0}; + int da22[4] = {36,-36,-4,4}; + int da23[4] = {-12,6,2,0}; + int da33[4] = {4,0,0,0}; + + S da[4]; + for(int i = 0; i < 4; ++i) + { + da[i] = Rd0Rd0 * da00[i] + Rd0Rd1 * da01[i] + Rd0Rd2 * da02[i] + Rd0Rd3 * da03[i] + + Rd0Rd1 * da01[i] + Rd1Rd1 * da11[i] + Rd1Rd2 * da12[i] + Rd1Rd3 * da13[i] + + Rd0Rd2 * da02[i] + Rd1Rd2 * da12[i] + Rd2Rd2 * da22[i] + Rd2Rd3 * da23[i] + + Rd0Rd3 * da03[i] + Rd1Rd3 * da13[i] + Rd2Rd3 * da23[i] + Rd3Rd3 * da33[i]; + da[i] /= 4.0; + } + + S roots[3]; + + int root_num = PolySolverd::solveCubic(da, roots); + + S dWdW_max = a[0] * tf_t * tf_t * tf_t + a[1] * tf_t * tf_t * tf_t + a[2] * tf_t * tf_t + a[3] * tf_t + a[4]; + S dWdW_1 = a[0] + a[1] + a[2] + a[3] + a[4]; + if(dWdW_max < dWdW_1) dWdW_max = dWdW_1; + for(int i = 0; i < root_num; ++i) + { + S v = roots[i]; + + if(v >= tf_t && v <= 1) + { + S value = a[0] * v * v * v * v + a[1] * v * v * v + a[2] * v * v + a[3] * v + a[4]; + if(value > dWdW_max) dWdW_max = value; + } + } + + return sqrt(dWdW_max); +} + +//============================================================================== +template +S SplineMotion::getWeight0(S t) const +{ + return (1 - 3 * t + 3 * t * t - t * t * t) / 6.0; +} + +//============================================================================== +template +S SplineMotion::getWeight1(S t) const +{ + return (4 - 6 * t * t + 3 * t * t * t) / 6.0; +} + +//============================================================================== +template +S SplineMotion::getWeight2(S t) const +{ + return (1 + 3 * t + 3 * t * t - 3 * t * t * t) / 6.0; } +//============================================================================== +template +S SplineMotion::getWeight3(S t) const +{ + return t * t * t / 6.0; +} + + + + +//============================================================================== +template +InterpMotion::InterpMotion() : MotionBase(), angular_axis(Vector3::UnitX()) +{ + // Default angular velocity is zero + angular_vel = 0; + + // Default reference point is local zero point + + // Default linear velocity is zero +} + +//============================================================================== +template +InterpMotion::InterpMotion(const Matrix3& R1, const Vector3& T1, + const Matrix3& R2, const Vector3& T2) : MotionBase(), + tf1(Transform3::Identity()), + tf2(Transform3::Identity()) +{ + tf1.linear() = R1; + tf1.translation() = T1; + + tf2.linear() = R2; + tf2.translation() = T2; + + tf = tf1; + + // Compute the velocities for the motion + computeVelocity(); +} + +//============================================================================== +template +InterpMotion::InterpMotion(const Transform3& tf1_, const Transform3& tf2_) : MotionBase(), + tf1(tf1_), + tf2(tf2_), + tf(tf1) +{ + // Compute the velocities for the motion + computeVelocity(); +} + +//============================================================================== +template +InterpMotion::InterpMotion(const Matrix3& R1, const Vector3& T1, + const Matrix3& R2, const Vector3& T2, + const Vector3& O) : MotionBase(), + tf1(Transform3::Identity()), + tf2(Transform3::Identity()), + reference_p(O) +{ + tf1.linear() = R1; + tf1.translation() = T1; + + tf2.linear() = R2; + tf2.translation() = T2; + + tf = tf1; + + // Compute the velocities for the motion + computeVelocity(); +} + +//============================================================================== +template +InterpMotion::InterpMotion(const Transform3& tf1_, const Transform3& tf2_, const Vector3& O) : MotionBase(), + tf1(tf1_), + tf2(tf2_), + tf(tf1), + reference_p(O) +{ +} + +//============================================================================== +template +bool InterpMotion::integrate(double dt) const +{ + if(dt > 1) dt = 1; + + tf.linear() = absoluteRotation(dt).toRotationMatrix(); + tf.translation() = linear_vel * dt + tf1 * reference_p - tf.linear() * reference_p; + + return true; +} + +//============================================================================== +template +void InterpMotion::computeVelocity() +{ + linear_vel = tf2 * reference_p - tf1 * reference_p; + + const AngleAxis aa(tf2.linear() * tf1.linear().transpose()); + angular_axis = aa.axis(); + angular_vel = aa.angle(); + + if(angular_vel < 0) + { + angular_vel = -angular_vel; + angular_axis = -angular_axis; + } +} + +//============================================================================== +template +Quaternion InterpMotion::deltaRotation(S dt) const +{ + return Quaternion(AngleAxis((S)(dt * angular_vel), angular_axis)); +} + +//============================================================================== +template +Quaternion InterpMotion::absoluteRotation(S dt) const +{ + Quaternion delta_t = deltaRotation(dt); + return delta_t * Quaternion(tf1.linear()); +} + +} // namespace fcl + #endif diff --git a/include/fcl/ccd/motion_base.h b/include/fcl/ccd/motion_base.h index c53daf261..1198c8b1e 100644 --- a/include/fcl/ccd/motion_base.h +++ b/include/fcl/ccd/motion_base.h @@ -46,145 +46,536 @@ namespace fcl { +template class MotionBase; +template class SplineMotion; + +template class ScrewMotion; + +template class InterpMotion; + +template class TranslationMotion; /// @brief Compute the motion bound for a bounding volume, given the closest direction n between two query objects +template class BVMotionBoundVisitor { public: - virtual FCL_REAL visit(const MotionBase& motion) const = 0; - virtual FCL_REAL visit(const SplineMotion& motion) const = 0; - virtual FCL_REAL visit(const ScrewMotion& motion) const = 0; - virtual FCL_REAL visit(const InterpMotion& motion) const = 0; - virtual FCL_REAL visit(const TranslationMotion& motion) const = 0; + virtual S visit(const MotionBase& motion) const = 0; + virtual S visit(const SplineMotion& motion) const = 0; + virtual S visit(const ScrewMotion& motion) const = 0; + virtual S visit(const InterpMotion& motion) const = 0; + virtual S visit(const TranslationMotion& motion) const = 0; }; template -class TBVMotionBoundVisitor : public BVMotionBoundVisitor +class TBVMotionBoundVisitor : public BVMotionBoundVisitor { public: - TBVMotionBoundVisitor(const BV& bv_, const Vector3d& n_) : bv(bv_), n(n_) {} + using S = typename BV::S; + + TBVMotionBoundVisitor(const BV& bv_, const Vector3& n_) : bv(bv_), n(n_) {} - virtual FCL_REAL visit(const MotionBase& motion) const { return 0; } - virtual FCL_REAL visit(const SplineMotion& motion) const { return 0; } - virtual FCL_REAL visit(const ScrewMotion& motion) const { return 0; } - virtual FCL_REAL visit(const InterpMotion& motion) const { return 0; } - virtual FCL_REAL visit(const TranslationMotion& motion) const { return 0; } + virtual S visit(const MotionBase& motion) const; + virtual S visit(const SplineMotion& motion) const; + virtual S visit(const ScrewMotion& motion) const; + virtual S visit(const InterpMotion& motion) const; + virtual S visit(const TranslationMotion& motion) const; protected: + template + friend struct TBVMotionBoundVisitorVisitImpl; + BV bv; - Vector3d n; + Vector3 n; }; -template<> -FCL_REAL TBVMotionBoundVisitor::visit(const SplineMotion& motion) const; - -template<> -FCL_REAL TBVMotionBoundVisitor::visit(const ScrewMotion& motion) const; - -template<> -FCL_REAL TBVMotionBoundVisitor::visit(const InterpMotion& motion) const; - -template<> -FCL_REAL TBVMotionBoundVisitor::visit(const TranslationMotion& motion) const; - - +template class TriangleMotionBoundVisitor { public: - TriangleMotionBoundVisitor(const Vector3d& a_, const Vector3d& b_, const Vector3d& c_, const Vector3d& n_) : + TriangleMotionBoundVisitor(const Vector3& a_, const Vector3& b_, const Vector3& c_, const Vector3& n_) : a(a_), b(b_), c(c_), n(n_) {} - virtual FCL_REAL visit(const MotionBase& motion) const { return 0; } - virtual FCL_REAL visit(const SplineMotion& motion) const; - virtual FCL_REAL visit(const ScrewMotion& motion) const; - virtual FCL_REAL visit(const InterpMotion& motion) const; - virtual FCL_REAL visit(const TranslationMotion& motion) const; + virtual S visit(const MotionBase& motion) const { return 0; } + virtual S visit(const SplineMotion& motion) const; + virtual S visit(const ScrewMotion& motion) const; + virtual S visit(const InterpMotion& motion) const; + virtual S visit(const TranslationMotion& motion) const; protected: - Vector3d a, b, c, n; -}; - + template + friend struct TriangleMotionBoundVisitorVisitImpl; + Vector3 a, b, c, n; +}; +template class MotionBase { public: - MotionBase() : time_interval_(std::shared_ptr(new TimeInterval(0, 1))) + MotionBase() + : time_interval_(std::shared_ptr>(new TimeInterval(0, 1))) { } virtual ~MotionBase() {} /** \brief Integrate the motion from 0 to dt */ - virtual bool integrate(double dt) const = 0; + virtual bool integrate(S dt) const = 0; /** \brief Compute the motion bound for a bounding volume, given the closest direction n between two query objects */ - virtual FCL_REAL computeMotionBound(const BVMotionBoundVisitor& mb_visitor) const = 0; + virtual S computeMotionBound(const BVMotionBoundVisitor& mb_visitor) const = 0; /** \brief Compute the motion bound for a triangle, given the closest direction n between two query objects */ - virtual FCL_REAL computeMotionBound(const TriangleMotionBoundVisitor& mb_visitor) const = 0; + virtual S computeMotionBound(const TriangleMotionBoundVisitor& mb_visitor) const = 0; /** \brief Get the rotation and translation in current step */ - void getCurrentTransform(Matrix3d& R, Vector3d& T) const + void getCurrentTransform(Matrix3& R, Vector3& T) const { - Transform3d tf; + Transform3 tf; getCurrentTransform(tf); R = tf.linear(); T = tf.translation(); } - void getCurrentTransform(Quaternion3d& Q, Vector3d& T) const + void getCurrentTransform(Quaternion& Q, Vector3& T) const { - Transform3d tf; + Transform3 tf; getCurrentTransform(tf); Q = tf.linear(); T = tf.translation(); } - void getCurrentRotation(Matrix3d& R) const + void getCurrentRotation(Matrix3& R) const { - Transform3d tf; + Transform3 tf; getCurrentTransform(tf); R = tf.linear(); } - void getCurrentRotation(Quaternion3d& Q) const + void getCurrentRotation(Quaternion& Q) const { - Transform3d tf; + Transform3 tf; getCurrentTransform(tf); Q = tf.linear(); } - void getCurrentTranslation(Vector3d& T) const + void getCurrentTranslation(Vector3& T) const { - Transform3d tf; + Transform3 tf; getCurrentTransform(tf); T = tf.translation(); } - virtual void getCurrentTransform(Transform3d& tf) const = 0; + virtual void getCurrentTransform(Transform3& tf) const = 0; - virtual void getTaylorModel(TMatrix3& tm, TVector3& tv) const = 0; + virtual void getTaylorModel(TMatrix3& tm, TVector3& tv) const = 0; - const std::shared_ptr& getTimeInterval() const + const std::shared_ptr>& getTimeInterval() const { return time_interval_; } protected: - std::shared_ptr time_interval_; + std::shared_ptr> time_interval_; }; -typedef std::shared_ptr MotionBasePtr; +using MotionBasef = MotionBase; +using MotionBased = MotionBase; + +template +using MotionBasePtr = std::shared_ptr>; + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +struct TBVMotionBoundVisitorVisitImpl +{ + static S run( + const TBVMotionBoundVisitor& /*visitor*/, + const MotionT& /*motion*/) + { + return 0; + } +}; + +//============================================================================== +template +typename BV::S TBVMotionBoundVisitor::visit( + const MotionBase& motion) const +{ + return 0; +} +//============================================================================== +template +typename BV::S TBVMotionBoundVisitor::visit( + const SplineMotion& motion) const +{ + using S = typename BV::S; + + return TBVMotionBoundVisitorVisitImpl< + S, BV, SplineMotion>::run(*this, motion); +} + +//============================================================================== +template +typename BV::S TBVMotionBoundVisitor::visit( + const ScrewMotion& motion) const +{ + using S = typename BV::S; + + return TBVMotionBoundVisitorVisitImpl< + S, BV, ScrewMotion>::run(*this, motion); +} +//============================================================================== +template +typename BV::S TBVMotionBoundVisitor::visit( + const InterpMotion& motion) const +{ + using S = typename BV::S; + return TBVMotionBoundVisitorVisitImpl< + S, BV, InterpMotion>::run(*this, motion); } +//============================================================================== +template +typename BV::S TBVMotionBoundVisitor::visit( + const TranslationMotion& motion) const +{ + using S = typename BV::S; + + return TBVMotionBoundVisitorVisitImpl< + S, BV, TranslationMotion>::run(*this, motion); +} + +//============================================================================== +template +struct TBVMotionBoundVisitorVisitImpl, SplineMotion> +{ + static S run( + const TBVMotionBoundVisitor>& visitor, + const SplineMotion& motion) + { + S T_bound = motion.computeTBound(visitor.n); + S tf_t = motion.getCurrentTime(); + + Vector3 c1 = visitor.bv.To; + Vector3 c2 = visitor.bv.To + visitor.bv.axis.col(0) * visitor.bv.l[0]; + Vector3 c3 = visitor.bv.To + visitor.bv.axis.col(1) * visitor.bv.l[1]; + Vector3 c4 = visitor.bv.To + visitor.bv.axis.col(0) * visitor.bv.l[0] + visitor.bv.axis.col(1) * visitor.bv.l[1]; + + S tmp; + // max_i |c_i * n| + S cn_max = std::abs(c1.dot(visitor.n)); + tmp = std::abs(c2.dot(visitor.n)); + if(tmp > cn_max) cn_max = tmp; + tmp = std::abs(c3.dot(visitor.n)); + if(tmp > cn_max) cn_max = tmp; + tmp = std::abs(c4.dot(visitor.n)); + if(tmp > cn_max) cn_max = tmp; + + // max_i ||c_i|| + S cmax = c1.squaredNorm(); + tmp = c2.squaredNorm(); + if(tmp > cmax) cmax = tmp; + tmp = c3.squaredNorm(); + if(tmp > cmax) cmax = tmp; + tmp = c4.squaredNorm(); + if(tmp > cmax) cmax = tmp; + cmax = sqrt(cmax); + + // max_i ||c_i x n|| + S cxn_max = (c1.cross(visitor.n)).squaredNorm(); + tmp = (c2.cross(visitor.n)).squaredNorm(); + if(tmp > cxn_max) cxn_max = tmp; + tmp = (c3.cross(visitor.n)).squaredNorm(); + if(tmp > cxn_max) cxn_max = tmp; + tmp = (c4.cross(visitor.n)).squaredNorm(); + if(tmp > cxn_max) cxn_max = tmp; + cxn_max = sqrt(cxn_max); + + S dWdW_max = motion.computeDWMax(); + S ratio = std::min(1 - tf_t, dWdW_max); + + S R_bound = 2 * (cn_max + cmax + cxn_max + 3 * visitor.bv.r) * ratio; + + + // std::cout << R_bound << " " << T_bound << std::endl; + + return R_bound + T_bound; + } +}; + +//============================================================================== +/// @brief Compute the motion bound for a bounding volume along a given direction n +/// according to mu < |v * n| + ||w x n||(r + max(||ci*||)) where ||ci*|| = ||R0(ci) x w||. w is the angular axis (normalized) +/// and ci are the endpoints of the generator primitives of RSSd. +/// Notice that all bv parameters are in the local frame of the object, but n should be in the global frame (the reason is that the motion (t1, t2 and t) is in global frame) +template +struct TBVMotionBoundVisitorVisitImpl, ScrewMotion> +{ + static S run( + const TBVMotionBoundVisitor>& visitor, + const ScrewMotion& motion) + { + Transform3 tf; + motion.getCurrentTransform(tf); + + const Vector3& axis = motion.getAxis(); + S linear_vel = motion.getLinearVelocity(); + S angular_vel = motion.getAngularVelocity(); + const Vector3& p = motion.getAxisOrigin(); + + S c_proj_max = ((tf.linear() * visitor.bv.To).cross(axis)).squaredNorm(); + S tmp; + tmp = ((tf.linear() * (visitor.bv.To + visitor.bv.axis.col(0) * visitor.bv.l[0])).cross(axis)).squaredNorm(); + if(tmp > c_proj_max) c_proj_max = tmp; + tmp = ((tf.linear() * (visitor.bv.To + visitor.bv.axis.col(1) * visitor.bv.l[1])).cross(axis)).squaredNorm(); + if(tmp > c_proj_max) c_proj_max = tmp; + tmp = ((tf.linear() * (visitor.bv.To + visitor.bv.axis.col(0) * visitor.bv.l[0] + visitor.bv.axis.col(1) * visitor.bv.l[1])).cross(axis)).squaredNorm(); + if(tmp > c_proj_max) c_proj_max = tmp; + + c_proj_max = sqrt(c_proj_max); + + S v_dot_n = axis.dot(visitor.n) * linear_vel; + S w_cross_n = (axis.cross(visitor.n)).norm() * angular_vel; + S origin_proj = ((tf.translation() - p).cross(axis)).norm(); + + S mu = v_dot_n + w_cross_n * (c_proj_max + visitor.bv.r + origin_proj); + + return mu; + } +}; + +//============================================================================== +/// @brief Compute the motion bound for a bounding volume along a given direction n +/// according to mu < |v * n| + ||w x n||(r + max(||ci*||)) where ||ci*|| = ||R0(ci) x w||. w is the angular axis (normalized) +/// and ci are the endpoints of the generator primitives of RSSd. +/// Notice that all bv parameters are in the local frame of the object, but n should be in the global frame (the reason is that the motion (t1, t2 and t) is in global frame) +template +struct TBVMotionBoundVisitorVisitImpl, InterpMotion> +{ + static S run( + const TBVMotionBoundVisitor>& visitor, + const InterpMotion& motion) + { + Transform3 tf; + motion.getCurrentTransform(tf); + + const Vector3& reference_p = motion.getReferencePoint(); + const Vector3& angular_axis = motion.getAngularAxis(); + S angular_vel = motion.getAngularVelocity(); + const Vector3& linear_vel = motion.getLinearVelocity(); + + S c_proj_max = ((tf.linear() * (visitor.bv.To - reference_p)).cross(angular_axis)).squaredNorm(); + S tmp; + tmp = ((tf.linear() * (visitor.bv.To + visitor.bv.axis.col(0) * visitor.bv.l[0] - reference_p)).cross(angular_axis)).squaredNorm(); + if(tmp > c_proj_max) c_proj_max = tmp; + tmp = ((tf.linear() * (visitor.bv.To + visitor.bv.axis.col(1) * visitor.bv.l[1] - reference_p)).cross(angular_axis)).squaredNorm(); + if(tmp > c_proj_max) c_proj_max = tmp; + tmp = ((tf.linear() * (visitor.bv.To + visitor.bv.axis.col(0) * visitor.bv.l[0] + visitor.bv.axis.col(1) * visitor.bv.l[1] - reference_p)).cross(angular_axis)).squaredNorm(); + if(tmp > c_proj_max) c_proj_max = tmp; + + c_proj_max = std::sqrt(c_proj_max); + + S v_dot_n = linear_vel.dot(visitor.n); + S w_cross_n = (angular_axis.cross(visitor.n)).norm() * angular_vel; + S mu = v_dot_n + w_cross_n * (visitor.bv.r + c_proj_max); + + return mu; + } +}; + +//============================================================================== +/// @brief Compute the motion bound for a bounding volume along a given direction n +template +struct TBVMotionBoundVisitorVisitImpl, TranslationMotion> +{ + static S run( + const TBVMotionBoundVisitor>& visitor, + const TranslationMotion& motion) + { + return motion.getVelocity().dot(visitor.n); + } +}; + +//============================================================================== +template +struct TriangleMotionBoundVisitorVisitImpl +{ + static S run( + const TriangleMotionBoundVisitor& /*visitor*/, + const MotionT& /*motion*/) + { + return 0; + } +}; + +//============================================================================== +template +S TriangleMotionBoundVisitor::visit( + const SplineMotion& motion) const +{ + return TriangleMotionBoundVisitorVisitImpl< + S, SplineMotion>::run(*this, motion); +} + +//============================================================================== +template +S TriangleMotionBoundVisitor::visit( + const ScrewMotion& motion) const +{ + return TriangleMotionBoundVisitorVisitImpl< + S, ScrewMotion>::run(*this, motion); +} + +//============================================================================== +template +S TriangleMotionBoundVisitor::visit( + const InterpMotion& motion) const +{ + return TriangleMotionBoundVisitorVisitImpl< + S, InterpMotion>::run(*this, motion); +} + +//============================================================================== +template +S TriangleMotionBoundVisitor::visit( + const TranslationMotion& motion) const +{ + return TriangleMotionBoundVisitorVisitImpl< + S, TranslationMotion>::run(*this, motion); +} + +//============================================================================== +/// @brief Compute the motion bound for a triangle along a given direction n +/// according to mu < |v * n| + ||w x n||(max||ci*||) where ||ci*|| = ||R0(ci) x w|| / \|w\|. w is the angular velocity +/// and ci are the triangle vertex coordinates. +/// Notice that the triangle is in the local frame of the object, but n should be in the global frame (the reason is that the motion (t1, t2 and t) is in global frame) +template +struct TriangleMotionBoundVisitorVisitImpl> +{ + static S run( + const TriangleMotionBoundVisitor& visitor, + const ScrewMotion& motion) + { + Transform3 tf; + motion.getCurrentTransform(tf); + + const Vector3& axis = motion.getAxis(); + S linear_vel = motion.getLinearVelocity(); + S angular_vel = motion.getAngularVelocity(); + const Vector3& p = motion.getAxisOrigin(); + + S proj_max = ((tf.linear() * visitor.a + tf.translation() - p).cross(axis)).squaredNorm(); + S tmp; + tmp = ((tf.linear() * visitor.b + tf.translation() - p).cross(axis)).squaredNorm(); + if(tmp > proj_max) proj_max = tmp; + tmp = ((tf.linear() * visitor.c + tf.translation() - p).cross(axis)).squaredNorm(); + if(tmp > proj_max) proj_max = tmp; + + proj_max = std::sqrt(proj_max); + + S v_dot_n = axis.dot(visitor.n) * linear_vel; + S w_cross_n = (axis.cross(visitor.n)).norm() * angular_vel; + S mu = v_dot_n + w_cross_n * proj_max; + + return mu; + } +}; + +//============================================================================== +/// @brief Compute the motion bound for a triangle along a given direction n +/// according to mu < |v * n| + ||w x n||(max||ci*||) where ||ci*|| = ||R0(ci) x w|| / \|w\|. w is the angular velocity +/// and ci are the triangle vertex coordinates. +/// Notice that the triangle is in the local frame of the object, but n should be in the global frame (the reason is that the motion (t1, t2 and t) is in global frame) +template +struct TriangleMotionBoundVisitorVisitImpl> +{ + static S run( + const TriangleMotionBoundVisitor& visitor, + const InterpMotion& motion) + { + Transform3 tf; + motion.getCurrentTransform(tf); + + const Vector3& reference_p = motion.getReferencePoint(); + const Vector3& angular_axis = motion.getAngularAxis(); + S angular_vel = motion.getAngularVelocity(); + const Vector3& linear_vel = motion.getLinearVelocity(); + + S proj_max = ((tf.linear() * (visitor.a - reference_p)).cross(angular_axis)).squaredNorm(); + S tmp; + tmp = ((tf.linear() * (visitor.b - reference_p)).cross(angular_axis)).squaredNorm(); + if(tmp > proj_max) proj_max = tmp; + tmp = ((tf.linear() * (visitor.c - reference_p)).cross(angular_axis)).squaredNorm(); + if(tmp > proj_max) proj_max = tmp; + + proj_max = std::sqrt(proj_max); + + S v_dot_n = linear_vel.dot(visitor.n); + S w_cross_n = (angular_axis.cross(visitor.n)).norm() * angular_vel; + S mu = v_dot_n + w_cross_n * proj_max; + + return mu; + } +}; + +//============================================================================== +template +struct TriangleMotionBoundVisitorVisitImpl> +{ + static S run( + const TriangleMotionBoundVisitor& visitor, + const SplineMotion& motion) + { + S T_bound = motion.computeTBound(visitor.n); + S tf_t = motion.getCurrentTime(); + + S R_bound = std::abs(visitor.a.dot(visitor.n)) + visitor.a.norm() + (visitor.a.cross(visitor.n)).norm(); + S R_bound_tmp = std::abs(visitor.b.dot(visitor.n)) + visitor.b.norm() + (visitor.b.cross(visitor.n)).norm(); + if(R_bound_tmp > R_bound) R_bound = R_bound_tmp; + R_bound_tmp = std::abs(visitor.c.dot(visitor.n)) + visitor.c.norm() + (visitor.c.cross(visitor.n)).norm(); + if(R_bound_tmp > R_bound) R_bound = R_bound_tmp; + + S dWdW_max = motion.computeDWMax(); + S ratio = std::min(1 - tf_t, dWdW_max); + + R_bound *= 2 * ratio; + + // std::cout << R_bound << " " << T_bound << std::endl; + + return R_bound + T_bound; + } +}; + +//============================================================================== +/// @brief Compute the motion bound for a triangle along a given direction n +template +struct TriangleMotionBoundVisitorVisitImpl> +{ + static S run( + const TriangleMotionBoundVisitor& visitor, + const TranslationMotion& motion) + { + return motion.getVelocity().dot(visitor.n); + } +}; + +} // namespace fcl + #endif diff --git a/include/fcl/ccd/taylor_matrix.h b/include/fcl/ccd/taylor_matrix.h index 9c56822ac..b4ed84959 100644 --- a/include/fcl/ccd/taylor_matrix.h +++ b/include/fcl/ccd/taylor_matrix.h @@ -44,76 +44,580 @@ namespace fcl { +template class TMatrix3 { - TVector3 v_[3]; + TVector3 v_[3]; public: TMatrix3(); - TMatrix3(const std::shared_ptr& time_interval); - TMatrix3(TaylorModel m[3][3]); - TMatrix3(const TVector3& v1, const TVector3& v2, const TVector3& v3); - TMatrix3(const Matrix3d& m, const std::shared_ptr& time_interval); + TMatrix3(const std::shared_ptr>& time_interval); + TMatrix3(TaylorModel m[3][3]); + TMatrix3(const TVector3& v1, const TVector3& v2, const TVector3& v3); + TMatrix3(const Matrix3& m, const std::shared_ptr>& time_interval); - TVector3 getColumn(size_t i) const; - const TVector3& getRow(size_t i) const; + TVector3 getColumn(size_t i) const; + const TVector3& getRow(size_t i) const; - const TaylorModel& operator () (size_t i, size_t j) const; - TaylorModel& operator () (size_t i, size_t j); + const TaylorModel& operator () (size_t i, size_t j) const; + TaylorModel& operator () (size_t i, size_t j); - TVector3 operator * (const Vector3d& v) const; - TVector3 operator * (const TVector3& v) const; - TMatrix3 operator * (const Matrix3d& m) const; + TVector3 operator * (const Vector3& v) const; + TVector3 operator * (const TVector3& v) const; + TMatrix3 operator * (const Matrix3& m) const; TMatrix3 operator * (const TMatrix3& m) const; - TMatrix3 operator * (const TaylorModel& d) const; - TMatrix3 operator * (FCL_REAL d) const; + TMatrix3 operator * (const TaylorModel& d) const; + TMatrix3 operator * (S d) const; - TMatrix3& operator *= (const Matrix3d& m); + TMatrix3& operator *= (const Matrix3& m); TMatrix3& operator *= (const TMatrix3& m); - TMatrix3& operator *= (const TaylorModel& d); - TMatrix3& operator *= (FCL_REAL d); + TMatrix3& operator *= (const TaylorModel& d); + TMatrix3& operator *= (S d); TMatrix3 operator + (const TMatrix3& m) const; TMatrix3& operator += (const TMatrix3& m); - TMatrix3 operator + (const Matrix3d& m) const; - TMatrix3& operator += (const Matrix3d& m); + TMatrix3 operator + (const Matrix3& m) const; + TMatrix3& operator += (const Matrix3& m); TMatrix3 operator - (const TMatrix3& m) const; TMatrix3& operator -= (const TMatrix3& m); - TMatrix3 operator - (const Matrix3d& m) const; - TMatrix3& operator -= (const Matrix3d& m); + TMatrix3 operator - (const Matrix3& m) const; + TMatrix3& operator -= (const Matrix3& m); TMatrix3 operator - () const; - IMatrix3 getBound() const; - IMatrix3 getBound(FCL_REAL l, FCL_REAL r) const; - IMatrix3 getBound(FCL_REAL t) const; - - IMatrix3 getTightBound() const; - IMatrix3 getTightBound(FCL_REAL l, FCL_REAL r) const; + IMatrix3 getBound() const; + IMatrix3 getBound(S l, S r) const; + IMatrix3 getBound(S t) const; + IMatrix3 getTightBound() const; + IMatrix3 getTightBound(S l, S r) const; void print() const; void setIdentity(); void setZero(); - FCL_REAL diameter() const; + S diameter() const; - void setTimeInterval(const std::shared_ptr& time_interval); - void setTimeInterval(FCL_REAL l, FCL_REAL r); + void setTimeInterval(const std::shared_ptr>& time_interval); + void setTimeInterval(S l, S r); - const std::shared_ptr& getTimeInterval() const; + const std::shared_ptr>& getTimeInterval() const; TMatrix3& rotationConstrain(); }; -TMatrix3 rotationConstrain(const TMatrix3& m); +template +TMatrix3 rotationConstrain(const TMatrix3& m); + +template +TMatrix3 operator * (const Matrix3& m, const TaylorModel& a); + +template +TMatrix3 operator * (const TaylorModel& a, const Matrix3& m); + +template +TMatrix3 operator * (const TaylorModel& a, const TMatrix3& m); + +template +TMatrix3 operator * (S d, const TMatrix3& m); + +template +TMatrix3 operator + (const Matrix3& m1, const TMatrix3& m2); + +template +TMatrix3 operator - (const Matrix3& m1, const TMatrix3& m2); + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +TMatrix3::TMatrix3() +{ +} + +//============================================================================== +template +TMatrix3::TMatrix3(const std::shared_ptr>& time_interval) +{ + setTimeInterval(time_interval); +} + +//============================================================================== +template +TMatrix3::TMatrix3(TaylorModel m[3][3]) +{ + v_[0] = TVector3(m[0]); + v_[1] = TVector3(m[1]); + v_[2] = TVector3(m[2]); +} + +//============================================================================== +template +TMatrix3::TMatrix3(const TVector3& v1, const TVector3& v2, const TVector3& v3) +{ + v_[0] = v1; + v_[1] = v2; + v_[2] = v3; +} + +//============================================================================== +template +TMatrix3::TMatrix3(const Matrix3& m, const std::shared_ptr>& time_interval) +{ + v_[0] = TVector3(m.row(0), time_interval); + v_[1] = TVector3(m.row(1), time_interval); + v_[2] = TVector3(m.row(2), time_interval); +} + +//============================================================================== +template +void TMatrix3::setIdentity() +{ + setZero(); + v_[0][0].coeff(0) = 1; + v_[1][1].coeff(0) = 1; + v_[2][2].coeff(0) = 1; + +} + +//============================================================================== +template +void TMatrix3::setZero() +{ + v_[0].setZero(); + v_[1].setZero(); + v_[2].setZero(); +} + +//============================================================================== +template +TVector3 TMatrix3::getColumn(size_t i) const +{ + return TVector3(v_[0][i], v_[1][i], v_[2][i]); +} + +//============================================================================== +template +const TVector3& TMatrix3::getRow(size_t i) const +{ + return v_[i]; +} + +//============================================================================== +template +const TaylorModel& TMatrix3::operator () (size_t i, size_t j) const +{ + return v_[i][j]; +} + +//============================================================================== +template +TaylorModel& TMatrix3::operator () (size_t i, size_t j) +{ + return v_[i][j]; +} + +//============================================================================== +template +TMatrix3 TMatrix3::operator * (const Matrix3& m) const +{ + const Vector3& mc0 = m.col(0); + const Vector3& mc1 = m.col(1); + const Vector3& mc2 = m.col(2); + + return TMatrix3(TVector3(v_[0].dot(mc0), v_[0].dot(mc1), v_[0].dot(mc2)), + TVector3(v_[1].dot(mc0), v_[1].dot(mc1), v_[1].dot(mc2)), + TVector3(v_[2].dot(mc0), v_[2].dot(mc1), v_[2].dot(mc2))); +} + +//============================================================================== +template +TVector3 TMatrix3::operator * (const Vector3& v) const +{ + return TVector3(v_[0].dot(v), v_[1].dot(v), v_[2].dot(v)); +} + +//============================================================================== +template +TVector3 TMatrix3::operator * (const TVector3& v) const +{ + return TVector3(v_[0].dot(v), v_[1].dot(v), v_[2].dot(v)); +} + +//============================================================================== +template +TMatrix3 TMatrix3::operator * (const TMatrix3& m) const +{ + const TVector3& mc0 = m.getColumn(0); + const TVector3& mc1 = m.getColumn(1); + const TVector3& mc2 = m.getColumn(2); + + return TMatrix3(TVector3(v_[0].dot(mc0), v_[0].dot(mc1), v_[0].dot(mc2)), + TVector3(v_[1].dot(mc0), v_[1].dot(mc1), v_[1].dot(mc2)), + TVector3(v_[2].dot(mc0), v_[2].dot(mc1), v_[2].dot(mc2))); +} + +//============================================================================== +template +TMatrix3 TMatrix3::operator * (const TaylorModel& d) const +{ + return TMatrix3(v_[0] * d, v_[1] * d, v_[2] * d); +} + +//============================================================================== +template +TMatrix3 TMatrix3::operator * (S d) const +{ + return TMatrix3(v_[0] * d, v_[1] * d, v_[2] * d); +} + +//============================================================================== +template +TMatrix3& TMatrix3::operator *= (const Matrix3& m) +{ + const Vector3& mc0 = m.col(0); + const Vector3& mc1 = m.col(1); + const Vector3& mc2 = m.col(2); + + v_[0] = TVector3(v_[0].dot(mc0), v_[0].dot(mc1), v_[0].dot(mc2)); + v_[1] = TVector3(v_[1].dot(mc0), v_[1].dot(mc1), v_[1].dot(mc2)); + v_[2] = TVector3(v_[2].dot(mc0), v_[2].dot(mc1), v_[2].dot(mc2)); + return *this; +} + +//============================================================================== +template +TMatrix3& TMatrix3::operator *= (const TMatrix3& m) +{ + const TVector3& mc0 = m.getColumn(0); + const TVector3& mc1 = m.getColumn(1); + const TVector3& mc2 = m.getColumn(2); + + v_[0] = TVector3(v_[0].dot(mc0), v_[0].dot(mc1), v_[0].dot(mc2)); + v_[1] = TVector3(v_[1].dot(mc0), v_[1].dot(mc1), v_[1].dot(mc2)); + v_[2] = TVector3(v_[2].dot(mc0), v_[2].dot(mc1), v_[2].dot(mc2)); + + return *this; +} + +//============================================================================== +template +TMatrix3& TMatrix3::operator *= (const TaylorModel& d) +{ + v_[0] *= d; + v_[1] *= d; + v_[2] *= d; + return *this; +} + +//============================================================================== +template +TMatrix3& TMatrix3::operator *= (S d) +{ + v_[0] *= d; + v_[1] *= d; + v_[2] *= d; + return *this; +} + +//============================================================================== +template +TMatrix3 TMatrix3::operator + (const TMatrix3& m) const +{ + return TMatrix3(v_[0] + m.v_[0], v_[1] + m.v_[1], v_[2] + m.v_[2]); +} + +//============================================================================== +template +TMatrix3& TMatrix3::operator += (const TMatrix3& m) +{ + v_[0] += m.v_[0]; + v_[1] += m.v_[1]; + v_[2] += m.v_[2]; + return *this; +} + +//============================================================================== +template +TMatrix3 TMatrix3::operator + (const Matrix3& m) const +{ + TMatrix3 res = *this; + res += m; + return res; +} + +//============================================================================== +template +TMatrix3& TMatrix3::operator += (const Matrix3& m) +{ + for(std::size_t i = 0; i < 3; ++i) + { + for(std::size_t j = 0; j < 3; ++j) + v_[i][j] += m(i, j); + } + + return *this; +} + +//============================================================================== +template +TMatrix3 TMatrix3::operator - (const TMatrix3& m) const +{ + return TMatrix3(v_[0] - m.v_[0], v_[1] - m.v_[1], v_[2] - m.v_[2]); +} + +//============================================================================== +template +TMatrix3& TMatrix3::operator -= (const TMatrix3& m) +{ + v_[0] -= m.v_[0]; + v_[1] -= m.v_[1]; + v_[2] -= m.v_[2]; + return *this; +} + +//============================================================================== +template +TMatrix3 TMatrix3::operator - (const Matrix3& m) const +{ + TMatrix3 res = *this; + res -= m; + return res; +} + +//============================================================================== +template +TMatrix3& TMatrix3::operator -= (const Matrix3& m) +{ + for(std::size_t i = 0; i < 3; ++i) + { + for(std::size_t j = 0; j < 3; ++j) + v_[i][j] -= m(i, j); + } + + return *this; +} + +//============================================================================== +template +TMatrix3 TMatrix3::operator - () const +{ + return TMatrix3(-v_[0], -v_[1], -v_[2]); +} + +//============================================================================== +template +void TMatrix3::print() const +{ + getColumn(0).print(); + getColumn(1).print(); + getColumn(2).print(); +} + +//============================================================================== +template +IMatrix3 TMatrix3::getBound() const +{ + return IMatrix3(v_[0].getBound(), v_[1].getBound(), v_[2].getBound()); +} + +//============================================================================== +template +IMatrix3 TMatrix3::getBound(S l, S r) const +{ + return IMatrix3(v_[0].getBound(l, r), v_[1].getBound(l, r), v_[2].getBound(l, r)); +} + +//============================================================================== +template +IMatrix3 TMatrix3::getBound(S t) const +{ + return IMatrix3(v_[0].getBound(t), v_[1].getBound(t), v_[2].getBound(t)); +} + +//============================================================================== +template +IMatrix3 TMatrix3::getTightBound() const +{ + return IMatrix3(v_[0].getTightBound(), v_[1].getTightBound(), v_[2].getTightBound()); +} + +//============================================================================== +template +IMatrix3 TMatrix3::getTightBound(S l, S r) const +{ + return IMatrix3(v_[0].getTightBound(l, r), v_[1].getTightBound(l, r), v_[2].getTightBound(l, r)); +} + +//============================================================================== +template +S TMatrix3::diameter() const +{ + S d = 0; + + S tmp = v_[0][0].remainder().diameter(); + if(tmp > d) d = tmp; + tmp = v_[0][1].remainder().diameter(); + if(tmp > d) d = tmp; + tmp = v_[0][2].remainder().diameter(); + if(tmp > d) d = tmp; + + tmp = v_[1][0].remainder().diameter(); + if(tmp > d) d = tmp; + tmp = v_[1][1].remainder().diameter(); + if(tmp > d) d = tmp; + tmp = v_[1][2].remainder().diameter(); + if(tmp > d) d = tmp; + + tmp = v_[2][0].remainder().diameter(); + if(tmp > d) d = tmp; + tmp = v_[2][1].remainder().diameter(); + if(tmp > d) d = tmp; + tmp = v_[2][2].remainder().diameter(); + if(tmp > d) d = tmp; -TMatrix3 operator * (const Matrix3d& m, const TaylorModel& a); -TMatrix3 operator * (const TaylorModel& a, const Matrix3d& m); -TMatrix3 operator * (const TaylorModel& a, const TMatrix3& m); -TMatrix3 operator * (FCL_REAL d, const TMatrix3& m); -TMatrix3 operator + (const Matrix3d& m1, const TMatrix3& m2); -TMatrix3 operator - (const Matrix3d& m1, const TMatrix3& m2); + return d; +} + +//============================================================================== +template +void TMatrix3::setTimeInterval(const std::shared_ptr>& time_interval) +{ + v_[0].setTimeInterval(time_interval); + v_[1].setTimeInterval(time_interval); + v_[2].setTimeInterval(time_interval); +} + +//============================================================================== +template +void TMatrix3::setTimeInterval(S l, S r) +{ + v_[0].setTimeInterval(l, r); + v_[1].setTimeInterval(l, r); + v_[2].setTimeInterval(l, r); +} + +//============================================================================== +template +const std::shared_ptr>& TMatrix3::getTimeInterval() const +{ + return v_[0].getTimeInterval(); +} + +//============================================================================== +template +TMatrix3& TMatrix3::rotationConstrain() +{ + for(std::size_t i = 0; i < 3; ++i) + { + for(std::size_t j = 0; j < 3; ++j) + { + if(v_[i][j].remainder()[0] < -1) v_[i][j].remainder()[0] = -1; + else if(v_[i][j].remainder()[0] > 1) v_[i][j].remainder()[0] = 1; + + if(v_[i][j].remainder()[1] < -1) v_[i][j].remainder()[1] = -1; + else if(v_[i][j].remainder()[1] > 1) v_[i][j].remainder()[1] = 1; + + if((v_[i][j].remainder()[0] == -1) && (v_[i][j].remainder()[1] == 1)) + { + v_[i][j].coeff(0) = 0; + v_[i][j].coeff(1) = 0; + v_[i][j].coeff(2) = 0; + v_[i][j].coeff(3) = 0; + } + } + } + return *this; } +//============================================================================== +template +TMatrix3 rotationConstrain(const TMatrix3& m) +{ + TMatrix3 res; + + for(std::size_t i = 0; i < 3; ++i) + { + for(std::size_t j = 0; j < 3; ++j) + { + if(m(i, j).remainder()[0] < -1) res(i, j).remainder()[0] = -1; + else if(m(i, j).remainder()[0] > 1) res(i, j).remainder()[0] = 1; + + if(m(i, j).remainder()[1] < -1) res(i, j).remainder()[1] = -1; + else if(m(i, j).remainder()[1] > 1) res(i, j).remainder()[1] = 1; + + if((m(i, j).remainder()[0] == -1) && (m(i, j).remainder()[1] == 1)) + { + res(i, j).coeff(0) = 0; + res(i, j).coeff(1) = 0; + res(i, j).coeff(2) = 0; + res(i, j).coeff(3) = 0; + } + } + } + + return res; +} + +//============================================================================== +template +TMatrix3 operator * (const Matrix3& m, const TaylorModel& a) +{ + TMatrix3 res(a.getTimeInterval()); + res(0, 0) = a * m(0, 0); + res(0, 1) = a * m(0, 1); + res(0, 1) = a * m(0, 2); + + res(1, 0) = a * m(1, 0); + res(1, 1) = a * m(1, 1); + res(1, 1) = a * m(1, 2); + + res(2, 0) = a * m(2, 0); + res(2, 1) = a * m(2, 1); + res(2, 1) = a * m(2, 2); + + return res; +} + +//============================================================================== +template +TMatrix3 operator * (const TaylorModel& a, const Matrix3& m) +{ + return m * a; +} + +//============================================================================== +template +TMatrix3 operator * (const TaylorModel& a, const TMatrix3& m) +{ + return m * a; +} + +//============================================================================== +template +TMatrix3 operator * (S d, const TMatrix3& m) +{ + return m * d; +} + +//============================================================================== +template +TMatrix3 operator + (const Matrix3& m1, const TMatrix3& m2) +{ + return m2 + m1; +} + +//============================================================================== +template +TMatrix3 operator - (const Matrix3& m1, const TMatrix3& m2) +{ + return -m2 + m1; +} + +} // namespace fcl + #endif diff --git a/include/fcl/ccd/taylor_model.h b/include/fcl/ccd/taylor_model.h index 47309d3f6..3a2c462fd 100644 --- a/include/fcl/ccd/taylor_model.h +++ b/include/fcl/ccd/taylor_model.h @@ -38,29 +38,32 @@ #ifndef FCL_CCD_TAYLOR_MODEL_H #define FCL_CCD_TAYLOR_MODEL_H -#include "fcl/ccd/interval.h" #include +#include +#include "fcl/math/constants.h" +#include "fcl/ccd/interval.h" namespace fcl { +template struct TimeInterval { - /// @brief time interval and different powers - Interval t_; // [t1, t2] - Interval t2_; // [t1, t2]^2 - Interval t3_; // [t1, t2]^3 - Interval t4_; // [t1, t2]^4 - Interval t5_; // [t1, t2]^5 - Interval t6_; // [t1, t2]^6 + /// @brief time Interval and different powers + Interval t_; // [t1, t2] + Interval t2_; // [t1, t2]^2 + Interval t3_; // [t1, t2]^3 + Interval t4_; // [t1, t2]^4 + Interval t5_; // [t1, t2]^5 + Interval t6_; // [t1, t2]^6 - TimeInterval() {} - TimeInterval(FCL_REAL l, FCL_REAL r) + TimeInterval() {} + TimeInterval(S l, S r) { setValue(l, r); } - void setValue(FCL_REAL l, FCL_REAL r) + void setValue(S l, S r) { t_.setValue(l, r); t2_.setValue(l * t_[0], r * t_[1]); @@ -74,45 +77,46 @@ struct TimeInterval /// @brief TaylorModel implements a third order Taylor model, i.e., a cubic approximation of a function /// over a time interval, with an interval remainder. /// All the operations on two Taylor models assume their time intervals are the same. +template class TaylorModel { /// @brief time interval - std::shared_ptr time_interval_; + std::shared_ptr> time_interval_; /// @brief Coefficients of the cubic polynomial approximation - FCL_REAL coeffs_[4]; + S coeffs_[4]; /// @brief interval remainder - Interval r_; + Interval r_; public: - void setTimeInterval(FCL_REAL l, FCL_REAL r) + void setTimeInterval(S l, S r) { time_interval_->setValue(l, r); } - void setTimeInterval(const std::shared_ptr& time_interval) + void setTimeInterval(const std::shared_ptr>& time_interval) { time_interval_ = time_interval; } - const std::shared_ptr& getTimeInterval() const + const std::shared_ptr>& getTimeInterval() const { return time_interval_; } - FCL_REAL coeff(std::size_t i) const { return coeffs_[i]; } - FCL_REAL& coeff(std::size_t i) { return coeffs_[i]; } - const Interval& remainder() const { return r_; } - Interval& remainder() { return r_; } + S coeff(std::size_t i) const { return coeffs_[i]; } + S& coeff(std::size_t i) { return coeffs_[i]; } + const Interval& remainder() const { return r_; } + Interval& remainder() { return r_; } TaylorModel(); - TaylorModel(const std::shared_ptr& time_interval); - TaylorModel(FCL_REAL coeff, const std::shared_ptr& time_interval); - TaylorModel(FCL_REAL coeffs[3], const Interval& r, const std::shared_ptr& time_interval); - TaylorModel(FCL_REAL c0, FCL_REAL c1, FCL_REAL c2, FCL_REAL c3, const Interval& r, const std::shared_ptr& time_interval); + TaylorModel(const std::shared_ptr>& time_interval); + TaylorModel(S coeff, const std::shared_ptr>& time_interval); + TaylorModel(S coeffs[3], const Interval& r, const std::shared_ptr>& time_interval); + TaylorModel(S c0, S c1, S c2, S c3, const Interval& r, const std::shared_ptr>& time_interval); TaylorModel operator + (const TaylorModel& other) const; TaylorModel& operator += (const TaylorModel& other); @@ -120,45 +124,577 @@ class TaylorModel TaylorModel operator - (const TaylorModel& other) const; TaylorModel& operator -= (const TaylorModel& other); - TaylorModel operator + (FCL_REAL d) const; - TaylorModel& operator += (FCL_REAL d); + TaylorModel operator + (S d) const; + TaylorModel& operator += (S d); - TaylorModel operator - (FCL_REAL d) const; - TaylorModel& operator -= (FCL_REAL d); + TaylorModel operator - (S d) const; + TaylorModel& operator -= (S d); TaylorModel operator * (const TaylorModel& other) const; - TaylorModel operator * (FCL_REAL d) const; + TaylorModel operator * (S d) const; TaylorModel& operator *= (const TaylorModel& other); - TaylorModel& operator *= (FCL_REAL d); + TaylorModel& operator *= (S d); TaylorModel operator - () const; void print() const; - Interval getBound() const; - Interval getBound(FCL_REAL l, FCL_REAL r) const; + Interval getBound() const; + Interval getBound(S l, S r) const; - Interval getTightBound() const; - Interval getTightBound(FCL_REAL l, FCL_REAL r) const; + Interval getTightBound() const; + Interval getTightBound(S l, S r) const; - Interval getBound(FCL_REAL t) const; + Interval getBound(S t) const; void setZero(); }; -TaylorModel operator * (FCL_REAL d, const TaylorModel& a); -TaylorModel operator + (FCL_REAL d, const TaylorModel& a); -TaylorModel operator - (FCL_REAL d, const TaylorModel& a); +template +TaylorModel operator * (S d, const TaylorModel& a); + +template +TaylorModel operator + (S d, const TaylorModel& a); + +template +TaylorModel operator - (S d, const TaylorModel& a); /// @brief Generate Taylor model for cos(w t + q0) -void generateTaylorModelForCosFunc(TaylorModel& tm, FCL_REAL w, FCL_REAL q0); +template +void generateTaylorModelForCosFunc(TaylorModel& tm, S w, S q0); /// @brief Generate Taylor model for sin(w t + q0) -void generateTaylorModelForSinFunc(TaylorModel& tm, FCL_REAL w, FCL_REAL q0); +template +void generateTaylorModelForSinFunc(TaylorModel& tm, S w, S q0); /// @brief Generate Taylor model for p + v t -void generateTaylorModelForLinearFunc(TaylorModel& tm, FCL_REAL p, FCL_REAL v); +template +void generateTaylorModelForLinearFunc(TaylorModel& tm, S p, S v); + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +TaylorModel::TaylorModel() +{ + coeffs_[0] = coeffs_[1] = coeffs_[2] = coeffs_[3] = 0; +} + +//============================================================================== +template +TaylorModel::TaylorModel(const std::shared_ptr>& time_interval) : time_interval_(time_interval) +{ + coeffs_[0] = coeffs_[1] = coeffs_[2] = coeffs_[3] = 0; +} + +//============================================================================== +template +TaylorModel::TaylorModel(S coeff, const std::shared_ptr>& time_interval) : time_interval_(time_interval) +{ + coeffs_[0] = coeff; + coeffs_[1] = coeffs_[2] = coeffs_[3] = r_[0] = r_[1] = 0; +} + +//============================================================================== +template +TaylorModel::TaylorModel(S coeffs[3], const Interval& r, const std::shared_ptr>& time_interval) : time_interval_(time_interval) +{ + coeffs_[0] = coeffs[0]; + coeffs_[1] = coeffs[1]; + coeffs_[2] = coeffs[2]; + coeffs_[3] = coeffs[3]; + + r_ = r; +} + +//============================================================================== +template +TaylorModel::TaylorModel(S c0, S c1, S c2, S c3, const Interval& r, const std::shared_ptr>& time_interval) : time_interval_(time_interval) +{ + coeffs_[0] = c0; + coeffs_[1] = c1; + coeffs_[2] = c2; + coeffs_[3] = c3; + r_ = r; } +//============================================================================== +template +TaylorModel TaylorModel::operator + (S d) const +{ + return TaylorModel(coeffs_[0] + d, coeffs_[1], coeffs_[2], coeffs_[3], r_, time_interval_); +} + +//============================================================================== +template +TaylorModel& TaylorModel::operator += (S d) +{ + coeffs_[0] += d; + return *this; +} + +//============================================================================== +template +TaylorModel TaylorModel::operator - (S d) const +{ + return TaylorModel(coeffs_[0] - d, coeffs_[1], coeffs_[2], coeffs_[3], r_, time_interval_); +} + +//============================================================================== +template +TaylorModel& TaylorModel::operator -= (S d) +{ + coeffs_[0] -= d; + return *this; +} + +//============================================================================== +template +TaylorModel TaylorModel::operator + (const TaylorModel& other) const +{ + assert(other.time_interval_ == time_interval_); + return TaylorModel(coeffs_[0] + other.coeffs_[0], coeffs_[1] + other.coeffs_[1], coeffs_[2] + other.coeffs_[2], coeffs_[3] + other.coeffs_[3], r_ + other.r_, time_interval_); +} + +//============================================================================== +template +TaylorModel TaylorModel::operator - (const TaylorModel& other) const +{ + assert(other.time_interval_ == time_interval_); + return TaylorModel(coeffs_[0] - other.coeffs_[0], coeffs_[1] - other.coeffs_[1], coeffs_[2] - other.coeffs_[2], coeffs_[3] - other.coeffs_[3], r_ - other.r_, time_interval_); +} + +//============================================================================== +template +TaylorModel& TaylorModel::operator += (const TaylorModel& other) +{ + assert(other.time_interval_ == time_interval_); + coeffs_[0] += other.coeffs_[0]; + coeffs_[1] += other.coeffs_[1]; + coeffs_[2] += other.coeffs_[2]; + coeffs_[3] += other.coeffs_[3]; + r_ += other.r_; + return *this; +} + +//============================================================================== +template +TaylorModel& TaylorModel::operator -= (const TaylorModel& other) +{ + assert(other.time_interval_ == time_interval_); + coeffs_[0] -= other.coeffs_[0]; + coeffs_[1] -= other.coeffs_[1]; + coeffs_[2] -= other.coeffs_[2]; + coeffs_[3] -= other.coeffs_[3]; + r_ -= other.r_; + return *this; +} + +//============================================================================== +/// @brief Taylor model multiplication: +/// f(t) = c0+c1*t+c2*t^2+c3*t^3+[a,b] +/// g(t) = c0'+c1'*t+c2'*t^2+c3'*t^2+[c,d] +/// f(t)g(t)= c0c0'+ +/// (c0c1'+c1c0')t+ +/// (c0c2'+c1c1'+c2c0')t^2+ +/// (c0c3'+c1c2'+c2c1'+c3c0')t^3+ +/// [a,b][c,d]+ +/// (c1c3'+c2c2'+c3c1')t^4+ +/// (c2c3'+c3c2')t^5+ +/// (c3c3')t^6+ +/// (c0+c1*t+c2*t^2+c3*t^3)[c,d]+ +/// (c0'+c1'*t+c2'*t^2+c3'*c^3)[a,b] +template +TaylorModel TaylorModel::operator * (const TaylorModel& other) const +{ + TaylorModel res(*this); + res *= other; + return res; +} + +//============================================================================== +template +TaylorModel TaylorModel::operator * (S d) const +{ + return TaylorModel(coeffs_[0] * d, coeffs_[1] * d, coeffs_[2] * d, coeffs_[3] * d, r_ * d, time_interval_); +} + +//============================================================================== +template +TaylorModel& TaylorModel::operator *= (const TaylorModel& other) +{ + assert(other.time_interval_ == time_interval_); + register S c0, c1, c2, c3; + register S c0b = other.coeffs_[0], c1b = other.coeffs_[1], c2b = other.coeffs_[2], c3b = other.coeffs_[3]; + + const Interval& rb = other.r_; + + c0 = coeffs_[0] * c0b; + c1 = coeffs_[0] * c1b + coeffs_[1] * c0b; + c2 = coeffs_[0] * c2b + coeffs_[1] * c1b + coeffs_[2] * c0b; + c3 = coeffs_[0] * c3b + coeffs_[1] * c2b + coeffs_[2] * c1b + coeffs_[3] * c0b; + + Interval remainder(r_ * rb); + register S tempVal = coeffs_[1] * c3b + coeffs_[2] * c2b + coeffs_[3] * c1b; + remainder += time_interval_->t4_ * tempVal; + + tempVal = coeffs_[2] * c3b + coeffs_[3] * c2b; + remainder += time_interval_->t5_ * tempVal; + + tempVal = coeffs_[3] * c3b; + remainder += time_interval_->t6_ * tempVal; + + remainder += ((Interval(coeffs_[0]) + time_interval_->t_ * coeffs_[1] + time_interval_->t2_ * coeffs_[2] + time_interval_->t3_ * coeffs_[3]) * rb + + (Interval(c0b) + time_interval_->t_ * c1b + time_interval_->t2_ * c2b + time_interval_->t3_ * c3b) * r_); + + coeffs_[0] = c0; + coeffs_[1] = c1; + coeffs_[2] = c2; + coeffs_[3] = c3; + + r_ = remainder; + + return *this; +} + +//============================================================================== +template +TaylorModel& TaylorModel::operator *= (S d) +{ + coeffs_[0] *= d; + coeffs_[1] *= d; + coeffs_[2] *= d; + coeffs_[3] *= d; + r_ *= d; + return *this; +} + +//============================================================================== +template +TaylorModel TaylorModel::operator - () const +{ + return TaylorModel(-coeffs_[0], -coeffs_[1], -coeffs_[2], -coeffs_[3], -r_, time_interval_); +} + +//============================================================================== +template +void TaylorModel::print() const +{ + std::cout << coeffs_[0] << "+" << coeffs_[1] << "*t+" << coeffs_[2] << "*t^2+" << coeffs_[3] << "*t^3+[" << r_[0] << "," << r_[1] << "]" << std::endl; +} + +//============================================================================== +template +Interval TaylorModel::getBound(S t) const +{ + return Interval(coeffs_[0] + t * (coeffs_[1] + t * (coeffs_[2] + t * coeffs_[3]))) + r_; +} + +//============================================================================== +template +Interval TaylorModel::getBound(S t0, S t1) const +{ + Interval t(t0, t1); + Interval t2(t0 * t0, t1 * t1); + Interval t3(t0 * t2[0], t1 * t2[1]); + + return Interval(coeffs_[0]) + t * coeffs_[1] + t2 * coeffs_[2] + t3 * coeffs_[3] + r_; +} + +//============================================================================== +template +Interval TaylorModel::getBound() const +{ + return Interval(coeffs_[0] + r_[0], coeffs_[1] + r_[1]) + time_interval_->t_ * coeffs_[1] + time_interval_->t2_ * coeffs_[2] + time_interval_->t3_ * coeffs_[3]; +} + +//============================================================================== +template +Interval TaylorModel::getTightBound(S t0, S t1) const +{ + if(t0 < time_interval_->t_[0]) t0 = time_interval_->t_[0]; + if(t1 > time_interval_->t_[1]) t1 = time_interval_->t_[1]; + + if(coeffs_[3] == 0) + { + register S a = -coeffs_[1] / (2 * coeffs_[2]); + Interval polybounds; + if(a <= t1 && a >= t0) + { + S AQ = coeffs_[0] + a * (coeffs_[1] + a * coeffs_[2]); + register S t = t0; + S LQ = coeffs_[0] + t * (coeffs_[1] + t * coeffs_[2]); + t = t1; + S RQ = coeffs_[0] + t * (coeffs_[1] + t * coeffs_[2]); + + S minQ = LQ, maxQ = RQ; + if(LQ > RQ) + { + minQ = RQ; + maxQ = LQ; + } + + if(minQ > AQ) minQ = AQ; + if(maxQ < AQ) maxQ = AQ; + + polybounds.setValue(minQ, maxQ); + } + else + { + register S t = t0; + S LQ = coeffs_[0] + t * (coeffs_[1] + t * coeffs_[2]); + t = t1; + S RQ = coeffs_[0] + t * (coeffs_[1] + t * coeffs_[2]); + + if(LQ > RQ) polybounds.setValue(RQ, LQ); + else polybounds.setValue(LQ, RQ); + } + + return polybounds + r_; + } + else + { + register S t = t0; + S LQ = coeffs_[0] + t * (coeffs_[1] + t * (coeffs_[2] + t * coeffs_[3])); + t = t1; + S RQ = coeffs_[0] + t * (coeffs_[1] + t * (coeffs_[2] + t * coeffs_[3])); + + if(LQ > RQ) + { + S tmp = LQ; + LQ = RQ; + RQ = tmp; + } + + // derivative: c1+2*c2*t+3*c3*t^2 + + S delta = coeffs_[2] * coeffs_[2] - 3 * coeffs_[1] * coeffs_[3]; + if(delta < 0) + return Interval(LQ, RQ) + r_; + + S r1 = (-coeffs_[2]-sqrt(delta))/(3*coeffs_[3]); + S r2 = (-coeffs_[2]+sqrt(delta))/(3*coeffs_[3]); + + if(r1 <= t1 && r1 >= t0) + { + S Q = coeffs_[0] + r1 * (coeffs_[1] + r1 * (coeffs_[2] + r1 * coeffs_[3])); + if(Q < LQ) LQ = Q; + else if(Q > RQ) RQ = Q; + } + + if(r2 <= t1 && r2 >= t0) + { + S Q = coeffs_[0] + r2 * (coeffs_[1] + r2 * (coeffs_[2] + r2 * coeffs_[3])); + if(Q < LQ) LQ = Q; + else if(Q > RQ) RQ = Q; + } + + return Interval(LQ, RQ) + r_; + } +} + +//============================================================================== +template +Interval TaylorModel::getTightBound() const +{ + return getTightBound(time_interval_->t_[0], time_interval_->t_[1]); +} + +//============================================================================== +template +void TaylorModel::setZero() +{ + coeffs_[0] = coeffs_[1] = coeffs_[2] = coeffs_[3] = 0; + r_.setValue(0); +} + +//============================================================================== +template +TaylorModel operator * (S d, const TaylorModel& a) +{ + TaylorModel res(a); + res.coeff(0) *= d; + res.coeff(1) *= d; + res.coeff(2) *= d; + res.coeff(3) *= d; + res.remainder() *= d; + return res; +} + +//============================================================================== +template +TaylorModel operator + (S d, const TaylorModel& a) +{ + return a + d; +} + +//============================================================================== +template +TaylorModel operator - (S d, const TaylorModel& a) +{ + return -a + d; +} + +//============================================================================== +template +void generateTaylorModelForCosFunc(TaylorModel& tm, S w, S q0) +{ + S a = tm.getTimeInterval()->t_.center(); + S t = w * a + q0; + S w2 = w * w; + S fa = cos(t); + S fda = -w*sin(t); + S fdda = -w2*fa; + S fddda = -w2*fda; + + tm.coeff(0) = fa-a*(fda-0.5*a*(fdda-1.0/3.0*a*fddda)); + tm.coeff(1) = fda-a*fdda+0.5*a*a*fddda; + tm.coeff(2) = 0.5*(fdda-a*fddda); + tm.coeff(3) = 1.0/6.0*fddda; + + // compute bounds for w^3 cos(wt+q0)/16, t \in [t0, t1] + Interval fddddBounds; + if(w == 0) fddddBounds.setValue(0); + else + { + S cosQL = cos(tm.getTimeInterval()->t_[0] * w + q0); + S cosQR = cos(tm.getTimeInterval()->t_[1] * w + q0); + + if(cosQL < cosQR) fddddBounds.setValue(cosQL, cosQR); + else fddddBounds.setValue(cosQR, cosQL); + + // enlarge to handle round-off errors + fddddBounds[0] -= 1e-15; + fddddBounds[1] += 1e-15; + + // cos reaches maximum if there exists an integer k in [(w*t0+q0)/2pi, (w*t1+q0)/2pi]; + // cos reaches minimum if there exists an integer k in [(w*t0+q0-pi)/2pi, (w*t1+q0-pi)/2pi] + + S k1 = (tm.getTimeInterval()->t_[0] * w + q0) / (2 * constants::pi()); + S k2 = (tm.getTimeInterval()->t_[1] * w + q0) / (2 * constants::pi()); + + + if(w > 0) + { + if(ceil(k2) - floor(k1) > 1) fddddBounds[1] = 1; + k1 -= 0.5; + k2 -= 0.5; + if(ceil(k2) - floor(k1) > 1) fddddBounds[0] = -1; + } + else + { + if(ceil(k1) - floor(k2) > 1) fddddBounds[1] = 1; + k1 -= 0.5; + k2 -= 0.5; + if(ceil(k1) - floor(k2) > 1) fddddBounds[0] = -1; + } + } + + S w4 = w2 * w2; + fddddBounds *= w4; + + S midSize = 0.5 * (tm.getTimeInterval()->t_[1] - tm.getTimeInterval()->t_[0]); + S midSize2 = midSize * midSize; + S midSize4 = midSize2 * midSize2; + + // [0, midSize4] * fdddBounds + if(fddddBounds[0] > 0) + tm.remainder().setValue(0, fddddBounds[1] * midSize4 * (1.0 / 24)); + else if(fddddBounds[0] < 0) + tm.remainder().setValue(fddddBounds[0] * midSize4 * (1.0 / 24), 0); + else + tm.remainder().setValue(fddddBounds[0] * midSize4 * (1.0 / 24), fddddBounds[1] * midSize4 * (1.0 / 24)); +} + +//============================================================================== +template +void generateTaylorModelForSinFunc(TaylorModel& tm, S w, S q0) +{ + S a = tm.getTimeInterval()->t_.center(); + S t = w * a + q0; + S w2 = w * w; + S fa = sin(t); + S fda = w*cos(t); + S fdda = -w2*fa; + S fddda = -w2*fda; + + tm.coeff(0) = fa-a*(fda-0.5*a*(fdda-1.0/3.0*a*fddda)); + tm.coeff(1) = fda-a*fdda+0.5*a*a*fddda; + tm.coeff(2) = 0.5*(fdda-a*fddda); + tm.coeff(3) = 1.0/6.0*fddda; + + // compute bounds for w^3 sin(wt+q0)/16, t \in [t0, t1] + + Interval fddddBounds; + + if(w == 0) fddddBounds.setValue(0); + else + { + S sinQL = sin(w * tm.getTimeInterval()->t_[0] + q0); + S sinQR = sin(w * tm.getTimeInterval()->t_[1] + q0); + + if(sinQL < sinQR) fddddBounds.setValue(sinQL, sinQR); + else fddddBounds.setValue(sinQR, sinQL); + + // enlarge to handle round-off errors + fddddBounds[0] -= 1e-15; + fddddBounds[1] += 1e-15; + + // sin reaches maximum if there exists an integer k in [(w*t0+q0-pi/2)/2pi, (w*t1+q0-pi/2)/2pi]; + // sin reaches minimum if there exists an integer k in [(w*t0+q0-pi-pi/2)/2pi, (w*t1+q0-pi-pi/2)/2pi] + + S k1 = (tm.getTimeInterval()->t_[0] * w + q0) / (2 * constants::pi()) - 0.25; + S k2 = (tm.getTimeInterval()->t_[1] * w + q0) / (2 * constants::pi()) - 0.25; + + if(w > 0) + { + if(ceil(k2) - floor(k1) > 1) fddddBounds[1] = 1; + k1 -= 0.5; + k2 -= 0.5; + if(ceil(k2) - floor(k1) > 1) fddddBounds[0] = -1; + } + else + { + if(ceil(k1) - floor(k2) > 1) fddddBounds[1] = 1; + k1 -= 0.5; + k2 -= 0.5; + if(ceil(k1) - floor(k2) > 1) fddddBounds[0] = -1; + } + + S w4 = w2 * w2; + fddddBounds *= w4; + + S midSize = 0.5 * (tm.getTimeInterval()->t_[1] - tm.getTimeInterval()->t_[0]); + S midSize2 = midSize * midSize; + S midSize4 = midSize2 * midSize2; + + // [0, midSize4] * fdddBounds + if(fddddBounds[0] > 0) + tm.remainder().setValue(0, fddddBounds[1] * midSize4 * (1.0 / 24)); + else if(fddddBounds[0] < 0) + tm.remainder().setValue(fddddBounds[0] * midSize4 * (1.0 / 24), 0); + else + tm.remainder().setValue(fddddBounds[0] * midSize4 * (1.0 / 24), fddddBounds[1] * midSize4 * (1.0 / 24)); + } +} + +//============================================================================== +template +void generateTaylorModelForLinearFunc(TaylorModel& tm, S p, S v) +{ + tm.coeff(0) = p; + tm.coeff(1) = v; + tm.coeff(2) = 0; + tm.coeff(3) = 0; + tm.remainder()[0] = 0; + tm.remainder()[1] = 0; +} + +} // namespace fcl + #endif diff --git a/include/fcl/ccd/taylor_vector.h b/include/fcl/ccd/taylor_vector.h index b03d0f3f0..f1998c534 100644 --- a/include/fcl/ccd/taylor_vector.h +++ b/include/fcl/ccd/taylor_vector.h @@ -44,71 +44,405 @@ namespace fcl { +template class TVector3 { - TaylorModel i_[3]; + TaylorModel i_[3]; public: TVector3(); - TVector3(const std::shared_ptr& time_interval); - TVector3(TaylorModel v[3]); - TVector3(const TaylorModel& v0, const TaylorModel& v1, const TaylorModel& v2); - TVector3(const Vector3d& v, const std::shared_ptr& time_interval); + TVector3(const std::shared_ptr>& time_interval); + TVector3(TaylorModel v[3]); + TVector3(const TaylorModel& v0, const TaylorModel& v1, const TaylorModel& v2); + TVector3(const Vector3& v, const std::shared_ptr>& time_interval); TVector3 operator + (const TVector3& other) const; TVector3& operator += (const TVector3& other); - TVector3 operator + (const Vector3d& other) const; - TVector3& operator += (const Vector3d& other); + TVector3 operator + (const Vector3& other) const; + TVector3& operator += (const Vector3& other); TVector3 operator - (const TVector3& other) const; TVector3& operator -= (const TVector3& other); - TVector3 operator - (const Vector3d& other) const; - TVector3& operator -= (const Vector3d& other); + TVector3 operator - (const Vector3& other) const; + TVector3& operator -= (const Vector3& other); TVector3 operator - () const; - TVector3 operator * (const TaylorModel& d) const; - TVector3& operator *= (const TaylorModel& d); - TVector3 operator * (FCL_REAL d) const; - TVector3& operator *= (FCL_REAL d); + TVector3 operator * (const TaylorModel& d) const; + TVector3& operator *= (const TaylorModel& d); + TVector3 operator * (S d) const; + TVector3& operator *= (S d); - const TaylorModel& operator [] (size_t i) const; - TaylorModel& operator [] (size_t i); + const TaylorModel& operator [] (size_t i) const; + TaylorModel& operator [] (size_t i); - TaylorModel dot(const TVector3& other) const; + TaylorModel dot(const TVector3& other) const; TVector3 cross(const TVector3& other) const; - TaylorModel dot(const Vector3d& other) const; - TVector3 cross(const Vector3d& other) const; + TaylorModel dot(const Vector3& other) const; + TVector3 cross(const Vector3& other) const; - IVector3 getBound() const; - IVector3 getBound(FCL_REAL l, FCL_REAL r) const; - IVector3 getBound(FCL_REAL t) const; + IVector3 getBound() const; + IVector3 getBound(S l, S r) const; + IVector3 getBound(S t) const; - IVector3 getTightBound() const; - IVector3 getTightBound(FCL_REAL l, FCL_REAL r) const; + IVector3 getTightBound() const; + IVector3 getTightBound(S l, S r) const; void print() const; - FCL_REAL volumn() const; + S volumn() const; void setZero(); - TaylorModel squareLength() const; + TaylorModel squareLength() const; - void setTimeInterval(const std::shared_ptr& time_interval); - void setTimeInterval(FCL_REAL l, FCL_REAL r); + void setTimeInterval(const std::shared_ptr>& time_interval); + void setTimeInterval(S l, S r); - const std::shared_ptr& getTimeInterval() const; + const std::shared_ptr>& getTimeInterval() const; }; -void generateTVector3ForLinearFunc(TVector3& v, const Vector3d& position, const Vector3d& velocity); +template +void generateTVector3ForLinearFunc(TVector3& v, const Vector3& position, const Vector3& velocity); +template +TVector3 operator * (const Vector3& v, const TaylorModel& a); -TVector3 operator * (const Vector3d& v, const TaylorModel& a); -TVector3 operator + (const Vector3d& v1, const TVector3& v2); -TVector3 operator - (const Vector3d& v1, const TVector3& v2); +template +TVector3 operator + (const Vector3& v1, const TVector3& v2); +template +TVector3 operator - (const Vector3& v1, const TVector3& v2); + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +TVector3::TVector3() +{ +} + +//============================================================================== +template +TVector3::TVector3(const std::shared_ptr>& time_interval) +{ + setTimeInterval(time_interval); +} + +//============================================================================== +template +TVector3::TVector3(TaylorModel v[3]) +{ + i_[0] = v[0]; + i_[1] = v[1]; + i_[2] = v[2]; +} + +//============================================================================== +template +TVector3::TVector3(const TaylorModel& v1, const TaylorModel& v2, const TaylorModel& v3) +{ + i_[0] = v1; + i_[1] = v2; + i_[2] = v3; +} + +//============================================================================== +template +TVector3::TVector3(const Vector3& v, const std::shared_ptr>& time_interval) +{ + i_[0] = TaylorModel(v[0], time_interval); + i_[1] = TaylorModel(v[1], time_interval); + i_[2] = TaylorModel(v[2], time_interval); +} + +//============================================================================== +template +void TVector3::setZero() +{ + i_[0].setZero(); + i_[1].setZero(); + i_[2].setZero(); +} + +//============================================================================== +template +TVector3 TVector3::operator + (const TVector3& other) const +{ + return TVector3(i_[0] + other.i_[0], i_[1] + other.i_[1], i_[2] + other.i_[2]); +} + +//============================================================================== +template +TVector3 TVector3::operator - (const TVector3& other) const +{ + return TVector3(i_[0] - other.i_[0], i_[1] - other.i_[1], i_[2] - other.i_[2]); +} + +//============================================================================== +template +TVector3 TVector3::operator - () const +{ + return TVector3(-i_[0], -i_[1], -i_[2]); +} + +//============================================================================== +template +TVector3& TVector3::operator += (const TVector3& other) +{ + i_[0] += other.i_[0]; + i_[1] += other.i_[1]; + i_[2] += other.i_[2]; + return *this; +} + +//============================================================================== +template +TVector3& TVector3::operator -= (const TVector3& other) +{ + i_[0] -= other.i_[0]; + i_[1] -= other.i_[1]; + i_[2] -= other.i_[2]; + return *this; +} + +//============================================================================== +template +TVector3 TVector3::operator + (const Vector3& other) const +{ + return TVector3(i_[0] + other[0], i_[1] + other[1], i_[2] + other[2]); +} + +//============================================================================== +template +TVector3& TVector3::operator += (const Vector3& other) +{ + i_[0] += other[0]; + i_[1] += other[1]; + i_[2] += other[2]; + return *this; +} + +//============================================================================== +template +TVector3 TVector3::operator - (const Vector3& other) const +{ + return TVector3(i_[0] - other[0], i_[1] - other[1], i_[2] - other[2]); +} + +//============================================================================== +template +TVector3& TVector3::operator -= (const Vector3& other) +{ + i_[0] -= other[0]; + i_[1] -= other[1]; + i_[2] -= other[2]; + return *this; +} + +//============================================================================== +template +TVector3 TVector3::operator * (const TaylorModel& d) const +{ + return TVector3(i_[0] * d, i_[1] * d, i_[2] * d); +} + +//============================================================================== +template +TVector3& TVector3::operator *= (const TaylorModel& d) +{ + i_[0] *= d; + i_[1] *= d; + i_[2] *= d; + return *this; +} + +//============================================================================== +template +TVector3 TVector3::operator * (S d) const +{ + return TVector3(i_[0] * d, i_[1] * d, i_[2] * d); +} + +//============================================================================== +template +TVector3& TVector3::operator *= (S d) +{ + i_[0] *= d; + i_[1] *= d; + i_[2] *= d; + return *this; +} + +//============================================================================== +template +const TaylorModel& TVector3::operator [] (size_t i) const +{ + return i_[i]; +} + +//============================================================================== +template +TaylorModel& TVector3::operator [] (size_t i) +{ + return i_[i]; +} + +//============================================================================== +template +TaylorModel TVector3::dot(const TVector3& other) const +{ + return i_[0] * other.i_[0] + i_[1] * other.i_[1] + i_[2] * other.i_[2]; +} + +//============================================================================== +template +TVector3 TVector3::cross(const TVector3& other) const +{ + return TVector3(i_[1] * other.i_[2] - i_[2] * other.i_[1], + i_[2] * other.i_[0] - i_[0] * other.i_[2], + i_[0] * other.i_[1] - i_[1] * other.i_[0]); } +//============================================================================== +template +TaylorModel TVector3::dot(const Vector3& other) const +{ + return i_[0] * other[0] + i_[1] * other[1] + i_[2] * other[2]; +} + +//============================================================================== +template +TVector3 TVector3::cross(const Vector3& other) const +{ + return TVector3(i_[1] * other[2] - i_[2] * other[1], + i_[2] * other[0] - i_[0] * other[2], + i_[0] * other[1] - i_[1] * other[0]); +} + +//============================================================================== +template +S TVector3::volumn() const +{ + return i_[0].getBound().diameter() * i_[1].getBound().diameter() * i_[2].getBound().diameter(); +} + +//============================================================================== +template +IVector3 TVector3::getBound() const +{ + return IVector3(i_[0].getBound(), i_[1].getBound(), i_[2].getBound()); +} + +//============================================================================== +template +IVector3 TVector3::getBound(S l, S r) const +{ + return IVector3(i_[0].getBound(l, r), i_[1].getBound(l, r), i_[2].getBound(l, r)); +} + +//============================================================================== +template +IVector3 TVector3::getBound(S t) const +{ + return IVector3(i_[0].getBound(t), i_[1].getBound(t), i_[2].getBound(t)); +} + +//============================================================================== +template +IVector3 TVector3::getTightBound() const +{ + return IVector3(i_[0].getTightBound(), i_[1].getTightBound(), i_[2].getTightBound()); +} + +//============================================================================== +template +IVector3 TVector3::getTightBound(S l, S r) const +{ + return IVector3(i_[0].getTightBound(l, r), i_[1].getTightBound(l, r), i_[2].getTightBound(l, r)); +} + +//============================================================================== +template +void TVector3::print() const +{ + i_[0].print(); + i_[1].print(); + i_[2].print(); +} + +//============================================================================== +template +TaylorModel TVector3::squareLength() const +{ + return i_[0] * i_[0] + i_[1] * i_[1] + i_[2] * i_[2]; +} + +//============================================================================== +template +void TVector3::setTimeInterval(const std::shared_ptr>& time_interval) +{ + i_[0].setTimeInterval(time_interval); + i_[1].setTimeInterval(time_interval); + i_[2].setTimeInterval(time_interval); +} + +//============================================================================== +template +void TVector3::setTimeInterval(S l, S r) +{ + i_[0].setTimeInterval(l, r); + i_[1].setTimeInterval(l, r); + i_[2].setTimeInterval(l, r); +} + +//============================================================================== +template +const std::shared_ptr>& TVector3::getTimeInterval() const +{ + return i_[0].getTimeInterval(); +} + +//============================================================================== +template +void generateTVector3ForLinearFunc(TVector3& v, const Vector3& position, const Vector3& velocity) +{ + generateTaylorModelForLinearFunc(v[0], position[0], velocity[0]); + generateTaylorModelForLinearFunc(v[1], position[1], velocity[1]); + generateTaylorModelForLinearFunc(v[2], position[2], velocity[2]); +} + +//============================================================================== +template +TVector3 operator * (const Vector3& v, const TaylorModel& a) +{ + TVector3 res(a.getTimeInterval()); + res[0] = a * v[0]; + res[1] = a * v[1]; + res[2] = a * v[2]; + + return res; +} + +//============================================================================== +template +TVector3 operator + (const Vector3& v1, const TVector3& v2) +{ + return v2 + v1; +} + +//============================================================================== +template +TVector3 operator - (const Vector3& v1, const TVector3& v2) +{ + return -v2 + v1; +} + +} // namespace fcl + #endif diff --git a/include/fcl/collision.h b/include/fcl/collision.h index ef8ea7940..2324e1b84 100644 --- a/include/fcl/collision.h +++ b/include/fcl/collision.h @@ -41,6 +41,9 @@ #include "fcl/collision_object.h" #include "fcl/collision_data.h" +#include "fcl/collision_func_matrix.h" +#include "fcl/narrowphase/gjk_solver_indep.h" +#include "fcl/narrowphase/gjk_solver_libccd.h" namespace fcl { @@ -49,15 +52,152 @@ namespace fcl /// returning all the contact points), whether return detailed contact information (i.e., normal, contact point, depth; otherwise only contact primitive id is returned), this function /// performs the collision between them. /// Return value is the number of contacts generated between the two objects. +template +std::size_t collide(const CollisionObject* o1, const CollisionObject* o2, + const CollisionRequest& request, + CollisionResult& result); -std::size_t collide(const CollisionObject* o1, const CollisionObject* o2, - const CollisionRequest& request, - CollisionResult& result); +template +std::size_t collide(const CollisionGeometry* o1, const Transform3& tf1, + const CollisionGeometry* o2, const Transform3& tf2, + const CollisionRequest& request, + CollisionResult& result); -std::size_t collide(const CollisionGeometry* o1, const Transform3d& tf1, - const CollisionGeometry* o2, const Transform3d& tf2, - const CollisionRequest& request, - CollisionResult& result); +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +CollisionFunctionMatrix& getCollisionFunctionLookTable() +{ + static CollisionFunctionMatrix table; + return table; +} + +template +std::size_t collide( + const CollisionObject* o1, + const CollisionObject* o2, + const NarrowPhaseSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result) +{ + return collide(o1->collisionGeometry().get(), o1->getTransform(), o2->collisionGeometry().get(), o2->getTransform(), + nsolver, request, result); +} + +template +std::size_t collide( + const CollisionGeometry* o1, + const Transform3& tf1, + const CollisionGeometry* o2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver_, + const CollisionRequest& request, + CollisionResult& result) +{ + const NarrowPhaseSolver* nsolver = nsolver_; + if(!nsolver_) + nsolver = new NarrowPhaseSolver(); + + const auto& looktable = getCollisionFunctionLookTable(); + + std::size_t res; + if(request.num_max_contacts == 0) + { + std::cerr << "Warning: should stop early as num_max_contact is " << request.num_max_contacts << " !" << std::endl; + res = 0; + } + else + { + OBJECT_TYPE object_type1 = o1->getObjectType(); + OBJECT_TYPE object_type2 = o2->getObjectType(); + NODE_TYPE node_type1 = o1->getNodeType(); + NODE_TYPE node_type2 = o2->getNodeType(); + + if(object_type1 == OT_GEOM && object_type2 == OT_BVH) + { + if(!looktable.collision_matrix[node_type2][node_type1]) + { + std::cerr << "Warning: collision function between node type " << node_type1 << " and node type " << node_type2 << " is not supported"<< std::endl; + res = 0; + } + else + res = looktable.collision_matrix[node_type2][node_type1](o2, tf2, o1, tf1, nsolver, request, result); + } + else + { + if(!looktable.collision_matrix[node_type1][node_type2]) + { + std::cerr << "Warning: collision function between node type " << node_type1 << " and node type " << node_type2 << " is not supported"<< std::endl; + res = 0; + } + else + res = looktable.collision_matrix[node_type1][node_type2](o1, tf1, o2, tf2, nsolver, request, result); + } + } + + if(!nsolver_) + delete nsolver; + + return res; +} + +//============================================================================== +template +std::size_t collide(const CollisionObject* o1, const CollisionObject* o2, + const CollisionRequest& request, CollisionResult& result) +{ + switch(request.gjk_solver_type) + { + case GST_LIBCCD: + { + GJKSolver_libccd solver; + return collide>(o1, o2, &solver, request, result); + } + case GST_INDEP: + { + GJKSolver_indep solver; + return collide>(o1, o2, &solver, request, result); + } + default: + return -1; // error + } } +//============================================================================== +template +std::size_t collide( + const CollisionGeometry* o1, + const Transform3& tf1, + const CollisionGeometry* o2, + const Transform3& tf2, + const CollisionRequest& request, + CollisionResult& result) +{ + switch(request.gjk_solver_type) + { + case GST_LIBCCD: + { + GJKSolver_libccd solver; + return collide>( + o1, tf1, o2, tf2, &solver, request, result); + } + case GST_INDEP: + { + GJKSolver_indep solver; + return collide>( + o1, tf1, o2, tf2, &solver, request, result); + } + default: + std::cerr << "Warning! Invalid GJK solver" << std::endl; + return -1; // error + } +} + +} // namespace fcl + #endif diff --git a/include/fcl/collision_data.h b/include/fcl/collision_data.h index dfd0cbb5b..a638a81c5 100644 --- a/include/fcl/collision_data.h +++ b/include/fcl/collision_data.h @@ -54,38 +54,49 @@ namespace fcl enum GJKSolverType {GST_LIBCCD, GST_INDEP}; /// @brief Minimal contact information returned by collision +template struct ContactPoint { /// @brief Contact normal, pointing from o1 to o2 - Vector3d normal; + Vector3 normal; /// @brief Contact position, in world space - Vector3d pos; + Vector3 pos; /// @brief Penetration depth - FCL_REAL penetration_depth; + S penetration_depth; /// @brief Constructor - ContactPoint() : normal(Vector3d::Zero()), pos(Vector3d::Zero()), penetration_depth(0.0) {} + ContactPoint(); /// @brief Constructor - ContactPoint(const Vector3d& n_, const Vector3d& p_, FCL_REAL d_) : normal(n_), - pos(p_), - penetration_depth(d_) - {} + ContactPoint(const Vector3& n_, const Vector3& p_, S d_); }; +using ContactPointf = ContactPoint; +using ContactPointd = ContactPoint; + +template +void flipNormal(std::vector>& contacts) +{ + for (auto& contact : contacts) + contact.normal *= -1.0; +} + /// @brief Return true if _cp1's penentration depth is less than _cp2's. -bool comparePenDepth(const ContactPoint& _cp1, const ContactPoint& _cp2); +template +bool comparePenDepth( + const ContactPoint& _cp1, const ContactPoint& _cp2); /// @brief Contact information returned by collision +template struct Contact { /// @brief collision object 1 - const CollisionGeometry* o1; + const CollisionGeometry* o1; /// @brief collision object 2 - const CollisionGeometry* o2; + const CollisionGeometry* o2; /// @brief contact primitive in object 1 /// if object 1 is mesh or point cloud, it is the triangle or point id @@ -93,7 +104,6 @@ struct Contact /// if object 1 is octree, it is the id of the cell int b1; - /// @brief contact primitive in object 2 /// if object 2 is mesh or point cloud, it is the triangle or point id /// if object 2 is geometry shape, it is NONE (-1), @@ -101,101 +111,63 @@ struct Contact int b2; /// @brief contact normal, pointing from o1 to o2 - Vector3d normal; + Vector3 normal; /// @brief contact position, in world space - Vector3d pos; + Vector3 pos; /// @brief penetration depth - FCL_REAL penetration_depth; + S penetration_depth; /// @brief invalid contact primitive information static const int NONE = -1; - Contact() : o1(NULL), - o2(NULL), - b1(NONE), - b2(NONE) - {} - - Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_) : o1(o1_), - o2(o2_), - b1(b1_), - b2(b2_) - {} - - Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_, - const Vector3d& pos_, const Vector3d& normal_, FCL_REAL depth_) : o1(o1_), - o2(o2_), - b1(b1_), - b2(b2_), - normal(normal_), - pos(pos_), - penetration_depth(depth_) - {} - - bool operator < (const Contact& other) const - { - if(b1 == other.b1) - return b2 < other.b2; - return b1 < other.b1; - } + Contact(); + + Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_); + + Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_, + const Vector3& pos_, const Vector3& normal_, S depth_); + + bool operator < (const Contact& other) const; }; -/// @brief Cost source describes an area with a cost. The area is described by an AABB region. +using Contactf = Contact; +using Contactd = Contact; + +/// @brief Cost source describes an area with a cost. The area is described by an AABB region. +template struct CostSource { /// @brief aabb lower bound - Vector3d aabb_min; + Vector3 aabb_min; /// @brief aabb upper bound - Vector3d aabb_max; + Vector3 aabb_max; - /// @brief cost density in the AABB region - FCL_REAL cost_density; + /// @brief cost density in the AABB region + S cost_density; - FCL_REAL total_cost; + S total_cost; - CostSource(const Vector3d& aabb_min_, const Vector3d& aabb_max_, FCL_REAL cost_density_) : aabb_min(aabb_min_), - aabb_max(aabb_max_), - cost_density(cost_density_) - { - total_cost = cost_density * (aabb_max[0] - aabb_min[0]) * (aabb_max[1] - aabb_min[1]) * (aabb_max[2] - aabb_min[2]); - } + CostSource(const Vector3& aabb_min_, const Vector3& aabb_max_, S cost_density_); - CostSource(const AABB& aabb, FCL_REAL cost_density_) : aabb_min(aabb.min_), - aabb_max(aabb.max_), - cost_density(cost_density_) - { - total_cost = cost_density * (aabb_max[0] - aabb_min[0]) * (aabb_max[1] - aabb_min[1]) * (aabb_max[2] - aabb_min[2]); - } + CostSource(const AABB& aabb, S cost_density_); - CostSource() {} + CostSource(); - bool operator < (const CostSource& other) const - { - if(total_cost < other.total_cost) - return false; - if(total_cost > other.total_cost) - return true; - - if(cost_density < other.cost_density) - return false; - if(cost_density > other.cost_density) - return true; - - for(size_t i = 0; i < 3; ++i) - if(aabb_min[i] != other.aabb_min[i]) - return aabb_min[i] < other.aabb_min[i]; - - return false; - } + bool operator < (const CostSource& other) const; }; +using CostSourcef = CostSource; +using CostSourced = CostSource; + +template struct CollisionResult; /// @brief request to the collision algorithm +template struct CollisionRequest { /// @brief The maximum number of contacts will return @@ -220,158 +192,115 @@ struct CollisionRequest bool enable_cached_gjk_guess; /// @brief the gjk intial guess set by user - Vector3d cached_gjk_guess; + Vector3 cached_gjk_guess; CollisionRequest(size_t num_max_contacts_ = 1, bool enable_contact_ = false, size_t num_max_cost_sources_ = 1, bool enable_cost_ = false, bool use_approximate_cost_ = true, - GJKSolverType gjk_solver_type_ = GST_LIBCCD) : num_max_contacts(num_max_contacts_), - enable_contact(enable_contact_), - num_max_cost_sources(num_max_cost_sources_), - enable_cost(enable_cost_), - use_approximate_cost(use_approximate_cost_), - gjk_solver_type(gjk_solver_type_) - { - enable_cached_gjk_guess = false; - cached_gjk_guess = Vector3d(1, 0, 0); - } + GJKSolverType gjk_solver_type_ = GST_LIBCCD); - bool isSatisfied(const CollisionResult& result) const; + bool isSatisfied(const CollisionResult& result) const; }; +using CollisionRequestf = CollisionRequest; +using CollisionRequestd = CollisionRequest; + /// @brief collision result +template struct CollisionResult { private: /// @brief contact information - std::vector contacts; + std::vector> contacts; /// @brief cost sources - std::set cost_sources; + std::set> cost_sources; public: - Vector3d cached_gjk_guess; + Vector3 cached_gjk_guess; public: - CollisionResult() - { - } - + CollisionResult(); /// @brief add one contact into result structure - inline void addContact(const Contact& c) - { - contacts.push_back(c); - } + void addContact(const Contact& c); /// @brief add one cost source into result structure - inline void addCostSource(const CostSource& c, std::size_t num_max_cost_sources) - { - cost_sources.insert(c); - while (cost_sources.size() > num_max_cost_sources) - cost_sources.erase(--cost_sources.end()); - } + void addCostSource(const CostSource& c, std::size_t num_max_cost_sources); /// @brief return binary collision result - bool isCollision() const - { - return contacts.size() > 0; - } + bool isCollision() const; /// @brief number of contacts found - size_t numContacts() const - { - return contacts.size(); - } + size_t numContacts() const; /// @brief number of cost sources found - size_t numCostSources() const - { - return cost_sources.size(); - } + size_t numCostSources() const; /// @brief get the i-th contact calculated - const Contact& getContact(size_t i) const - { - if(i < contacts.size()) - return contacts[i]; - else - return contacts.back(); - } + const Contact& getContact(size_t i) const; /// @brief get all the contacts - void getContacts(std::vector& contacts_) - { - contacts_.resize(contacts.size()); - std::copy(contacts.begin(), contacts.end(), contacts_.begin()); - } + void getContacts(std::vector>& contacts_); /// @brief get all the cost sources - void getCostSources(std::vector& cost_sources_) - { - cost_sources_.resize(cost_sources.size()); - std::copy(cost_sources.begin(), cost_sources.end(), cost_sources_.begin()); - } + void getCostSources(std::vector>& cost_sources_); /// @brief clear the results obtained - void clear() - { - contacts.clear(); - cost_sources.clear(); - } + void clear(); }; +using CollisionResultf = CollisionResult; +using CollisionResultd = CollisionResult; +template struct DistanceResult; /// @brief request to the distance computation +template struct DistanceRequest { /// @brief whether to return the nearest points bool enable_nearest_points; /// @brief error threshold for approximate distance - FCL_REAL rel_err; // relative error, between 0 and 1 - FCL_REAL abs_err; // absoluate error + S rel_err; // relative error, between 0 and 1 + S abs_err; // absoluate error /// @brief narrow phase solver type GJKSolverType gjk_solver_type; - - DistanceRequest(bool enable_nearest_points_ = false, - FCL_REAL rel_err_ = 0.0, - FCL_REAL abs_err_ = 0.0, - GJKSolverType gjk_solver_type_ = GST_LIBCCD) : enable_nearest_points(enable_nearest_points_), - rel_err(rel_err_), - abs_err(abs_err_), - gjk_solver_type(gjk_solver_type_) - { - } + S rel_err_ = 0.0, + S abs_err_ = 0.0, + GJKSolverType gjk_solver_type_ = GST_LIBCCD); - bool isSatisfied(const DistanceResult& result) const; + bool isSatisfied(const DistanceResult& result) const; }; +using DistanceRequestf = DistanceRequest; +using DistanceRequestd = DistanceRequest; /// @brief distance result +template struct DistanceResult { public: /// @brief minimum distance between two objects. if two objects are in collision, min_distance <= 0. - FCL_REAL min_distance; + S min_distance; /// @brief nearest points - Vector3d nearest_points[2]; + Vector3 nearest_points[2]; /// @brief collision object 1 - const CollisionGeometry* o1; + const CollisionGeometry* o1; /// @brief collision object 2 - const CollisionGeometry* o2; + const CollisionGeometry* o2; /// @brief information about the nearest point in object 1 /// if object 1 is mesh or point cloud, it is the triangle or point id @@ -388,81 +317,35 @@ struct DistanceResult /// @brief invalid contact primitive information static const int NONE = -1; - DistanceResult(FCL_REAL min_distance_ = std::numeric_limits::max()) : min_distance(min_distance_), - o1(NULL), - o2(NULL), - b1(NONE), - b2(NONE) - { - } - + DistanceResult(S min_distance_ = std::numeric_limits::max()); /// @brief add distance information into the result - void update(FCL_REAL distance, const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_) - { - if(min_distance > distance) - { - min_distance = distance; - o1 = o1_; - o2 = o2_; - b1 = b1_; - b2 = b2_; - } - } + void update(S distance, const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_); /// @brief add distance information into the result - void update(FCL_REAL distance, const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_, const Vector3d& p1, const Vector3d& p2) - { - if(min_distance > distance) - { - min_distance = distance; - o1 = o1_; - o2 = o2_; - b1 = b1_; - b2 = b2_; - nearest_points[0] = p1; - nearest_points[1] = p2; - } - } + void update(S distance, const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_, const Vector3& p1, const Vector3& p2); /// @brief add distance information into the result - void update(const DistanceResult& other_result) - { - if(min_distance > other_result.min_distance) - { - min_distance = other_result.min_distance; - o1 = other_result.o1; - o2 = other_result.o2; - b1 = other_result.b1; - b2 = other_result.b2; - nearest_points[0] = other_result.nearest_points[0]; - nearest_points[1] = other_result.nearest_points[1]; - } - } + void update(const DistanceResult& other_result); /// @brief clear the result - void clear() - { - min_distance = std::numeric_limits::max(); - o1 = NULL; - o2 = NULL; - b1 = NONE; - b2 = NONE; - } + void clear(); }; +using DistanceResultf = DistanceResult; +using DistanceResultd = DistanceResult; enum CCDMotionType {CCDM_TRANS, CCDM_LINEAR, CCDM_SCREW, CCDM_SPLINE}; enum CCDSolverType {CCDC_NAIVE, CCDC_CONSERVATIVE_ADVANCEMENT, CCDC_RAY_SHOOTING, CCDC_POLYNOMIAL_SOLVER}; - +template struct ContinuousCollisionRequest { /// @brief maximum num of iterations std::size_t num_max_iterations; /// @brief error in first contact time - FCL_REAL toc_err; + S toc_err; /// @brief ccd motion type CCDMotionType ccd_motion_type; @@ -474,38 +357,39 @@ struct ContinuousCollisionRequest CCDSolverType ccd_solver_type; ContinuousCollisionRequest(std::size_t num_max_iterations_ = 10, - FCL_REAL toc_err_ = 0.0001, + S toc_err_ = 0.0001, CCDMotionType ccd_motion_type_ = CCDM_TRANS, GJKSolverType gjk_solver_type_ = GST_LIBCCD, - CCDSolverType ccd_solver_type_ = CCDC_NAIVE) : num_max_iterations(num_max_iterations_), - toc_err(toc_err_), - ccd_motion_type(ccd_motion_type_), - gjk_solver_type(gjk_solver_type_), - ccd_solver_type(ccd_solver_type_) - { - } + CCDSolverType ccd_solver_type_ = CCDC_NAIVE); }; + +using ContinuousCollisionRequestf = ContinuousCollisionRequest; +using ContinuousCollisionRequestd = ContinuousCollisionRequest; + /// @brief continuous collision result +template struct ContinuousCollisionResult { /// @brief collision or not bool is_collide; /// @brief time of contact in [0, 1] - FCL_REAL time_of_contact; + S time_of_contact; - Transform3d contact_tf1, contact_tf2; + Transform3 contact_tf1, contact_tf2; - ContinuousCollisionResult() : is_collide(false), time_of_contact(1.0) - { - } + ContinuousCollisionResult(); + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; +using ContinuousCollisionResultf = ContinuousCollisionResult; +using ContinuousCollisionResultd = ContinuousCollisionResult; enum PenetrationDepthType {PDT_TRANSLATIONAL, PDT_GENERAL_EULER, PDT_GENERAL_QUAT, PDT_GENERAL_EULER_BALL, PDT_GENERAL_QUAT_BALL}; - +template struct PenetrationDepthRequest { void* classifier; @@ -516,29 +400,363 @@ struct PenetrationDepthRequest /// @brief gjk solver type GJKSolverType gjk_solver_type; - std::vector contact_vectors; + Eigen::aligned_vector> contact_vectors; PenetrationDepthRequest(void* classifier_, PenetrationDepthType pd_type_ = PDT_TRANSLATIONAL, - GJKSolverType gjk_solver_type_ = GST_LIBCCD) : classifier(classifier_), - pd_type(pd_type_), - gjk_solver_type(gjk_solver_type_) - { - } + GJKSolverType gjk_solver_type_ = GST_LIBCCD); }; +template struct PenetrationDepthResult { /// @brief penetration depth value - FCL_REAL pd_value; + S pd_value; /// @brief the transform where the collision is resolved - Transform3d resolved_tf; + Transform3 resolved_tf; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +ContactPoint::ContactPoint() + : normal(Vector3::Zero()), + pos(Vector3::Zero()), + penetration_depth(0.0) +{ + // Do nothing +} + +//============================================================================== +template +ContactPoint::ContactPoint( + const Vector3& n_, const Vector3& p_, S d_) + : normal(n_), pos(p_), penetration_depth(d_) +{ + // Do nothing +} + +//============================================================================== +template +bool comparePenDepth(const ContactPoint& _cp1, const ContactPoint& _cp2) +{ + return _cp1.penetration_depth < _cp2.penetration_depth; +} + +//============================================================================== +template +Contact::Contact() + : o1(nullptr), + o2(nullptr), + b1(NONE), + b2(NONE) +{ + // Do nothing +} + +//============================================================================== +template +Contact::Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_) : o1(o1_), + o2(o2_), + b1(b1_), + b2(b2_) +{ + // Do nothing +} + +//============================================================================== +template +Contact::Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_, const Vector3& pos_, const Vector3& normal_, S depth_) : o1(o1_), + o2(o2_), + b1(b1_), + b2(b2_), + normal(normal_), + pos(pos_), + penetration_depth(depth_) +{ + // Do nothing +} + +//============================================================================== +template +bool Contact::operator <(const Contact& other) const +{ + if(b1 == other.b1) + return b2 < other.b2; + return b1 < other.b1; +} +//============================================================================== +template +CostSource::CostSource(const Vector3& aabb_min_, const Vector3& aabb_max_, S cost_density_) : aabb_min(aabb_min_), + aabb_max(aabb_max_), + cost_density(cost_density_) +{ + total_cost = cost_density * (aabb_max[0] - aabb_min[0]) * (aabb_max[1] - aabb_min[1]) * (aabb_max[2] - aabb_min[2]); +} +//============================================================================== +template +CostSource::CostSource(const AABB& aabb, S cost_density_) : aabb_min(aabb.min_), + aabb_max(aabb.max_), + cost_density(cost_density_) +{ + total_cost = cost_density * (aabb_max[0] - aabb_min[0]) * (aabb_max[1] - aabb_min[1]) * (aabb_max[2] - aabb_min[2]); +} +//============================================================================== +template +CostSource::CostSource() +{ + // Do nothing } +//============================================================================== +template +bool CostSource::operator <(const CostSource& other) const +{ + if(total_cost < other.total_cost) + return false; + if(total_cost > other.total_cost) + return true; + + if(cost_density < other.cost_density) + return false; + if(cost_density > other.cost_density) + return true; + + for(size_t i = 0; i < 3; ++i) + if(aabb_min[i] != other.aabb_min[i]) + return aabb_min[i] < other.aabb_min[i]; + + return false; +} + +//============================================================================== +template +CollisionRequest::CollisionRequest(size_t num_max_contacts_, bool enable_contact_, size_t num_max_cost_sources_, bool enable_cost_, bool use_approximate_cost_, GJKSolverType gjk_solver_type_) : num_max_contacts(num_max_contacts_), + enable_contact(enable_contact_), + num_max_cost_sources(num_max_cost_sources_), + enable_cost(enable_cost_), + use_approximate_cost(use_approximate_cost_), + gjk_solver_type(gjk_solver_type_) +{ + enable_cached_gjk_guess = false; + cached_gjk_guess = Vector3(1, 0, 0); +} + +//============================================================================== +template +bool CollisionRequest::isSatisfied( + const CollisionResult& result) const +{ + return (!enable_cost) + && result.isCollision() + && (num_max_contacts <= result.numContacts()); +} + +//============================================================================== +template +CollisionResult::CollisionResult() +{ + // Do nothing +} + +//============================================================================== +template +void CollisionResult::addContact(const Contact& c) +{ + contacts.push_back(c); +} + +//============================================================================== +template +void CollisionResult::addCostSource( + const CostSource& c, std::size_t num_max_cost_sources) +{ + cost_sources.insert(c); + while (cost_sources.size() > num_max_cost_sources) + cost_sources.erase(--cost_sources.end()); +} + +//============================================================================== +template +bool CollisionResult::isCollision() const +{ + return contacts.size() > 0; +} + +//============================================================================== +template +size_t CollisionResult::numContacts() const +{ + return contacts.size(); +} + +//============================================================================== +template +size_t CollisionResult::numCostSources() const +{ + return cost_sources.size(); +} + +//============================================================================== +template +const Contact& CollisionResult::getContact(size_t i) const +{ + if(i < contacts.size()) + return contacts[i]; + else + return contacts.back(); +} + +//============================================================================== +template +void CollisionResult::getContacts( + std::vector>& contacts_) +{ + contacts_.resize(contacts.size()); + std::copy(contacts.begin(), contacts.end(), contacts_.begin()); +} + +//============================================================================== +template +void CollisionResult::getCostSources( + std::vector>& cost_sources_) +{ + cost_sources_.resize(cost_sources.size()); + std::copy(cost_sources.begin(), cost_sources.end(), cost_sources_.begin()); +} + +//============================================================================== +template +void CollisionResult::clear() +{ + contacts.clear(); + cost_sources.clear(); +} + +//============================================================================== +template +DistanceRequest::DistanceRequest(bool enable_nearest_points_, S rel_err_, S abs_err_, GJKSolverType gjk_solver_type_) : enable_nearest_points(enable_nearest_points_), + rel_err(rel_err_), + abs_err(abs_err_), + gjk_solver_type(gjk_solver_type_) +{ + // Do nothing +} + +//============================================================================== +template +bool DistanceRequest::isSatisfied( + const DistanceResult& result) const +{ + return (result.min_distance <= 0); +} + +//============================================================================== +template +DistanceResult::DistanceResult(S min_distance_) + : min_distance(min_distance_), + o1(nullptr), + o2(nullptr), + b1(NONE), + b2(NONE) +{ + // Do nothing +} + +//============================================================================== +template +void DistanceResult::update(S distance, const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_) +{ + if(min_distance > distance) + { + min_distance = distance; + o1 = o1_; + o2 = o2_; + b1 = b1_; + b2 = b2_; + } +} + +//============================================================================== +template +void DistanceResult::update(S distance, const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_, const Vector3& p1, const Vector3& p2) +{ + if(min_distance > distance) + { + min_distance = distance; + o1 = o1_; + o2 = o2_; + b1 = b1_; + b2 = b2_; + nearest_points[0] = p1; + nearest_points[1] = p2; + } +} + +//============================================================================== +template +void DistanceResult::update(const DistanceResult& other_result) +{ + if(min_distance > other_result.min_distance) + { + min_distance = other_result.min_distance; + o1 = other_result.o1; + o2 = other_result.o2; + b1 = other_result.b1; + b2 = other_result.b2; + nearest_points[0] = other_result.nearest_points[0]; + nearest_points[1] = other_result.nearest_points[1]; + } +} + +//============================================================================== +template +void DistanceResult::clear() +{ + min_distance = std::numeric_limits::max(); + o1 = nullptr; + o2 = nullptr; + b1 = NONE; + b2 = NONE; +} + +//============================================================================== +template +ContinuousCollisionRequest::ContinuousCollisionRequest(std::size_t num_max_iterations_, S toc_err_, CCDMotionType ccd_motion_type_, GJKSolverType gjk_solver_type_, CCDSolverType ccd_solver_type_) : num_max_iterations(num_max_iterations_), + toc_err(toc_err_), + ccd_motion_type(ccd_motion_type_), + gjk_solver_type(gjk_solver_type_), + ccd_solver_type(ccd_solver_type_) +{ + // Do nothing +} + +//============================================================================== +template +ContinuousCollisionResult::ContinuousCollisionResult() : is_collide(false), time_of_contact(1.0) +{ + // Do nothing +} + +//============================================================================== +template +PenetrationDepthRequest::PenetrationDepthRequest(void* classifier_, PenetrationDepthType pd_type_, GJKSolverType gjk_solver_type_) : classifier(classifier_), + pd_type(pd_type_), + gjk_solver_type(gjk_solver_type_) +{ +} + +} // namespace fcl + #endif diff --git a/include/fcl/collision_func_matrix.h b/include/fcl/collision_func_matrix.h index 086c39f84..cf6c1cbd9 100644 --- a/include/fcl/collision_func_matrix.h +++ b/include/fcl/collision_func_matrix.h @@ -39,32 +39,874 @@ #ifndef FCL_COLLISION_FUNC_MATRIX_H #define FCL_COLLISION_FUNC_MATRIX_H - #include "fcl/collision_object.h" #include "fcl/collision_data.h" +#include "fcl/collision_node.h" + +#include "fcl/shape/box.h" +#include "fcl/shape/capsule.h" +#include "fcl/shape/cone.h" +#include "fcl/shape/convex.h" +#include "fcl/shape/cylinder.h" +#include "fcl/shape/ellipsoid.h" +#include "fcl/shape/halfspace.h" +#include "fcl/shape/plane.h" +#include "fcl/shape/sphere.h" +#include "fcl/shape/triangle_p.h" +#include "fcl/shape/construct_box.h" + +#include "fcl/traversal/traversal_nodes.h" namespace fcl { /// @brief collision matrix stores the functions for collision between different types of objects and provides a uniform call interface -template +template struct CollisionFunctionMatrix { + using S = typename NarrowPhaseSolver::S; + /// @brief the uniform call interface for collision: for collision, we need know /// 1. two objects o1 and o2 and their configuration in world coordinate tf1 and tf2; /// 2. the solver for narrow phase collision, this is for the collision between geometric shapes; /// 3. the request setting for collision (e.g., whether need to return normal information, whether need to compute cost); /// 4. the structure to return collision result - typedef std::size_t (*CollisionFunc)(const CollisionGeometry* o1, const Transform3d& tf1, const CollisionGeometry* o2, const Transform3d& tf2, const NarrowPhaseSolver* nsolver, const CollisionRequest& request, CollisionResult& result); + using CollisionFunc = std::size_t (*)( + const CollisionGeometry* o1, + const Transform3& tf1, + const CollisionGeometry* o2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result); - /// @brief each item in the collision matrix is a function to handle collision between objects of type1 and type2 + /// @brief each item in the collision matrix is a function to handle collision + /// between objects of type1 and type2 CollisionFunc collision_matrix[NODE_COUNT][NODE_COUNT]; CollisionFunctionMatrix(); }; +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +#if FCL_HAVE_OCTOMAP + +//============================================================================== +template +std::size_t ShapeOcTreeCollide( + const CollisionGeometry* o1, + const Transform3& tf1, + const CollisionGeometry* o2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result) +{ + using S = typename Shape::S; + + if(request.isSatisfied(result)) return result.numContacts(); + + ShapeOcTreeCollisionTraversalNode node; + const Shape* obj1 = static_cast(o1); + const OcTree* obj2 = static_cast*>(o2); + OcTreeSolver otsolver(nsolver); + + initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result); + collide(&node); + + return result.numContacts(); +} + +//============================================================================== +template +std::size_t OcTreeShapeCollide( + const CollisionGeometry* o1, + const Transform3& tf1, + const CollisionGeometry* o2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result) +{ + using S = typename Shape::S; + + if(request.isSatisfied(result)) return result.numContacts(); + + OcTreeShapeCollisionTraversalNode node; + const OcTree* obj1 = static_cast*>(o1); + const Shape* obj2 = static_cast(o2); + OcTreeSolver otsolver(nsolver); + + initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result); + collide(&node); + + return result.numContacts(); +} + +//============================================================================== +template +std::size_t OcTreeCollide( + const CollisionGeometry* o1, + const Transform3& tf1, + const CollisionGeometry* o2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result) +{ + using S = typename NarrowPhaseSolver::S; + + if(request.isSatisfied(result)) return result.numContacts(); + + OcTreeCollisionTraversalNode node; + const OcTree* obj1 = static_cast*>(o1); + const OcTree* obj2 = static_cast*>(o2); + OcTreeSolver otsolver(nsolver); + + initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result); + collide(&node); + + return result.numContacts(); +} + +//============================================================================== +template +std::size_t OcTreeBVHCollide( + const CollisionGeometry* o1, + const Transform3& tf1, + const CollisionGeometry* o2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result) +{ + using S = typename BV::S; + + if(request.isSatisfied(result)) return result.numContacts(); + + if(request.enable_cost && request.use_approximate_cost) + { + CollisionRequest no_cost_request(request); // request remove cost to avoid the exact but expensive cost computation between mesh and octree + no_cost_request.enable_cost = false; // disable cost computation + + OcTreeMeshCollisionTraversalNode node; + const OcTree* obj1 = static_cast*>(o1); + const BVHModel* obj2 = static_cast*>(o2); + OcTreeSolver otsolver(nsolver); + + initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, no_cost_request, result); + collide(&node); + + Box box; + Transform3 box_tf; + constructBox(obj2->getBV(0).bv, tf2, box, box_tf); // compute the box for BVH's root node + + box.cost_density = obj2->cost_density; + box.threshold_occupied = obj2->threshold_occupied; + box.threshold_free = obj2->threshold_free; + + CollisionRequest only_cost_request(result.numContacts(), false, request.num_max_cost_sources, true, false); // additional cost request, no contacts + OcTreeShapeCollide, NarrowPhaseSolver>(o1, tf1, &box, box_tf, nsolver, only_cost_request, result); + } + else + { + OcTreeMeshCollisionTraversalNode node; + const OcTree* obj1 = static_cast*>(o1); + const BVHModel* obj2 = static_cast*>(o2); + OcTreeSolver otsolver(nsolver); + + initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result); + collide(&node); + } + + return result.numContacts(); +} + +//============================================================================== +template +std::size_t BVHOcTreeCollide( + const CollisionGeometry* o1, + const Transform3& tf1, + const CollisionGeometry* o2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result) +{ + using S = typename BV::S; + + if(request.isSatisfied(result)) return result.numContacts(); + + if(request.enable_cost && request.use_approximate_cost) + { + CollisionRequest no_cost_request(request); // request remove cost to avoid the exact but expensive cost computation between mesh and octree + no_cost_request.enable_cost = false; // disable cost computation + + MeshOcTreeCollisionTraversalNode node; + const BVHModel* obj1 = static_cast*>(o1); + const OcTree* obj2 = static_cast*>(o2); + OcTreeSolver otsolver(nsolver); + + initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, no_cost_request, result); + collide(&node); + + Box box; + Transform3 box_tf; + constructBox(obj1->getBV(0).bv, tf1, box, box_tf); + + box.cost_density = obj1->cost_density; + box.threshold_occupied = obj1->threshold_occupied; + box.threshold_free = obj1->threshold_free; + + CollisionRequest only_cost_request(result.numContacts(), false, request.num_max_cost_sources, true, false); + ShapeOcTreeCollide, NarrowPhaseSolver>(&box, box_tf, o2, tf2, nsolver, only_cost_request, result); + } + else + { + MeshOcTreeCollisionTraversalNode node; + const BVHModel* obj1 = static_cast*>(o1); + const OcTree* obj2 = static_cast*>(o2); + OcTreeSolver otsolver(nsolver); + + initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result); + collide(&node); + } + + return result.numContacts(); +} + +#endif + +//============================================================================== +template +std::size_t ShapeShapeCollide( + const CollisionGeometry* o1, + const Transform3& tf1, + const CollisionGeometry* o2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result) +{ + if(request.isSatisfied(result)) return result.numContacts(); + + ShapeCollisionTraversalNode node; + const Shape1* obj1 = static_cast(o1); + const Shape2* obj2 = static_cast(o2); + + if(request.enable_cached_gjk_guess) + { + nsolver->enableCachedGuess(true); + nsolver->setCachedGuess(request.cached_gjk_guess); + } + else + { + nsolver->enableCachedGuess(true); + } + + initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result); + collide(&node); + + if(request.enable_cached_gjk_guess) + result.cached_gjk_guess = nsolver->getCachedGuess(); + + return result.numContacts(); +} + +//============================================================================== +template +struct BVHShapeCollider +{ + using S = typename BV::S; + + static std::size_t collide( + const CollisionGeometry* o1, + const Transform3& tf1, + const CollisionGeometry* o2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result) + { + if(request.isSatisfied(result)) return result.numContacts(); + + if(request.enable_cost && request.use_approximate_cost) + { + CollisionRequest no_cost_request(request); + no_cost_request.enable_cost = false; + + MeshShapeCollisionTraversalNode node; + const BVHModel* obj1 = static_cast* >(o1); + BVHModel* obj1_tmp = new BVHModel(*obj1); + Transform3 tf1_tmp = tf1; + const Shape* obj2 = static_cast(o2); + + initialize(node, *obj1_tmp, tf1_tmp, *obj2, tf2, nsolver, no_cost_request, result); + fcl::collide(&node); + + delete obj1_tmp; + + Box box; + Transform3 box_tf; + constructBox(obj1->getBV(0).bv, tf1, box, box_tf); + + box.cost_density = obj1->cost_density; + box.threshold_occupied = obj1->threshold_occupied; + box.threshold_free = obj1->threshold_free; + + CollisionRequest only_cost_request(result.numContacts(), false, request.num_max_cost_sources, true, false); + ShapeShapeCollide, Shape>(&box, box_tf, o2, tf2, nsolver, only_cost_request, result); + } + else + { + MeshShapeCollisionTraversalNode node; + const BVHModel* obj1 = static_cast* >(o1); + BVHModel* obj1_tmp = new BVHModel(*obj1); + Transform3 tf1_tmp = tf1; + const Shape* obj2 = static_cast(o2); + + initialize(node, *obj1_tmp, tf1_tmp, *obj2, tf2, nsolver, request, result); + fcl::collide(&node); + + delete obj1_tmp; + } + + return result.numContacts(); + } +}; + +namespace details +{ + +//============================================================================== +template +std::size_t orientedBVHShapeCollide( + const CollisionGeometry* o1, + const Transform3& tf1, + const CollisionGeometry* o2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result) +{ + using S = typename BV::S; + + if(request.isSatisfied(result)) return result.numContacts(); + + if(request.enable_cost && request.use_approximate_cost) + { + CollisionRequest no_cost_request(request); + no_cost_request.enable_cost = false; + + OrientMeshShapeCollisionTraveralNode node; + const BVHModel* obj1 = static_cast* >(o1); + const Shape* obj2 = static_cast(o2); + + initialize(node, *obj1, tf1, *obj2, tf2, nsolver, no_cost_request, result); + fcl::collide(&node); + + Box box; + Transform3 box_tf; + constructBox(obj1->getBV(0).bv, tf1, box, box_tf); + + box.cost_density = obj1->cost_density; + box.threshold_occupied = obj1->threshold_occupied; + box.threshold_free = obj1->threshold_free; + + CollisionRequest only_cost_request(result.numContacts(), false, request.num_max_cost_sources, true, false); + ShapeShapeCollide, Shape>(&box, box_tf, o2, tf2, nsolver, only_cost_request, result); + } + else + { + OrientMeshShapeCollisionTraveralNode node; + const BVHModel* obj1 = static_cast* >(o1); + const Shape* obj2 = static_cast(o2); + + initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result); + fcl::collide(&node); + } + + return result.numContacts(); +} + +} // namespace details + +//============================================================================== +template +struct BVHShapeCollider< + OBB, Shape, NarrowPhaseSolver> +{ + using S = typename Shape::S; + + static std::size_t collide( + const CollisionGeometry* o1, + const Transform3& tf1, + const CollisionGeometry* o2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result) + { + return details::orientedBVHShapeCollide< + MeshShapeCollisionTraversalNodeOBB, + OBB, + Shape, + NarrowPhaseSolver>( + o1, tf1, o2, tf2, nsolver, request, result); + } +}; + +//============================================================================== +template +struct BVHShapeCollider, Shape, NarrowPhaseSolver> +{ + using S = typename Shape::S; + + static std::size_t collide( + const CollisionGeometry* o1, + const Transform3& tf1, + const CollisionGeometry* o2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result) + { + return details::orientedBVHShapeCollide< + MeshShapeCollisionTraversalNodeRSS, + RSS, + Shape, + NarrowPhaseSolver>( + o1, tf1, o2, tf2, nsolver, request, result); + } +}; + +//============================================================================== +template +struct BVHShapeCollider, Shape, NarrowPhaseSolver> +{ + using S = typename Shape::S; + static std::size_t collide( + const CollisionGeometry* o1, + const Transform3& tf1, + const CollisionGeometry* o2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result) + { + return details::orientedBVHShapeCollide< + MeshShapeCollisionTraversalNodekIOS, + kIOS, + Shape, + NarrowPhaseSolver>( + o1, tf1, o2, tf2, nsolver, request, result); + } +}; + +//============================================================================== +template +struct BVHShapeCollider, Shape, NarrowPhaseSolver> +{ + using S = typename Shape::S; + static std::size_t collide( + const CollisionGeometry* o1, + const Transform3& tf1, + const CollisionGeometry* o2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result) + { + return details::orientedBVHShapeCollide< + MeshShapeCollisionTraversalNodeOBBRSS, + OBBRSS, + Shape, + NarrowPhaseSolver>( + o1, tf1, o2, tf2, nsolver, request, result); + } +}; + +//============================================================================== +template +struct BVHCollideImpl +{ + static std::size_t run( + const CollisionGeometry* o1, + const Transform3& tf1, + const CollisionGeometry* o2, + const Transform3& tf2, + const CollisionRequest& request, + CollisionResult& result) + { + if(request.isSatisfied(result)) return result.numContacts(); + + MeshCollisionTraversalNode node; + const BVHModel* obj1 = static_cast* >(o1); + const BVHModel* obj2 = static_cast* >(o2); + BVHModel* obj1_tmp = new BVHModel(*obj1); + Transform3 tf1_tmp = tf1; + BVHModel* obj2_tmp = new BVHModel(*obj2); + Transform3 tf2_tmp = tf2; + + initialize(node, *obj1_tmp, tf1_tmp, *obj2_tmp, tf2_tmp, request, result); + collide(&node); + + delete obj1_tmp; + delete obj2_tmp; + + return result.numContacts(); + } +}; + +//============================================================================== +template +std::size_t BVHCollide( + const CollisionGeometry* o1, + const Transform3& tf1, + const CollisionGeometry* o2, + const Transform3& tf2, + const CollisionRequest& request, + CollisionResult& result) +{ + return BVHCollideImpl::run( + o1, tf1, o2, tf2, request, result); } +namespace details +{ + +//============================================================================== +template +std::size_t orientedMeshCollide( + const CollisionGeometry* o1, + const Transform3& tf1, + const CollisionGeometry* o2, + const Transform3& tf2, + const CollisionRequest& request, + CollisionResult& result) +{ + if(request.isSatisfied(result)) return result.numContacts(); + + OrientedMeshCollisionTraversalNode node; + const BVHModel* obj1 = static_cast* >(o1); + const BVHModel* obj2 = static_cast* >(o2); + + initialize(node, *obj1, tf1, *obj2, tf2, request, result); + collide(&node); + + return result.numContacts(); +} + +} // namespace details + +//============================================================================== +template +struct BVHCollideImpl> +{ + static std::size_t run( + const CollisionGeometry* o1, + const Transform3& tf1, + const CollisionGeometry* o2, + const Transform3& tf2, + const CollisionRequest& request, + CollisionResult& result) + { + return details::orientedMeshCollide< + MeshCollisionTraversalNodeOBB, OBB>( + o1, tf1, o2, tf2, request, result); + } +}; + +//============================================================================== +template +struct BVHCollideImpl> +{ + static std::size_t run( + const CollisionGeometry* o1, + const Transform3& tf1, + const CollisionGeometry* o2, + const Transform3& tf2, + const CollisionRequest& request, + CollisionResult& result) + { + return details::orientedMeshCollide< + MeshCollisionTraversalNodeOBBRSS, OBBRSS>( + o1, tf1, o2, tf2, request, result); + } +}; + +//============================================================================== +template +struct BVHCollideImpl> +{ + static std::size_t run( + const CollisionGeometry* o1, + const Transform3& tf1, + const CollisionGeometry* o2, + const Transform3& tf2, + const CollisionRequest& request, + CollisionResult& result) + { + return details::orientedMeshCollide< + MeshCollisionTraversalNodekIOS, kIOS>( + o1, tf1, o2, tf2, request, result); + } +}; + +//============================================================================== +template +std::size_t BVHCollide( + const CollisionGeometry* o1, + const Transform3& tf1, + const CollisionGeometry* o2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result) +{ + return BVHCollide(o1, tf1, o2, tf2, request, result); +} + +//============================================================================== +template +CollisionFunctionMatrix::CollisionFunctionMatrix() +{ + using S = typename NarrowPhaseSolver::S; + + for(int i = 0; i < NODE_COUNT; ++i) + { + for(int j = 0; j < NODE_COUNT; ++j) + collision_matrix[i][j] = nullptr; + } + + collision_matrix[GEOM_BOX][GEOM_BOX] = &ShapeShapeCollide, Box, NarrowPhaseSolver>; + collision_matrix[GEOM_BOX][GEOM_SPHERE] = &ShapeShapeCollide, Sphere, NarrowPhaseSolver>; + collision_matrix[GEOM_BOX][GEOM_ELLIPSOID] = &ShapeShapeCollide, Ellipsoid, NarrowPhaseSolver>; + collision_matrix[GEOM_BOX][GEOM_CAPSULE] = &ShapeShapeCollide, Capsule, NarrowPhaseSolver>; + collision_matrix[GEOM_BOX][GEOM_CONE] = &ShapeShapeCollide, Cone, NarrowPhaseSolver>; + collision_matrix[GEOM_BOX][GEOM_CYLINDER] = &ShapeShapeCollide, Cylinder, NarrowPhaseSolver>; + collision_matrix[GEOM_BOX][GEOM_CONVEX] = &ShapeShapeCollide, Convex, NarrowPhaseSolver>; + collision_matrix[GEOM_BOX][GEOM_PLANE] = &ShapeShapeCollide, Plane, NarrowPhaseSolver>; + collision_matrix[GEOM_BOX][GEOM_HALFSPACE] = &ShapeShapeCollide, Halfspace, NarrowPhaseSolver>; + + collision_matrix[GEOM_SPHERE][GEOM_BOX] = &ShapeShapeCollide, Box, NarrowPhaseSolver>; + collision_matrix[GEOM_SPHERE][GEOM_SPHERE] = &ShapeShapeCollide, Sphere, NarrowPhaseSolver>; + collision_matrix[GEOM_SPHERE][GEOM_ELLIPSOID] = &ShapeShapeCollide, Ellipsoid, NarrowPhaseSolver>; + collision_matrix[GEOM_SPHERE][GEOM_CAPSULE] = &ShapeShapeCollide, Capsule, NarrowPhaseSolver>; + collision_matrix[GEOM_SPHERE][GEOM_CONE] = &ShapeShapeCollide, Cone, NarrowPhaseSolver>; + collision_matrix[GEOM_SPHERE][GEOM_CYLINDER] = &ShapeShapeCollide, Cylinder, NarrowPhaseSolver>; + collision_matrix[GEOM_SPHERE][GEOM_CONVEX] = &ShapeShapeCollide, Convex, NarrowPhaseSolver>; + collision_matrix[GEOM_SPHERE][GEOM_PLANE] = &ShapeShapeCollide, Plane, NarrowPhaseSolver>; + collision_matrix[GEOM_SPHERE][GEOM_HALFSPACE] = &ShapeShapeCollide, Halfspace, NarrowPhaseSolver>; + + collision_matrix[GEOM_ELLIPSOID][GEOM_BOX] = &ShapeShapeCollide, Box, NarrowPhaseSolver>; + collision_matrix[GEOM_ELLIPSOID][GEOM_SPHERE] = &ShapeShapeCollide, Sphere, NarrowPhaseSolver>; + collision_matrix[GEOM_ELLIPSOID][GEOM_ELLIPSOID] = &ShapeShapeCollide, Ellipsoid, NarrowPhaseSolver>; + collision_matrix[GEOM_ELLIPSOID][GEOM_CAPSULE] = &ShapeShapeCollide, Capsule, NarrowPhaseSolver>; + collision_matrix[GEOM_ELLIPSOID][GEOM_CONE] = &ShapeShapeCollide, Cone, NarrowPhaseSolver>; + collision_matrix[GEOM_ELLIPSOID][GEOM_CYLINDER] = &ShapeShapeCollide, Cylinder, NarrowPhaseSolver>; + collision_matrix[GEOM_ELLIPSOID][GEOM_CONVEX] = &ShapeShapeCollide, Convex, NarrowPhaseSolver>; + collision_matrix[GEOM_ELLIPSOID][GEOM_PLANE] = &ShapeShapeCollide, Plane, NarrowPhaseSolver>; + collision_matrix[GEOM_ELLIPSOID][GEOM_HALFSPACE] = &ShapeShapeCollide, Halfspace, NarrowPhaseSolver>; + + collision_matrix[GEOM_CAPSULE][GEOM_BOX] = &ShapeShapeCollide, Box, NarrowPhaseSolver>; + collision_matrix[GEOM_CAPSULE][GEOM_SPHERE] = &ShapeShapeCollide, Sphere, NarrowPhaseSolver>; + collision_matrix[GEOM_CAPSULE][GEOM_ELLIPSOID] = &ShapeShapeCollide, Ellipsoid, NarrowPhaseSolver>; + collision_matrix[GEOM_CAPSULE][GEOM_CAPSULE] = &ShapeShapeCollide, Capsule, NarrowPhaseSolver>; + collision_matrix[GEOM_CAPSULE][GEOM_CONE] = &ShapeShapeCollide, Cone, NarrowPhaseSolver>; + collision_matrix[GEOM_CAPSULE][GEOM_CYLINDER] = &ShapeShapeCollide, Cylinder, NarrowPhaseSolver>; + collision_matrix[GEOM_CAPSULE][GEOM_CONVEX] = &ShapeShapeCollide, Convex, NarrowPhaseSolver>; + collision_matrix[GEOM_CAPSULE][GEOM_PLANE] = &ShapeShapeCollide, Plane, NarrowPhaseSolver>; + collision_matrix[GEOM_CAPSULE][GEOM_HALFSPACE] = &ShapeShapeCollide, Halfspace, NarrowPhaseSolver>; + + collision_matrix[GEOM_CONE][GEOM_BOX] = &ShapeShapeCollide, Box, NarrowPhaseSolver>; + collision_matrix[GEOM_CONE][GEOM_SPHERE] = &ShapeShapeCollide, Sphere, NarrowPhaseSolver>; + collision_matrix[GEOM_CONE][GEOM_ELLIPSOID] = &ShapeShapeCollide, Ellipsoid, NarrowPhaseSolver>; + collision_matrix[GEOM_CONE][GEOM_CAPSULE] = &ShapeShapeCollide, Capsule, NarrowPhaseSolver>; + collision_matrix[GEOM_CONE][GEOM_CONE] = &ShapeShapeCollide, Cone, NarrowPhaseSolver>; + collision_matrix[GEOM_CONE][GEOM_CYLINDER] = &ShapeShapeCollide, Cylinder, NarrowPhaseSolver>; + collision_matrix[GEOM_CONE][GEOM_CONVEX] = &ShapeShapeCollide, Convex, NarrowPhaseSolver>; + collision_matrix[GEOM_CONE][GEOM_PLANE] = &ShapeShapeCollide, Plane, NarrowPhaseSolver>; + collision_matrix[GEOM_CONE][GEOM_HALFSPACE] = &ShapeShapeCollide, Halfspace, NarrowPhaseSolver>; + + collision_matrix[GEOM_CYLINDER][GEOM_BOX] = &ShapeShapeCollide, Box, NarrowPhaseSolver>; + collision_matrix[GEOM_CYLINDER][GEOM_SPHERE] = &ShapeShapeCollide, Sphere, NarrowPhaseSolver>; + collision_matrix[GEOM_CYLINDER][GEOM_ELLIPSOID] = &ShapeShapeCollide, Ellipsoid, NarrowPhaseSolver>; + collision_matrix[GEOM_CYLINDER][GEOM_CAPSULE] = &ShapeShapeCollide, Capsule, NarrowPhaseSolver>; + collision_matrix[GEOM_CYLINDER][GEOM_CONE] = &ShapeShapeCollide, Cone, NarrowPhaseSolver>; + collision_matrix[GEOM_CYLINDER][GEOM_CYLINDER] = &ShapeShapeCollide, Cylinder, NarrowPhaseSolver>; + collision_matrix[GEOM_CYLINDER][GEOM_CONVEX] = &ShapeShapeCollide, Convex, NarrowPhaseSolver>; + collision_matrix[GEOM_CYLINDER][GEOM_PLANE] = &ShapeShapeCollide, Plane, NarrowPhaseSolver>; + collision_matrix[GEOM_CYLINDER][GEOM_HALFSPACE] = &ShapeShapeCollide, Halfspace, NarrowPhaseSolver>; + + collision_matrix[GEOM_CONVEX][GEOM_BOX] = &ShapeShapeCollide, Box, NarrowPhaseSolver>; + collision_matrix[GEOM_CONVEX][GEOM_SPHERE] = &ShapeShapeCollide, Sphere, NarrowPhaseSolver>; + collision_matrix[GEOM_CONVEX][GEOM_ELLIPSOID] = &ShapeShapeCollide, Ellipsoid, NarrowPhaseSolver>; + collision_matrix[GEOM_CONVEX][GEOM_CAPSULE] = &ShapeShapeCollide, Capsule, NarrowPhaseSolver>; + collision_matrix[GEOM_CONVEX][GEOM_CONE] = &ShapeShapeCollide, Cone, NarrowPhaseSolver>; + collision_matrix[GEOM_CONVEX][GEOM_CYLINDER] = &ShapeShapeCollide, Cylinder, NarrowPhaseSolver>; + collision_matrix[GEOM_CONVEX][GEOM_CONVEX] = &ShapeShapeCollide, Convex, NarrowPhaseSolver>; + collision_matrix[GEOM_CONVEX][GEOM_PLANE] = &ShapeShapeCollide, Plane, NarrowPhaseSolver>; + collision_matrix[GEOM_CONVEX][GEOM_HALFSPACE] = &ShapeShapeCollide, Halfspace, NarrowPhaseSolver>; + + collision_matrix[GEOM_PLANE][GEOM_BOX] = &ShapeShapeCollide, Box, NarrowPhaseSolver>; + collision_matrix[GEOM_PLANE][GEOM_SPHERE] = &ShapeShapeCollide, Sphere, NarrowPhaseSolver>; + collision_matrix[GEOM_PLANE][GEOM_ELLIPSOID] = &ShapeShapeCollide, Ellipsoid, NarrowPhaseSolver>; + collision_matrix[GEOM_PLANE][GEOM_CAPSULE] = &ShapeShapeCollide, Capsule, NarrowPhaseSolver>; + collision_matrix[GEOM_PLANE][GEOM_CONE] = &ShapeShapeCollide, Cone, NarrowPhaseSolver>; + collision_matrix[GEOM_PLANE][GEOM_CYLINDER] = &ShapeShapeCollide, Cylinder, NarrowPhaseSolver>; + collision_matrix[GEOM_PLANE][GEOM_CONVEX] = &ShapeShapeCollide, Convex, NarrowPhaseSolver>; + collision_matrix[GEOM_PLANE][GEOM_PLANE] = &ShapeShapeCollide, Plane, NarrowPhaseSolver>; + collision_matrix[GEOM_PLANE][GEOM_HALFSPACE] = &ShapeShapeCollide, Halfspace, NarrowPhaseSolver>; + + collision_matrix[GEOM_HALFSPACE][GEOM_BOX] = &ShapeShapeCollide, Box, NarrowPhaseSolver>; + collision_matrix[GEOM_HALFSPACE][GEOM_SPHERE] = &ShapeShapeCollide, Sphere, NarrowPhaseSolver>; + collision_matrix[GEOM_HALFSPACE][GEOM_CAPSULE] = &ShapeShapeCollide, Capsule, NarrowPhaseSolver>; + collision_matrix[GEOM_HALFSPACE][GEOM_CONE] = &ShapeShapeCollide, Cone, NarrowPhaseSolver>; + collision_matrix[GEOM_HALFSPACE][GEOM_CYLINDER] = &ShapeShapeCollide, Cylinder, NarrowPhaseSolver>; + collision_matrix[GEOM_HALFSPACE][GEOM_CONVEX] = &ShapeShapeCollide, Convex, NarrowPhaseSolver>; + collision_matrix[GEOM_HALFSPACE][GEOM_PLANE] = &ShapeShapeCollide, Plane, NarrowPhaseSolver>; + collision_matrix[GEOM_HALFSPACE][GEOM_HALFSPACE] = &ShapeShapeCollide, Halfspace, NarrowPhaseSolver>; + + collision_matrix[BV_AABB][GEOM_BOX] = &BVHShapeCollider, Box, NarrowPhaseSolver>::collide; + collision_matrix[BV_AABB][GEOM_SPHERE] = &BVHShapeCollider, Sphere, NarrowPhaseSolver>::collide; + collision_matrix[BV_AABB][GEOM_ELLIPSOID] = &BVHShapeCollider, Ellipsoid, NarrowPhaseSolver>::collide; + collision_matrix[BV_AABB][GEOM_CAPSULE] = &BVHShapeCollider, Capsule, NarrowPhaseSolver>::collide; + collision_matrix[BV_AABB][GEOM_CONE] = &BVHShapeCollider, Cone, NarrowPhaseSolver>::collide; + collision_matrix[BV_AABB][GEOM_CYLINDER] = &BVHShapeCollider, Cylinder, NarrowPhaseSolver>::collide; + collision_matrix[BV_AABB][GEOM_CONVEX] = &BVHShapeCollider, Convex, NarrowPhaseSolver>::collide; + collision_matrix[BV_AABB][GEOM_PLANE] = &BVHShapeCollider, Plane, NarrowPhaseSolver>::collide; + collision_matrix[BV_AABB][GEOM_HALFSPACE] = &BVHShapeCollider, Halfspace, NarrowPhaseSolver>::collide; + + collision_matrix[BV_OBB][GEOM_BOX] = &BVHShapeCollider, Box, NarrowPhaseSolver>::collide; + collision_matrix[BV_OBB][GEOM_SPHERE] = &BVHShapeCollider, Sphere, NarrowPhaseSolver>::collide; + collision_matrix[BV_OBB][GEOM_ELLIPSOID] = &BVHShapeCollider, Ellipsoid, NarrowPhaseSolver>::collide; + collision_matrix[BV_OBB][GEOM_CAPSULE] = &BVHShapeCollider, Capsule, NarrowPhaseSolver>::collide; + collision_matrix[BV_OBB][GEOM_CONE] = &BVHShapeCollider, Cone, NarrowPhaseSolver>::collide; + collision_matrix[BV_OBB][GEOM_CYLINDER] = &BVHShapeCollider, Cylinder, NarrowPhaseSolver>::collide; + collision_matrix[BV_OBB][GEOM_CONVEX] = &BVHShapeCollider, Convex, NarrowPhaseSolver>::collide; + collision_matrix[BV_OBB][GEOM_PLANE] = &BVHShapeCollider, Plane, NarrowPhaseSolver>::collide; + collision_matrix[BV_OBB][GEOM_HALFSPACE] = &BVHShapeCollider, Halfspace, NarrowPhaseSolver>::collide; + + collision_matrix[BV_RSS][GEOM_BOX] = &BVHShapeCollider, Box, NarrowPhaseSolver>::collide; + collision_matrix[BV_RSS][GEOM_SPHERE] = &BVHShapeCollider, Sphere, NarrowPhaseSolver>::collide; + collision_matrix[BV_RSS][GEOM_ELLIPSOID] = &BVHShapeCollider, Ellipsoid, NarrowPhaseSolver>::collide; + collision_matrix[BV_RSS][GEOM_CAPSULE] = &BVHShapeCollider, Capsule, NarrowPhaseSolver>::collide; + collision_matrix[BV_RSS][GEOM_CONE] = &BVHShapeCollider, Cone, NarrowPhaseSolver>::collide; + collision_matrix[BV_RSS][GEOM_CYLINDER] = &BVHShapeCollider, Cylinder, NarrowPhaseSolver>::collide; + collision_matrix[BV_RSS][GEOM_CONVEX] = &BVHShapeCollider, Convex, NarrowPhaseSolver>::collide; + collision_matrix[BV_RSS][GEOM_PLANE] = &BVHShapeCollider, Plane, NarrowPhaseSolver>::collide; + collision_matrix[BV_RSS][GEOM_HALFSPACE] = &BVHShapeCollider, Halfspace, NarrowPhaseSolver>::collide; + + collision_matrix[BV_KDOP16][GEOM_BOX] = &BVHShapeCollider, Box, NarrowPhaseSolver>::collide; + collision_matrix[BV_KDOP16][GEOM_SPHERE] = &BVHShapeCollider, Sphere, NarrowPhaseSolver>::collide; + collision_matrix[BV_KDOP16][GEOM_ELLIPSOID] = &BVHShapeCollider, Ellipsoid, NarrowPhaseSolver>::collide; + collision_matrix[BV_KDOP16][GEOM_CAPSULE] = &BVHShapeCollider, Capsule, NarrowPhaseSolver>::collide; + collision_matrix[BV_KDOP16][GEOM_CONE] = &BVHShapeCollider, Cone, NarrowPhaseSolver>::collide; + collision_matrix[BV_KDOP16][GEOM_CYLINDER] = &BVHShapeCollider, Cylinder, NarrowPhaseSolver>::collide; + collision_matrix[BV_KDOP16][GEOM_CONVEX] = &BVHShapeCollider, Convex, NarrowPhaseSolver>::collide; + collision_matrix[BV_KDOP16][GEOM_PLANE] = &BVHShapeCollider, Plane, NarrowPhaseSolver>::collide; + collision_matrix[BV_KDOP16][GEOM_HALFSPACE] = &BVHShapeCollider, Halfspace, NarrowPhaseSolver>::collide; + + collision_matrix[BV_KDOP18][GEOM_BOX] = &BVHShapeCollider, Box, NarrowPhaseSolver>::collide; + collision_matrix[BV_KDOP18][GEOM_SPHERE] = &BVHShapeCollider, Sphere, NarrowPhaseSolver>::collide; + collision_matrix[BV_KDOP18][GEOM_ELLIPSOID] = &BVHShapeCollider, Ellipsoid, NarrowPhaseSolver>::collide; + collision_matrix[BV_KDOP18][GEOM_CAPSULE] = &BVHShapeCollider, Capsule, NarrowPhaseSolver>::collide; + collision_matrix[BV_KDOP18][GEOM_CONE] = &BVHShapeCollider, Cone, NarrowPhaseSolver>::collide; + collision_matrix[BV_KDOP18][GEOM_CYLINDER] = &BVHShapeCollider, Cylinder, NarrowPhaseSolver>::collide; + collision_matrix[BV_KDOP18][GEOM_CONVEX] = &BVHShapeCollider, Convex, NarrowPhaseSolver>::collide; + collision_matrix[BV_KDOP18][GEOM_PLANE] = &BVHShapeCollider, Plane, NarrowPhaseSolver>::collide; + collision_matrix[BV_KDOP18][GEOM_HALFSPACE] = &BVHShapeCollider, Halfspace, NarrowPhaseSolver>::collide; + + collision_matrix[BV_KDOP24][GEOM_BOX] = &BVHShapeCollider, Box, NarrowPhaseSolver>::collide; + collision_matrix[BV_KDOP24][GEOM_SPHERE] = &BVHShapeCollider, Sphere, NarrowPhaseSolver>::collide; + collision_matrix[BV_KDOP24][GEOM_ELLIPSOID] = &BVHShapeCollider, Ellipsoid, NarrowPhaseSolver>::collide; + collision_matrix[BV_KDOP24][GEOM_CAPSULE] = &BVHShapeCollider, Capsule, NarrowPhaseSolver>::collide; + collision_matrix[BV_KDOP24][GEOM_CONE] = &BVHShapeCollider, Cone, NarrowPhaseSolver>::collide; + collision_matrix[BV_KDOP24][GEOM_CYLINDER] = &BVHShapeCollider, Cylinder, NarrowPhaseSolver>::collide; + collision_matrix[BV_KDOP24][GEOM_CONVEX] = &BVHShapeCollider, Convex, NarrowPhaseSolver>::collide; + collision_matrix[BV_KDOP24][GEOM_PLANE] = &BVHShapeCollider, Plane, NarrowPhaseSolver>::collide; + collision_matrix[BV_KDOP24][GEOM_HALFSPACE] = &BVHShapeCollider, Halfspace, NarrowPhaseSolver>::collide; + + collision_matrix[BV_kIOS][GEOM_BOX] = &BVHShapeCollider, Box, NarrowPhaseSolver>::collide; + collision_matrix[BV_kIOS][GEOM_SPHERE] = &BVHShapeCollider, Sphere, NarrowPhaseSolver>::collide; + collision_matrix[BV_kIOS][GEOM_ELLIPSOID] = &BVHShapeCollider, Ellipsoid, NarrowPhaseSolver>::collide; + collision_matrix[BV_kIOS][GEOM_CAPSULE] = &BVHShapeCollider, Capsule, NarrowPhaseSolver>::collide; + collision_matrix[BV_kIOS][GEOM_CONE] = &BVHShapeCollider, Cone, NarrowPhaseSolver>::collide; + collision_matrix[BV_kIOS][GEOM_CYLINDER] = &BVHShapeCollider, Cylinder, NarrowPhaseSolver>::collide; + collision_matrix[BV_kIOS][GEOM_CONVEX] = &BVHShapeCollider, Convex, NarrowPhaseSolver>::collide; + collision_matrix[BV_kIOS][GEOM_PLANE] = &BVHShapeCollider, Plane, NarrowPhaseSolver>::collide; + collision_matrix[BV_kIOS][GEOM_HALFSPACE] = &BVHShapeCollider, Halfspace, NarrowPhaseSolver>::collide; + + collision_matrix[BV_OBBRSS][GEOM_BOX] = &BVHShapeCollider, Box, NarrowPhaseSolver>::collide; + collision_matrix[BV_OBBRSS][GEOM_SPHERE] = &BVHShapeCollider, Sphere, NarrowPhaseSolver>::collide; + collision_matrix[BV_OBBRSS][GEOM_ELLIPSOID] = &BVHShapeCollider, Ellipsoid, NarrowPhaseSolver>::collide; + collision_matrix[BV_OBBRSS][GEOM_CAPSULE] = &BVHShapeCollider, Capsule, NarrowPhaseSolver>::collide; + collision_matrix[BV_OBBRSS][GEOM_CONE] = &BVHShapeCollider, Cone, NarrowPhaseSolver>::collide; + collision_matrix[BV_OBBRSS][GEOM_CYLINDER] = &BVHShapeCollider, Cylinder, NarrowPhaseSolver>::collide; + collision_matrix[BV_OBBRSS][GEOM_CONVEX] = &BVHShapeCollider, Convex, NarrowPhaseSolver>::collide; + collision_matrix[BV_OBBRSS][GEOM_PLANE] = &BVHShapeCollider, Plane, NarrowPhaseSolver>::collide; + collision_matrix[BV_OBBRSS][GEOM_HALFSPACE] = &BVHShapeCollider, Halfspace, NarrowPhaseSolver>::collide; + + collision_matrix[BV_AABB][BV_AABB] = &BVHCollide, NarrowPhaseSolver>; + collision_matrix[BV_OBB][BV_OBB] = &BVHCollide, NarrowPhaseSolver>; + collision_matrix[BV_RSS][BV_RSS] = &BVHCollide, NarrowPhaseSolver>; + collision_matrix[BV_KDOP16][BV_KDOP16] = &BVHCollide, NarrowPhaseSolver>; + collision_matrix[BV_KDOP18][BV_KDOP18] = &BVHCollide, NarrowPhaseSolver>; + collision_matrix[BV_KDOP24][BV_KDOP24] = &BVHCollide, NarrowPhaseSolver>; + collision_matrix[BV_kIOS][BV_kIOS] = &BVHCollide, NarrowPhaseSolver>; + collision_matrix[BV_OBBRSS][BV_OBBRSS] = &BVHCollide, NarrowPhaseSolver>; + +#if FCL_HAVE_OCTOMAP + collision_matrix[GEOM_OCTREE][GEOM_BOX] = &OcTreeShapeCollide, NarrowPhaseSolver>; + collision_matrix[GEOM_OCTREE][GEOM_SPHERE] = &OcTreeShapeCollide, NarrowPhaseSolver>; + collision_matrix[GEOM_OCTREE][GEOM_ELLIPSOID] = &OcTreeShapeCollide, NarrowPhaseSolver>; + collision_matrix[GEOM_OCTREE][GEOM_CAPSULE] = &OcTreeShapeCollide, NarrowPhaseSolver>; + collision_matrix[GEOM_OCTREE][GEOM_CONE] = &OcTreeShapeCollide, NarrowPhaseSolver>; + collision_matrix[GEOM_OCTREE][GEOM_CYLINDER] = &OcTreeShapeCollide, NarrowPhaseSolver>; + collision_matrix[GEOM_OCTREE][GEOM_CONVEX] = &OcTreeShapeCollide, NarrowPhaseSolver>; + collision_matrix[GEOM_OCTREE][GEOM_PLANE] = &OcTreeShapeCollide, NarrowPhaseSolver>; + collision_matrix[GEOM_OCTREE][GEOM_HALFSPACE] = &OcTreeShapeCollide, NarrowPhaseSolver>; + + collision_matrix[GEOM_BOX][GEOM_OCTREE] = &ShapeOcTreeCollide, NarrowPhaseSolver>; + collision_matrix[GEOM_SPHERE][GEOM_OCTREE] = &ShapeOcTreeCollide, NarrowPhaseSolver>; + collision_matrix[GEOM_ELLIPSOID][GEOM_OCTREE] = &ShapeOcTreeCollide, NarrowPhaseSolver>; + collision_matrix[GEOM_CAPSULE][GEOM_OCTREE] = &ShapeOcTreeCollide, NarrowPhaseSolver>; + collision_matrix[GEOM_CONE][GEOM_OCTREE] = &ShapeOcTreeCollide, NarrowPhaseSolver>; + collision_matrix[GEOM_CYLINDER][GEOM_OCTREE] = &ShapeOcTreeCollide, NarrowPhaseSolver>; + collision_matrix[GEOM_CONVEX][GEOM_OCTREE] = &ShapeOcTreeCollide, NarrowPhaseSolver>; + collision_matrix[GEOM_PLANE][GEOM_OCTREE] = &ShapeOcTreeCollide, NarrowPhaseSolver>; + collision_matrix[GEOM_HALFSPACE][GEOM_OCTREE] = &ShapeOcTreeCollide, NarrowPhaseSolver>; + + collision_matrix[GEOM_OCTREE][GEOM_OCTREE] = &OcTreeCollide; + + collision_matrix[GEOM_OCTREE][BV_AABB] = &OcTreeBVHCollide, NarrowPhaseSolver>; + collision_matrix[GEOM_OCTREE][BV_OBB] = &OcTreeBVHCollide, NarrowPhaseSolver>; + collision_matrix[GEOM_OCTREE][BV_RSS] = &OcTreeBVHCollide, NarrowPhaseSolver>; + collision_matrix[GEOM_OCTREE][BV_OBBRSS] = &OcTreeBVHCollide, NarrowPhaseSolver>; + collision_matrix[GEOM_OCTREE][BV_kIOS] = &OcTreeBVHCollide, NarrowPhaseSolver>; + collision_matrix[GEOM_OCTREE][BV_KDOP16] = &OcTreeBVHCollide, NarrowPhaseSolver>; + collision_matrix[GEOM_OCTREE][BV_KDOP18] = &OcTreeBVHCollide, NarrowPhaseSolver>; + collision_matrix[GEOM_OCTREE][BV_KDOP24] = &OcTreeBVHCollide, NarrowPhaseSolver>; + + collision_matrix[BV_AABB][GEOM_OCTREE] = &BVHOcTreeCollide, NarrowPhaseSolver>; + collision_matrix[BV_OBB][GEOM_OCTREE] = &BVHOcTreeCollide, NarrowPhaseSolver>; + collision_matrix[BV_RSS][GEOM_OCTREE] = &BVHOcTreeCollide, NarrowPhaseSolver>; + collision_matrix[BV_OBBRSS][GEOM_OCTREE] = &BVHOcTreeCollide, NarrowPhaseSolver>; + collision_matrix[BV_kIOS][GEOM_OCTREE] = &BVHOcTreeCollide, NarrowPhaseSolver>; + collision_matrix[BV_KDOP16][GEOM_OCTREE] = &BVHOcTreeCollide, NarrowPhaseSolver>; + collision_matrix[BV_KDOP18][GEOM_OCTREE] = &BVHOcTreeCollide, NarrowPhaseSolver>; + collision_matrix[BV_KDOP24][GEOM_OCTREE] = &BVHOcTreeCollide, NarrowPhaseSolver>; +#endif +} + +} // namespace fcl + #endif diff --git a/include/fcl/collision_geometry.h b/include/fcl/collision_geometry.h new file mode 100644 index 000000000..2d756dddf --- /dev/null +++ b/include/fcl/collision_geometry.h @@ -0,0 +1,248 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_COLLISION_GEOMETRY_H +#define FCL_COLLISION_GEOMETRY_H + +#include + +#include "fcl/deprecated.h" +#include "fcl/BV/AABB.h" +#include "fcl/ccd/motion_base.h" + +namespace fcl +{ + +/// @brief object type: BVH (mesh, points), basic geometry, octree +enum OBJECT_TYPE {OT_UNKNOWN, OT_BVH, OT_GEOM, OT_OCTREE, OT_COUNT}; + +/// @brief traversal node type: bounding volume (AABBd, OBBd, RSSd, kIOSd, OBBRSSd, KDOP16, KDOP18, kDOP24), basic shape (box, sphere, ellipsoid, capsule, cone, cylinder, convex, plane, halfspace, triangle), and octree +enum NODE_TYPE {BV_UNKNOWN, BV_AABB, BV_OBB, BV_RSS, BV_kIOS, BV_OBBRSS, BV_KDOP16, BV_KDOP18, BV_KDOP24, + GEOM_BOX, GEOM_SPHERE, GEOM_ELLIPSOID, GEOM_CAPSULE, GEOM_CONE, GEOM_CYLINDER, GEOM_CONVEX, GEOM_PLANE, GEOM_HALFSPACE, GEOM_TRIANGLE, GEOM_OCTREE, NODE_COUNT}; + +/// @brief The geometry for the object for collision or distance computation +template +class CollisionGeometry +{ +public: + CollisionGeometry(); + + virtual ~CollisionGeometry(); + + /// @brief get the type of the object + virtual OBJECT_TYPE getObjectType() const; + + /// @brief get the node type + virtual NODE_TYPE getNodeType() const; + + /// @brief compute the AABBd for object in local coordinate + virtual void computeLocalAABB() = 0; + + /// @brief get user data in geometry + void* getUserData() const; + + /// @brief set user data in geometry + void setUserData(void *data); + + /// @brief whether the object is completely occupied + bool isOccupied() const; + + /// @brief whether the object is completely free + bool isFree() const; + + /// @brief whether the object has some uncertainty + bool isUncertain() const; + + /// @brief AABBd center in local coordinate + Vector3 aabb_center; + + /// @brief AABBd radius + S aabb_radius; + + /// @brief AABBd in local coordinate, used for tight AABBd when only translation transform + AABB aabb_local; + + /// @brief pointer to user defined data specific to this object + void* user_data; + + /// @brief collision cost for unit volume + S cost_density; + + /// @brief threshold for occupied ( >= is occupied) + S threshold_occupied; + + /// @brief threshold for free (<= is free) + S threshold_free; + + /// @brief compute center of mass + virtual Vector3 computeCOM() const; + + /// @brief compute the inertia matrix, related to the origin + virtual Matrix3 computeMomentofInertia() const; + + /// @brief compute the volume + virtual S computeVolume() const; + + /// @brief compute the inertia matrix, related to the com + virtual Matrix3 computeMomentofInertiaRelatedToCOM() const; + +}; + +using CollisionGeometryf = CollisionGeometry; +using CollisionGeometryd = CollisionGeometry; + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +CollisionGeometry::CollisionGeometry() + : aabb_center(Vector3::Zero()), + aabb_radius((S)0), + user_data(nullptr), + cost_density((S)1), + threshold_occupied((S)1), + threshold_free((S)0) +{ + // Do nothing +} + +//============================================================================== +template +CollisionGeometry::~CollisionGeometry() +{ + // Do nothing +} + +//============================================================================== +template +OBJECT_TYPE CollisionGeometry::getObjectType() const +{ + return OT_UNKNOWN; +} + +//============================================================================== +template +NODE_TYPE CollisionGeometry::getNodeType() const +{ + return BV_UNKNOWN; +} + +//============================================================================== +template +void* CollisionGeometry::getUserData() const +{ + return user_data; +} + +//============================================================================== +template +void CollisionGeometry::setUserData(void* data) +{ + user_data = data; +} + +//============================================================================== +template +bool CollisionGeometry::isOccupied() const +{ + return cost_density >= threshold_occupied; +} + +//============================================================================== +template +bool CollisionGeometry::isFree() const +{ + return cost_density <= threshold_free; +} + +//============================================================================== +template +bool CollisionGeometry::isUncertain() const +{ + return !isOccupied() && !isFree(); +} + +//============================================================================== +template +Vector3 CollisionGeometry::computeCOM() const +{ + return Vector3::Zero(); +} + +//============================================================================== +template +Matrix3 CollisionGeometry::computeMomentofInertia() const +{ + return Matrix3::Zero(); +} + +//============================================================================== +template +S CollisionGeometry::computeVolume() const +{ + return 0; +} + +//============================================================================== +template +Matrix3 CollisionGeometry::computeMomentofInertiaRelatedToCOM() const +{ + Matrix3 C = computeMomentofInertia(); + Vector3 com = computeCOM(); + S V = computeVolume(); + + Matrix3 m; + m << C(0, 0) - V * (com[1] * com[1] + com[2] * com[2]), + C(0, 1) + V * com[0] * com[1], + C(0, 2) + V * com[0] * com[2], + C(1, 0) + V * com[1] * com[0], + C(1, 1) - V * (com[0] * com[0] + com[2] * com[2]), + C(1, 2) + V * com[1] * com[2], + C(2, 0) + V * com[2] * com[0], + C(2, 1) + V * com[2] * com[1], + C(2, 2) - V * (com[0] * com[0] + com[1] * com[1]); + + return m; +} + +} + +#endif diff --git a/include/fcl/collision_node.h b/include/fcl/collision_node.h index 554ff3325..2e310bdf9 100644 --- a/include/fcl/collision_node.h +++ b/include/fcl/collision_node.h @@ -39,32 +39,121 @@ #ifndef FCL_COLLISION_NODE_H #define FCL_COLLISION_NODE_H -#include "fcl/traversal/traversal_node_base.h" -#include "fcl/traversal/traversal_node_bvhs.h" #include "fcl/BVH/BVH_front.h" - - +#include "fcl/traversal/traversal_recurse.h" +#include "fcl/traversal/collision/collision_traversal_node_base.h" +#include "fcl/traversal/collision/mesh_collision_traversal_node.h" +#include "fcl/traversal/distance/distance_traversal_node_base.h" /// @brief collision and distance function on traversal nodes. these functions provide a higher level abstraction for collision functions provided in collision_func_matrix namespace fcl { - /// @brief collision on collision traversal node; can use front list to accelerate -void collide(CollisionTraversalNodeBase* node, BVHFrontList* front_list = NULL); +template +void collide(CollisionTraversalNodeBase* node, BVHFrontList* front_list = nullptr); /// @brief self collision on collision traversal node; can use front list to accelerate -void selfCollide(CollisionTraversalNodeBase* node, BVHFrontList* front_list = NULL); +template +void selfCollide(CollisionTraversalNodeBase* node, BVHFrontList* front_list = nullptr); /// @brief distance computation on distance traversal node; can use front list to accelerate -void distance(DistanceTraversalNodeBase* node, BVHFrontList* front_list = NULL, int qsize = 2); +template +void distance(DistanceTraversalNodeBase* node, BVHFrontList* front_list = nullptr, int qsize = 2); + +/// @brief special collision on OBBd traversal node +template +void collide2(MeshCollisionTraversalNodeOBB* node, BVHFrontList* front_list = nullptr); + +/// @brief special collision on RSSd traversal node +template +void collide2(MeshCollisionTraversalNodeRSS* node, BVHFrontList* front_list = nullptr); + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +void collide(CollisionTraversalNodeBase* node, BVHFrontList* front_list) +{ + if(front_list && front_list->size() > 0) + { + propagateBVHFrontListCollisionRecurse(node, front_list); + } + else + { + collisionRecurse(node, 0, 0, front_list); + } +} + +//============================================================================== +template +void collide2(MeshCollisionTraversalNodeOBB* node, BVHFrontList* front_list) +{ + if(front_list && front_list->size() > 0) + { + propagateBVHFrontListCollisionRecurse(node, front_list); + } + else + { + Matrix3 Rtemp, R; + Vector3 Ttemp, T; + Rtemp = node->tf.linear() * node->model2->getBV(0).getOrientation(); + R = node->model1->getBV(0).getOrientation().transpose() * Rtemp; + Ttemp = node->tf.linear() * node->model2->getBV(0).getCenter() + node->tf.translation(); + Ttemp -= node->model1->getBV(0).getCenter(); + T = node->model1->getBV(0).getOrientation().transpose() * Ttemp; -/// @brief special collision on OBB traversal node -void collide2(MeshCollisionTraversalNodeOBB* node, BVHFrontList* front_list = NULL); + collisionRecurse(node, 0, 0, R, T, front_list); + } +} + +//============================================================================== +template +void collide2(MeshCollisionTraversalNodeRSS* node, BVHFrontList* front_list) +{ + if(front_list && front_list->size() > 0) + { + propagateBVHFrontListCollisionRecurse(node, front_list); + } + else + { + collisionRecurse(node, 0, 0, node->tf.linear(), node->tf.translation(), front_list); + } +} -/// @brief special collision on RSS traversal node -void collide2(MeshCollisionTraversalNodeRSS* node, BVHFrontList* front_list = NULL); +//============================================================================== +template +void selfCollide(CollisionTraversalNodeBase* node, BVHFrontList* front_list) +{ + if(front_list && front_list->size() > 0) + { + propagateBVHFrontListCollisionRecurse(node, front_list); + } + else + { + selfCollisionRecurse(node, 0, front_list); + } } +//============================================================================== +template +void distance(DistanceTraversalNodeBase* node, BVHFrontList* front_list, int qsize) +{ + node->preprocess(); + + if(qsize <= 2) + distanceRecurse(node, 0, 0, front_list); + else + distanceQueueRecurse(node, 0, 0, front_list, qsize); + + node->postprocess(); +} + +} // namespace fcl + #endif diff --git a/include/fcl/collision_object.h b/include/fcl/collision_object.h index fe4d0bcbf..858d5cd38 100644 --- a/include/fcl/collision_object.h +++ b/include/fcl/collision_object.h @@ -36,125 +36,24 @@ /** \author Jia Pan */ -#ifndef FCL_COLLISION_OBJECT_BASE_H -#define FCL_COLLISION_OBJECT_BASE_H +#ifndef FCL_COLLISION_OBJECT_H +#define FCL_COLLISION_OBJECT_H -#include -#include "fcl/BV/AABB.h" -#include "fcl/ccd/motion_base.h" #include -namespace fcl -{ - -/// @brief object type: BVH (mesh, points), basic geometry, octree -enum OBJECT_TYPE {OT_UNKNOWN, OT_BVH, OT_GEOM, OT_OCTREE, OT_COUNT}; - -/// @brief traversal node type: bounding volume (AABB, OBB, RSS, kIOS, OBBRSS, KDOP16, KDOP18, kDOP24), basic shape (box, sphere, ellipsoid, capsule, cone, cylinder, convex, plane, halfspace, triangle), and octree -enum NODE_TYPE {BV_UNKNOWN, BV_AABB, BV_OBB, BV_RSS, BV_kIOS, BV_OBBRSS, BV_KDOP16, BV_KDOP18, BV_KDOP24, - GEOM_BOX, GEOM_SPHERE, GEOM_ELLIPSOID, GEOM_CAPSULE, GEOM_CONE, GEOM_CYLINDER, GEOM_CONVEX, GEOM_PLANE, GEOM_HALFSPACE, GEOM_TRIANGLE, GEOM_OCTREE, NODE_COUNT}; +#include "fcl/collision_geometry.h" -/// @brief The geometry for the object for collision or distance computation -class CollisionGeometry +namespace fcl { -public: - CollisionGeometry() : cost_density(1), - threshold_occupied(1), - threshold_free(0) - { - } - - virtual ~CollisionGeometry() {} - - /// @brief get the type of the object - virtual OBJECT_TYPE getObjectType() const { return OT_UNKNOWN; } - - /// @brief get the node type - virtual NODE_TYPE getNodeType() const { return BV_UNKNOWN; } - - /// @brief compute the AABB for object in local coordinate - virtual void computeLocalAABB() = 0; - - /// @brief get user data in geometry - void* getUserData() const - { - return user_data; - } - - /// @brief set user data in geometry - void setUserData(void *data) - { - user_data = data; - } - - /// @brief whether the object is completely occupied - inline bool isOccupied() const { return cost_density >= threshold_occupied; } - - /// @brief whether the object is completely free - inline bool isFree() const { return cost_density <= threshold_free; } - - /// @brief whether the object has some uncertainty - inline bool isUncertain() const { return !isOccupied() && !isFree(); } - /// @brief AABB center in local coordinate - Vector3d aabb_center; - - /// @brief AABB radius - FCL_REAL aabb_radius; - - /// @brief AABB in local coordinate, used for tight AABB when only translation transform - AABB aabb_local; - - /// @brief pointer to user defined data specific to this object - void *user_data; - - /// @brief collision cost for unit volume - FCL_REAL cost_density; - - /// @brief threshold for occupied ( >= is occupied) - FCL_REAL threshold_occupied; - - /// @brief threshold for free (<= is free) - FCL_REAL threshold_free; - - /// @brief compute center of mass - virtual Vector3d computeCOM() const { return Vector3d::Zero(); } - - /// @brief compute the inertia matrix, related to the origin - virtual Matrix3d computeMomentofInertia() const { return Matrix3d::Zero(); } - - /// @brief compute the volume - virtual FCL_REAL computeVolume() const { return 0; } - - /// @brief compute the inertia matrix, related to the com - virtual Matrix3d computeMomentofInertiaRelatedToCOM() const - { - Matrix3d C = computeMomentofInertia(); - Vector3d com = computeCOM(); - FCL_REAL V = computeVolume(); - - Matrix3d m; - m << C(0, 0) - V * (com[1] * com[1] + com[2] * com[2]), - C(0, 1) + V * com[0] * com[1], - C(0, 2) + V * com[0] * com[2], - C(1, 0) + V * com[1] * com[0], - C(1, 1) - V * (com[0] * com[0] + com[2] * com[2]), - C(1, 2) + V * com[1] * com[2], - C(2, 0) + V * com[2] * com[0], - C(2, 1) + V * com[2] * com[1], - C(2, 2) - V * (com[0] * com[0] + com[1] * com[1]); - - return m; - } - -}; - -/// @brief the object for collision or distance computation, contains the geometry and the transform information +/// @brief the object for collision or distance computation, contains the +/// geometry and the transform information +template class CollisionObject { public: - CollisionObject(const std::shared_ptr &cgeom_) : - cgeom(cgeom_), cgeom_const(cgeom_), t(Transform3d::Identity()) + CollisionObject(const std::shared_ptr> &cgeom_) : + cgeom(cgeom_), cgeom_const(cgeom_), t(Transform3::Identity()) { if (cgeom) { @@ -163,15 +62,15 @@ class CollisionObject } } - CollisionObject(const std::shared_ptr &cgeom_, const Transform3d& tf) : + CollisionObject(const std::shared_ptr> &cgeom_, const Transform3& tf) : cgeom(cgeom_), cgeom_const(cgeom_), t(tf) { cgeom->computeLocalAABB(); computeAABB(); } - CollisionObject(const std::shared_ptr &cgeom_, const Matrix3d& R, const Vector3d& T): - cgeom(cgeom_), cgeom_const(cgeom_), t(Transform3d::Identity()) + CollisionObject(const std::shared_ptr> &cgeom_, const Matrix3& R, const Vector3& T): + cgeom(cgeom_), cgeom_const(cgeom_), t(Transform3::Identity()) { t.linear() = R; t.translation() = T; @@ -196,13 +95,13 @@ class CollisionObject } /// @brief get the AABB in world space - inline const AABB& getAABB() const + const AABB& getAABB() const { return aabb; } /// @brief compute the AABB in world space - inline void computeAABB() + void computeAABB() { if(t.linear().isIdentity()) { @@ -210,8 +109,8 @@ class CollisionObject } else { - Vector3d center = t * cgeom->aabb_center; - Vector3d delta = Vector3d::Constant(cgeom->aabb_radius); + Vector3 center = t * cgeom->aabb_center; + Vector3 delta = Vector3::Constant(cgeom->aabb_radius); aabb.min_ = center - delta; aabb.max_ = center + delta; } @@ -230,63 +129,63 @@ class CollisionObject } /// @brief get translation of the object - inline const Vector3d getTranslation() const + const Vector3 getTranslation() const { return t.translation(); } /// @brief get matrix rotation of the object - inline const Matrix3d getRotation() const + const Matrix3 getRotation() const { return t.linear(); } /// @brief get quaternion rotation of the object - inline const Quaternion3d getQuatRotation() const + const Quaternion getQuatRotation() const { - return Quaternion3d(t.linear()); + return Quaternion(t.linear()); } /// @brief get object's transform - inline const Transform3d& getTransform() const + const Transform3& getTransform() const { return t; } /// @brief set object's rotation matrix - void setRotation(const Matrix3d& R) + void setRotation(const Matrix3& R) { t.linear() = R; } /// @brief set object's translation - void setTranslation(const Vector3d& T) + void setTranslation(const Vector3& T) { t.translation() = T; } /// @brief set object's quatenrion rotation - void setQuatRotation(const Quaternion3d& q) + void setQuatRotation(const Quaternion& q) { t.linear() = q.toRotationMatrix(); } /// @brief set object's transform - void setTransform(const Matrix3d& R, const Vector3d& T) + void setTransform(const Matrix3& R, const Vector3& T) { setRotation(R); setTranslation(T); } /// @brief set object's transform - void setTransform(const Quaternion3d& q, const Vector3d& T) + void setTransform(const Quaternion& q, const Vector3& T) { setQuatRotation(q); setTranslation(T); } /// @brief set object's transform - void setTransform(const Transform3d& tf) + void setTransform(const Transform3& tf) { t = tf; } @@ -294,7 +193,7 @@ class CollisionObject /// @brief whether the object is in local coordinate bool isIdentityTransform() const { - return (t.linear().isIdentity() && t.translation().isZero()); + return t.matrix().isIdentity(); } /// @brief set the object in local coordinate @@ -305,181 +204,68 @@ class CollisionObject /// @brief get geometry from the object instance FCL_DEPRECATED - const CollisionGeometry* getCollisionGeometry() const + const CollisionGeometry* getCollisionGeometry() const { return cgeom.get(); } /// @brief get geometry from the object instance - const std::shared_ptr& collisionGeometry() const + const std::shared_ptr>& collisionGeometry() const { return cgeom_const; } /// @brief get object's cost density - FCL_REAL getCostDensity() const + S getCostDensity() const { return cgeom->cost_density; } /// @brief set object's cost density - void setCostDensity(FCL_REAL c) + void setCostDensity(S c) { cgeom->cost_density = c; } /// @brief whether the object is completely occupied - inline bool isOccupied() const + bool isOccupied() const { return cgeom->isOccupied(); } /// @brief whether the object is completely free - inline bool isFree() const + bool isFree() const { return cgeom->isFree(); } /// @brief whether the object is uncertain - inline bool isUncertain() const + bool isUncertain() const { return cgeom->isUncertain(); } protected: - std::shared_ptr cgeom; - std::shared_ptr cgeom_const; + std::shared_ptr> cgeom; + std::shared_ptr> cgeom_const; - Transform3d t; + Transform3 t; - /// @brief AABB in global coordinate - mutable AABB aabb; + /// @brief AABB in global coordinate + mutable AABB aabb; /// @brief pointer to user defined data specific to this object void *user_data; -}; - -/// @brief the object for continuous collision or distance computation, contains the geometry and the motion information -class ContinuousCollisionObject -{ public: - ContinuousCollisionObject(const std::shared_ptr& cgeom_) : - cgeom(cgeom_), cgeom_const(cgeom_) - { - } - ContinuousCollisionObject(const std::shared_ptr& cgeom_, const std::shared_ptr& motion_) : - cgeom(cgeom_), cgeom_const(cgeom), motion(motion_) - { - } - - ~ContinuousCollisionObject() {} - - /// @brief get the type of the object - OBJECT_TYPE getObjectType() const - { - return cgeom->getObjectType(); - } - - /// @brief get the node type - NODE_TYPE getNodeType() const - { - return cgeom->getNodeType(); - } - - /// @brief get the AABB in the world space for the motion - inline const AABB& getAABB() const - { - return aabb; - } - - /// @brief compute the AABB in the world space for the motion - inline void computeAABB() - { - IVector3 box; - TMatrix3 R; - TVector3 T; - motion->getTaylorModel(R, T); - - Vector3d p = cgeom->aabb_local.min_; - box = (R * p + T).getTightBound(); - - p[2] = cgeom->aabb_local.max_[2]; - box = bound(box, (R * p + T).getTightBound()); - - p[1] = cgeom->aabb_local.max_[1]; - p[2] = cgeom->aabb_local.min_[2]; - box = bound(box, (R * p + T).getTightBound()); - - p[2] = cgeom->aabb_local.max_[2]; - box = bound(box, (R * p + T).getTightBound()); - - p[0] = cgeom->aabb_local.max_[0]; - p[1] = cgeom->aabb_local.min_[1]; - p[2] = cgeom->aabb_local.min_[2]; - box = bound(box, (R * p + T).getTightBound()); - - p[2] = cgeom->aabb_local.max_[2]; - box = bound(box, (R * p + T).getTightBound()); - - p[1] = cgeom->aabb_local.max_[1]; - p[2] = cgeom->aabb_local.min_[2]; - box = bound(box, (R * p + T).getTightBound()); - - p[2] = cgeom->aabb_local.max_[2]; - box = bound(box, (R * p + T).getTightBound()); - - aabb.min_ = box.getLow(); - aabb.max_ = box.getHigh(); - } - - /// @brief get user data in object - void* getUserData() const - { - return user_data; - } - - /// @brief set user data in object - void setUserData(void* data) - { - user_data = data; - } - - /// @brief get motion from the object instance - inline MotionBase* getMotion() const - { - return motion.get(); - } - - /// @brief get geometry from the object instance - FCL_DEPRECATED - inline const CollisionGeometry* getCollisionGeometry() const - { - return cgeom.get(); - } - - /// @brief get geometry from the object instance - inline const std::shared_ptr& collisionGeometry() const - { - return cgeom_const; - } - -protected: - - std::shared_ptr cgeom; - std::shared_ptr cgeom_const; - - std::shared_ptr motion; - - /// @brief AABB in the global coordinate for the motion - mutable AABB aabb; - - /// @brief pointer to user defined data specific to this object - void* user_data; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -} +using CollisionObjectf = CollisionObject; +using CollisionObjectd = CollisionObject; + +} // namespace fcl #endif diff --git a/include/fcl/common/detail/profiler.h b/include/fcl/common/detail/profiler.h new file mode 100644 index 000000000..3a83411c7 --- /dev/null +++ b/include/fcl/common/detail/profiler.h @@ -0,0 +1,608 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2008-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* Author Ioan Sucan */ + +#ifndef FCL_COMMON_DETAIL_PROFILER_H +#define FCL_COMMON_DETAIL_PROFILER_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "fcl/common/time.h" +#include "fcl/common/detail/profiler.h" + +namespace fcl { +namespace detail { + +/// @brief This is a simple thread-safe tool for counting time +/// spent in various chunks of code. This is different from +/// external profiling tools in that it allows the user to count +/// time spent in various bits of code (sub-function granularity) +/// or count how many times certain pieces of code are executed. +class Profiler +{ +public: + // non-copyable + Profiler(const Profiler&) = delete; + Profiler& operator=(const Profiler&) = delete; + + /// @brief This instance will call Profiler::begin() when constructed and + /// Profiler::end() when it goes out of scope. + class ScopedBlock; + + /// @brief This instance will call Profiler::start() when constructed and + /// Profiler::stop() when it goes out of scope. + /// If the profiler was already started, this block's constructor and + /// destructor take no action + class ScopedStart; + + /// @brief Return an instance of the class + static Profiler& Instance(void); + + /// @brief Constructor. It is allowed to separately instantiate this + /// class (not only as a singleton) + Profiler(bool printOnDestroy = false, bool autoStart = false); + + /// @brief Destructor + ~Profiler(void); + + /// @brief Start counting time + static void Start(void); + + /// @brief Stop counting time + static void Stop(void); + + /// @brief Clear counted time and events + static void Clear(void); + + /// @brief Start counting time + void start(void); + + /// @brief Stop counting time + void stop(void); + + /// @brief Clear counted time and events + void clear(void); + + /// @brief Count a specific event for a number of times + static void Event(const std::string& name, const unsigned int times = 1); + + /// @brief Count a specific event for a number of times + void event(const std::string &name, const unsigned int times = 1); + + /// @brief Maintain the average of a specific value + static void Average(const std::string& name, const double value); + + /// @brief Maintain the average of a specific value + void average(const std::string &name, const double value); + + /// @brief Begin counting time for a specific chunk of code + static void Begin(const std::string &name); + + /// @brief Stop counting time for a specific chunk of code + static void End(const std::string &name); + + /// @brief Begin counting time for a specific chunk of code + void begin(const std::string &name); + + /// @brief Stop counting time for a specific chunk of code + void end(const std::string &name); + + /// @brief Print the status of the profiled code chunks and + /// events. Optionally, computation done by different threads + /// can be printed separately. + static void Status(std::ostream &out = std::cout, bool merge = true); + + /// @brief Print the status of the profiled code chunks and + /// events. Optionally, computation done by different threads + /// can be printed separately. + void status(std::ostream &out = std::cout, bool merge = true); + + /// @brief Check if the profiler is counting time or not + bool running(void) const; + + /// @brief Check if the profiler is counting time or not + static bool Running(void); + +private: + + /// @brief Information about time spent in a section of the code + struct TimeInfo + { + TimeInfo(void); + + /// @brief Total time counted. + time::duration total; + + /// @brief The shortest counted time interval + time::duration shortest; + + /// @brief The longest counted time interval + time::duration longest; + + /// @brief Number of times a chunk of time was added to this structure + unsigned long int parts; + + /// @brief The point in time when counting time started + time::point start; + + /// @brief Begin counting time + void set(void); + + /// @brief Add the counted time to the total time + void update(void); + }; + + /// @brief Information maintained about averaged values + struct AvgInfo + { + /// @brief The sum of the values to average + double total; + + /// @brief The sub of squares of the values to average + double totalSqr; + + /// @brief Number of times a value was added to this structure + unsigned long int parts; + }; + + /// @brief Information to be maintained for each thread + struct PerThread + { + /// @brief The stored events + std::map events; + + /// @brief The stored averages + std::map avg; + + /// @brief The amount of time spent in various places + std::map time; + }; + + void printThreadInfo(std::ostream &out, const PerThread &data); + + std::mutex lock_; + std::map data_; + TimeInfo tinfo_; + bool running_; + bool printOnDestroy_; + +}; + +/// @brief This instance will call Profiler::begin() when constructed and +/// Profiler::end() when it goes out of scope. +class Profiler::ScopedBlock +{ +public: + /// @brief Start counting time for the block named \e name of the profiler + /// \e prof + ScopedBlock(const std::string &name, Profiler &prof = Profiler::Instance()); + + ~ScopedBlock(void); + +private: + + std::string name_; + Profiler &prof_; +}; + +/// @brief This instance will call Profiler::start() when constructed and +/// Profiler::stop() when it goes out of scope. +/// If the profiler was already started, this block's constructor and +/// destructor take no action +class Profiler::ScopedStart +{ +public: + + /// @brief Take as argument the profiler instance to operate on (\e prof) + ScopedStart(Profiler &prof = Profiler::Instance()); + + ~ScopedStart(void); + +private: + + Profiler &prof_; + bool wasRunning_; +}; + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +inline Profiler& Profiler::Instance(void) +{ + static Profiler p(true, false); + return p; +} + +//============================================================================== +inline Profiler::Profiler(bool printOnDestroy, bool autoStart) + : running_(false), printOnDestroy_(printOnDestroy) +{ + if (autoStart) + start(); +} + +//============================================================================== +inline Profiler::~Profiler() +{ + if (printOnDestroy_ && !data_.empty()) + status(); +} + +//============================================================================== +inline void Profiler::Start() +{ + Instance().start(); +} + +//============================================================================== +inline void Profiler::Stop() +{ + Instance().stop(); +} + +//============================================================================== +inline void Profiler::Clear() +{ + Instance().clear(); +} + +//============================================================================== +inline void Profiler::start(void) +{ + lock_.lock(); + if (!running_) + { + tinfo_.set(); + running_ = true; + } + lock_.unlock(); +} + +//============================================================================== +inline void Profiler::stop(void) +{ + lock_.lock(); + if (running_) + { + tinfo_.update(); + running_ = false; + } + lock_.unlock(); +} + +//============================================================================== +inline void Profiler::clear(void) +{ + lock_.lock(); + data_.clear(); + tinfo_ = TimeInfo(); + if (running_) + tinfo_.set(); + lock_.unlock(); +} + +//============================================================================== +inline void Profiler::Event(const std::string& name, const unsigned int times) +{ + Instance().event(name, times); +} + +//============================================================================== +inline void Profiler::event(const std::string &name, const unsigned int times) +{ + lock_.lock(); + data_[std::this_thread::get_id()].events[name] += times; + lock_.unlock(); +} + +//============================================================================== +inline void Profiler::Average(const std::string& name, const double value) +{ + Instance().average(name, value); +} + +//============================================================================== +inline void Profiler::average(const std::string &name, const double value) +{ + lock_.lock(); + AvgInfo &a = data_[std::this_thread::get_id()].avg[name]; + a.total += value; + a.totalSqr += value*value; + a.parts++; + lock_.unlock(); +} + +//============================================================================== +inline void Profiler::Begin(const std::string& name) +{ + Instance().begin(name); +} + +//============================================================================== +inline void Profiler::End(const std::string& name) +{ + Instance().end(name); +} + +//============================================================================== +inline void Profiler::begin(const std::string &name) +{ + lock_.lock(); + data_[std::this_thread::get_id()].time[name].set(); + lock_.unlock(); +} + +//============================================================================== +inline void Profiler::end(const std::string &name) +{ + lock_.lock(); + data_[std::this_thread::get_id()].time[name].update(); + lock_.unlock(); +} + +//============================================================================== +inline void Profiler::Status(std::ostream& out, bool merge) +{ + Instance().status(out, merge); +} + +//============================================================================== +inline void Profiler::status(std::ostream &out, bool merge) +{ + stop(); + lock_.lock(); + printOnDestroy_ = false; + + out << std::endl; + out << " *** Profiling statistics. Total counted time : " << time::seconds(tinfo_.total) << " seconds" << std::endl; + + if (merge) + { + PerThread combined; + for (std::map::const_iterator it = data_.begin() ; it != data_.end() ; ++it) + { + for (std::map::const_iterator iev = it->second.events.begin() ; iev != it->second.events.end(); ++iev) + combined.events[iev->first] += iev->second; + for (std::map::const_iterator iavg = it->second.avg.begin() ; iavg != it->second.avg.end(); ++iavg) + { + combined.avg[iavg->first].total += iavg->second.total; + combined.avg[iavg->first].totalSqr += iavg->second.totalSqr; + combined.avg[iavg->first].parts += iavg->second.parts; + } + for (std::map::const_iterator itm = it->second.time.begin() ; itm != it->second.time.end(); ++itm) + { + TimeInfo &tc = combined.time[itm->first]; + tc.total = tc.total + itm->second.total; + tc.parts = tc.parts + itm->second.parts; + if (tc.shortest > itm->second.shortest) + tc.shortest = itm->second.shortest; + if (tc.longest < itm->second.longest) + tc.longest = itm->second.longest; + } + } + printThreadInfo(out, combined); + } + else + for (std::map::const_iterator it = data_.begin() ; it != data_.end() ; ++it) + { + out << "Thread " << it->first << ":" << std::endl; + printThreadInfo(out, it->second); + } + lock_.unlock(); +} + +//============================================================================== +inline bool Profiler::running() const +{ + return running_; +} + +//============================================================================== +inline bool Profiler::Running() +{ + return Instance().running(); +} + +struct dataIntVal +{ + std::string name; + unsigned long int value; +}; + +struct SortIntByValue +{ + bool operator()(const dataIntVal &a, const dataIntVal &b) const + { + return a.value > b.value; + } +}; + +struct dataDoubleVal +{ + std::string name; + double value; +}; + +struct SortDoubleByValue +{ + bool operator()(const dataDoubleVal &a, const dataDoubleVal &b) const + { + return a.value > b.value; + } +}; + +//============================================================================== +inline void Profiler::printThreadInfo(std::ostream &out, const PerThread &data) +{ + double total = time::seconds(tinfo_.total); + + std::vector events; + for (std::map::const_iterator iev = data.events.begin() ; iev != data.events.end() ; ++iev) + { + detail::dataIntVal next = {iev->first, iev->second}; + events.push_back(next); + } + std::sort(events.begin(), events.end(), SortIntByValue()); + if (!events.empty()) + out << "Events:" << std::endl; + for (unsigned int i = 0 ; i < events.size() ; ++i) + out << events[i].name << ": " << events[i].value << std::endl; + + std::vector avg; + for (std::map::const_iterator ia = data.avg.begin() ; ia != data.avg.end() ; ++ia) + { + detail::dataDoubleVal next = {ia->first, ia->second.total / (double)ia->second.parts}; + avg.push_back(next); + } + std::sort(avg.begin(), avg.end(), SortDoubleByValue()); + if (!avg.empty()) + out << "Averages:" << std::endl; + for (unsigned int i = 0 ; i < avg.size() ; ++i) + { + const AvgInfo &a = data.avg.find(avg[i].name)->second; + out << avg[i].name << ": " << avg[i].value << " (stddev = " << + std::sqrt(std::abs(a.totalSqr - (double)a.parts * avg[i].value * avg[i].value) / ((double)a.parts - 1.)) << ")" << std::endl; + } + + std::vector time; + + for (std::map::const_iterator itm = data.time.begin() ; itm != data.time.end() ; ++itm) + { + detail::dataDoubleVal next = {itm->first, time::seconds(itm->second.total)}; + time.push_back(next); + } + + std::sort(time.begin(), time.end(), detail::SortDoubleByValue()); + if (!time.empty()) + out << "Blocks of time:" << std::endl; + + double unaccounted = total; + for (unsigned int i = 0 ; i < time.size() ; ++i) + { + const TimeInfo &d = data.time.find(time[i].name)->second; + + double tS = time::seconds(d.shortest); + double tL = time::seconds(d.longest); + out << time[i].name << ": " << time[i].value << "s (" << (100.0 * time[i].value/total) << "%), [" + << tS << "s --> " << tL << " s], " << d.parts << " parts"; + if (d.parts > 0) + out << ", " << (time::seconds(d.total) / (double)d.parts) << " s on average"; + out << std::endl; + unaccounted -= time[i].value; + } + out << "Unaccounted time : " << unaccounted; + if (total > 0.0) + out << " (" << (100.0 * unaccounted / total) << " %)"; + out << std::endl; + + out << std::endl; +} + +//============================================================================== +inline Profiler::TimeInfo::TimeInfo() + : total(time::seconds(0.)), + shortest(time::duration::max()), + longest(time::duration::min()), + parts(0) +{ + // Do nothing +} + +//============================================================================== +inline void Profiler::TimeInfo::set() +{ + start = time::now(); +} + +//============================================================================== +inline void Profiler::TimeInfo::update() +{ + const time::duration &dt = time::now() - start; + + if (dt > longest) + longest = dt; + + if (dt < shortest) + shortest = dt; + + total = total + dt; + ++parts; +} + +//============================================================================== +inline Profiler::ScopedStart::ScopedStart(Profiler& prof) + : prof_(prof), wasRunning_(prof_.running()) +{ + if (!wasRunning_) + prof_.start(); +} + +//============================================================================== +inline Profiler::ScopedStart::~ScopedStart() +{ + if (!wasRunning_) + prof_.stop(); +} + +//============================================================================== +inline Profiler::ScopedBlock::ScopedBlock(const std::string& name, Profiler& prof) + : name_(name), prof_(prof) +{ + prof_.begin(name); +} + +//============================================================================== +inline Profiler::ScopedBlock::~ScopedBlock() +{ + prof_.end(name_); +} + +} // namespace tools +} // namespace fcl + +#endif // #ifndef FCL_COMMON_DETAIL_PROFILER_H diff --git a/include/fcl/common/profiler.h b/include/fcl/common/profiler.h new file mode 100644 index 000000000..ff151bc4c --- /dev/null +++ b/include/fcl/common/profiler.h @@ -0,0 +1,63 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2008-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* Author Jeongseok Lee */ + +#ifndef FCL_COMMON_PROFILER_H +#define FCL_COMMON_PROFILER_H + +#include "fcl/config.h" + +#if FCL_ENABLE_PROFILING + + #define FCL_PROFILE_START ::fcl::detail::Profiler::Start(); + #define FCL_PROFILE_STOP ::fcl::detail::Profiler::Stop(); + #define FCL_PROFILE_BLOCK_BEGIN(name) ::fcl::detail::Profiler::Begin(name); + #define FCL_PROFILE_BLOCK_END(name) ::fcl::detail::Profiler::End(name); + #define FCL_PROFILE_STATUS(stream) ::fcl::detail::Profiler::Status(stream); + +#else + + #define FCL_PROFILE_START + #define FCL_PROFILE_STOP + #define FCL_PROFILE_BLOCK_BEGIN(name) + #define FCL_PROFILE_BLOCK_END(name) + #define FCL_PROFILE_STATUS(stream) + +#endif // #if FCL_ENABLE_PROFILING + +#include "fcl/common/detail/profiler.h" + +#endif // #ifndef FCL_COMMON_PROFILER_H diff --git a/include/fcl/common/time.h b/include/fcl/common/time.h new file mode 100644 index 000000000..5aa2e5cbe --- /dev/null +++ b/include/fcl/common/time.h @@ -0,0 +1,99 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2008-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* Author Ioan Sucan */ + +#ifndef FCL_COMMON_TIME_H +#define FCL_COMMON_TIME_H + +#include + +namespace fcl +{ + +/// @brief Namespace containing time datatypes and time operations +namespace time +{ + +/// @brief Representation of a point in time +using point = std::chrono::system_clock::time_point; + +/// @brief Representation of a time duration +using duration = std::chrono::system_clock::duration; + +/// @brief Get the current time point +point now(void); + +/// @brief Return the time duration representing a given number of seconds +duration seconds(double sec); + +/// @brief Return the number of seconds that a time duration represents +double seconds(const duration &d); + +} // namespace time + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +namespace time +{ + +//============================================================================== +inline point now(void) +{ + return std::chrono::system_clock::now(); +} + +//============================================================================== +inline duration seconds(double sec) +{ + long s = (long)sec; + long us = (long)((sec - s) * 1000000); + return std::chrono::seconds(s) + std::chrono::microseconds(us); +} + +//============================================================================== +inline double seconds(const duration &d) +{ + return std::chrono::duration(d).count(); +} + +} // namespace time +} // namespace fcl + +#endif diff --git a/include/fcl/common/warning.h b/include/fcl/common/warning.h new file mode 100644 index 000000000..ae3cf508d --- /dev/null +++ b/include/fcl/common/warning.h @@ -0,0 +1,120 @@ +/* + * Software License Agreement (BSD License) + * + * 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 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 Jeongseok Lee + +#ifndef FCL_COMMON_WARNING_H +#define FCL_COMMON_WARNING_H + +#include "fcl/config.h" + +// We define two convenient macros that can be used to suppress +// deprecated-warnings for a specific code block rather than a whole project. +// This macros would be useful when you need to call deprecated function for +// some reason (e.g., for backward compatibility) but don't want warnings. +// +// Example code: +// +// deprecated_function() // warning +// +// FCL_SUPPRESS_DEPRECATED_BEGIN +// deprecated_function() // okay, no warning +// FCL_SUPPRESS_DEPRECATED_END +// +#if defined (FCL_COMPILER_GCC) + + #define FCL_SUPPRESS_DEPRECATED_BEGIN \ + _Pragma("GCC diagnostic push") \ + _Pragma("GCC diagnostic ignored \"-Wdeprecated-declarations\"") + + #define FCL_SUPPRESS_DEPRECATED_END \ + _Pragma("GCC diagnostic pop") + + #define FCL_SUPPRESS_UNINITIALIZED_BEGIN \ + _Pragma("GCC diagnostic push") \ + _Pragma("GCC diagnostic ignored \"-Wuninitialized\"") + + #define FCL_SUPPRESS_UNINITIALIZED_END \ + _Pragma("GCC diagnostic pop") + + #define FCL_SUPPRESS_MAYBE_UNINITIALIZED_BEGIN \ + _Pragma("GCC diagnostic push") \ + _Pragma("GCC diagnostic ignored \"-Wmaybe-uninitialized\"") + + #define FCL_SUPPRESS_MAYBE_UNINITIALIZED_END \ + _Pragma("GCC diagnostic pop") + +#elif defined (FCL_COMPILER_CLANG) + + #define FCL_SUPPRESS_DEPRECATED_BEGIN \ + _Pragma("clang diagnostic push") \ + _Pragma("clang diagnostic ignored \"-Wdeprecated-declarations\"") + + #define FCL_SUPPRESS_DEPRECATED_END \ + _Pragma("clang diagnostic pop") + + #define FCL_SUPPRESS_UNINITIALIZED_BEGIN \ + _Pragma("clang diagnostic push") \ + _Pragma("clang diagnostic ignored \"-Wuninitialized\"") + + #define FCL_SUPPRESS_UNINITIALIZED_END \ + _Pragma("clang diagnostic pop") + + #define FCL_SUPPRESS_MAYBE_UNINITIALIZED_BEGIN \ + _Pragma("clang diagnostic push") \ + _Pragma("clang diagnostic ignored \"-Wmaybe-uninitialized\"") + + #define FCL_SUPPRESS_MAYBE_UNINITIALIZED_END \ + _Pragma("clang diagnostic pop") + +#elif defined (FCL_COMPILER_MSVC) + + #define FCL_SUPPRESS_DEPRECATED_BEGIN \ + __pragma(warning(push)) \ + __pragma(warning(disable:4996)) + + #define FCL_SUPPRESS_DEPRECATED_END \ + __pragma(warning(pop)) + + #define FCL_SUPPRESS_UNINITIALIZED_BEGIN // TODO + + #define FCL_SUPPRESS_UNINITIALIZED_END // TODO + + #define FCL_SUPPRESS_MAYBE_UNINITIALIZED_BEGIN // TODO + + #define FCL_SUPPRESS_MAYBE_UNINITIALIZED_END // TODO + +#endif + +#endif // FCL_COMMON_WARNING_H diff --git a/include/fcl/config.h.in b/include/fcl/config.h.in index 3c53952ac..687902de9 100644 --- a/include/fcl/config.h.in +++ b/include/fcl/config.h.in @@ -33,8 +33,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef FCL_CONFIG_ -#define FCL_CONFIG_ +#ifndef FCL_CONFIG_H +#define FCL_CONFIG_H #define FCL_VERSION "@FCL_VERSION@" #define FCL_MAJOR_VERSION @FCL_MAJOR_VERSION@ @@ -45,8 +45,16 @@ #cmakedefine01 FCL_HAVE_OCTOMAP #cmakedefine01 FCL_HAVE_TINYXML -#cmakedefine01 FCL_BUILD_TYPE_DEBUG -#cmakedefine01 FCL_BUILD_TYPE_RELEASE +#cmakedefine01 FCL_ENABLE_PROFILING + +// Detect the compiler +#if defined(__clang__) + #define FCL_COMPILER_CLANG +#elif defined(__GNUC__) || defined(__GNUG__) + #define FCL_COMPILER_GCC +#elif defined(_MSC_VER) + #define FCL_COMPILER_MSVC +#endif #if FCL_HAVE_OCTOMAP #define OCTOMAP_MAJOR_VERSION @OCTOMAP_MAJOR_VERSION@ diff --git a/include/fcl/continuous_collision.h b/include/fcl/continuous_collision.h index 3514b1e38..ac4bb087c 100644 --- a/include/fcl/continuous_collision.h +++ b/include/fcl/continuous_collision.h @@ -38,27 +38,412 @@ #ifndef FCL_CONTINUOUS_COLLISION_H #define FCL_CONTINUOUS_COLLISION_H +#include #include "fcl/collision_object.h" +#include "fcl/continuous_collision_object.h" #include "fcl/collision_data.h" +#include "fcl/ccd/conservative_advancement.h" +#include "fcl/ccd/motion.h" +#include "fcl/BVH/BVH_model.h" namespace fcl { /// @brief continous collision checking between two objects -FCL_REAL continuousCollide(const CollisionGeometry* o1, const Transform3d& tf1_beg, const Transform3d& tf1_end, - const CollisionGeometry* o2, const Transform3d& tf2_beg, const Transform3d& tf2_end, - const ContinuousCollisionRequest& request, - ContinuousCollisionResult& result); - -FCL_REAL continuousCollide(const CollisionObject* o1, const Transform3d& tf1_end, - const CollisionObject* o2, const Transform3d& tf2_end, - const ContinuousCollisionRequest& request, - ContinuousCollisionResult& result); - -FCL_REAL collide(const ContinuousCollisionObject* o1, const ContinuousCollisionObject* o2, - const ContinuousCollisionRequest& request, - ContinuousCollisionResult& result); - +template +S continuousCollide( + const CollisionGeometry* o1, + const Transform3& tf1_beg, + const Transform3& tf1_end, + const CollisionGeometry* o2, + const Transform3& tf2_beg, + const Transform3& tf2_end, + const ContinuousCollisionRequest& request, + ContinuousCollisionResult& result); + +template +S continuousCollide( + const CollisionObject* o1, + const Transform3& tf1_end, + const CollisionObject* o2, + const Transform3& tf2_end, + const ContinuousCollisionRequest& request, + ContinuousCollisionResult& result); + +template +S collide( + const ContinuousCollisionObject* o1, + const ContinuousCollisionObject* o2, + const ContinuousCollisionRequest& request, + ContinuousCollisionResult& result); + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +ConservativeAdvancementFunctionMatrix& getConservativeAdvancementFunctionLookTable() +{ + static ConservativeAdvancementFunctionMatrix table; + return table; +} + +template +MotionBasePtr getMotionBase( + const Transform3& tf_beg, + const Transform3& tf_end, + CCDMotionType motion_type) +{ + switch(motion_type) + { + case CCDM_TRANS: + return MotionBasePtr(new TranslationMotion(tf_beg, tf_end)); + break; + case CCDM_LINEAR: + return MotionBasePtr(new InterpMotion(tf_beg, tf_end)); + break; + case CCDM_SCREW: + return MotionBasePtr(new ScrewMotion(tf_beg, tf_end)); + break; + case CCDM_SPLINE: + return MotionBasePtr(new SplineMotion(tf_beg, tf_end)); + break; + default: + return MotionBasePtr(); + } +} + +template +S continuousCollideNaive( + const CollisionGeometry* o1, + const MotionBased* motion1, + const CollisionGeometry* o2, + const MotionBased* motion2, + const ContinuousCollisionRequest& request, + ContinuousCollisionResult& result) +{ + std::size_t n_iter = std::min(request.num_max_iterations, (std::size_t)ceil(1 / request.toc_err)); + Transform3 cur_tf1, cur_tf2; + for(std::size_t i = 0; i < n_iter; ++i) + { + S t = i / (S) (n_iter - 1); + motion1->integrate(t); + motion2->integrate(t); + + motion1->getCurrentTransform(cur_tf1); + motion2->getCurrentTransform(cur_tf2); + + CollisionRequest c_request; + CollisionResult c_result; + + if(collide(o1, cur_tf1, o2, cur_tf2, c_request, c_result)) + { + result.is_collide = true; + result.time_of_contact = t; + result.contact_tf1 = cur_tf1; + result.contact_tf2 = cur_tf2; + return t; + } + } + + result.is_collide = false; + result.time_of_contact = S(1); + return result.time_of_contact; +} + +namespace details +{ +template +typename BV::S continuousCollideBVHPolynomial( + const CollisionGeometry* o1_, + const TranslationMotion* motion1, + const CollisionGeometry* o2_, + const TranslationMotion* motion2, + const ContinuousCollisionRequest& request, + ContinuousCollisionResult& result) +{ + using S = typename BV::S; + + const BVHModel* o1__ = static_cast*>(o1_); + const BVHModel* o2__ = static_cast*>(o2_); + + // ugly, but lets do it now. + BVHModel* o1 = const_cast*>(o1__); + BVHModel* o2 = const_cast*>(o2__); + std::vector> new_v1(o1->num_vertices); + std::vector> new_v2(o2->num_vertices); + + for(std::size_t i = 0; i < new_v1.size(); ++i) + new_v1[i] = o1->vertices[i] + motion1->getVelocity(); + for(std::size_t i = 0; i < new_v2.size(); ++i) + new_v2[i] = o2->vertices[i] + motion2->getVelocity(); + + o1->beginUpdateModel(); + o1->updateSubModel(new_v1); + o1->endUpdateModel(true, true); + + o2->beginUpdateModel(); + o2->updateSubModel(new_v2); + o2->endUpdateModel(true, true); + + MeshContinuousCollisionTraversalNode node; + CollisionRequest c_request; + + motion1->integrate(0); + motion2->integrate(0); + Transform3 tf1, tf2; + motion1->getCurrentTransform(tf1); + motion2->getCurrentTransform(tf2); + if(!initialize(node, *o1, tf1, *o2, tf2, c_request)) + return -1.0; + + collide(&node); + + result.is_collide = (node.pairs.size() > 0); + result.time_of_contact = node.time_of_contact; + + if(result.is_collide) + { + motion1->integrate(node.time_of_contact); + motion2->integrate(node.time_of_contact); + motion1->getCurrentTransform(tf1); + motion2->getCurrentTransform(tf2); + result.contact_tf1 = tf1; + result.contact_tf2 = tf2; + } + + return result.time_of_contact; +} + +} // namespace details + +template +S continuousCollideBVHPolynomial( + const CollisionGeometry* o1, + const TranslationMotion* motion1, + const CollisionGeometry* o2, + const TranslationMotion* motion2, + const ContinuousCollisionRequest& request, + ContinuousCollisionResult& result) +{ + switch(o1->getNodeType()) + { + case BV_AABB: + if(o2->getNodeType() == BV_AABB) + return details::continuousCollideBVHPolynomial>(o1, motion1, o2, motion2, request, result); + break; + case BV_OBB: + if(o2->getNodeType() == BV_OBB) + return details::continuousCollideBVHPolynomial>(o1, motion1, o2, motion2, request, result); + break; + case BV_RSS: + if(o2->getNodeType() == BV_RSS) + return details::continuousCollideBVHPolynomial>(o1, motion1, o2, motion2, request, result); + break; + case BV_kIOS: + if(o2->getNodeType() == BV_kIOS) + return details::continuousCollideBVHPolynomial>(o1, motion1, o2, motion2, request, result); + break; + case BV_OBBRSS: + if(o2->getNodeType() == BV_OBBRSS) + return details::continuousCollideBVHPolynomial>(o1, motion1, o2, motion2, request, result); + break; + case BV_KDOP16: + if(o2->getNodeType() == BV_KDOP16) + return details::continuousCollideBVHPolynomial >(o1, motion1, o2, motion2, request, result); + break; + case BV_KDOP18: + if(o2->getNodeType() == BV_KDOP18) + return details::continuousCollideBVHPolynomial >(o1, motion1, o2, motion2, request, result); + break; + case BV_KDOP24: + if(o2->getNodeType() == BV_KDOP24) + return details::continuousCollideBVHPolynomial >(o1, motion1, o2, motion2, request, result); + break; + default: + ; + } + + std::cerr << "Warning: BV type not supported by polynomial solver CCD" << std::endl; + + return -1; +} + +namespace details +{ + +template +typename NarrowPhaseSolver::S continuousCollideConservativeAdvancement( + const CollisionGeometry* o1, + const MotionBased* motion1, + const CollisionGeometry* o2, + const MotionBased* motion2, + const NarrowPhaseSolver* nsolver_, + const ContinuousCollisionRequest& request, + ContinuousCollisionResult& result) +{ + using S = typename NarrowPhaseSolver::S; + + const NarrowPhaseSolver* nsolver = nsolver_; + if(!nsolver_) + nsolver = new NarrowPhaseSolver(); + + const auto& looktable = getConservativeAdvancementFunctionLookTable(); + + NODE_TYPE node_type1 = o1->getNodeType(); + NODE_TYPE node_type2 = o2->getNodeType(); + + S res = -1; + + if(!looktable.conservative_advancement_matrix[node_type1][node_type2]) + { + std::cerr << "Warning: collision function between node type " << node_type1 << " and node type " << node_type2 << " is not supported"<< std::endl; + } + else + { + res = looktable.conservative_advancement_matrix[node_type1][node_type2](o1, motion1, o2, motion2, nsolver, request, result); + } + + if(!nsolver_) + delete nsolver; + + if(result.is_collide) + { + motion1->integrate(result.time_of_contact); + motion2->integrate(result.time_of_contact); + + Transform3 tf1, tf2; + motion1->getCurrentTransform(tf1); + motion2->getCurrentTransform(tf2); + result.contact_tf1 = tf1; + result.contact_tf2 = tf2; + } + + return res; } +} // namespace details + +template +S continuousCollideConservativeAdvancement( + const CollisionGeometry* o1, + const MotionBased* motion1, + const CollisionGeometry* o2, + const MotionBased* motion2, + const ContinuousCollisionRequest& request, + ContinuousCollisionResult& result) +{ + switch(request.gjk_solver_type) + { + case GST_LIBCCD: + { + GJKSolver_libccd solver; + return details::continuousCollideConservativeAdvancement(o1, motion1, o2, motion2, &solver, request, result); + } + case GST_INDEP: + { + GJKSolver_indep solver; + return details::continuousCollideConservativeAdvancement(o1, motion1, o2, motion2, &solver, request, result); + } + default: + return -1; + } +} + +template +S continuousCollide( + const CollisionGeometry* o1, + const MotionBased* motion1, + const CollisionGeometry* o2, + const MotionBased* motion2, + const ContinuousCollisionRequest& request, + ContinuousCollisionResult& result) +{ + switch(request.ccd_solver_type) + { + case CCDC_NAIVE: + return continuousCollideNaive(o1, motion1, + o2, motion2, + request, + result); + break; + case CCDC_CONSERVATIVE_ADVANCEMENT: + return continuousCollideConservativeAdvancement(o1, motion1, + o2, motion2, + request, + result); + break; + case CCDC_RAY_SHOOTING: + if(o1->getObjectType() == OT_GEOM && o2->getObjectType() == OT_GEOM && request.ccd_motion_type == CCDM_TRANS) + { + + } + else + std::cerr << "Warning! Invalid continuous collision setting" << std::endl; + break; + case CCDC_POLYNOMIAL_SOLVER: + if(o1->getObjectType() == OT_BVH && o2->getObjectType() == OT_BVH && request.ccd_motion_type == CCDM_TRANS) + { + return continuousCollideBVHPolynomial(o1, (const TranslationMotion*)motion1, + o2, (const TranslationMotion*)motion2, + request, result); + } + else + std::cerr << "Warning! Invalid continuous collision checking" << std::endl; + break; + default: + std::cerr << "Warning! Invalid continuous collision setting" << std::endl; + } + + return -1; +} + +template +S continuousCollide( + const CollisionGeometry* o1, + const Transform3& tf1_beg, + const Transform3& tf1_end, + const CollisionGeometry* o2, + const Transform3& tf2_beg, + const Transform3& tf2_end, + const ContinuousCollisionRequest& request, + ContinuousCollisionResult& result) +{ + MotionBasePtr motion1 = getMotionBase(tf1_beg, tf1_end, request.ccd_motion_type); + MotionBasePtr motion2 = getMotionBase(tf2_beg, tf2_end, request.ccd_motion_type); + + return continuousCollide(o1, motion1.get(), o2, motion2.get(), request, result); +} + +template +S continuousCollide( + const CollisionObject* o1, + const Transform3& tf1_end, + const CollisionObject* o2, + const Transform3& tf2_end, + const ContinuousCollisionRequest& request, + ContinuousCollisionResult& result) +{ + return continuousCollide(o1->collisionGeometry().get(), o1->getTransform(), tf1_end, + o2->collisionGeometry().get(), o2->getTransform(), tf2_end, + request, result); +} + +template +S collide( + const ContinuousCollisionObject* o1, + const ContinuousCollisionObject* o2, + const ContinuousCollisionRequest& request, + ContinuousCollisionResult& result) +{ + return continuousCollide(o1->collisionGeometry().get(), o1->getMotion(), + o2->collisionGeometry().get(), o2->getMotion(), + request, result); +} + +} // namespace fcl + #endif diff --git a/include/fcl/continuous_collision_object.h b/include/fcl/continuous_collision_object.h new file mode 100644 index 000000000..49202e326 --- /dev/null +++ b/include/fcl/continuous_collision_object.h @@ -0,0 +1,174 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_CONTINUOUS_COLLISION_OBJECT_H +#define FCL_CONTINUOUS_COLLISION_OBJECT_H + +#include + +#include "fcl/collision_geometry.h" + +namespace fcl +{ + +/// @brief the object for continuous collision or distance computation, contains +/// the geometry and the motion information +template +class ContinuousCollisionObject +{ +public: + ContinuousCollisionObject(const std::shared_ptr>& cgeom_) : + cgeom(cgeom_), cgeom_const(cgeom_) + { + } + + ContinuousCollisionObject(const std::shared_ptr>& cgeom_, const std::shared_ptr& motion_) : + cgeom(cgeom_), cgeom_const(cgeom), motion(motion_) + { + } + + ~ContinuousCollisionObject() {} + + /// @brief get the type of the object + OBJECT_TYPE getObjectType() const + { + return cgeom->getObjectType(); + } + + /// @brief get the node type + NODE_TYPE getNodeType() const + { + return cgeom->getNodeType(); + } + + /// @brief get the AABB in the world space for the motion + const AABB& getAABB() const + { + return aabb; + } + + /// @brief compute the AABB in the world space for the motion + void computeAABB() + { + IVector3 box; + TMatrix3 R; + TVector3 T; + motion->getTaylorModel(R, T); + + Vector3 p = cgeom->aabb_local.min_; + box = (R * p + T).getTightBound(); + + p[2] = cgeom->aabb_local.max_[2]; + box = bound(box, (R * p + T).getTightBound()); + + p[1] = cgeom->aabb_local.max_[1]; + p[2] = cgeom->aabb_local.min_[2]; + box = bound(box, (R * p + T).getTightBound()); + + p[2] = cgeom->aabb_local.max_[2]; + box = bound(box, (R * p + T).getTightBound()); + + p[0] = cgeom->aabb_local.max_[0]; + p[1] = cgeom->aabb_local.min_[1]; + p[2] = cgeom->aabb_local.min_[2]; + box = bound(box, (R * p + T).getTightBound()); + + p[2] = cgeom->aabb_local.max_[2]; + box = bound(box, (R * p + T).getTightBound()); + + p[1] = cgeom->aabb_local.max_[1]; + p[2] = cgeom->aabb_local.min_[2]; + box = bound(box, (R * p + T).getTightBound()); + + p[2] = cgeom->aabb_local.max_[2]; + box = bound(box, (R * p + T).getTightBound()); + + aabb.min_ = box.getLow(); + aabb.max_ = box.getHigh(); + } + + /// @brief get user data in object + void* getUserData() const + { + return user_data; + } + + /// @brief set user data in object + void setUserData(void* data) + { + user_data = data; + } + + /// @brief get motion from the object instance + MotionBased* getMotion() const + { + return motion.get(); + } + + /// @brief get geometry from the object instance + FCL_DEPRECATED + const CollisionGeometry* getCollisionGeometry() const + { + return cgeom.get(); + } + + /// @brief get geometry from the object instance + const std::shared_ptr>& collisionGeometry() const + { + return cgeom_const; + } + +protected: + + std::shared_ptr> cgeom; + std::shared_ptr> cgeom_const; + + std::shared_ptr motion; + + /// @brief AABB in the global coordinate for the motion + mutable AABB aabb; + + /// @brief pointer to user defined data specific to this object + void* user_data; +}; + +using ContinuousCollisionObjectf = ContinuousCollisionObject; +using ContinuousCollisionObjectd = ContinuousCollisionObject; + +} // namespace fcl + +#endif diff --git a/include/fcl/data_types.h b/include/fcl/data_types.h index b624feedd..1ff1db6ed 100644 --- a/include/fcl/data_types.h +++ b/include/fcl/data_types.h @@ -40,7 +40,11 @@ #include #include +#include +#include +#include #include +#include #include "fcl/deprecated.h" namespace fcl @@ -59,23 +63,41 @@ using uint64 = std::uint64_t; using int32 = std::int32_t; using uint32 = std::uint32_t; -template -using Vector3 = Eigen::Matrix; +template +using Vector2 = Eigen::Matrix; -template -using VectorN = Eigen::Matrix; +template +using Vector3 = Eigen::Matrix; -template -using VectorX = Eigen::Matrix; +template +using Vector6 = Eigen::Matrix; -template -using Matrix3 = Eigen::Matrix; +template +using Vector7 = Eigen::Matrix; -template -using Quaternion3 = Eigen::Quaternion; +template +using VectorN = Eigen::Matrix; -template -using Isometry3 = Eigen::Transform; +template +using VectorX = Eigen::Matrix; + +template +using Matrix3 = Eigen::Matrix; + +template +using Quaternion = Eigen::Quaternion; + +template +using Transform3 = Eigen::Transform; + +template +using Isometry3 = Eigen::Transform; + +template +using Translation3 = Eigen::Translation; + +template +using AngleAxis = Eigen::AngleAxis; // float types using Vector3f = Vector3; @@ -83,9 +105,11 @@ template using VectorNf = VectorN; using VectorXf = VectorX; using Matrix3f = Matrix3; -using Quaternion3f = Quaternion3; +using Quaternionf = Quaternion; using Isometry3f = Isometry3; using Transform3f = Isometry3f; +using Translation3f = Translation3; +using AngleAxisf = AngleAxis; // double types using Vector3d = Vector3; @@ -93,10 +117,91 @@ template using VectorNd = VectorN; using VectorXd = VectorX; using Matrix3d = Matrix3; -using Quaternion3d = Quaternion3; +using Quaterniond = Quaternion; using Isometry3d = Isometry3; using Transform3d = Isometry3d; +using Translation3d = Translation3; +using AngleAxisd = AngleAxis; } // namespace fcl +namespace Eigen { + +template +using aligned_vector = std::vector<_Tp, Eigen::aligned_allocator<_Tp>>; + +template > +using aligned_map = std::map<_Key, _Tp, _Compare, + Eigen::aligned_allocator>>; + +#if EIGEN_VERSION_AT_LEAST(3,2,1) + +/// Aligned allocator that is compatible with c++11 +// Ref: https://bitbucket.org/eigen/eigen/commits/f5b7700 +// TODO: Remove this and use Eigen::aligned_allocator once new version of Eigen +// is released with above commit. +template +class aligned_allocator_cpp11 : public std::allocator +{ +public: + typedef std::size_t size_type; + typedef std::ptrdiff_t difference_type; + typedef T* pointer; + typedef const T* const_pointer; + typedef T& reference; + typedef const T& const_reference; + typedef T value_type; + + template + struct rebind + { + typedef aligned_allocator_cpp11 other; + }; + + aligned_allocator_cpp11() + : std::allocator() {} + + aligned_allocator_cpp11(const aligned_allocator_cpp11& other) + : std::allocator(other) {} + + template + aligned_allocator_cpp11(const aligned_allocator_cpp11& other) + : std::allocator(other) {} + + ~aligned_allocator_cpp11() {} + + pointer allocate(size_type num, const void* /*hint*/ = 0) + { + internal::check_size_for_overflow(num); + return static_cast( internal::aligned_malloc(num * sizeof(T)) ); + } + + void deallocate(pointer p, size_type /*num*/) + { + internal::aligned_free(p); + } +}; + +template +inline std::shared_ptr<_Tp> make_aligned_shared(_Args&&... __args) +{ + typedef typename std::remove_const<_Tp>::type _Tp_nc; + return std::allocate_shared<_Tp>(Eigen::aligned_allocator_cpp11<_Tp_nc>(), + std::forward<_Args>(__args)...); +} + +#else + +template +inline std::shared_ptr<_Tp> make_aligned_shared(_Args&&... __args) +{ + typedef typename std::remove_const<_Tp>::type _Tp_nc; + return std::allocate_shared<_Tp>(Eigen::aligned_allocator<_Tp_nc>(), + std::forward<_Args>(__args)...); +} + +#endif + +} // namespace Eigen + #endif diff --git a/include/fcl/distance.h b/include/fcl/distance.h index 036606e0d..25b2ddf68 100644 --- a/include/fcl/distance.h +++ b/include/fcl/distance.h @@ -40,20 +40,161 @@ #include "fcl/collision_object.h" #include "fcl/collision_data.h" +#include "fcl/distance_func_matrix.h" +#include "fcl/narrowphase/gjk_solver_indep.h" +#include "fcl/narrowphase/gjk_solver_libccd.h" namespace fcl { /// @brief Main distance interface: given two collision objects, and the requirements for contacts, including whether return the nearest points, this function performs the distance between them. /// Return value is the minimum distance generated between the two objects. +template +S distance( + const CollisionObject* o1, const CollisionObject* o2, + const DistanceRequest& request, DistanceResult& result); -FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2, - const DistanceRequest& request, DistanceResult& result); +template +S distance( + const CollisionGeometry* o1, const Transform3& tf1, + const CollisionGeometry* o2, const Transform3& tf2, + const DistanceRequest& request, DistanceResult& result); -FCL_REAL distance(const CollisionGeometry* o1, const Transform3d& tf1, - const CollisionGeometry* o2, const Transform3d& tf2, - const DistanceRequest& request, DistanceResult& result); +//============================================================================// +// // +// Implementations // +// // +//============================================================================// +//============================================================================== +template +DistanceFunctionMatrix& getDistanceFunctionLookTable() +{ + static DistanceFunctionMatrix table; + return table; +} + +template +typename NarrowPhaseSolver::S distance( + const CollisionObject* o1, + const CollisionObject* o2, + const NarrowPhaseSolver* nsolver, + const DistanceRequest& request, + DistanceResult& result) +{ + return distance( + o1->collisionGeometry().get(), + o1->getTransform(), + o2->collisionGeometry().get(), + o2->getTransform(), + nsolver, + request, + result); } +template +typename NarrowPhaseSolver::S distance( + const CollisionGeometry* o1, + const Transform3& tf1, + const CollisionGeometry* o2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver_, + const DistanceRequest& request, + DistanceResult& result) +{ + using S = typename NarrowPhaseSolver::S; + + const NarrowPhaseSolver* nsolver = nsolver_; + if(!nsolver_) + nsolver = new NarrowPhaseSolver(); + + const auto& looktable = getDistanceFunctionLookTable(); + + OBJECT_TYPE object_type1 = o1->getObjectType(); + NODE_TYPE node_type1 = o1->getNodeType(); + OBJECT_TYPE object_type2 = o2->getObjectType(); + NODE_TYPE node_type2 = o2->getNodeType(); + + S res = std::numeric_limits::max(); + + if(object_type1 == OT_GEOM && object_type2 == OT_BVH) + { + if(!looktable.distance_matrix[node_type2][node_type1]) + { + std::cerr << "Warning: distance function between node type " << node_type1 << " and node type " << node_type2 << " is not supported" << std::endl; + } + else + { + res = looktable.distance_matrix[node_type2][node_type1](o2, tf2, o1, tf1, nsolver, request, result); + } + } + else + { + if(!looktable.distance_matrix[node_type1][node_type2]) + { + std::cerr << "Warning: distance function between node type " << node_type1 << " and node type " << node_type2 << " is not supported" << std::endl; + } + else + { + res = looktable.distance_matrix[node_type1][node_type2](o1, tf1, o2, tf2, nsolver, request, result); + } + } + + if(!nsolver_) + delete nsolver; + + return res; +} + +//============================================================================== +template +S distance( + const CollisionObject* o1, + const CollisionObject* o2, + const DistanceRequest& request, + DistanceResult& result) +{ + switch(request.gjk_solver_type) + { + case GST_LIBCCD: + { + GJKSolver_libccd solver; + return distance>(o1, o2, &solver, request, result); + } + case GST_INDEP: + { + GJKSolver_indep solver; + return distance>(o1, o2, &solver, request, result); + } + default: + return -1; // error + } +} + +//============================================================================== +template +S distance( + const CollisionGeometry* o1, const Transform3& tf1, + const CollisionGeometry* o2, const Transform3& tf2, + const DistanceRequest& request, DistanceResult& result) +{ + switch(request.gjk_solver_type) + { + case GST_LIBCCD: + { + GJKSolver_libccd solver; + return distance>(o1, tf1, o2, tf2, &solver, request, result); + } + case GST_INDEP: + { + GJKSolver_indep solver; + return distance>(o1, tf1, o2, tf2, &solver, request, result); + } + default: + return -1; + } +} + +} // namespace fcl + #endif diff --git a/include/fcl/distance_func_matrix.h b/include/fcl/distance_func_matrix.h index 865e65915..87addc0b3 100644 --- a/include/fcl/distance_func_matrix.h +++ b/include/fcl/distance_func_matrix.h @@ -40,20 +40,32 @@ #include "fcl/collision_object.h" #include "fcl/collision_data.h" +#include "fcl/collision_node.h" +#include "fcl/narrowphase/gjk_solver_indep.h" +#include "fcl/narrowphase/gjk_solver_libccd.h" +#include "fcl/traversal/traversal_nodes.h" namespace fcl { /// @brief distance matrix stores the functions for distance between different types of objects and provides a uniform call interface -template +template struct DistanceFunctionMatrix { + using S = typename NarrowPhaseSolver::S; + /// @brief the uniform call interface for distance: for distance, we need know /// 1. two objects o1 and o2 and their configuration in world coordinate tf1 and tf2; /// 2. the solver for narrow phase collision, this is for distance computation between geometric shapes; /// 3. the request setting for distance (e.g., whether need to return nearest points); - typedef FCL_REAL (*DistanceFunc)(const CollisionGeometry* o1, const Transform3d& tf1, const CollisionGeometry* o2, const Transform3d& tf2, const NarrowPhaseSolver* nsolver, - const DistanceRequest& request, DistanceResult& result); + using DistanceFunc = S (*)( + const CollisionGeometry* o1, + const Transform3& tf1, + const CollisionGeometry* o2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const DistanceRequest& request, + DistanceResult& result); /// @brief each item in the distance matrix is a function to handle distance between objects of type1 and type2 DistanceFunc distance_matrix[NODE_COUNT][NODE_COUNT]; @@ -61,6 +73,646 @@ struct DistanceFunctionMatrix DistanceFunctionMatrix(); }; +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +#if FCL_HAVE_OCTOMAP +template +typename Shape::S ShapeOcTreeDistance( + const CollisionGeometry* o1, + const Transform3& tf1, + const CollisionGeometry* o2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const DistanceRequest& request, + DistanceResult& result) +{ + using S = typename Shape::S; + + if(request.isSatisfied(result)) return result.min_distance; + ShapeOcTreeDistanceTraversalNode node; + const Shape* obj1 = static_cast(o1); + const OcTree* obj2 = static_cast*>(o2); + OcTreeSolver otsolver(nsolver); + + initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result); + distance(&node); + + return result.min_distance; +} + +template +typename Shape::S OcTreeShapeDistance( + const CollisionGeometry* o1, + const Transform3& tf1, + const CollisionGeometry* o2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const DistanceRequest& request, + DistanceResult& result) +{ + using S = typename Shape::S; + + if(request.isSatisfied(result)) return result.min_distance; + OcTreeShapeDistanceTraversalNode node; + const OcTree* obj1 = static_cast*>(o1); + const Shape* obj2 = static_cast(o2); + OcTreeSolver otsolver(nsolver); + + initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result); + distance(&node); + + return result.min_distance; +} + +template +typename NarrowPhaseSolver::S OcTreeDistance( + const CollisionGeometry* o1, + const Transform3& tf1, + const CollisionGeometry* o2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const DistanceRequest& request, + DistanceResult& result) +{ + using S = typename NarrowPhaseSolver::S; + + if(request.isSatisfied(result)) return result.min_distance; + OcTreeDistanceTraversalNode node; + const OcTree* obj1 = static_cast*>(o1); + const OcTree* obj2 = static_cast*>(o2); + OcTreeSolver otsolver(nsolver); + + initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result); + distance(&node); + + return result.min_distance; +} + +template +typename BV::S BVHOcTreeDistance( + const CollisionGeometry* o1, + const Transform3& tf1, + const CollisionGeometry* o2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const DistanceRequest& request, + DistanceResult& result) +{ + using S = typename BV::S; + + if(request.isSatisfied(result)) return result.min_distance; + MeshOcTreeDistanceTraversalNode node; + const BVHModel* obj1 = static_cast*>(o1); + const OcTree* obj2 = static_cast*>(o2); + OcTreeSolver otsolver(nsolver); + + initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result); + distance(&node); + + return result.min_distance; +} + +template +typename BV::S OcTreeBVHDistance( + const CollisionGeometry* o1, + const Transform3& tf1, + const CollisionGeometry* o2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const DistanceRequest& request, + DistanceResult& result) +{ + using S = typename BV::S; + + if(request.isSatisfied(result)) return result.min_distance; + OcTreeMeshDistanceTraversalNode node; + const OcTree* obj1 = static_cast*>(o1); + const BVHModel* obj2 = static_cast*>(o2); + OcTreeSolver otsolver(nsolver); + + initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result); + distance(&node); + + return result.min_distance; } #endif + +template +typename Shape1::S ShapeShapeDistance( + const CollisionGeometry* o1, + const Transform3& tf1, + const CollisionGeometry* o2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const DistanceRequest& request, + DistanceResult& result) +{ + if(request.isSatisfied(result)) return result.min_distance; + ShapeDistanceTraversalNode node; + const Shape1* obj1 = static_cast(o1); + const Shape2* obj2 = static_cast(o2); + + initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result); + distance(&node); + + return result.min_distance; +} + +template +struct BVHShapeDistancer +{ + using S = typename BV::S; + + static S distance( + const CollisionGeometry* o1, + const Transform3& tf1, + const CollisionGeometry* o2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const DistanceRequest& request, + DistanceResult& result) + { + if(request.isSatisfied(result)) return result.min_distance; + MeshShapeDistanceTraversalNode node; + const BVHModel* obj1 = static_cast* >(o1); + BVHModel* obj1_tmp = new BVHModel(*obj1); + Transform3 tf1_tmp = tf1; + const Shape* obj2 = static_cast(o2); + + initialize(node, *obj1_tmp, tf1_tmp, *obj2, tf2, nsolver, request, result); + ::fcl::distance(&node); + + delete obj1_tmp; + return result.min_distance; + } +}; + +namespace details +{ + +template +typename Shape::S orientedBVHShapeDistance( + const CollisionGeometry* o1, + const Transform3& tf1, + const CollisionGeometry* o2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const DistanceRequest& + request, DistanceResult& result) +{ + if(request.isSatisfied(result)) return result.min_distance; + OrientedMeshShapeDistanceTraversalNode node; + const BVHModel* obj1 = static_cast* >(o1); + const Shape* obj2 = static_cast(o2); + + initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result); + distance(&node); + + return result.min_distance; +} + +} // namespace details + +template +struct BVHShapeDistancer, Shape, NarrowPhaseSolver> +{ + static typename Shape::S distance( + const CollisionGeometry* o1, + const Transform3& tf1, + const CollisionGeometry* o2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const DistanceRequest& request, + DistanceResult& result) + { + return details::orientedBVHShapeDistance< + MeshShapeDistanceTraversalNodeRSS, + RSS, + Shape, + NarrowPhaseSolver>( + o1, tf1, o2, tf2, nsolver, request, result); + } +}; + +template +struct BVHShapeDistancer, Shape, NarrowPhaseSolver> +{ + static typename Shape::S distance( + const CollisionGeometry* o1, + const Transform3& tf1, + const CollisionGeometry* o2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const DistanceRequest& request, + DistanceResult& result) + { + return details::orientedBVHShapeDistance< + MeshShapeDistanceTraversalNodekIOS, + kIOS, + Shape, + NarrowPhaseSolver>( + o1, tf1, o2, tf2, nsolver, request, result); + } +}; + +template +struct BVHShapeDistancer, Shape, NarrowPhaseSolver> +{ + static typename Shape::S distance( + const CollisionGeometry* o1, + const Transform3& tf1, + const CollisionGeometry* o2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const DistanceRequest& request, + DistanceResult& result) + { + return details::orientedBVHShapeDistance< + MeshShapeDistanceTraversalNodeOBBRSS, + OBBRSS, + Shape, + NarrowPhaseSolver>( + o1, tf1, o2, tf2, nsolver, request, result); + } +}; + +//============================================================================== +template +struct BVHDistanceImpl +{ + static S run( + const CollisionGeometry* o1, + const Transform3& tf1, + const CollisionGeometry* o2, + const Transform3& tf2, + const DistanceRequest& request, + DistanceResult& result) + { + if(request.isSatisfied(result)) return result.min_distance; + MeshDistanceTraversalNode node; + const BVHModel* obj1 = static_cast* >(o1); + const BVHModel* obj2 = static_cast* >(o2); + BVHModel* obj1_tmp = new BVHModel(*obj1); + Transform3 tf1_tmp = tf1; + BVHModel* obj2_tmp = new BVHModel(*obj2); + Transform3 tf2_tmp = tf2; + + initialize(node, *obj1_tmp, tf1_tmp, *obj2_tmp, tf2_tmp, request, result); + distance(&node); + delete obj1_tmp; + delete obj2_tmp; + + return result.min_distance; + } +}; + +//============================================================================== +template +typename BV::S BVHDistance( + const CollisionGeometry* o1, + const Transform3& tf1, + const CollisionGeometry* o2, + const Transform3& tf2, + const DistanceRequest& request, + DistanceResult& result) +{ + return BVHDistanceImpl::run( + o1, tf1, o2, tf2, request, result); +} + +namespace details +{ + +template +typename BV::S orientedMeshDistance( + const CollisionGeometry* o1, + const Transform3& tf1, + const CollisionGeometry* o2, + const Transform3& tf2, + const DistanceRequest& request, + DistanceResult& result) +{ + if(request.isSatisfied(result)) return result.min_distance; + OrientedMeshDistanceTraversalNode node; + const BVHModel* obj1 = static_cast* >(o1); + const BVHModel* obj2 = static_cast* >(o2); + + initialize(node, *obj1, tf1, *obj2, tf2, request, result); + distance(&node); + + return result.min_distance; +} + +} // namespace details + +//============================================================================== +template +struct BVHDistanceImpl> +{ + static S run( + const CollisionGeometry* o1, + const Transform3& tf1, + const CollisionGeometry* o2, + const Transform3& tf2, + const DistanceRequest& request, + DistanceResult& result) + { + return details::orientedMeshDistance< + MeshDistanceTraversalNodeRSS, RSS>( + o1, tf1, o2, tf2, request, result); + } +}; + +//============================================================================== +template +struct BVHDistanceImpl> +{ + static S run( + const CollisionGeometry* o1, + const Transform3& tf1, + const CollisionGeometry* o2, + const Transform3& tf2, + const DistanceRequest& request, + DistanceResult& result) + { + return details::orientedMeshDistance< + MeshDistanceTraversalNodekIOS, kIOS>( + o1, tf1, o2, tf2, request, result); + } +}; + +//============================================================================== +template +struct BVHDistanceImpl> +{ + static S run( + const CollisionGeometry* o1, + const Transform3& tf1, + const CollisionGeometry* o2, + const Transform3& tf2, + const DistanceRequest& request, + DistanceResult& result) + { + return details::orientedMeshDistance< + MeshDistanceTraversalNodeOBBRSS, OBBRSS>( + o1, tf1, o2, tf2, request, result); + } +}; + +//============================================================================== +template +typename BV::S BVHDistance( + const CollisionGeometry* o1, + const Transform3& tf1, + const CollisionGeometry* o2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const DistanceRequest& request, + DistanceResult& result) +{ + return BVHDistance(o1, tf1, o2, tf2, request, result); +} + +template +DistanceFunctionMatrix::DistanceFunctionMatrix() +{ + for(int i = 0; i < NODE_COUNT; ++i) + { + for(int j = 0; j < NODE_COUNT; ++j) + distance_matrix[i][j] = nullptr; + } + + distance_matrix[GEOM_BOX][GEOM_BOX] = &ShapeShapeDistance, Box, NarrowPhaseSolver>; + distance_matrix[GEOM_BOX][GEOM_SPHERE] = &ShapeShapeDistance, Sphere, NarrowPhaseSolver>; + distance_matrix[GEOM_BOX][GEOM_ELLIPSOID] = &ShapeShapeDistance, Ellipsoid, NarrowPhaseSolver>; + distance_matrix[GEOM_BOX][GEOM_CAPSULE] = &ShapeShapeDistance, Capsule, NarrowPhaseSolver>; + distance_matrix[GEOM_BOX][GEOM_CONE] = &ShapeShapeDistance, Cone, NarrowPhaseSolver>; + distance_matrix[GEOM_BOX][GEOM_CYLINDER] = &ShapeShapeDistance, Cylinder, NarrowPhaseSolver>; + distance_matrix[GEOM_BOX][GEOM_CONVEX] = &ShapeShapeDistance, Convex, NarrowPhaseSolver>; + distance_matrix[GEOM_BOX][GEOM_PLANE] = &ShapeShapeDistance, Plane, NarrowPhaseSolver>; + distance_matrix[GEOM_BOX][GEOM_HALFSPACE] = &ShapeShapeDistance, Halfspace, NarrowPhaseSolver>; + + distance_matrix[GEOM_SPHERE][GEOM_BOX] = &ShapeShapeDistance, Box, NarrowPhaseSolver>; + distance_matrix[GEOM_SPHERE][GEOM_SPHERE] = &ShapeShapeDistance, Sphere, NarrowPhaseSolver>; + distance_matrix[GEOM_SPHERE][GEOM_ELLIPSOID] = &ShapeShapeDistance, Ellipsoid, NarrowPhaseSolver>; + distance_matrix[GEOM_SPHERE][GEOM_CAPSULE] = &ShapeShapeDistance, Capsule, NarrowPhaseSolver>; + distance_matrix[GEOM_SPHERE][GEOM_CONE] = &ShapeShapeDistance, Cone, NarrowPhaseSolver>; + distance_matrix[GEOM_SPHERE][GEOM_CYLINDER] = &ShapeShapeDistance, Cylinder, NarrowPhaseSolver>; + distance_matrix[GEOM_SPHERE][GEOM_CONVEX] = &ShapeShapeDistance, Convex, NarrowPhaseSolver>; + distance_matrix[GEOM_SPHERE][GEOM_PLANE] = &ShapeShapeDistance, Plane, NarrowPhaseSolver>; + distance_matrix[GEOM_SPHERE][GEOM_HALFSPACE] = &ShapeShapeDistance, Halfspace, NarrowPhaseSolver>; + + distance_matrix[GEOM_ELLIPSOID][GEOM_BOX] = &ShapeShapeDistance, Box, NarrowPhaseSolver>; + distance_matrix[GEOM_ELLIPSOID][GEOM_SPHERE] = &ShapeShapeDistance, Sphere, NarrowPhaseSolver>; + distance_matrix[GEOM_ELLIPSOID][GEOM_ELLIPSOID] = &ShapeShapeDistance, Ellipsoid, NarrowPhaseSolver>; + distance_matrix[GEOM_ELLIPSOID][GEOM_CAPSULE] = &ShapeShapeDistance, Capsule, NarrowPhaseSolver>; + distance_matrix[GEOM_ELLIPSOID][GEOM_CONE] = &ShapeShapeDistance, Cone, NarrowPhaseSolver>; + distance_matrix[GEOM_ELLIPSOID][GEOM_CYLINDER] = &ShapeShapeDistance, Cylinder, NarrowPhaseSolver>; + distance_matrix[GEOM_ELLIPSOID][GEOM_CONVEX] = &ShapeShapeDistance, Convex, NarrowPhaseSolver>; + distance_matrix[GEOM_ELLIPSOID][GEOM_PLANE] = &ShapeShapeDistance, Plane, NarrowPhaseSolver>; + distance_matrix[GEOM_ELLIPSOID][GEOM_HALFSPACE] = &ShapeShapeDistance, Halfspace, NarrowPhaseSolver>; + + distance_matrix[GEOM_CAPSULE][GEOM_BOX] = &ShapeShapeDistance, Box, NarrowPhaseSolver>; + distance_matrix[GEOM_CAPSULE][GEOM_SPHERE] = &ShapeShapeDistance, Sphere, NarrowPhaseSolver>; + distance_matrix[GEOM_CAPSULE][GEOM_ELLIPSOID] = &ShapeShapeDistance, Ellipsoid, NarrowPhaseSolver>; + distance_matrix[GEOM_CAPSULE][GEOM_CAPSULE] = &ShapeShapeDistance, Capsule, NarrowPhaseSolver>; + distance_matrix[GEOM_CAPSULE][GEOM_CONE] = &ShapeShapeDistance, Cone, NarrowPhaseSolver>; + distance_matrix[GEOM_CAPSULE][GEOM_CYLINDER] = &ShapeShapeDistance, Cylinder, NarrowPhaseSolver>; + distance_matrix[GEOM_CAPSULE][GEOM_CONVEX] = &ShapeShapeDistance, Convex, NarrowPhaseSolver>; + distance_matrix[GEOM_CAPSULE][GEOM_PLANE] = &ShapeShapeDistance, Plane, NarrowPhaseSolver>; + distance_matrix[GEOM_CAPSULE][GEOM_HALFSPACE] = &ShapeShapeDistance, Halfspace, NarrowPhaseSolver>; + + distance_matrix[GEOM_CONE][GEOM_BOX] = &ShapeShapeDistance, Box, NarrowPhaseSolver>; + distance_matrix[GEOM_CONE][GEOM_SPHERE] = &ShapeShapeDistance, Sphere, NarrowPhaseSolver>; + distance_matrix[GEOM_CONE][GEOM_ELLIPSOID] = &ShapeShapeDistance, Ellipsoid, NarrowPhaseSolver>; + distance_matrix[GEOM_CONE][GEOM_CAPSULE] = &ShapeShapeDistance, Capsule, NarrowPhaseSolver>; + distance_matrix[GEOM_CONE][GEOM_CONE] = &ShapeShapeDistance, Cone, NarrowPhaseSolver>; + distance_matrix[GEOM_CONE][GEOM_CYLINDER] = &ShapeShapeDistance, Cylinder, NarrowPhaseSolver>; + distance_matrix[GEOM_CONE][GEOM_CONVEX] = &ShapeShapeDistance, Convex, NarrowPhaseSolver>; + distance_matrix[GEOM_CONE][GEOM_PLANE] = &ShapeShapeDistance, Plane, NarrowPhaseSolver>; + distance_matrix[GEOM_CONE][GEOM_HALFSPACE] = &ShapeShapeDistance, Halfspace, NarrowPhaseSolver>; + + distance_matrix[GEOM_CYLINDER][GEOM_BOX] = &ShapeShapeDistance, Box, NarrowPhaseSolver>; + distance_matrix[GEOM_CYLINDER][GEOM_SPHERE] = &ShapeShapeDistance, Sphere, NarrowPhaseSolver>; + distance_matrix[GEOM_CYLINDER][GEOM_ELLIPSOID] = &ShapeShapeDistance, Ellipsoid, NarrowPhaseSolver>; + distance_matrix[GEOM_CYLINDER][GEOM_CAPSULE] = &ShapeShapeDistance, Capsule, NarrowPhaseSolver>; + distance_matrix[GEOM_CYLINDER][GEOM_CONE] = &ShapeShapeDistance, Cone, NarrowPhaseSolver>; + distance_matrix[GEOM_CYLINDER][GEOM_CYLINDER] = &ShapeShapeDistance, Cylinder, NarrowPhaseSolver>; + distance_matrix[GEOM_CYLINDER][GEOM_CONVEX] = &ShapeShapeDistance, Convex, NarrowPhaseSolver>; + distance_matrix[GEOM_CYLINDER][GEOM_PLANE] = &ShapeShapeDistance, Plane, NarrowPhaseSolver>; + distance_matrix[GEOM_CYLINDER][GEOM_HALFSPACE] = &ShapeShapeDistance, Halfspace, NarrowPhaseSolver>; + + distance_matrix[GEOM_CONVEX][GEOM_BOX] = &ShapeShapeDistance, Box, NarrowPhaseSolver>; + distance_matrix[GEOM_CONVEX][GEOM_SPHERE] = &ShapeShapeDistance, Sphere, NarrowPhaseSolver>; + distance_matrix[GEOM_CONVEX][GEOM_ELLIPSOID] = &ShapeShapeDistance, Ellipsoid, NarrowPhaseSolver>; + distance_matrix[GEOM_CONVEX][GEOM_CAPSULE] = &ShapeShapeDistance, Capsule, NarrowPhaseSolver>; + distance_matrix[GEOM_CONVEX][GEOM_CONE] = &ShapeShapeDistance, Cone, NarrowPhaseSolver>; + distance_matrix[GEOM_CONVEX][GEOM_CYLINDER] = &ShapeShapeDistance, Cylinder, NarrowPhaseSolver>; + distance_matrix[GEOM_CONVEX][GEOM_CONVEX] = &ShapeShapeDistance, Convex, NarrowPhaseSolver>; + distance_matrix[GEOM_CONVEX][GEOM_PLANE] = &ShapeShapeDistance, Plane, NarrowPhaseSolver>; + distance_matrix[GEOM_CONVEX][GEOM_HALFSPACE] = &ShapeShapeDistance, Halfspace, NarrowPhaseSolver>; + + distance_matrix[GEOM_PLANE][GEOM_BOX] = &ShapeShapeDistance, Box, NarrowPhaseSolver>; + distance_matrix[GEOM_PLANE][GEOM_SPHERE] = &ShapeShapeDistance, Sphere, NarrowPhaseSolver>; + distance_matrix[GEOM_PLANE][GEOM_ELLIPSOID] = &ShapeShapeDistance, Ellipsoid, NarrowPhaseSolver>; + distance_matrix[GEOM_PLANE][GEOM_CAPSULE] = &ShapeShapeDistance, Capsule, NarrowPhaseSolver>; + distance_matrix[GEOM_PLANE][GEOM_CONE] = &ShapeShapeDistance, Cone, NarrowPhaseSolver>; + distance_matrix[GEOM_PLANE][GEOM_CYLINDER] = &ShapeShapeDistance, Cylinder, NarrowPhaseSolver>; + distance_matrix[GEOM_PLANE][GEOM_CONVEX] = &ShapeShapeDistance, Convex, NarrowPhaseSolver>; + distance_matrix[GEOM_PLANE][GEOM_PLANE] = &ShapeShapeDistance, Plane, NarrowPhaseSolver>; + distance_matrix[GEOM_PLANE][GEOM_HALFSPACE] = &ShapeShapeDistance, Halfspace, NarrowPhaseSolver>; + + distance_matrix[GEOM_HALFSPACE][GEOM_BOX] = &ShapeShapeDistance, Box, NarrowPhaseSolver>; + distance_matrix[GEOM_HALFSPACE][GEOM_SPHERE] = &ShapeShapeDistance, Sphere, NarrowPhaseSolver>; + distance_matrix[GEOM_HALFSPACE][GEOM_CAPSULE] = &ShapeShapeDistance, Capsule, NarrowPhaseSolver>; + distance_matrix[GEOM_HALFSPACE][GEOM_CONE] = &ShapeShapeDistance, Cone, NarrowPhaseSolver>; + distance_matrix[GEOM_HALFSPACE][GEOM_CYLINDER] = &ShapeShapeDistance, Cylinder, NarrowPhaseSolver>; + distance_matrix[GEOM_HALFSPACE][GEOM_CONVEX] = &ShapeShapeDistance, Convex, NarrowPhaseSolver>; + distance_matrix[GEOM_HALFSPACE][GEOM_PLANE] = &ShapeShapeDistance, Plane, NarrowPhaseSolver>; + distance_matrix[GEOM_HALFSPACE][GEOM_HALFSPACE] = &ShapeShapeDistance, Halfspace, NarrowPhaseSolver>; + + /* AABB distance not implemented */ + /* + distance_matrix[BV_AABB][GEOM_BOX] = &BVHShapeDistancer, Box, NarrowPhaseSolver>::distance; + distance_matrix[BV_AABB][GEOM_SPHERE] = &BVHShapeDistancer, Sphere, NarrowPhaseSolver>::distance; + distance_matrix[BV_AABB][GEOM_ELLIPSOID] = &BVHShapeDistancer, Ellipsoid, NarrowPhaseSolver>::distance; + distance_matrix[BV_AABB][GEOM_CAPSULE] = &BVHShapeDistancer, Capsule, NarrowPhaseSolver>::distance; + distance_matrix[BV_AABB][GEOM_CONE] = &BVHShapeDistancer, Cone, NarrowPhaseSolver>::distance; + distance_matrix[BV_AABB][GEOM_CYLINDER] = &BVHShapeDistancer, Cylinder, NarrowPhaseSolver>::distance; + distance_matrix[BV_AABB][GEOM_CONVEX] = &BVHShapeDistancer, Convex, NarrowPhaseSolver>::distance; + distance_matrix[BV_AABB][GEOM_PLANE] = &BVHShapeDistancer, Plane, NarrowPhaseSolver>::distance; + distance_matrix[BV_AABB][GEOM_HALFSPACE] = &BVHShapeDistancer, Halfspace, NarrowPhaseSolver>::distance; + + distance_matrix[BV_OBB][GEOM_BOX] = &BVHShapeDistancer, Box, NarrowPhaseSolver>::distance; + distance_matrix[BV_OBB][GEOM_SPHERE] = &BVHShapeDistancer, Sphere, NarrowPhaseSolver>::distance; + distance_matrix[BV_OBB][GEOM_ELLIPSOID] = &BVHShapeDistancer, Ellipsoid, NarrowPhaseSolver>::distance; + distance_matrix[BV_OBB][GEOM_CAPSULE] = &BVHShapeDistancer, Capsule, NarrowPhaseSolver>::distance; + distance_matrix[BV_OBB][GEOM_CONE] = &BVHShapeDistancer, Cone, NarrowPhaseSolver>::distance; + distance_matrix[BV_OBB][GEOM_CYLINDER] = &BVHShapeDistancer, Cylinder, NarrowPhaseSolver>::distance; + distance_matrix[BV_OBB][GEOM_CONVEX] = &BVHShapeDistancer, Convex, NarrowPhaseSolver>::distance; + distance_matrix[BV_OBB][GEOM_PLANE] = &BVHShapeDistancer, Plane, NarrowPhaseSolver>::distance; + distance_matrix[BV_OBB][GEOM_HALFSPACE] = &BVHShapeDistancer, Halfspace, NarrowPhaseSolver>::distance; + */ + + distance_matrix[BV_RSS][GEOM_BOX] = &BVHShapeDistancer, Box, NarrowPhaseSolver>::distance; + distance_matrix[BV_RSS][GEOM_SPHERE] = &BVHShapeDistancer, Sphere, NarrowPhaseSolver>::distance; + distance_matrix[BV_RSS][GEOM_ELLIPSOID] = &BVHShapeDistancer, Ellipsoid, NarrowPhaseSolver>::distance; + distance_matrix[BV_RSS][GEOM_CAPSULE] = &BVHShapeDistancer, Capsule, NarrowPhaseSolver>::distance; + distance_matrix[BV_RSS][GEOM_CONE] = &BVHShapeDistancer, Cone, NarrowPhaseSolver>::distance; + distance_matrix[BV_RSS][GEOM_CYLINDER] = &BVHShapeDistancer, Cylinder, NarrowPhaseSolver>::distance; + distance_matrix[BV_RSS][GEOM_CONVEX] = &BVHShapeDistancer, Convex, NarrowPhaseSolver>::distance; + distance_matrix[BV_RSS][GEOM_PLANE] = &BVHShapeDistancer, Plane, NarrowPhaseSolver>::distance; + distance_matrix[BV_RSS][GEOM_HALFSPACE] = &BVHShapeDistancer, Halfspace, NarrowPhaseSolver>::distance; + + /* KDOPd distance not implemented */ + /* + distance_matrix[BV_KDOP16][GEOM_BOX] = &BVHShapeDistancer, Box, NarrowPhaseSolver>::distance; + distance_matrix[BV_KDOP16][GEOM_SPHERE] = &BVHShapeDistancer, Sphere, NarrowPhaseSolver>::distance; + distance_matrix[BV_KDOP16][GEOM_ELLIPSOID] = &BVHShapeDistancer, Ellipsoid, NarrowPhaseSolver>::distance; + distance_matrix[BV_KDOP16][GEOM_CAPSULE] = &BVHShapeDistancer, Capsule, NarrowPhaseSolver>::distance; + distance_matrix[BV_KDOP16][GEOM_CONE] = &BVHShapeDistancer, Cone, NarrowPhaseSolver>::distance; + distance_matrix[BV_KDOP16][GEOM_CYLINDER] = &BVHShapeDistancer, Cylinder, NarrowPhaseSolver>::distance; + distance_matrix[BV_KDOP16][GEOM_CONVEX] = &BVHShapeDistancer, Convex, NarrowPhaseSolver>::distance; + distance_matrix[BV_KDOP16][GEOM_PLANE] = &BVHShapeDistancer, Plane, NarrowPhaseSolver>::distance; + distance_matrix[BV_KDOP16][GEOM_HALFSPACE] = &BVHShapeDistancer, Halfspace, NarrowPhaseSolver>::distance; + + distance_matrix[BV_KDOP18][GEOM_BOX] = &BVHShapeDistancer, Box, NarrowPhaseSolver>::distance; + distance_matrix[BV_KDOP18][GEOM_SPHERE] = &BVHShapeDistancer, Sphere, NarrowPhaseSolver>::distance; + distance_matrix[BV_KDOP18][GEOM_ELLIPSOID] = &BVHShapeDistancer, Ellipsoid, NarrowPhaseSolver>::distance; + distance_matrix[BV_KDOP18][GEOM_CAPSULE] = &BVHShapeDistancer, Capsule, NarrowPhaseSolver>::distance; + distance_matrix[BV_KDOP18][GEOM_CONE] = &BVHShapeDistancer, Cone, NarrowPhaseSolver>::distance; + distance_matrix[BV_KDOP18][GEOM_CYLINDER] = &BVHShapeDistancer, Cylinder, NarrowPhaseSolver>::distance; + distance_matrix[BV_KDOP18][GEOM_CONVEX] = &BVHShapeDistancer, Convex, NarrowPhaseSolver>::distance; + distance_matrix[BV_KDOP18][GEOM_PLANE] = &BVHShapeDistancer, Plane, NarrowPhaseSolver>::distance; + distance_matrix[BV_KDOP18][GEOM_HALFSPACE] = &BVHShapeDistancer, Halfspace, NarrowPhaseSolver>::distance; + + distance_matrix[BV_KDOP24][GEOM_BOX] = &BVHShapeDistancer, Box, NarrowPhaseSolver>::distance; + distance_matrix[BV_KDOP24][GEOM_SPHERE] = &BVHShapeDistancer, Sphere, NarrowPhaseSolver>::distance; + distance_matrix[BV_KDOP24][GEOM_ELLIPSOID] = &BVHShapeDistancer, Ellipsoid, NarrowPhaseSolver>::distance; + distance_matrix[BV_KDOP24][GEOM_CAPSULE] = &BVHShapeDistancer, Capsule, NarrowPhaseSolver>::distance; + distance_matrix[BV_KDOP24][GEOM_CONE] = &BVHShapeDistancer, Cone, NarrowPhaseSolver>::distance; + distance_matrix[BV_KDOP24][GEOM_CYLINDER] = &BVHShapeDistancer, Cylinder, NarrowPhaseSolver>::distance; + distance_matrix[BV_KDOP24][GEOM_CONVEX] = &BVHShapeDistancer, Convex, NarrowPhaseSolver>::distance; + distance_matrix[BV_KDOP24][GEOM_PLANE] = &BVHShapeDistancer, Plane, NarrowPhaseSolver>::distance; + distance_matrix[BV_KDOP24][GEOM_HALFSPACE] = &BVHShapeDistancer, Halfspace, NarrowPhaseSolver>::distance; + */ + + distance_matrix[BV_kIOS][GEOM_BOX] = &BVHShapeDistancer, Box, NarrowPhaseSolver>::distance; + distance_matrix[BV_kIOS][GEOM_SPHERE] = &BVHShapeDistancer, Sphere, NarrowPhaseSolver>::distance; + distance_matrix[BV_kIOS][GEOM_ELLIPSOID] = &BVHShapeDistancer, Ellipsoid, NarrowPhaseSolver>::distance; + distance_matrix[BV_kIOS][GEOM_CAPSULE] = &BVHShapeDistancer, Capsule, NarrowPhaseSolver>::distance; + distance_matrix[BV_kIOS][GEOM_CONE] = &BVHShapeDistancer, Cone, NarrowPhaseSolver>::distance; + distance_matrix[BV_kIOS][GEOM_CYLINDER] = &BVHShapeDistancer, Cylinder, NarrowPhaseSolver>::distance; + distance_matrix[BV_kIOS][GEOM_CONVEX] = &BVHShapeDistancer, Convex, NarrowPhaseSolver>::distance; + distance_matrix[BV_kIOS][GEOM_PLANE] = &BVHShapeDistancer, Plane, NarrowPhaseSolver>::distance; + distance_matrix[BV_kIOS][GEOM_HALFSPACE] = &BVHShapeDistancer, Halfspace, NarrowPhaseSolver>::distance; + + distance_matrix[BV_OBBRSS][GEOM_BOX] = &BVHShapeDistancer, Box, NarrowPhaseSolver>::distance; + distance_matrix[BV_OBBRSS][GEOM_SPHERE] = &BVHShapeDistancer, Sphere, NarrowPhaseSolver>::distance; + distance_matrix[BV_OBBRSS][GEOM_ELLIPSOID] = &BVHShapeDistancer, Ellipsoid, NarrowPhaseSolver>::distance; + distance_matrix[BV_OBBRSS][GEOM_CAPSULE] = &BVHShapeDistancer, Capsule, NarrowPhaseSolver>::distance; + distance_matrix[BV_OBBRSS][GEOM_CONE] = &BVHShapeDistancer, Cone, NarrowPhaseSolver>::distance; + distance_matrix[BV_OBBRSS][GEOM_CYLINDER] = &BVHShapeDistancer, Cylinder, NarrowPhaseSolver>::distance; + distance_matrix[BV_OBBRSS][GEOM_CONVEX] = &BVHShapeDistancer, Convex, NarrowPhaseSolver>::distance; + distance_matrix[BV_OBBRSS][GEOM_PLANE] = &BVHShapeDistancer, Plane, NarrowPhaseSolver>::distance; + distance_matrix[BV_OBBRSS][GEOM_HALFSPACE] = &BVHShapeDistancer, Halfspace, NarrowPhaseSolver>::distance; + + distance_matrix[BV_AABB][BV_AABB] = &BVHDistance, NarrowPhaseSolver>; + distance_matrix[BV_RSS][BV_RSS] = &BVHDistance, NarrowPhaseSolver>; + distance_matrix[BV_kIOS][BV_kIOS] = &BVHDistance, NarrowPhaseSolver>; + distance_matrix[BV_OBBRSS][BV_OBBRSS] = &BVHDistance, NarrowPhaseSolver>; + +#if FCL_HAVE_OCTOMAP + distance_matrix[GEOM_OCTREE][GEOM_BOX] = &OcTreeShapeDistance, NarrowPhaseSolver>; + distance_matrix[GEOM_OCTREE][GEOM_SPHERE] = &OcTreeShapeDistance, NarrowPhaseSolver>; + distance_matrix[GEOM_OCTREE][GEOM_ELLIPSOID] = &OcTreeShapeDistance, NarrowPhaseSolver>; + distance_matrix[GEOM_OCTREE][GEOM_CAPSULE] = &OcTreeShapeDistance, NarrowPhaseSolver>; + distance_matrix[GEOM_OCTREE][GEOM_CONE] = &OcTreeShapeDistance, NarrowPhaseSolver>; + distance_matrix[GEOM_OCTREE][GEOM_CYLINDER] = &OcTreeShapeDistance, NarrowPhaseSolver>; + distance_matrix[GEOM_OCTREE][GEOM_CONVEX] = &OcTreeShapeDistance, NarrowPhaseSolver>; + distance_matrix[GEOM_OCTREE][GEOM_PLANE] = &OcTreeShapeDistance, NarrowPhaseSolver>; + distance_matrix[GEOM_OCTREE][GEOM_HALFSPACE] = &OcTreeShapeDistance, NarrowPhaseSolver>; + + distance_matrix[GEOM_BOX][GEOM_OCTREE] = &ShapeOcTreeDistance, NarrowPhaseSolver>; + distance_matrix[GEOM_SPHERE][GEOM_OCTREE] = &ShapeOcTreeDistance, NarrowPhaseSolver>; + distance_matrix[GEOM_ELLIPSOID][GEOM_OCTREE] = &ShapeOcTreeDistance, NarrowPhaseSolver>; + distance_matrix[GEOM_CAPSULE][GEOM_OCTREE] = &ShapeOcTreeDistance, NarrowPhaseSolver>; + distance_matrix[GEOM_CONE][GEOM_OCTREE] = &ShapeOcTreeDistance, NarrowPhaseSolver>; + distance_matrix[GEOM_CYLINDER][GEOM_OCTREE] = &ShapeOcTreeDistance, NarrowPhaseSolver>; + distance_matrix[GEOM_CONVEX][GEOM_OCTREE] = &ShapeOcTreeDistance, NarrowPhaseSolver>; + distance_matrix[GEOM_PLANE][GEOM_OCTREE] = &ShapeOcTreeDistance, NarrowPhaseSolver>; + distance_matrix[GEOM_HALFSPACE][GEOM_OCTREE] = &ShapeOcTreeDistance, NarrowPhaseSolver>; + + distance_matrix[GEOM_OCTREE][GEOM_OCTREE] = &OcTreeDistance; + + distance_matrix[GEOM_OCTREE][BV_AABB] = &OcTreeBVHDistance, NarrowPhaseSolver>; + distance_matrix[GEOM_OCTREE][BV_OBB] = &OcTreeBVHDistance, NarrowPhaseSolver>; + distance_matrix[GEOM_OCTREE][BV_RSS] = &OcTreeBVHDistance, NarrowPhaseSolver>; + distance_matrix[GEOM_OCTREE][BV_OBBRSS] = &OcTreeBVHDistance, NarrowPhaseSolver>; + distance_matrix[GEOM_OCTREE][BV_kIOS] = &OcTreeBVHDistance, NarrowPhaseSolver>; + distance_matrix[GEOM_OCTREE][BV_KDOP16] = &OcTreeBVHDistance, NarrowPhaseSolver>; + distance_matrix[GEOM_OCTREE][BV_KDOP18] = &OcTreeBVHDistance, NarrowPhaseSolver>; + distance_matrix[GEOM_OCTREE][BV_KDOP24] = &OcTreeBVHDistance, NarrowPhaseSolver>; + + distance_matrix[BV_AABB][GEOM_OCTREE] = &BVHOcTreeDistance, NarrowPhaseSolver>; + distance_matrix[BV_OBB][GEOM_OCTREE] = &BVHOcTreeDistance, NarrowPhaseSolver>; + distance_matrix[BV_RSS][GEOM_OCTREE] = &BVHOcTreeDistance, NarrowPhaseSolver>; + distance_matrix[BV_OBBRSS][GEOM_OCTREE] = &BVHOcTreeDistance, NarrowPhaseSolver>; + distance_matrix[BV_kIOS][GEOM_OCTREE] = &BVHOcTreeDistance, NarrowPhaseSolver>; + distance_matrix[BV_KDOP16][GEOM_OCTREE] = &BVHOcTreeDistance, NarrowPhaseSolver>; + distance_matrix[BV_KDOP18][GEOM_OCTREE] = &BVHOcTreeDistance, NarrowPhaseSolver>; + distance_matrix[BV_KDOP24][GEOM_OCTREE] = &BVHOcTreeDistance, NarrowPhaseSolver>; +#endif + +} + +} // namespace fcl + +#endif diff --git a/include/fcl/intersect.h b/include/fcl/intersect.h index f05ac0065..198efac05 100644 --- a/include/fcl/intersect.h +++ b/include/fcl/intersect.h @@ -38,36 +38,44 @@ #ifndef FCL_INTERSECT_H #define FCL_INTERSECT_H +#include +#include +#include #include "fcl/data_types.h" +#include "fcl/math/geometry.h" namespace fcl { /// @brief A class solves polynomial degree (1,2,3) equations +template class PolySolver { public: /// @brief Solve a linear equation with coefficients c, return roots s and number of roots - static int solveLinear(FCL_REAL c[2], FCL_REAL s[1]); + static int solveLinear(S c[2], S s[1]); /// @brief Solve a quadratic function with coefficients c, return roots s and number of roots - static int solveQuadric(FCL_REAL c[3], FCL_REAL s[2]); + static int solveQuadric(S c[3], S s[2]); /// @brief Solve a cubic function with coefficients c, return roots s and number of roots - static int solveCubic(FCL_REAL c[4], FCL_REAL s[3]); + static int solveCubic(S c[4], S s[3]); private: /// @brief Check whether v is zero - static inline bool isZero(FCL_REAL v); + static inline bool isZero(S v); /// @brief Compute v^{1/3} - static inline bool cbrt(FCL_REAL v); + static inline bool cbrt(S v); - static const FCL_REAL NEAR_ZERO_THRESHOLD; + static constexpr S getNearZeroThreshold() { return 1e-9; } }; +using PolySolverf = PolySolver; +using PolySolverd = PolySolver; /// @brief CCD intersect kernel among primitives +template class Intersect { @@ -77,215 +85,234 @@ class Intersect /// [a0, b0, c0] and [a1, b1, c1] are points for the triangle face in time t0 and t1 /// p0 and p1 are points for vertex in time t0 and t1 /// p_i returns the coordinate of the collision point - static bool intersect_VF(const Vector3d& a0, const Vector3d& b0, const Vector3d& c0, const Vector3d& p0, - const Vector3d& a1, const Vector3d& b1, const Vector3d& c1, const Vector3d& p1, - FCL_REAL* collision_time, Vector3d* p_i, bool useNewton = true); + static bool intersect_VF(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& p0, + const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& p1, + S* collision_time, Vector3* p_i, bool useNewton = true); /// @brief CCD intersect between two edges /// [a0, b0] and [a1, b1] are points for one edge in time t0 and t1 /// [c0, d0] and [c1, d1] are points for the other edge in time t0 and t1 /// p_i returns the coordinate of the collision point - static bool intersect_EE(const Vector3d& a0, const Vector3d& b0, const Vector3d& c0, const Vector3d& d0, - const Vector3d& a1, const Vector3d& b1, const Vector3d& c1, const Vector3d& d1, - FCL_REAL* collision_time, Vector3d* p_i, bool useNewton = true); + static bool intersect_EE(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, + const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& d1, + S* collision_time, Vector3* p_i, bool useNewton = true); /// @brief CCD intersect between one vertex and one face, using additional filter - static bool intersect_VF_filtered(const Vector3d& a0, const Vector3d& b0, const Vector3d& c0, const Vector3d& p0, - const Vector3d& a1, const Vector3d& b1, const Vector3d& c1, const Vector3d& p1, - FCL_REAL* collision_time, Vector3d* p_i, bool useNewton = true); + static bool intersect_VF_filtered(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& p0, + const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& p1, + S* collision_time, Vector3* p_i, bool useNewton = true); /// @brief CCD intersect between two edges, using additional filter - static bool intersect_EE_filtered(const Vector3d& a0, const Vector3d& b0, const Vector3d& c0, const Vector3d& d0, - const Vector3d& a1, const Vector3d& b1, const Vector3d& c1, const Vector3d& d1, - FCL_REAL* collision_time, Vector3d* p_i, bool useNewton = true); + static bool intersect_EE_filtered(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, + const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& d1, + S* collision_time, Vector3* p_i, bool useNewton = true); /// @brief CCD intersect between one vertex and and one edge - static bool intersect_VE(const Vector3d& a0, const Vector3d& b0, const Vector3d& p0, - const Vector3d& a1, const Vector3d& b1, const Vector3d& p1, - const Vector3d& L); + static bool intersect_VE(const Vector3& a0, const Vector3& b0, const Vector3& p0, + const Vector3& a1, const Vector3& b1, const Vector3& p1, + const Vector3& L); /// @brief CD intersect between two triangles [P1, P2, P3] and [Q1, Q2, Q3] - static bool intersect_Triangle(const Vector3d& P1, const Vector3d& P2, const Vector3d& P3, - const Vector3d& Q1, const Vector3d& Q2, const Vector3d& Q3, - Vector3d* contact_points = NULL, - unsigned int* num_contact_points = NULL, - FCL_REAL* penetration_depth = NULL, - Vector3d* normal = NULL); - - static bool intersect_Triangle(const Vector3d& P1, const Vector3d& P2, const Vector3d& P3, - const Vector3d& Q1, const Vector3d& Q2, const Vector3d& Q3, - const Matrix3d& R, const Vector3d& T, - Vector3d* contact_points = NULL, - unsigned int* num_contact_points = NULL, - FCL_REAL* penetration_depth = NULL, - Vector3d* normal = NULL); - - static bool intersect_Triangle(const Vector3d& P1, const Vector3d& P2, const Vector3d& P3, - const Vector3d& Q1, const Vector3d& Q2, const Vector3d& Q3, - const Transform3d& tf, - Vector3d* contact_points = NULL, - unsigned int* num_contact_points = NULL, - FCL_REAL* penetration_depth = NULL, - Vector3d* normal = NULL); + static bool intersect_Triangle(const Vector3& P1, const Vector3& P2, const Vector3& P3, + const Vector3& Q1, const Vector3& Q2, const Vector3& Q3, + Vector3* contact_points = nullptr, + unsigned int* num_contact_points = nullptr, + S* penetration_depth = nullptr, + Vector3* normal = nullptr); + + static bool intersect_Triangle( + const Vector3& P1, + const Vector3& P2, + const Vector3& P3, + const Vector3& Q1, + const Vector3& Q2, + const Vector3& Q3, + const Matrix3& R, + const Vector3& T, + Vector3* contact_points = nullptr, + unsigned int* num_contact_points = nullptr, + S* penetration_depth = nullptr, + Vector3* normal = nullptr); + + static bool intersect_Triangle( + const Vector3& P1, + const Vector3& P2, + const Vector3& P3, + const Vector3& Q1, + const Vector3& Q2, + const Vector3& Q3, + const Transform3& tf, + Vector3* contact_points = nullptr, + unsigned int* num_contact_points = nullptr, + S* penetration_depth = nullptr, + Vector3* normal = nullptr); private: /// @brief Project function used in intersect_Triangle() - static int project6(const Vector3d& ax, - const Vector3d& p1, const Vector3d& p2, const Vector3d& p3, - const Vector3d& q1, const Vector3d& q2, const Vector3d& q3); + static int project6(const Vector3& ax, + const Vector3& p1, const Vector3& p2, const Vector3& p3, + const Vector3& q1, const Vector3& q2, const Vector3& q3); /// @brief Check whether one value is zero - static inline bool isZero(FCL_REAL v); + static inline bool isZero(S v); /// @brief Solve the cubic function using Newton method, also satisfies the interval restriction - static bool solveCubicWithIntervalNewton(const Vector3d& a0, const Vector3d& b0, const Vector3d& c0, const Vector3d& d0, - const Vector3d& va, const Vector3d& vb, const Vector3d& vc, const Vector3d& vd, - FCL_REAL& l, FCL_REAL& r, bool bVF, FCL_REAL coeffs[], Vector3d* data = NULL); + static bool solveCubicWithIntervalNewton(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, + const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vd, + S& l, S& r, bool bVF, S coeffs[], Vector3* data = nullptr); /// @brief Check whether one point p is within triangle [a, b, c] - static bool insideTriangle(const Vector3d& a, const Vector3d& b, const Vector3d& c, const Vector3d&p); + static bool insideTriangle(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3&p); /// @brief Check whether one point p is within a line segment [a, b] - static bool insideLineSegment(const Vector3d& a, const Vector3d& b, const Vector3d& p); + static bool insideLineSegment(const Vector3& a, const Vector3& b, const Vector3& p); /// @brief Calculate the line segment papb that is the shortest route between /// two lines p1p2 and p3p4. Calculate also the values of mua and mub where /// pa = p1 + mua (p2 - p1) /// pb = p3 + mub (p4 - p3) /// return FALSE if no solution exists. - static bool linelineIntersect(const Vector3d& p1, const Vector3d& p2, const Vector3d& p3, const Vector3d& p4, - Vector3d* pa, Vector3d* pb, FCL_REAL* mua, FCL_REAL* mub); + static bool linelineIntersect(const Vector3& p1, const Vector3& p2, const Vector3& p3, const Vector3& p4, + Vector3* pa, Vector3* pb, S* mua, S* mub); /// @brief Check whether a root for VF intersection is valid (i.e. within the triangle at intersection t - static bool checkRootValidity_VF(const Vector3d& a0, const Vector3d& b0, const Vector3d& c0, const Vector3d& p0, - const Vector3d& va, const Vector3d& vb, const Vector3d& vc, const Vector3d& vp, - FCL_REAL t); + static bool checkRootValidity_VF(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& p0, + const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vp, + S t); /// @brief Check whether a root for EE intersection is valid (i.e. within the two edges intersected at the given time - static bool checkRootValidity_EE(const Vector3d& a0, const Vector3d& b0, const Vector3d& c0, const Vector3d& d0, - const Vector3d& va, const Vector3d& vb, const Vector3d& vc, const Vector3d& vd, - FCL_REAL t, Vector3d* q_i = NULL); + static bool checkRootValidity_EE(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, + const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vd, + S t, Vector3* q_i = nullptr); /// @brief Check whether a root for VE intersection is valid - static bool checkRootValidity_VE(const Vector3d& a0, const Vector3d& b0, const Vector3d& p0, - const Vector3d& va, const Vector3d& vb, const Vector3d& vp, - FCL_REAL t); + static bool checkRootValidity_VE(const Vector3& a0, const Vector3& b0, const Vector3& p0, + const Vector3& va, const Vector3& vb, const Vector3& vp, + S t); /// @brief Solve a square function for EE intersection (with interval restriction) - static bool solveSquare(FCL_REAL a, FCL_REAL b, FCL_REAL c, - const Vector3d& a0, const Vector3d& b0, const Vector3d& c0, const Vector3d& d0, - const Vector3d& va, const Vector3d& vb, const Vector3d& vc, const Vector3d& vd, + static bool solveSquare(S a, S b, S c, + const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, + const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vd, bool bVF, - FCL_REAL* ret); + S* ret); /// @brief Solve a square function for VE intersection (with interval restriction) - static bool solveSquare(FCL_REAL a, FCL_REAL b, FCL_REAL c, - const Vector3d& a0, const Vector3d& b0, const Vector3d& p0, - const Vector3d& va, const Vector3d& vb, const Vector3d& vp); + static bool solveSquare(S a, S b, S c, + const Vector3& a0, const Vector3& b0, const Vector3& p0, + const Vector3& va, const Vector3& vb, const Vector3& vp); /// @brief Compute the cubic coefficients for VF intersection /// See Paper "Interactive Continuous Collision Detection between Deformable Models using Connectivity-Based Culling", Equation 1. - static void computeCubicCoeff_VF(const Vector3d& a0, const Vector3d& b0, const Vector3d& c0, const Vector3d& p0, - const Vector3d& va, const Vector3d& vb, const Vector3d& vc, const Vector3d& vp, - FCL_REAL* a, FCL_REAL* b, FCL_REAL* c, FCL_REAL* d); + static void computeCubicCoeff_VF(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& p0, + const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vp, + S* a, S* b, S* c, S* d); /// @brief Compute the cubic coefficients for EE intersection - static void computeCubicCoeff_EE(const Vector3d& a0, const Vector3d& b0, const Vector3d& c0, const Vector3d& d0, - const Vector3d& va, const Vector3d& vb, const Vector3d& vc, const Vector3d& vd, - FCL_REAL* a, FCL_REAL* b, FCL_REAL* c, FCL_REAL* d); + static void computeCubicCoeff_EE(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, + const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vd, + S* a, S* b, S* c, S* d); /// @brief Compute the cubic coefficients for VE intersection - static void computeCubicCoeff_VE(const Vector3d& a0, const Vector3d& b0, const Vector3d& p0, - const Vector3d& va, const Vector3d& vb, const Vector3d& vp, - const Vector3d& L, - FCL_REAL* a, FCL_REAL* b, FCL_REAL* c); + static void computeCubicCoeff_VE(const Vector3& a0, const Vector3& b0, const Vector3& p0, + const Vector3& va, const Vector3& vb, const Vector3& vp, + const Vector3& L, + S* a, S* b, S* c); /// @brief filter for intersection, works for both VF and EE - static bool intersectPreFiltering(const Vector3d& a0, const Vector3d& b0, const Vector3d& c0, const Vector3d& d0, - const Vector3d& a1, const Vector3d& b1, const Vector3d& c1, const Vector3d& d1); + static bool intersectPreFiltering(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, + const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& d1); /// @brief distance of point v to a plane n * x - t = 0 - static FCL_REAL distanceToPlane(const Vector3d& n, FCL_REAL t, const Vector3d& v); + static S distanceToPlane(const Vector3& n, S t, const Vector3& v); /// @brief check wether points v1, v2, v2 are on the same side of plane n * x - t = 0 - static bool sameSideOfPlane(const Vector3d& v1, const Vector3d& v2, const Vector3d& v3, const Vector3d& n, FCL_REAL t); + static bool sameSideOfPlane(const Vector3& v1, const Vector3& v2, const Vector3& v3, const Vector3& n, S t); /// @brief clip triangle v1, v2, v3 by the prism made by t1, t2 and t3. The normal of the prism is tn and is cutted up by to - static void clipTriangleByTriangleAndEdgePlanes(const Vector3d& v1, const Vector3d& v2, const Vector3d& v3, - const Vector3d& t1, const Vector3d& t2, const Vector3d& t3, - const Vector3d& tn, FCL_REAL to, - Vector3d clipped_points[], unsigned int* num_clipped_points, bool clip_triangle = false); + static void clipTriangleByTriangleAndEdgePlanes(const Vector3& v1, const Vector3& v2, const Vector3& v3, + const Vector3& t1, const Vector3& t2, const Vector3& t3, + const Vector3& tn, S to, + Vector3 clipped_points[], unsigned int* num_clipped_points, bool clip_triangle = false); /// @brief build a plane passed through triangle v1 v2 v3 - static bool buildTrianglePlane(const Vector3d& v1, const Vector3d& v2, const Vector3d& v3, Vector3d* n, FCL_REAL* t); + static bool buildTrianglePlane(const Vector3& v1, const Vector3& v2, const Vector3& v3, Vector3* n, S* t); /// @brief build a plane pass through edge v1 and v2, normal is tn - static bool buildEdgePlane(const Vector3d& v1, const Vector3d& v2, const Vector3d& tn, Vector3d* n, FCL_REAL* t); + static bool buildEdgePlane(const Vector3& v1, const Vector3& v2, const Vector3& tn, Vector3* n, S* t); /// @brief compute the points which has deepest penetration depth - static void computeDeepestPoints(Vector3d* clipped_points, unsigned int num_clipped_points, const Vector3d& n, FCL_REAL t, FCL_REAL* penetration_depth, Vector3d* deepest_points, unsigned int* num_deepest_points); + static void computeDeepestPoints(Vector3* clipped_points, unsigned int num_clipped_points, const Vector3& n, S t, S* penetration_depth, Vector3* deepest_points, unsigned int* num_deepest_points); /// @brief clip polygon by plane - static void clipPolygonByPlane(Vector3d* polygon_points, unsigned int num_polygon_points, const Vector3d& n, FCL_REAL t, Vector3d clipped_points[], unsigned int* num_clipped_points); + static void clipPolygonByPlane(Vector3* polygon_points, unsigned int num_polygon_points, const Vector3& n, S t, Vector3 clipped_points[], unsigned int* num_clipped_points); /// @brief clip a line segment by plane - static void clipSegmentByPlane(const Vector3d& v1, const Vector3d& v2, const Vector3d& n, FCL_REAL t, Vector3d* clipped_point); + static void clipSegmentByPlane(const Vector3& v1, const Vector3& v2, const Vector3& n, S t, Vector3* clipped_point); /// @brief compute the cdf(x) - static FCL_REAL gaussianCDF(FCL_REAL x) + static S gaussianCDF(S x) { return 0.5 * std::erfc(-x / sqrt(2.0)); } - static const FCL_REAL EPSILON; - static const FCL_REAL NEAR_ZERO_THRESHOLD; - static const FCL_REAL CCD_RESOLUTION; - static const unsigned int MAX_TRIANGLE_CLIPS = 8; + static constexpr S getEpsilon() { return 1e-5; } + static constexpr S getNearZeroThreshold() { return 1e-7; } + static constexpr S getCcdResolution() { return 1e-7; } + static constexpr unsigned int getMaxTriangleClips() { return 8; } }; +using Intersectf = Intersect; +using Intersectd = Intersect; + /// @brief Project functions +template class Project { public: struct ProjectResult { /// @brief Parameterization of the projected point (based on the simplex to be projected, use 2 or 3 or 4 of the array) - FCL_REAL parameterization[4]; + S parameterization[4]; /// @brief square distance from the query point to the projected simplex - FCL_REAL sqr_distance; + S sqr_distance; /// @brief the code of the projection type unsigned int encode; - ProjectResult() : sqr_distance(-1), encode(0) + ProjectResult() : parameterization{0.0, 0.0, 0.0, 0.0}, sqr_distance(-1), encode(0) { } }; /// @brief Project point p onto line a-b - static ProjectResult projectLine(const Vector3d& a, const Vector3d& b, const Vector3d& p); + static ProjectResult projectLine(const Vector3& a, const Vector3& b, const Vector3& p); /// @brief Project point p onto triangle a-b-c - static ProjectResult projectTriangle(const Vector3d& a, const Vector3d& b, const Vector3d& c, const Vector3d& p); + static ProjectResult projectTriangle(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& p); /// @brief Project point p onto tetrahedra a-b-c-d - static ProjectResult projectTetrahedra(const Vector3d& a, const Vector3d& b, const Vector3d& c, const Vector3d& d, const Vector3d& p); + static ProjectResult projectTetrahedra(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& d, const Vector3& p); /// @brief Project origin (0) onto line a-b - static ProjectResult projectLineOrigin(const Vector3d& a, const Vector3d& b); + static ProjectResult projectLineOrigin(const Vector3& a, const Vector3& b); /// @brief Project origin (0) onto triangle a-b-c - static ProjectResult projectTriangleOrigin(const Vector3d& a, const Vector3d& b, const Vector3d& c); + static ProjectResult projectTriangleOrigin(const Vector3& a, const Vector3& b, const Vector3& c); /// @brief Project origin (0) onto tetrahedran a-b-c-d - static ProjectResult projectTetrahedraOrigin(const Vector3d& a, const Vector3d& b, const Vector3d& c, const Vector3d& d); + static ProjectResult projectTetrahedraOrigin(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& d); }; +using Projectf = Project; +using Projectd = Project; + /// @brief Triangle distance functions +template class TriangleDistance { public: @@ -295,48 +322,1917 @@ class TriangleDistance /// The second segment is Q + t * B /// X, Y are the closest points on the two segments /// VEC is the vector between X and Y - static void segPoints(const Vector3d& P, const Vector3d& A, const Vector3d& Q, const Vector3d& B, - Vector3d& VEC, Vector3d& X, Vector3d& Y); + static void segPoints(const Vector3& P, const Vector3& A, const Vector3& Q, const Vector3& B, + Vector3& VEC, Vector3& X, Vector3& Y); /// @brief Compute the closest points on two triangles given their absolute coordinate, and returns the distance between them - /// S and T are two triangles - /// If the triangles are disjoint, P and Q give the closet points of S and T respectively. However, + /// T1 and T2 are two triangles + /// If the triangles are disjoint, P and Q give the closet points of T1 and T2 respectively. However, /// if the triangles overlap, P and Q are basically a random pair of points from the triangles, not /// coincident points on the intersection of the triangles, as might be expected. - static FCL_REAL triDistance(const Vector3d S[3], const Vector3d T[3], Vector3d& P, Vector3d& Q); + static S triDistance(const Vector3 T1[3], const Vector3 T2[3], Vector3& P, Vector3& Q); - static FCL_REAL triDistance(const Vector3d& S1, const Vector3d& S2, const Vector3d& S3, - const Vector3d& T1, const Vector3d& T2, const Vector3d& T3, - Vector3d& P, Vector3d& Q); + static S triDistance(const Vector3& S1, const Vector3& S2, const Vector3& S3, + const Vector3& T1, const Vector3& T2, const Vector3& T3, + Vector3& P, Vector3& Q); /// @brief Compute the closest points on two triangles given the relative transform between them, and returns the distance between them - /// S and T are two triangles - /// If the triangles are disjoint, P and Q give the closet points of S and T respectively. However, + /// T1 and T2 are two triangles + /// If the triangles are disjoint, P and Q give the closet points of T1 and T2 respectively. However, /// if the triangles overlap, P and Q are basically a random pair of points from the triangles, not /// coincident points on the intersection of the triangles, as might be expected. /// The returned P and Q are both in the coordinate of the first triangle's coordinate - static FCL_REAL triDistance(const Vector3d S[3], const Vector3d T[3], - const Matrix3d& R, const Vector3d& Tl, - Vector3d& P, Vector3d& Q); + static S triDistance(const Vector3 T1[3], const Vector3 T2[3], + const Matrix3& R, const Vector3& Tl, + Vector3& P, Vector3& Q); + + static S triDistance(const Vector3 T1[3], const Vector3 T2[3], + const Transform3& tf, + Vector3& P, Vector3& Q); + + FCL_DEPRECATED + static S triDistance(const Vector3& S1, const Vector3& S2, const Vector3& S3, + const Vector3& T1, const Vector3& T2, const Vector3& T3, + const Matrix3& R, const Vector3& Tl, + Vector3& P, Vector3& Q); + + static S triDistance( + const Vector3& S1, + const Vector3& S2, + const Vector3& S3, + const Vector3& T1, + const Vector3& T2, + const Vector3& T3, + const Transform3& tf, + Vector3& P, + Vector3& Q); - static FCL_REAL triDistance(const Vector3d S[3], const Vector3d T[3], - const Transform3d& tf, - Vector3d& P, Vector3d& Q); +}; - static FCL_REAL triDistance(const Vector3d& S1, const Vector3d& S2, const Vector3d& S3, - const Vector3d& T1, const Vector3d& T2, const Vector3d& T3, - const Matrix3d& R, const Vector3d& Tl, - Vector3d& P, Vector3d& Q); +using TriangleDistancef = TriangleDistance; +using TriangleDistanced = TriangleDistance; - static FCL_REAL triDistance(const Vector3d& S1, const Vector3d& S2, const Vector3d& S3, - const Vector3d& T1, const Vector3d& T2, const Vector3d& T3, - const Transform3d& tf, - Vector3d& P, Vector3d& Q); +//============================================================================// +// // +// Implementations // +// // +//============================================================================// -}; +//============================================================================== +template +bool PolySolver::isZero(S v) +{ + return (v < getNearZeroThreshold()) && (v > -getNearZeroThreshold()); +} + +//============================================================================== +template +bool PolySolver::cbrt(S v) +{ + return powf(v, 1.0 / 3.0); +} + +//============================================================================== +template +int PolySolver::solveLinear(S c[2], S s[1]) +{ + if(isZero(c[1])) + return 0; + s[0] = - c[0] / c[1]; + return 1; +} + +//============================================================================== +template +int PolySolver::solveQuadric(S c[3], S s[2]) +{ + S p, q, D; + + // make sure we have a d2 equation + + if(isZero(c[2])) + return solveLinear(c, s); + + // normal for: x^2 + px + q + p = c[1] / (2.0 * c[2]); + q = c[0] / c[2]; + D = p * p - q; + + if(isZero(D)) + { + // one S root + s[0] = s[1] = -p; + return 1; + } + + if(D < 0.0) + // no real root + return 0; + else + { + // two real roots + S sqrt_D = sqrt(D); + s[0] = sqrt_D - p; + s[1] = -sqrt_D - p; + return 2; + } +} + +//============================================================================== +template +int PolySolver::solveCubic(S c[4], S s[3]) +{ + int i, num; + S sub, A, B, C, sq_A, p, q, cb_p, D; + const S ONE_OVER_THREE = 1 / 3.0; + const S PI = 3.14159265358979323846; + + // make sure we have a d2 equation + if(isZero(c[3])) + return solveQuadric(c, s); + + // normalize the equation:x ^ 3 + Ax ^ 2 + Bx + C = 0 + A = c[2] / c[3]; + B = c[1] / c[3]; + C = c[0] / c[3]; + + // substitute x = y - A / 3 to eliminate the quadratic term: x^3 + px + q = 0 + sq_A = A * A; + p = (-ONE_OVER_THREE * sq_A + B) * ONE_OVER_THREE; + q = 0.5 * (2.0 / 27.0 * A * sq_A - ONE_OVER_THREE * A * B + C); + + // use Cardano's formula + cb_p = p * p * p; + D = q * q + cb_p; + + if(isZero(D)) + { + if(isZero(q)) + { + // one triple solution + s[0] = 0.0; + num = 1; + } + else + { + // one single and one S solution + S u = cbrt(-q); + s[0] = 2.0 * u; + s[1] = -u; + num = 2; + } + } + else + { + if(D < 0.0) + { + // three real solutions + S phi = ONE_OVER_THREE * acos(-q / sqrt(-cb_p)); + S t = 2.0 * sqrt(-p); + s[0] = t * cos(phi); + s[1] = -t * cos(phi + PI / 3.0); + s[2] = -t * cos(phi - PI / 3.0); + num = 3; + } + else + { + // one real solution + S sqrt_D = sqrt(D); + S u = cbrt(sqrt_D + fabs(q)); + if(q > 0.0) + s[0] = - u + p / u ; + else + s[0] = u - p / u; + num = 1; + } + } + + // re-substitute + sub = ONE_OVER_THREE * A; + for(i = 0; i < num; i++) + s[i] -= sub; + return num; +} + +//============================================================================== +template +bool Intersect::isZero(S v) +{ + return (v < getNearZeroThreshold()) && (v > -getNearZeroThreshold()); +} + +//============================================================================== +/// @brief data: only used for EE, return the intersect point +template +bool Intersect::solveCubicWithIntervalNewton(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, + const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vd, + S& l, S& r, bool bVF, S coeffs[], Vector3* data) +{ + S v2[2]= {l*l,r*r}; + S v[2]= {l,r}; + S r_backup; + + unsigned char min3, min2, min1, max3, max2, max1; + + min3= *((unsigned char*)&coeffs[3]+7)>>7; max3=min3^1; + min2= *((unsigned char*)&coeffs[2]+7)>>7; max2=min2^1; + min1= *((unsigned char*)&coeffs[1]+7)>>7; max1=min1^1; + + // bound the cubic + + S minor = coeffs[3]*v2[min3]*v[min3]+coeffs[2]*v2[min2]+coeffs[1]*v[min1]+coeffs[0]; + S major = coeffs[3]*v2[max3]*v[max3]+coeffs[2]*v2[max2]+coeffs[1]*v[max1]+coeffs[0]; + + if(major<0) return false; + if(minor>0) return false; + + // starting here, the bounds have opposite values + S m = 0.5 * (r + l); + + // bound the derivative + S dminor = 3.0*coeffs[3]*v2[min3]+2.0*coeffs[2]*v[min2]+coeffs[1]; + S dmajor = 3.0*coeffs[3]*v2[max3]+2.0*coeffs[2]*v[max2]+coeffs[1]; + + if((dminor > 0)||(dmajor < 0)) // we can use Newton + { + S m2 = m*m; + S fm = coeffs[3]*m2*m+coeffs[2]*m2+coeffs[1]*m+coeffs[0]; + S nl = m; + S nu = m; + if(fm>0) + { + nl-=(fm/dminor); + nu-=(fm/dmajor); + } + else + { + nu-=(fm/dminor); + nl-=(fm/dmajor); + } + + //intersect with [l,r] + + if(nl>r) return false; + if(nul) + { + if(nu +bool Intersect::insideTriangle(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3&p) +{ + Vector3 ab = b - a; + Vector3 ac = c - a; + Vector3 n = ab.cross(ac); + + Vector3 pa = a - p; + Vector3 pb = b - p; + Vector3 pc = c - p; + + if((pb.cross(pc)).dot(n) < -getEpsilon()) return false; + if((pc.cross(pa)).dot(n) < -getEpsilon()) return false; + if((pa.cross(pb)).dot(n) < -getEpsilon()) return false; + + return true; +} + +//============================================================================== +template +bool Intersect::insideLineSegment(const Vector3& a, const Vector3& b, const Vector3& p) +{ + return (p - a).dot(p - b) <= 0; +} + +//============================================================================== +/// @brief Calculate the line segment papb that is the shortest route between +/// two lines p1p2 and p3p4. Calculate also the values of mua and mub where +/// pa = p1 + mua (p2 - p1) +/// pb = p3 + mub (p4 - p3) +/// Return FALSE if no solution exists. +template +bool Intersect::linelineIntersect(const Vector3& p1, const Vector3& p2, const Vector3& p3, const Vector3& p4, + Vector3* pa, Vector3* pb, S* mua, S* mub) +{ + Vector3 p31 = p1 - p3; + Vector3 p34 = p4 - p3; + if(fabs(p34[0]) < getEpsilon() && fabs(p34[1]) < getEpsilon() && fabs(p34[2]) < getEpsilon()) + return false; + + Vector3 p12 = p2 - p1; + if(fabs(p12[0]) < getEpsilon() && fabs(p12[1]) < getEpsilon() && fabs(p12[2]) < getEpsilon()) + return false; + + S d3134 = p31.dot(p34); + S d3412 = p34.dot(p12); + S d3112 = p31.dot(p12); + S d3434 = p34.dot(p34); + S d1212 = p12.dot(p12); + + S denom = d1212 * d3434 - d3412 * d3412; + if(fabs(denom) < getEpsilon()) + return false; + S numer = d3134 * d3412 - d3112 * d3434; + + *mua = numer / denom; + if(*mua < 0 || *mua > 1) + return false; + + *mub = (d3134 + d3412 * (*mua)) / d3434; + if(*mub < 0 || *mub > 1) + return false; + + *pa = p1 + p12 * (*mua); + *pb = p3 + p34 * (*mub); + return true; +} + +//============================================================================== +template +bool Intersect::checkRootValidity_VF(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& p0, + const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vp, + S t) +{ + return insideTriangle(a0 + va * t, b0 + vb * t, c0 + vc * t, p0 + vp * t); +} + +//============================================================================== +template +bool Intersect::checkRootValidity_EE(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, + const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vd, + S t, Vector3* q_i) +{ + Vector3 a = a0 + va * t; + Vector3 b = b0 + vb * t; + Vector3 c = c0 + vc * t; + Vector3 d = d0 + vd * t; + Vector3 p1, p2; + S t_ab, t_cd; + if(linelineIntersect(a, b, c, d, &p1, &p2, &t_ab, &t_cd)) + { + if(q_i) *q_i = p1; + return true; + } + + return false; +} + +//============================================================================== +template +bool Intersect::checkRootValidity_VE(const Vector3& a0, const Vector3& b0, const Vector3& p0, + const Vector3& va, const Vector3& vb, const Vector3& vp, + S t) +{ + return insideLineSegment(a0 + va * t, b0 + vb * t, p0 + vp * t); +} + +//============================================================================== +template +bool Intersect::solveSquare(S a, S b, S c, + const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, + const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vd, + bool bVF, + S* ret) +{ + S discriminant = b * b - 4 * a * c; + if(discriminant < 0) + return false; + + S sqrt_dis = sqrt(discriminant); + S r1 = (-b + sqrt_dis) / (2 * a); + bool v1 = (r1 >= 0.0 && r1 <= 1.0) ? ((bVF) ? checkRootValidity_VF(a0, b0, c0, d0, va, vb, vc, vd, r1) : checkRootValidity_EE(a0, b0, c0, d0, va, vb, vc, vd, r1)) : false; + + S r2 = (-b - sqrt_dis) / (2 * a); + bool v2 = (r2 >= 0.0 && r2 <= 1.0) ? ((bVF) ? checkRootValidity_VF(a0, b0, c0, d0, va, vb, vc, vd, r2) : checkRootValidity_EE(a0, b0, c0, d0, va, vb, vc, vd, r2)) : false; + + if(v1 && v2) + { + *ret = (r1 > r2) ? r2 : r1; + return true; + } + if(v1) + { + *ret = r1; + return true; + } + if(v2) + { + *ret = r2; + return true; + } + + return false; +} + +//============================================================================== +template +bool Intersect::solveSquare(S a, S b, S c, + const Vector3& a0, const Vector3& b0, const Vector3& p0, + const Vector3& va, const Vector3& vb, const Vector3& vp) +{ + if(isZero(a)) + { + S t = -c/b; + return (t >= 0 && t <= 1) ? checkRootValidity_VE(a0, b0, p0, va, vb, vp, t) : false; + } + + S discriminant = b*b-4*a*c; + if(discriminant < 0) + return false; + + S sqrt_dis = sqrt(discriminant); + + S r1 = (-b+sqrt_dis) / (2 * a); + bool v1 = (r1 >= 0.0 && r1 <= 1.0) ? checkRootValidity_VE(a0, b0, p0, va, vb, vp, r1) : false; + if(v1) return true; + + S r2 = (-b-sqrt_dis) / (2 * a); + bool v2 = (r2 >= 0.0 && r2 <= 1.0) ? checkRootValidity_VE(a0, b0, p0, va, vb, vp, r2) : false; + return v2; +} + +//============================================================================== +/// @brief Compute the cubic coefficients for VF case +/// See Paper "Interactive Continuous Collision Detection between Deformable Models using Connectivity-Based Culling", Equation 1. +template +void Intersect::computeCubicCoeff_VF(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& p0, + const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vp, + S* a, S* b, S* c, S* d) +{ + Vector3 vavb = vb - va; + Vector3 vavc = vc - va; + Vector3 vavp = vp - va; + Vector3 a0b0 = b0 - a0; + Vector3 a0c0 = c0 - a0; + Vector3 a0p0 = p0 - a0; + + Vector3 vavb_cross_vavc = vavb.cross(vavc); + Vector3 vavb_cross_a0c0 = vavb.cross(a0c0); + Vector3 a0b0_cross_vavc = a0b0.cross(vavc); + Vector3 a0b0_cross_a0c0 = a0b0.cross(a0c0); + + *a = vavp.dot(vavb_cross_vavc); + *b = a0p0.dot(vavb_cross_vavc) + vavp.dot(vavb_cross_a0c0 + a0b0_cross_vavc); + *c = vavp.dot(a0b0_cross_a0c0) + a0p0.dot(vavb_cross_a0c0 + a0b0_cross_vavc); + *d = a0p0.dot(a0b0_cross_a0c0); +} + +//============================================================================== +template +void Intersect::computeCubicCoeff_EE(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, + const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vd, + S* a, S* b, S* c, S* d) +{ + Vector3 vavb = vb - va; + Vector3 vcvd = vd - vc; + Vector3 vavc = vc - va; + Vector3 c0d0 = d0 - c0; + Vector3 a0b0 = b0 - a0; + Vector3 a0c0 = c0 - a0; + Vector3 vavb_cross_vcvd = vavb.cross(vcvd); + Vector3 vavb_cross_c0d0 = vavb.cross(c0d0); + Vector3 a0b0_cross_vcvd = a0b0.cross(vcvd); + Vector3 a0b0_cross_c0d0 = a0b0.cross(c0d0); + + *a = vavc.dot(vavb_cross_vcvd); + *b = a0c0.dot(vavb_cross_vcvd) + vavc.dot(vavb_cross_c0d0 + a0b0_cross_vcvd); + *c = vavc.dot(a0b0_cross_c0d0) + a0c0.dot(vavb_cross_c0d0 + a0b0_cross_vcvd); + *d = a0c0.dot(a0b0_cross_c0d0); +} + +//============================================================================== +template +void Intersect::computeCubicCoeff_VE(const Vector3& a0, const Vector3& b0, const Vector3& p0, + const Vector3& va, const Vector3& vb, const Vector3& vp, + const Vector3& L, + S* a, S* b, S* c) +{ + Vector3 vbva = va - vb; + Vector3 vbvp = vp - vb; + Vector3 b0a0 = a0 - b0; + Vector3 b0p0 = p0 - b0; + + Vector3 L_cross_vbvp = L.cross(vbvp); + Vector3 L_cross_b0p0 = L.cross(b0p0); + + *a = L_cross_vbvp.dot(vbva); + *b = L_cross_vbvp.dot(b0a0) + L_cross_b0p0.dot(vbva); + *c = L_cross_b0p0.dot(b0a0); +} +//============================================================================== +template +bool Intersect::intersect_VF(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& p0, + const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& p1, + S* collision_time, Vector3* p_i, bool useNewton) +{ + *collision_time = 2.0; + + Vector3 vp, va, vb, vc; + vp = p1 - p0; + va = a1 - a0; + vb = b1 - b0; + vc = c1 - c0; + + S a, b, c, d; + computeCubicCoeff_VF(a0, b0, c0, p0, va, vb, vc, vp, &a, &b, &c, &d); + + if(isZero(a) && isZero(b) && isZero(c) && isZero(d)) + { + return false; + } + + + /// if(isZero(a)) + /// { + /// return solveSquare(b, c, d, a0, b0, c0, p0, va, vb, vc, vp, true, collision_time); + /// } + + S coeffs[4]; + coeffs[3] = a, coeffs[2] = b, coeffs[1] = c, coeffs[0] = d; + + if(useNewton) + { + S l = 0; + S r = 1; + + if(solveCubicWithIntervalNewton(a0, b0, c0, p0, va, vb, vc, vp, l, r, true, coeffs)) + { + *collision_time = 0.5 * (l + r); + } + } + else + { + S roots[3]; + int num = PolySolver::solveCubic(coeffs, roots); + for(int i = 0; i < num; ++i) + { + S r = roots[i]; + if(r < 0 || r > 1) continue; + if(checkRootValidity_VF(a0, b0, c0, p0, va, vb, vc, vp, r)) + { + *collision_time = r; + break; + } + } + } + + if(*collision_time > 1) + { + return false; + } + + *p_i = vp * (*collision_time) + p0; + return true; +} + +//============================================================================== +template +bool Intersect::intersect_EE(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, + const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& d1, + S* collision_time, Vector3* p_i, bool useNewton) +{ + *collision_time = 2.0; + + Vector3 va, vb, vc, vd; + va = a1 - a0; + vb = b1 - b0; + vc = c1 - c0; + vd = d1 - d0; + + S a, b, c, d; + computeCubicCoeff_EE(a0, b0, c0, d0, va, vb, vc, vd, &a, &b, &c, &d); + + if(isZero(a) && isZero(b) && isZero(c) && isZero(d)) + { + return false; + } + + /// if(isZero(a)) + /// { + /// return solveSquare(b, c, d, a0, b0, c0, d0, va, vb, vc, vd, collision_time, false); + /// } + + + S coeffs[4]; + coeffs[3] = a, coeffs[2] = b, coeffs[1] = c, coeffs[0] = d; + + if(useNewton) + { + S l = 0; + S r = 1; + + if(solveCubicWithIntervalNewton(a0, b0, c0, d0, va, vb, vc, vd, l, r, false, coeffs, p_i)) + { + *collision_time = (l + r) * 0.5; + } + } + else + { + S roots[3]; + int num = PolySolver::solveCubic(coeffs, roots); + for(int i = 0; i < num; ++i) + { + S r = roots[i]; + if(r < 0 || r > 1) continue; + + if(checkRootValidity_EE(a0, b0, c0, d0, va, vb, vc, vd, r, p_i)) + { + *collision_time = r; + break; + } + } + } + + if(*collision_time > 1) + { + return false; + } + + return true; +} + +//============================================================================== +template +bool Intersect::intersect_VE(const Vector3& a0, const Vector3& b0, const Vector3& p0, + const Vector3& a1, const Vector3& b1, const Vector3& p1, + const Vector3& L) +{ + Vector3 va, vb, vp; + va = a1 - a0; + vb = b1 - b0; + vp = p1 - p0; + + S a, b, c; + computeCubicCoeff_VE(a0, b0, p0, va, vb, vp, L, &a, &b, &c); + + if(isZero(a) && isZero(b) && isZero(c)) + return true; + + return solveSquare(a, b, c, a0, b0, p0, va, vb, vp); + +} + +//============================================================================== +/// @brief Prefilter for intersection, works for both VF and EE +template +bool Intersect::intersectPreFiltering(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, + const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& d1) +{ + Vector3 n0 = (b0 - a0).cross(c0 - a0); + Vector3 n1 = (b1 - a1).cross(c1 - a1); + Vector3 a0a1 = a1 - a0; + Vector3 b0b1 = b1 - b0; + Vector3 c0c1 = c1 - c0; + Vector3 delta = (b0b1 - a0a1).cross(c0c1 - a0a1); + Vector3 nx = (n0 + n1 - delta) * 0.5; + + Vector3 a0d0 = d0 - a0; + Vector3 a1d1 = d1 - a1; + + S A = n0.dot(a0d0); + S B = n1.dot(a1d1); + S C = nx.dot(a0d0); + S D = nx.dot(a1d1); + S E = n1.dot(a0d0); + S F = n0.dot(a1d1); + + if(A > 0 && B > 0 && (2*C +F) > 0 && (2*D+E) > 0) + return false; + if(A < 0 && B < 0 && (2*C +F) < 0 && (2*D+E) < 0) + return false; + + return true; +} + +//============================================================================== +template +bool Intersect::intersect_VF_filtered(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& p0, + const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& p1, + S* collision_time, Vector3* p_i, bool useNewton) +{ + if(intersectPreFiltering(a0, b0, c0, p0, a1, b1, c1, p1)) + { + return intersect_VF(a0, b0, c0, p0, a1, b1, c1, p1, collision_time, p_i, useNewton); + } + else + return false; +} +//============================================================================== +template +bool Intersect::intersect_EE_filtered(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, + const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& d1, + S* collision_time, Vector3* p_i, bool useNewton) +{ + if(intersectPreFiltering(a0, b0, c0, d0, a1, b1, c1, d1)) + { + return intersect_EE(a0, b0, c0, d0, a1, b1, c1, d1, collision_time, p_i, useNewton); + } + else + return false; +} + +//============================================================================== +template +bool Intersect::intersect_Triangle(const Vector3& P1, const Vector3& P2, const Vector3& P3, + const Vector3& Q1, const Vector3& Q2, const Vector3& Q3, + const Matrix3& R, const Vector3& T, + Vector3* contact_points, + unsigned int* num_contact_points, + S* penetration_depth, + Vector3* normal) +{ + Vector3 Q1_ = R * Q1 + T; + Vector3 Q2_ = R * Q2 + T; + Vector3 Q3_ = R * Q3 + T; + + return intersect_Triangle(P1, P2, P3, Q1_, Q2_, Q3_, contact_points, num_contact_points, penetration_depth, normal); +} + +//============================================================================== +template +bool Intersect::intersect_Triangle( + const Vector3& P1, + const Vector3& P2, + const Vector3& P3, + const Vector3& Q1, + const Vector3& Q2, + const Vector3& Q3, + const Transform3& tf, + Vector3* contact_points, + unsigned int* num_contact_points, + S* penetration_depth, + Vector3* normal) +{ + Vector3 Q1_ = tf * Q1; + Vector3 Q2_ = tf * Q2; + Vector3 Q3_ = tf * Q3; + + return intersect_Triangle(P1, P2, P3, Q1_, Q2_, Q3_, contact_points, num_contact_points, penetration_depth, normal); +} + + +#if ODE_STYLE +//============================================================================== +template +bool Intersect::intersect_Triangle(const Vector3& P1, const Vector3& P2, const Vector3& P3, + const Vector3& Q1, const Vector3& Q2, const Vector3& Q3, + Vector3* contact_points, + unsigned int* num_contact_points, + S* penetration_depth, + Vector3* normal) +{ + + + Vector3 n1; + S t1; + bool b1 = buildTrianglePlane(P1, P2, P3, &n1, &t1); + if(!b1) return false; + + Vector3 n2; + S t2; + bool b2 = buildTrianglePlane(Q1, Q2, Q3, &n2, &t2); + if(!b2) return false; + + if(sameSideOfPlane(P1, P2, P3, n2, t2)) + return false; + + if(sameSideOfPlane(Q1, Q2, Q3, n1, t1)) + return false; + + Vector3 clipped_points1[getMaxTriangleClips()]; + unsigned int num_clipped_points1 = 0; + Vector3 clipped_points2[getMaxTriangleClips()]; + unsigned int num_clipped_points2 = 0; + + Vector3 deepest_points1[getMaxTriangleClips()]; + unsigned int num_deepest_points1 = 0; + Vector3 deepest_points2[getMaxTriangleClips()]; + unsigned int num_deepest_points2 = 0; + S penetration_depth1 = -1, penetration_depth2 = -1; + + clipTriangleByTriangleAndEdgePlanes(Q1, Q2, Q3, P1, P2, P3, n1, t1, clipped_points2, &num_clipped_points2); + + if(num_clipped_points2 == 0) + return false; + + computeDeepestPoints(clipped_points2, num_clipped_points2, n1, t1, &penetration_depth2, deepest_points2, &num_deepest_points2); + if(num_deepest_points2 == 0) + return false; + + clipTriangleByTriangleAndEdgePlanes(P1, P2, P3, Q1, Q2, Q3, n2, t2, clipped_points1, &num_clipped_points1); + if(num_clipped_points1 == 0) + return false; + + computeDeepestPoints(clipped_points1, num_clipped_points1, n2, t2, &penetration_depth1, deepest_points1, &num_deepest_points1); + if(num_deepest_points1 == 0) + return false; + + + /// Return contact information + if(contact_points && num_contact_points && penetration_depth && normal) + { + if(penetration_depth1 > penetration_depth2) + { + *num_contact_points = num_deepest_points2; + for(unsigned int i = 0; i < num_deepest_points2; ++i) + { + contact_points[i] = deepest_points2[i]; + } + + *normal = n1; + *penetration_depth = penetration_depth2; + } + else + { + *num_contact_points = num_deepest_points1; + for(unsigned int i = 0; i < num_deepest_points1; ++i) + { + contact_points[i] = deepest_points1[i]; + } + + *normal = -n2; + *penetration_depth = penetration_depth1; + } + } + + return true; +} +#else +//============================================================================== +template +bool Intersect::intersect_Triangle(const Vector3& P1, const Vector3& P2, const Vector3& P3, + const Vector3& Q1, const Vector3& Q2, const Vector3& Q3, + Vector3* contact_points, + unsigned int* num_contact_points, + S* penetration_depth, + Vector3* normal) +{ + Vector3 p1 = P1 - P1; + Vector3 p2 = P2 - P1; + Vector3 p3 = P3 - P1; + Vector3 q1 = Q1 - P1; + Vector3 q2 = Q2 - P1; + Vector3 q3 = Q3 - P1; + + Vector3 e1 = p2 - p1; + Vector3 e2 = p3 - p2; + Vector3 n1 = e1.cross(e2); + if (!project6(n1, p1, p2, p3, q1, q2, q3)) return false; + + Vector3 f1 = q2 - q1; + Vector3 f2 = q3 - q2; + Vector3 m1 = f1.cross(f2); + if (!project6(m1, p1, p2, p3, q1, q2, q3)) return false; + + Vector3 ef11 = e1.cross(f1); + if (!project6(ef11, p1, p2, p3, q1, q2, q3)) return false; + + Vector3 ef12 = e1.cross(f2); + if (!project6(ef12, p1, p2, p3, q1, q2, q3)) return false; + + Vector3 f3 = q1 - q3; + Vector3 ef13 = e1.cross(f3); + if (!project6(ef13, p1, p2, p3, q1, q2, q3)) return false; + + Vector3 ef21 = e2.cross(f1); + if (!project6(ef21, p1, p2, p3, q1, q2, q3)) return false; + + Vector3 ef22 = e2.cross(f2); + if (!project6(ef22, p1, p2, p3, q1, q2, q3)) return false; + + Vector3 ef23 = e2.cross(f3); + if (!project6(ef23, p1, p2, p3, q1, q2, q3)) return false; + + Vector3 e3 = p1 - p3; + Vector3 ef31 = e3.cross(f1); + if (!project6(ef31, p1, p2, p3, q1, q2, q3)) return false; + + Vector3 ef32 = e3.cross(f2); + if (!project6(ef32, p1, p2, p3, q1, q2, q3)) return false; + + Vector3 ef33 = e3.cross(f3); + if (!project6(ef33, p1, p2, p3, q1, q2, q3)) return false; + + Vector3 g1 = e1.cross(n1); + if (!project6(g1, p1, p2, p3, q1, q2, q3)) return false; + + Vector3 g2 = e2.cross(n1); + if (!project6(g2, p1, p2, p3, q1, q2, q3)) return false; + + Vector3 g3 = e3.cross(n1); + if (!project6(g3, p1, p2, p3, q1, q2, q3)) return false; + + Vector3 h1 = f1.cross(m1); + if (!project6(h1, p1, p2, p3, q1, q2, q3)) return false; + + Vector3 h2 = f2.cross(m1); + if (!project6(h2, p1, p2, p3, q1, q2, q3)) return false; + + Vector3 h3 = f3.cross(m1); + if (!project6(h3, p1, p2, p3, q1, q2, q3)) return false; + + if(contact_points && num_contact_points && penetration_depth && normal) + { + Vector3 n1, n2; + S t1, t2; + buildTrianglePlane(P1, P2, P3, &n1, &t1); + buildTrianglePlane(Q1, Q2, Q3, &n2, &t2); + + Vector3 deepest_points1[3]; + unsigned int num_deepest_points1 = 0; + Vector3 deepest_points2[3]; + unsigned int num_deepest_points2 = 0; + S penetration_depth1, penetration_depth2; + + Vector3 P[3] = {P1, P2, P3}; + Vector3 Q[3] = {Q1, Q2, Q3}; + + computeDeepestPoints(Q, 3, n1, t1, &penetration_depth2, deepest_points2, &num_deepest_points2); + computeDeepestPoints(P, 3, n2, t2, &penetration_depth1, deepest_points1, &num_deepest_points1); + + + if(penetration_depth1 > penetration_depth2) + { + *num_contact_points = std::min(num_deepest_points2, (unsigned int)2); + for(unsigned int i = 0; i < *num_contact_points; ++i) + { + contact_points[i] = deepest_points2[i]; + } + + *normal = n1; + *penetration_depth = penetration_depth2; + } + else + { + *num_contact_points = std::min(num_deepest_points1, (unsigned int)2); + for(unsigned int i = 0; i < *num_contact_points; ++i) + { + contact_points[i] = deepest_points1[i]; + } + + *normal = -n2; + *penetration_depth = penetration_depth1; + } + } + + return true; +} +#endif + +//============================================================================== +template +void Intersect::computeDeepestPoints(Vector3* clipped_points, unsigned int num_clipped_points, const Vector3& n, S t, S* penetration_depth, Vector3* deepest_points, unsigned int* num_deepest_points) +{ + *num_deepest_points = 0; + S max_depth = -std::numeric_limits::max(); + unsigned int num_deepest_points_ = 0; + unsigned int num_neg = 0; + unsigned int num_pos = 0; + unsigned int num_zero = 0; + + for(unsigned int i = 0; i < num_clipped_points; ++i) + { + S dist = -distanceToPlane(n, t, clipped_points[i]); + if(dist > getEpsilon()) num_pos++; + else if(dist < -getEpsilon()) num_neg++; + else num_zero++; + if(dist > max_depth) + { + max_depth = dist; + num_deepest_points_ = 1; + deepest_points[num_deepest_points_ - 1] = clipped_points[i]; + } + else if(dist + 1e-6 >= max_depth) + { + num_deepest_points_++; + deepest_points[num_deepest_points_ - 1] = clipped_points[i]; + } + } + + if(max_depth < -getEpsilon()) + num_deepest_points_ = 0; + + if(num_zero == 0 && ((num_neg == 0) || (num_pos == 0))) + num_deepest_points_ = 0; + + *penetration_depth = max_depth; + *num_deepest_points = num_deepest_points_; +} + +//============================================================================== +template +void Intersect::clipTriangleByTriangleAndEdgePlanes(const Vector3& v1, const Vector3& v2, const Vector3& v3, + const Vector3& t1, const Vector3& t2, const Vector3& t3, + const Vector3& tn, S to, + Vector3 clipped_points[], unsigned int* num_clipped_points, + bool clip_triangle) +{ + *num_clipped_points = 0; + Vector3 temp_clip[getMaxTriangleClips()]; + Vector3 temp_clip2[getMaxTriangleClips()]; + unsigned int num_temp_clip = 0; + unsigned int num_temp_clip2 = 0; + Vector3 v[3] = {v1, v2, v3}; + + Vector3 plane_n; + S plane_dist; + + if(buildEdgePlane(t1, t2, tn, &plane_n, &plane_dist)) + { + clipPolygonByPlane(v, 3, plane_n, plane_dist, temp_clip, &num_temp_clip); + if(num_temp_clip > 0) + { + if(buildEdgePlane(t2, t3, tn, &plane_n, &plane_dist)) + { + clipPolygonByPlane(temp_clip, num_temp_clip, plane_n, plane_dist, temp_clip2, &num_temp_clip2); + if(num_temp_clip2 > 0) + { + if(buildEdgePlane(t3, t1, tn, &plane_n, &plane_dist)) + { + if(clip_triangle) + { + num_temp_clip = 0; + clipPolygonByPlane(temp_clip2, num_temp_clip2, plane_n, plane_dist, temp_clip, &num_temp_clip); + if(num_temp_clip > 0) + { + clipPolygonByPlane(temp_clip, num_temp_clip, tn, to, clipped_points, num_clipped_points); + } + } + else + { + clipPolygonByPlane(temp_clip2, num_temp_clip2, plane_n, plane_dist, clipped_points, num_clipped_points); + } + } + } + } + } + } +} + +//============================================================================== +template +void Intersect::clipPolygonByPlane(Vector3* polygon_points, unsigned int num_polygon_points, const Vector3& n, S t, Vector3 clipped_points[], unsigned int* num_clipped_points) +{ + *num_clipped_points = 0; + + unsigned int num_clipped_points_ = 0; + unsigned int vi; + unsigned int prev_classify = 2; + unsigned int classify; + for(unsigned int i = 0; i <= num_polygon_points; ++i) + { + vi = (i % num_polygon_points); + S d = distanceToPlane(n, t, polygon_points[i]); + classify = ((d > getEpsilon()) ? 1 : 0); + if(classify == 0) + { + if(prev_classify == 1) + { + if(num_clipped_points_ < getMaxTriangleClips()) + { + Vector3 tmp; + clipSegmentByPlane(polygon_points[i - 1], polygon_points[vi], n, t, &tmp); + if(num_clipped_points_ > 0) + { + if((tmp - clipped_points[num_clipped_points_ - 1]).squaredNorm() > getEpsilon()) + { + clipped_points[num_clipped_points_] = tmp; + num_clipped_points_++; + } + } + else + { + clipped_points[num_clipped_points_] = tmp; + num_clipped_points_++; + } + } + } + + if(num_clipped_points_ < getMaxTriangleClips() && i < num_polygon_points) + { + clipped_points[num_clipped_points_] = polygon_points[vi]; + num_clipped_points_++; + } + } + else + { + if(prev_classify == 0) + { + if(num_clipped_points_ < getMaxTriangleClips()) + { + Vector3 tmp; + clipSegmentByPlane(polygon_points[i - 1], polygon_points[vi], n, t, &tmp); + if(num_clipped_points_ > 0) + { + if((tmp - clipped_points[num_clipped_points_ - 1]).squaredNorm() > getEpsilon()) + { + clipped_points[num_clipped_points_] = tmp; + num_clipped_points_++; + } + } + else + { + clipped_points[num_clipped_points_] = tmp; + num_clipped_points_++; + } + } + } + } + + prev_classify = classify; + } + + if(num_clipped_points_ > 2) + { + if((clipped_points[0] - clipped_points[num_clipped_points_ - 1]).squaredNorm() < getEpsilon()) + { + num_clipped_points_--; + } + } + + *num_clipped_points = num_clipped_points_; +} + +//============================================================================== +template +void Intersect::clipSegmentByPlane(const Vector3& v1, const Vector3& v2, const Vector3& n, S t, Vector3* clipped_point) +{ + S dist1 = distanceToPlane(n, t, v1); + Vector3 tmp = v2 - v1; + S dist2 = tmp.dot(n); + *clipped_point = tmp * (-dist1 / dist2) + v1; +} + +//============================================================================== +template +S Intersect::distanceToPlane(const Vector3& n, S t, const Vector3& v) +{ + return n.dot(v) - t; +} + +//============================================================================== +template +bool Intersect::buildTrianglePlane(const Vector3& v1, const Vector3& v2, const Vector3& v3, Vector3* n, S* t) +{ + Vector3 n_ = (v2 - v1).cross(v3 - v1); + bool can_normalize = false; + normalize(n_, &can_normalize); + if(can_normalize) + { + *n = n_; + *t = n_.dot(v1); + return true; + } + + return false; +} + +//============================================================================== +template +bool Intersect::buildEdgePlane(const Vector3& v1, const Vector3& v2, const Vector3& tn, Vector3* n, S* t) +{ + Vector3 n_ = (v2 - v1).cross(tn); + bool can_normalize = false; + normalize(n_, &can_normalize); + if(can_normalize) + { + *n = n_; + *t = n_.dot(v1); + return true; + } + + return false; +} + +//============================================================================== +template +bool Intersect::sameSideOfPlane(const Vector3& v1, const Vector3& v2, const Vector3& v3, const Vector3& n, S t) +{ + S dist1 = distanceToPlane(n, t, v1); + S dist2 = dist1 * distanceToPlane(n, t, v2); + S dist3 = dist1 * distanceToPlane(n, t, v3); + if((dist2 > 0) && (dist3 > 0)) + return true; + return false; +} + +//============================================================================== +template +int Intersect::project6(const Vector3& ax, + const Vector3& p1, const Vector3& p2, const Vector3& p3, + const Vector3& q1, const Vector3& q2, const Vector3& q3) +{ + S P1 = ax.dot(p1); + S P2 = ax.dot(p2); + S P3 = ax.dot(p3); + S Q1 = ax.dot(q1); + S Q2 = ax.dot(q2); + S Q3 = ax.dot(q3); + + S mn1 = std::min(P1, std::min(P2, P3)); + S mx2 = std::max(Q1, std::max(Q2, Q3)); + if(mn1 > mx2) return 0; + + S mx1 = std::max(P1, std::max(P2, P3)); + S mn2 = std::min(Q1, std::min(Q2, Q3)); + + if(mn2 > mx1) return 0; + return 1; +} + +//============================================================================== +template +void TriangleDistance::segPoints(const Vector3& P, const Vector3& A, const Vector3& Q, const Vector3& B, + Vector3& VEC, Vector3& X, Vector3& Y) +{ + Vector3 T; + S A_dot_A, B_dot_B, A_dot_B, A_dot_T, B_dot_T; + Vector3 TMP; + + T = Q - P; + A_dot_A = A.dot(A); + B_dot_B = B.dot(B); + A_dot_B = A.dot(B); + A_dot_T = A.dot(T); + B_dot_T = B.dot(T); + + // t parameterizes ray P,A + // u parameterizes ray Q,B + + S t, u; + + // compute t for the closest point on ray P,A to + // ray Q,B + + S denom = A_dot_A*B_dot_B - A_dot_B*A_dot_B; + + t = (A_dot_T*B_dot_B - B_dot_T*A_dot_B) / denom; + + // clamp result so t is on the segment P,A + + if((t < 0) || std::isnan(t)) t = 0; else if(t > 1) t = 1; + + // find u for point on ray Q,B closest to point at t + + u = (t*A_dot_B - B_dot_T) / B_dot_B; + + // if u is on segment Q,B, t and u correspond to + // closest points, otherwise, clamp u, recompute and + // clamp t + + if((u <= 0) || std::isnan(u)) + { + Y = Q; + + t = A_dot_T / A_dot_A; + + if((t <= 0) || std::isnan(t)) + { + X = P; + VEC = Q - P; + } + else if(t >= 1) + { + X = P + A; + VEC = Q - X; + } + else + { + X = P + A * t; + TMP = T.cross(A); + VEC = A.cross(TMP); + } + } + else if (u >= 1) + { + Y = Q + B; + + t = (A_dot_B + A_dot_T) / A_dot_A; + + if((t <= 0) || std::isnan(t)) + { + X = P; + VEC = Y - P; + } + else if(t >= 1) + { + X = P + A; + VEC = Y - X; + } + else + { + X = P + A * t; + T = Y - P; + TMP = T.cross(A); + VEC= A.cross(TMP); + } + } + else + { + Y = Q + B * u; + + if((t <= 0) || std::isnan(t)) + { + X = P; + TMP = T.cross(B); + VEC = B.cross(TMP); + } + else if(t >= 1) + { + X = P + A; + T = Q - X; + TMP = T.cross(B); + VEC = B.cross(TMP); + } + else + { + X = P + A * t; + VEC = A.cross(B); + if(VEC.dot(T) < 0) + { + VEC = VEC * (-1); + } + } + } +} + +//============================================================================== +template +S TriangleDistance::triDistance(const Vector3 T1[3], const Vector3 T2[3], Vector3& P, Vector3& Q) +{ + // Compute vectors along the 6 sides + + Vector3 Sv[3]; + Vector3 Tv[3]; + Vector3 VEC; + + Sv[0] = T1[1] - T1[0]; + Sv[1] = T1[2] - T1[1]; + Sv[2] = T1[0] - T1[2]; + + Tv[0] = T2[1] - T2[0]; + Tv[1] = T2[2] - T2[1]; + Tv[2] = T2[0] - T2[2]; + + // For each edge pair, the vector connecting the closest points + // of the edges defines a slab (parallel planes at head and tail + // enclose the slab). If we can show that the off-edge vertex of + // each triangle is outside of the slab, then the closest points + // of the edges are the closest points for the triangles. + // Even if these tests fail, it may be helpful to know the closest + // points found, and whether the triangles were shown disjoint + + Vector3 V; + Vector3 Z; + Vector3 minP = Vector3::Zero(); + Vector3 minQ = Vector3::Zero(); + S mindd; + int shown_disjoint = 0; + + mindd = (T1[0] - T2[0]).squaredNorm() + 1; // Set first minimum safely high + + for(int i = 0; i < 3; ++i) + { + for(int j = 0; j < 3; ++j) + { + // Find closest points on edges i & j, plus the + // vector (and distance squared) between these points + segPoints(T1[i], Sv[i], T2[j], Tv[j], VEC, P, Q); + + V = Q - P; + S dd = V.dot(V); + + // Verify this closest point pair only if the distance + // squared is less than the minimum found thus far. + + if(dd <= mindd) + { + minP = P; + minQ = Q; + mindd = dd; + + Z = T1[(i+2)%3] - P; + S a = Z.dot(VEC); + Z = T2[(j+2)%3] - Q; + S b = Z.dot(VEC); + + if((a <= 0) && (b >= 0)) return sqrt(dd); + + S p = V.dot(VEC); + + if(a < 0) a = 0; + if(b > 0) b = 0; + if((p - a + b) > 0) shown_disjoint = 1; + } + } + } + + // No edge pairs contained the closest points. + // either: + // 1. one of the closest points is a vertex, and the + // other point is interior to a face. + // 2. the triangles are overlapping. + // 3. an edge of one triangle is parallel to the other's face. If + // cases 1 and 2 are not true, then the closest points from the 9 + // edge pairs checks above can be taken as closest points for the + // triangles. + // 4. possibly, the triangles were degenerate. When the + // triangle points are nearly colinear or coincident, one + // of above tests might fail even though the edges tested + // contain the closest points. + + // First check for case 1 + + Vector3 Sn; + S Snl; + + Sn = Sv[0].cross(Sv[1]); // Compute normal to T1 triangle + Snl = Sn.dot(Sn); // Compute square of length of normal + + // If cross product is long enough, + + if(Snl > 1e-15) + { + // Get projection lengths of T2 points + + Vector3 Tp; + + V = T1[0] - T2[0]; + Tp[0] = V.dot(Sn); + + V = T1[0] - T2[1]; + Tp[1] = V.dot(Sn); + + V = T1[0] - T2[2]; + Tp[2] = V.dot(Sn); + + // If Sn is a separating direction, + // find point with smallest projection + + int point = -1; + if((Tp[0] > 0) && (Tp[1] > 0) && (Tp[2] > 0)) + { + if(Tp[0] < Tp[1]) point = 0; else point = 1; + if(Tp[2] < Tp[point]) point = 2; + } + else if((Tp[0] < 0) && (Tp[1] < 0) && (Tp[2] < 0)) + { + if(Tp[0] > Tp[1]) point = 0; else point = 1; + if(Tp[2] > Tp[point]) point = 2; + } + + // If Sn is a separating direction, + + if(point >= 0) + { + shown_disjoint = 1; + + // Test whether the point found, when projected onto the + // other triangle, lies within the face. + + V = T2[point] - T1[0]; + Z = Sn.cross(Sv[0]); + if(V.dot(Z) > 0) + { + V = T2[point] - T1[1]; + Z = Sn.cross(Sv[1]); + if(V.dot(Z) > 0) + { + V = T2[point] - T1[2]; + Z = Sn.cross(Sv[2]); + if(V.dot(Z) > 0) + { + // T[point] passed the test - it's a closest point for + // the T2 triangle; the other point is on the face of T1 + P = T2[point] + Sn * (Tp[point] / Snl); + Q = T2[point]; + return (P - Q).norm(); + } + } + } + } + } + + Vector3 Tn; + S Tnl; + + Tn = Tv[0].cross(Tv[1]); + Tnl = Tn.dot(Tn); + + if(Tnl > 1e-15) + { + Vector3 Sp; + + V = T2[0] - T1[0]; + Sp[0] = V.dot(Tn); + + V = T2[0] - T1[1]; + Sp[1] = V.dot(Tn); + + V = T2[0] - T1[2]; + Sp[2] = V.dot(Tn); + + int point = -1; + if((Sp[0] > 0) && (Sp[1] > 0) && (Sp[2] > 0)) + { + if(Sp[0] < Sp[1]) point = 0; else point = 1; + if(Sp[2] < Sp[point]) point = 2; + } + else if((Sp[0] < 0) && (Sp[1] < 0) && (Sp[2] < 0)) + { + if(Sp[0] > Sp[1]) point = 0; else point = 1; + if(Sp[2] > Sp[point]) point = 2; + } + + if(point >= 0) + { + shown_disjoint = 1; + + V = T1[point] - T2[0]; + Z = Tn.cross(Tv[0]); + if(V.dot(Z) > 0) + { + V = T1[point] - T2[1]; + Z = Tn.cross(Tv[1]); + if(V.dot(Z) > 0) + { + V = T1[point] - T2[2]; + Z = Tn.cross(Tv[2]); + if(V.dot(Z) > 0) + { + P = T1[point]; + Q = T1[point] + Tn * (Sp[point] / Tnl); + return (P - Q).norm(); + } + } + } + } + } + + // Case 1 can't be shown. + // If one of these tests showed the triangles disjoint, + // we assume case 3 or 4, otherwise we conclude case 2, + // that the triangles overlap. + + if(shown_disjoint) + { + P = minP; + Q = minQ; + return sqrt(mindd); + } + else return 0; +} + +//============================================================================== +template +S TriangleDistance::triDistance(const Vector3& S1, const Vector3& S2, const Vector3& S3, + const Vector3& T1, const Vector3& T2, const Vector3& T3, + Vector3& P, Vector3& Q) +{ + Vector3 U[3]; + Vector3 T[3]; + U[0] = S1; U[1] = S2; U[2] = S3; + T[0] = T1; T[1] = T2; T[2] = T3; + + return triDistance(U, T, P, Q); +} + +//============================================================================== +template +S TriangleDistance::triDistance(const Vector3 T1[3], const Vector3 T2[3], + const Matrix3& R, const Vector3& Tl, + Vector3& P, Vector3& Q) +{ + Vector3 T_transformed[3]; + T_transformed[0] = R * T2[0] + Tl; + T_transformed[1] = R * T2[1] + Tl; + T_transformed[2] = R * T2[2] + Tl; + + return triDistance(T1, T_transformed, P, Q); +} + +//============================================================================== +template +S TriangleDistance::triDistance(const Vector3 T1[3], const Vector3 T2[3], + const Transform3& tf, + Vector3& P, Vector3& Q) +{ + Vector3 T_transformed[3]; + T_transformed[0] = tf * T2[0]; + T_transformed[1] = tf * T2[1]; + T_transformed[2] = tf * T2[2]; + + return triDistance(T1, T_transformed, P, Q); +} + +//============================================================================== +template +S TriangleDistance::triDistance(const Vector3& S1, const Vector3& S2, const Vector3& S3, + const Vector3& T1, const Vector3& T2, const Vector3& T3, + const Matrix3& R, const Vector3& Tl, + Vector3& P, Vector3& Q) +{ + Vector3 T1_transformed = R * T1 + Tl; + Vector3 T2_transformed = R * T2 + Tl; + Vector3 T3_transformed = R * T3 + Tl; + return triDistance(S1, S2, S3, T1_transformed, T2_transformed, T3_transformed, P, Q); +} + +//============================================================================== +template +S TriangleDistance::triDistance(const Vector3& S1, const Vector3& S2, const Vector3& S3, + const Vector3& T1, const Vector3& T2, const Vector3& T3, + const Transform3& tf, + Vector3& P, Vector3& Q) +{ + Vector3 T1_transformed = tf * T1; + Vector3 T2_transformed = tf * T2; + Vector3 T3_transformed = tf * T3; + return triDistance(S1, S2, S3, T1_transformed, T2_transformed, T3_transformed, P, Q); +} + +//============================================================================== +template +typename Project::ProjectResult Project::projectLine(const Vector3& a, const Vector3& b, const Vector3& p) +{ + ProjectResult res; + + const Vector3 d = b - a; + const S l = d.squaredNorm(); + + if(l > 0) + { + const S t = (p - a).dot(d); + res.parameterization[1] = (t >= l) ? 1 : ((t <= 0) ? 0 : (t / l)); + res.parameterization[0] = 1 - res.parameterization[1]; + if(t >= l) { res.sqr_distance = (p - b).squaredNorm(); res.encode = 2; /* 0x10 */ } + else if(t <= 0) { res.sqr_distance = (p - a).squaredNorm(); res.encode = 1; /* 0x01 */ } + else { res.sqr_distance = (a + d * res.parameterization[1] - p).squaredNorm(); res.encode = 3; /* 0x00 */ } + } + + return res; +} + +//============================================================================== +template +typename Project::ProjectResult Project::projectTriangle(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& p) +{ + ProjectResult res; + + static const size_t nexti[3] = {1, 2, 0}; + const Vector3* vt[] = {&a, &b, &c}; + const Vector3 dl[] = {a - b, b - c, c - a}; + const Vector3& n = dl[0].cross(dl[1]); + const S l = n.squaredNorm(); + + if(l > 0) + { + S mindist = -1; + for(size_t i = 0; i < 3; ++i) + { + if((*vt[i] - p).dot(dl[i].cross(n)) > 0) // origin is to the outside part of the triangle edge, then the optimal can only be on the edge + { + size_t j = nexti[i]; + ProjectResult res_line = projectLine(*vt[i], *vt[j], p); + + if(mindist < 0 || res_line.sqr_distance < mindist) + { + mindist = res_line.sqr_distance; + res.encode = static_cast(((res_line.encode&1)?1< p_to_project = n * (d / l); + mindist = p_to_project.squaredNorm(); + res.encode = 7; // m = 0x111 + res.parameterization[0] = dl[1].cross(b - p -p_to_project).norm() / s; + res.parameterization[1] = dl[2].cross(c - p -p_to_project).norm() / s; + res.parameterization[2] = 1 - res.parameterization[0] - res.parameterization[1]; + } + + res.sqr_distance = mindist; + } + + return res; + +} + +//============================================================================== +template +typename Project::ProjectResult Project::projectTetrahedra(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& d, const Vector3& p) +{ + ProjectResult res; + + static const size_t nexti[] = {1, 2, 0}; + const Vector3* vt[] = {&a, &b, &c, &d}; + const Vector3 dl[3] = {a-d, b-d, c-d}; + S vl = triple(dl[0], dl[1], dl[2]); + bool ng = (vl * (a-p).dot((b-c).cross(a-b))) <= 0; + if(ng && std::abs(vl) > 0) // abs(vl) == 0, the tetrahedron is degenerated; if ng is false, then the last vertex in the tetrahedron does not grow toward the origin (in fact origin is on the other side of the abc face) + { + S mindist = -1; + + for(size_t i = 0; i < 3; ++i) + { + size_t j = nexti[i]; + S s = vl * (d-p).dot(dl[i].cross(dl[j])); + if(s > 0) // the origin is to the outside part of a triangle face, then the optimal can only be on the triangle face + { + ProjectResult res_triangle = projectTriangle(*vt[i], *vt[j], d, p); + if(mindist < 0 || res_triangle.sqr_distance < mindist) + { + mindist = res_triangle.sqr_distance; + res.encode = static_cast( (res_triangle.encode&1?1< +typename Project::ProjectResult Project::projectLineOrigin(const Vector3& a, const Vector3& b) +{ + ProjectResult res; + + const Vector3 d = b - a; + const S l = d.squaredNorm(); + + if(l > 0) + { + const S t = - a.dot(d); + res.parameterization[1] = (t >= l) ? 1 : ((t <= 0) ? 0 : (t / l)); + res.parameterization[0] = 1 - res.parameterization[1]; + if(t >= l) { res.sqr_distance = b.squaredNorm(); res.encode = 2; /* 0x10 */ } + else if(t <= 0) { res.sqr_distance = a.squaredNorm(); res.encode = 1; /* 0x01 */ } + else { res.sqr_distance = (a + d * res.parameterization[1]).squaredNorm(); res.encode = 3; /* 0x00 */ } + } + + return res; +} + +//============================================================================== +template +typename Project::ProjectResult Project::projectTriangleOrigin(const Vector3& a, const Vector3& b, const Vector3& c) +{ + ProjectResult res; + + static const size_t nexti[3] = {1, 2, 0}; + const Vector3* vt[] = {&a, &b, &c}; + const Vector3 dl[] = {a - b, b - c, c - a}; + const Vector3& n = dl[0].cross(dl[1]); + const S l = n.squaredNorm(); + + if(l > 0) + { + S mindist = -1; + for(size_t i = 0; i < 3; ++i) + { + if(vt[i]->dot(dl[i].cross(n)) > 0) // origin is to the outside part of the triangle edge, then the optimal can only be on the edge + { + size_t j = nexti[i]; + ProjectResult res_line = projectLineOrigin(*vt[i], *vt[j]); + + if(mindist < 0 || res_line.sqr_distance < mindist) + { + mindist = res_line.sqr_distance; + res.encode = static_cast(((res_line.encode&1)?1< o_to_project = n * (d / l); + mindist = o_to_project.squaredNorm(); + res.encode = 7; // m = 0x111 + res.parameterization[0] = dl[1].cross(b - o_to_project).norm() / s; + res.parameterization[1] = dl[2].cross(c - o_to_project).norm() / s; + res.parameterization[2] = 1 - res.parameterization[0] - res.parameterization[1]; + } + + res.sqr_distance = mindist; + } + + return res; + +} + +//============================================================================== +template +typename Project::ProjectResult Project::projectTetrahedraOrigin(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& d) +{ + ProjectResult res; + + static const size_t nexti[] = {1, 2, 0}; + const Vector3* vt[] = {&a, &b, &c, &d}; + const Vector3 dl[3] = {a-d, b-d, c-d}; + S vl = triple(dl[0], dl[1], dl[2]); + bool ng = (vl * a.dot((b-c).cross(a-b))) <= 0; + if(ng && std::abs(vl) > 0) // abs(vl) == 0, the tetrahedron is degenerated; if ng is false, then the last vertex in the tetrahedron does not grow toward the origin (in fact origin is on the other side of the abc face) + { + S mindist = -1; + + for(size_t i = 0; i < 3; ++i) + { + size_t j = nexti[i]; + S s = vl * d.dot(dl[i].cross(dl[j])); + if(s > 0) // the origin is to the outside part of a triangle face, then the optimal can only be on the triangle face + { + ProjectResult res_triangle = projectTriangleOrigin(*vt[i], *vt[j], d); + if(mindist < 0 || res_triangle.sqr_distance < mindist) + { + mindist = res_triangle.sqr_distance; + res.encode = static_cast( (res_triangle.encode&1?1< +struct constants +{ +/// The mathematical constant pi +static constexpr S pi() { return S(3.141592653589793238462643383279502884197169399375105820974944592L); } + +/// The golden ratio +static constexpr S phi() { return S(1.618033988749894848204586834365638117720309179805762862135448623L); } +}; + +using constantsf = constants; +using constantsd = constants; + +} // namespace fcl #endif diff --git a/include/fcl/math/detail/seed.h b/include/fcl/math/detail/seed.h new file mode 100644 index 000000000..06b6de185 --- /dev/null +++ b/include/fcl/math/detail/seed.h @@ -0,0 +1,164 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_MATH_DETAIL_SEED_H +#define FCL_MATH_DETAIL_SEED_H + +#include +#include + +#include "fcl/math/constants.h" + +namespace fcl +{ +namespace detail +{ + +class Seed +{ +public: + + static bool isFirstSeedGenerated(); + + static std::uint_fast32_t getUserSetSeed(); + + static void setUserSetSeed(std::uint_fast32_t seed); + + static std::uint_fast32_t getFirstSeed(); + + /// We use a different random number generator for the seeds of the + /// Other random generators. The root seed is from the number of + /// nano-seconds in the current time. + static std::uint_fast32_t getNextSeed(); + +protected: + + Seed(); + + static Seed& getInstance(); + + /// The seed the user asked for (cannot be 0) + std::uint_fast32_t userSetSeed; + + /// Flag indicating whether the first seed has already been generated or not + bool firstSeedGenerated; + + /// The value of the first seed + std::uint_fast32_t firstSeedValue; +}; + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +bool Seed::isFirstSeedGenerated() +{ + return getInstance().firstSeedGenerated; +} + +//============================================================================== +uint_fast32_t Seed::getUserSetSeed() +{ + return getInstance().userSetSeed; +} + +//============================================================================== +void Seed::setUserSetSeed(uint_fast32_t seed) +{ + getInstance().userSetSeed = seed; +} + +//============================================================================== +uint_fast32_t Seed::getFirstSeed() +{ + // Compute the first seed to be used; this function should be called only once + static std::mutex fsLock; + std::unique_lock slock(fsLock); + + if (getInstance().firstSeedGenerated) + return getInstance().firstSeedValue; + + if (getInstance().userSetSeed != 0) + { + getInstance().firstSeedValue = getInstance().userSetSeed; + } + else + { + getInstance().firstSeedValue + = static_cast( + std::chrono::duration_cast( + std::chrono::system_clock::now() + - std::chrono::system_clock::time_point()).count()); + } + + getInstance().firstSeedGenerated = true; + + return getInstance().firstSeedValue; +} + +//============================================================================== +uint_fast32_t Seed::getNextSeed() +{ + static std::mutex rngMutex; + std::unique_lock slock(rngMutex); + static std::ranlux24_base sGen; + static std::uniform_int_distribution<> sDist(1, 1000000000); + + return sDist(sGen); +} + +//============================================================================== +Seed::Seed() : userSetSeed(0), firstSeedGenerated(false), firstSeedValue(0) +{ + // Do nothing +} + +//============================================================================== +Seed& Seed::getInstance() +{ + static Seed seed; + + return seed; +} + +} // namespace detail +} // namespace fcl + +#endif diff --git a/include/fcl/math/geometry.h b/include/fcl/math/geometry.h index 77e78bf7e..59047c3c1 100644 --- a/include/fcl/math/geometry.h +++ b/include/fcl/math/geometry.h @@ -35,21 +35,152 @@ /** \author Jia Pan */ -#ifndef FCL_GEOMETRY_H -#define FCL_GEOMETRY_H +#ifndef FCL_MATH_GEOMETRY_H +#define FCL_MATH_GEOMETRY_H #include #include +#include #include "fcl/config.h" #include "fcl/data_types.h" +#include "fcl/math/triangle.h" -namespace fcl -{ +namespace fcl { + +template +void normalize(Vector3& v, bool* signal); + +template +typename Derived::RealScalar triple(const Eigen::MatrixBase& x, + const Eigen::MatrixBase& y, + const Eigen::MatrixBase& z); + +template +void generateCoordinateSystem( + const Eigen::MatrixBase& w, + Eigen::MatrixBase& u, + Eigen::MatrixBase& v); + +template +VectorN combine( + const VectorN& v1, const VectorN& v2); + +template +void hat(Matrix3& mat, const Vector3& vec); + +/// @brief compute the eigen vector and eigen vector of a matrix. dout is the +/// eigen values, vout is the eigen vectors +template +void eigen(const Matrix3& m, Vector3& dout, Matrix3& vout); + +/// @brief compute the eigen vector and eigen vector of a matrix. dout is the +/// eigen values, vout is the eigen vectors +template +void eigen_old(const Matrix3& m, Vector3& dout, Matrix3& vout); + +template +void axisFromEigen(const Matrix3& eigenV, + const Vector3& eigenS, + Matrix3& axis); + +template +void axisFromEigen(const Matrix3& eigenV, + const Vector3& eigenS, + Transform3& tf); + +template +void generateCoordinateSystem(Matrix3& axis); + +template +void generateCoordinateSystem(Transform3& tf); -inline void normalize(Vector3d& v, bool* signal) +template +void relativeTransform( + const Eigen::MatrixBase& R1, const Eigen::MatrixBase& t1, + const Eigen::MatrixBase& R2, const Eigen::MatrixBase& t2, + Eigen::MatrixBase& R, Eigen::MatrixBase& t); + +template +void relativeTransform( + const Eigen::Transform& T1, + const Eigen::Transform& T2, + Eigen::MatrixBase& R, Eigen::MatrixBase& t); + +/// @brief Compute the RSS bounding volume parameters: radius, rectangle size +/// and the origin, given the BV axises. +template +void getRadiusAndOriginAndRectangleSize( + Vector3* ps, + Vector3* ps2, + Triangle* ts, + unsigned int* indices, + int n, + const Matrix3& axis, + Vector3& origin, + S l[2], + S& r); + +/// @brief Compute the RSS bounding volume parameters: radius, rectangle size +/// and the origin, given the BV axises. +template +void getRadiusAndOriginAndRectangleSize( + Vector3* ps, + Vector3* ps2, + Triangle* ts, + unsigned int* indices, + int n, + Transform3& tf, + S l[2], + S& r); + +/// @brief Compute the center and radius for a triangle's circumcircle +template +void circumCircleComputation( + const Vector3& a, + const Vector3& b, + const Vector3& c, + Vector3& center, + S& radius); + +template +S maximumDistance_mesh( + Vector3* ps, + Vector3* ps2, + Triangle* ts, + unsigned int* indices, + int n, + const Vector3& query); + +template +S maximumDistance_pointcloud( + Vector3* ps, + Vector3* ps2, + unsigned int* indices, + int n, + const Vector3& query); + +/// @brief Compute the maximum distance from a given center point to a point cloud +template +S maximumDistance( + Vector3* ps, + Vector3* ps2, + Triangle* ts, + unsigned int* indices, + int n, + const Vector3& query); + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +void normalize(Vector3& v, bool* signal) { - Vector3d::Scalar sqr_length = v.squaredNorm(); + S sqr_length = v.squaredNorm(); if (sqr_length > 0) { @@ -62,21 +193,29 @@ inline void normalize(Vector3d& v, bool* signal) } } -inline Vector3d::Scalar triple(const Vector3d& x, const Vector3d& y, const Vector3d& z) +//============================================================================== +template +typename Derived::RealScalar triple(const Eigen::MatrixBase& x, + const Eigen::MatrixBase& y, + const Eigen::MatrixBase& z) { return x.dot(y.cross(z)); } -template -void generateCoordinateSystem(const Vector3& w, Vector3& u, Vector3& v) +//============================================================================== +template +void generateCoordinateSystem( + const Eigen::MatrixBase& w, + Eigen::MatrixBase& u, + Eigen::MatrixBase& v) { - T inv_length; + typename Derived::RealScalar inv_length; if(std::abs(w[0]) >= std::abs(w[1])) { - inv_length = (T)1.0 / sqrt(w[0] * w[0] + w[2] * w[2]); + inv_length = 1.0 / std::sqrt(w[0] * w[0] + w[2] * w[2]); u[0] = -w[2] * inv_length; - u[1] = (T)0; + u[1] = 0; u[2] = w[0] * inv_length; v[0] = w[1] * u[2]; v[1] = w[2] * u[0] - w[0] * u[2]; @@ -84,8 +223,8 @@ void generateCoordinateSystem(const Vector3& w, Vector3& u, Vector3& v) } else { - inv_length = (T)1.0 / sqrt(w[1] * w[1] + w[2] * w[2]); - u[0] = (T)0; + inv_length = 1.0 / std::sqrt(w[1] * w[1] + w[2] * w[2]); + u[0] = 0; u[1] = w[2] * inv_length; u[2] = -w[1] * inv_length; v[0] = w[1] * u[2] - w[2] * u[1]; @@ -94,28 +233,30 @@ void generateCoordinateSystem(const Vector3& w, Vector3& u, Vector3& v) } } -template -VectorN combine(const VectorN& v1, const VectorN& v2) +//============================================================================== +template +VectorN combine( + const VectorN& v1, const VectorN& v2) { - VectorN v; + VectorN v; v << v1, v2; return v; } -template -void hat(Matrix3& mat, const Vector3& vec) +//============================================================================== +template +void hat(Matrix3& mat, const Vector3& vec) { mat << 0, -vec[2], vec[1], vec[2], 0, -vec[0], -vec[1], vec[0], 0; } -/// @brief compute the eigen vector and eigen vector of a matrix. dout is the -/// eigen values, vout is the eigen vectors -template -void eigen(const Matrix3& m, Vector3& dout, Matrix3& vout) +//============================================================================== +template +void eigen(const Matrix3& m, Vector3& dout, Matrix3& vout) { // We assume that m is symmetric matrix. - Eigen::SelfAdjointEigenSolver> eigensolver(m); + Eigen::SelfAdjointEigenSolver> eigensolver(m); if (eigensolver.info() != Eigen::Success) { std::cerr << "[eigen] Failed to compute eigendecomposition.\n"; @@ -125,8 +266,176 @@ void eigen(const Matrix3& m, Vector3& dout, Matrix3& vout) vout = eigensolver.eigenvectors(); } -template -void generateCoordinateSystem(Matrix3& axis) +//============================================================================== +template +void eigen_old(const Matrix3& m, Vector3& dout, Matrix3& vout) +{ + Matrix3 R(m); + int n = 3; + int j, iq, ip, i; + S tresh, theta, tau, t, sm, s, h, g, c; + int nrot; + S b[3]; + S z[3]; + S v[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; + S d[3]; + + for(ip = 0; ip < n; ++ip) + { + b[ip] = d[ip] = R(ip, ip); + z[ip] = 0; + } + + nrot = 0; + + for(i = 0; i < 50; ++i) + { + sm = 0; + for(ip = 0; ip < n; ++ip) + for(iq = ip + 1; iq < n; ++iq) + sm += std::abs(R(ip, iq)); + if(sm == 0.0) + { + vout.col(0) << v[0][0], v[0][1], v[0][2]; + vout.col(1) << v[1][0], v[1][1], v[1][2]; + vout.col(2) << v[2][0], v[2][1], v[2][2]; + dout[0] = d[0]; dout[1] = d[1]; dout[2] = d[2]; + return; + } + + if(i < 3) tresh = 0.2 * sm / (n * n); + else tresh = 0.0; + + for(ip = 0; ip < n; ++ip) + { + for(iq= ip + 1; iq < n; ++iq) + { + g = 100.0 * std::abs(R(ip, iq)); + if(i > 3 && + std::abs(d[ip]) + g == std::abs(d[ip]) && + std::abs(d[iq]) + g == std::abs(d[iq])) + R(ip, iq) = 0.0; + else if(std::abs(R(ip, iq)) > tresh) + { + h = d[iq] - d[ip]; + if(std::abs(h) + g == std::abs(h)) t = (R(ip, iq)) / h; + else + { + theta = 0.5 * h / (R(ip, iq)); + t = 1.0 /(std::abs(theta) + std::sqrt(1.0 + theta * theta)); + if(theta < 0.0) t = -t; + } + c = 1.0 / std::sqrt(1 + t * t); + s = t * c; + tau = s / (1.0 + c); + h = t * R(ip, iq); + z[ip] -= h; + z[iq] += h; + d[ip] -= h; + d[iq] += h; + R(ip, iq) = 0.0; + for(j = 0; j < ip; ++j) { g = R(j, ip); h = R(j, iq); R(j, ip) = g - s * (h + g * tau); R(j, iq) = h + s * (g - h * tau); } + for(j = ip + 1; j < iq; ++j) { g = R(ip, j); h = R(j, iq); R(ip, j) = g - s * (h + g * tau); R(j, iq) = h + s * (g - h * tau); } + for(j = iq + 1; j < n; ++j) { g = R(ip, j); h = R(iq, j); R(ip, j) = g - s * (h + g * tau); R(iq, j) = h + s * (g - h * tau); } + for(j = 0; j < n; ++j) { g = v[j][ip]; h = v[j][iq]; v[j][ip] = g - s * (h + g * tau); v[j][iq] = h + s * (g - h * tau); } + nrot++; + } + } + } + for(ip = 0; ip < n; ++ip) + { + b[ip] += z[ip]; + d[ip] = b[ip]; + z[ip] = 0.0; + } + } + + std::cerr << "eigen: too many iterations in Jacobi transform." << std::endl; + + return; +} + +//============================================================================== +template +void axisFromEigen(const Matrix3& eigenV, + const Vector3& eigenS, + Matrix3& axis) +{ + int min, mid, max; + + if(eigenS[0] > eigenS[1]) + { + max = 0; + min = 1; + } + else + { + min = 0; + max = 1; + } + + if(eigenS[2] < eigenS[min]) + { + mid = min; + min = 2; + } + else if(eigenS[2] > eigenS[max]) + { + mid = max; + max = 2; + } + else + { + mid = 2; + } + + axis.col(0) = eigenV.row(max); + axis.col(1) = eigenV.row(mid); + axis.col(2).noalias() = axis.col(0).cross(axis.col(1)); +} + +//============================================================================== +template +void axisFromEigen(const Matrix3& eigenV, + const Vector3& eigenS, + Transform3& tf) +{ + int min, mid, max; + + if(eigenS[0] > eigenS[1]) + { + max = 0; + min = 1; + } + else + { + min = 0; + max = 1; + } + + if(eigenS[2] < eigenS[min]) + { + mid = min; + min = 2; + } + else if(eigenS[2] > eigenS[max]) + { + mid = max; + max = 2; + } + else + { + mid = 2; + } + + tf.linear().col(0) = eigenV.col(max); + tf.linear().col(1) = eigenV.col(mid); + tf.linear().col(2).noalias() = tf.linear().col(0).cross(tf.linear().col(1)); +} + +//============================================================================== +template +void generateCoordinateSystem(Matrix3& axis) { // Assum axis.col(0) is closest to z-axis assert(axis.col(0).maxCoeff() == 2); @@ -139,7 +448,7 @@ void generateCoordinateSystem(Matrix3& axis) // // othorgonal // axis.col(2) = axis.col(0).cross(axis.col(1)) - T inv_length = 1.0 / sqrt(std::pow(axis(0, 0), 2) + std::pow(axis(2, 0), 2)); + S inv_length = 1.0 / sqrt(std::pow(axis(0, 0), 2) + std::pow(axis(2, 0), 2)); axis(0, 1) = -axis(2, 0) * inv_length; axis(1, 1) = 0; @@ -157,7 +466,7 @@ void generateCoordinateSystem(Matrix3& axis) // // othorgonal // axis.col(2) = axis.col(0).cross(axis.col(1)) - T inv_length = 1.0 / sqrt(std::pow(axis(1, 0), 2) + std::pow(axis(2, 0), 2)); + S inv_length = 1.0 / sqrt(std::pow(axis(1, 0), 2) + std::pow(axis(2, 0), 2)); axis(0, 1) = 0; axis(1, 1) = axis(2, 0) * inv_length; @@ -169,54 +478,767 @@ void generateCoordinateSystem(Matrix3& axis) } } -template +//============================================================================== +template +void generateCoordinateSystem(Transform3& tf) +{ + // Assum axis.col(0) is closest to z-axis + assert(tf.linear().col(0).maxCoeff() == 2); + + if(std::abs(tf.linear()(0, 0)) >= std::abs(tf.linear()(1, 0))) + { + // let axis.col(0) = (x, y, z) + // axis.col(1) = (-z, 0, x) / length((-z, 0, x)) // so that axis.col(0) and + // // axis.col(1) are + // // othorgonal + // axis.col(2) = axis.col(0).cross(axis.col(1)) + + S inv_length = 1.0 / sqrt(std::pow(tf.linear()(0, 0), 2) + std::pow(tf.linear()(2, 0), 2)); + + tf.linear()(0, 1) = -tf.linear()(2, 0) * inv_length; + tf.linear()(1, 1) = 0; + tf.linear()(2, 1) = tf.linear()(0, 0) * inv_length; + + tf.linear()(0, 2) = tf.linear()(1, 0) * tf.linear()(2, 1); + tf.linear()(1, 2) = tf.linear()(2, 0) * tf.linear()(0, 1) - tf.linear()(0, 0) * tf.linear()(2, 1); + tf.linear()(2, 2) = -tf.linear()(1, 0) * tf.linear()(0, 1); + } + else + { + // let axis.col(0) = (x, y, z) + // axis.col(1) = (0, z, -y) / length((0, z, -y)) // so that axis.col(0) and + // // axis.col(1) are + // // othorgonal + // axis.col(2) = axis.col(0).cross(axis.col(1)) + + S inv_length = 1.0 / sqrt(std::pow(tf.linear()(1, 0), 2) + std::pow(tf.linear()(2, 0), 2)); + + tf.linear()(0, 1) = 0; + tf.linear()(1, 1) = tf.linear()(2, 0) * inv_length; + tf.linear()(2, 1) = -tf.linear()(1, 0) * inv_length; + + tf.linear()(0, 2) = tf.linear()(1, 0) * tf.linear()(2, 1) - tf.linear()(2, 0) * tf.linear()(1, 1); + tf.linear()(1, 2) = -tf.linear()(0, 0) * tf.linear()(2, 1); + tf.linear()(2, 2) = tf.linear()(0, 0) * tf.linear()(1, 1); + } +} + +//============================================================================== +template void relativeTransform( - const Eigen::MatrixBase& R1, const Eigen::MatrixBase& t1, - const Eigen::MatrixBase& R2, const Eigen::MatrixBase& t2, - Eigen::MatrixBase& R, Eigen::MatrixBase& t) + const Eigen::MatrixBase& R1, const Eigen::MatrixBase& t1, + const Eigen::MatrixBase& R2, const Eigen::MatrixBase& t2, + Eigen::MatrixBase& R, Eigen::MatrixBase& t) { EIGEN_STATIC_ASSERT( - Derived1::RowsAtCompileTime == 3 - && Derived1::ColsAtCompileTime == 3, + DerivedA::RowsAtCompileTime == 3 + && DerivedA::ColsAtCompileTime == 3, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE); EIGEN_STATIC_ASSERT( - Derived2::RowsAtCompileTime == 3 - && Derived2::ColsAtCompileTime == 1, + DerivedB::RowsAtCompileTime == 3 + && DerivedB::ColsAtCompileTime == 1, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE); EIGEN_STATIC_ASSERT( - Derived3::RowsAtCompileTime == 3 - && Derived3::ColsAtCompileTime == 3, + DerivedC::RowsAtCompileTime == 3 + && DerivedC::ColsAtCompileTime == 3, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE); EIGEN_STATIC_ASSERT( - Derived4::RowsAtCompileTime == 3 - && Derived4::ColsAtCompileTime == 1, + DerivedD::RowsAtCompileTime == 3 + && DerivedD::ColsAtCompileTime == 1, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE); - R = R1.transpose() * R2; - t = R1.transpose() * (t2 - t1); + R.noalias() = R1.transpose() * R2; + t.noalias() = R1.transpose() * (t2 - t1); } -template +//============================================================================== +template void relativeTransform( - const Eigen::Transform& T1, - const Eigen::Transform& T2, - Eigen::MatrixBase& R, Eigen::MatrixBase& t) + const Transform3& T1, + const Transform3& T2, + Eigen::MatrixBase& R, Eigen::MatrixBase& t) { EIGEN_STATIC_ASSERT( - Derived1::RowsAtCompileTime == 3 - && Derived1::ColsAtCompileTime == 3, + DerivedA::RowsAtCompileTime == 3 + && DerivedA::ColsAtCompileTime == 3, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE); EIGEN_STATIC_ASSERT( - Derived2::RowsAtCompileTime == 3 - && Derived2::ColsAtCompileTime == 1, + DerivedB::RowsAtCompileTime == 3 + && DerivedB::ColsAtCompileTime == 1, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE); - relativeTransform( - T1.linear(), T1.translation(), T2.linear(), T2.translation(), R, t); + R.noalias() = T1.linear().transpose() * T2.linear(); + t.noalias() = T1.linear().transpose() * (T2.translation() - T1.translation()); +} + +//============================================================================== +template +void getRadiusAndOriginAndRectangleSize( + Vector3* ps, + Vector3* ps2, + Triangle* ts, + unsigned int* indices, + int n, + const Matrix3& axis, + Vector3& origin, + S l[2], + S& r) +{ + bool indirect_index = true; + if(!indices) indirect_index = false; + + int size_P = ((ps2) ? 2 : 1) * ((ts) ? 3 : 1) * n; + + std::vector> P(size_P); + + int P_id = 0; + + if(ts) + { + for(int i = 0; i < n; ++i) + { + int index = indirect_index ? indices[i] : i; + const Triangle& t = ts[index]; + + for(int j = 0; j < 3; ++j) + { + int point_id = t[j]; + const Vector3& p = ps[point_id]; + P[P_id][0] = axis.col(0).dot(p); + P[P_id][1] = axis.col(1).dot(p); + P[P_id][2] = axis.col(2).dot(p); + P_id++; + } + + if(ps2) + { + for(int j = 0; j < 3; ++j) + { + int point_id = t[j]; + const Vector3& p = ps2[point_id]; + P[P_id][0] = axis.col(0).dot(p); + P[P_id][1] = axis.col(1).dot(p); + P[P_id][2] = axis.col(2).dot(p); + P_id++; + } + } + } + } + else + { + for(int i = 0; i < n; ++i) + { + int index = indirect_index ? indices[i] : i; + + const Vector3& p = ps[index]; + P[P_id][0] = axis.col(0).dot(p); + P[P_id][1] = axis.col(1).dot(p); + P[P_id][2] = axis.col(2).dot(p); + P_id++; + + if(ps2) + { + const Vector3& v = ps2[index]; + P[P_id][0] = axis.col(0).dot(v); + P[P_id][1] = axis.col(1).dot(v); + P[P_id][2] = axis.col(2).dot(v); + P_id++; + } + } + } + + S minx, maxx, miny, maxy, minz, maxz; + + S cz, radsqr; + + minz = maxz = P[0][2]; + + for(int i = 1; i < size_P; ++i) + { + S z_value = P[i][2]; + if(z_value < minz) minz = z_value; + else if(z_value > maxz) maxz = z_value; + } + + r = (S)0.5 * (maxz - minz); + radsqr = r * r; + cz = (S)0.5 * (maxz + minz); + + // compute an initial length of rectangle along x direction + + // find minx and maxx as starting points + + int minindex, maxindex; + minindex = maxindex = 0; + S mintmp, maxtmp; + mintmp = maxtmp = P[0][0]; + + for(int i = 1; i < size_P; ++i) + { + S x_value = P[i][0]; + if(x_value < mintmp) + { + minindex = i; + mintmp = x_value; + } + else if(x_value > maxtmp) + { + maxindex = i; + maxtmp = x_value; + } + } + + S x, dz; + dz = P[minindex][2] - cz; + minx = P[minindex][0] + std::sqrt(std::max(radsqr - dz * dz, 0)); + dz = P[maxindex][2] - cz; + maxx = P[maxindex][0] - std::sqrt(std::max(radsqr - dz * dz, 0)); + + + // grow minx + + for(int i = 0; i < size_P; ++i) + { + if(P[i][0] < minx) + { + dz = P[i][2] - cz; + x = P[i][0] + std::sqrt(std::max(radsqr - dz * dz, 0)); + if(x < minx) minx = x; + } + } + + // grow maxx + + for(int i = 0; i < size_P; ++i) + { + if(P[i][0] > maxx) + { + dz = P[i][2] - cz; + x = P[i][0] - std::sqrt(std::max(radsqr - dz * dz, 0)); + if(x > maxx) maxx = x; + } + } + + // compute an initial length of rectangle along y direction + + // find miny and maxy as starting points + + minindex = maxindex = 0; + mintmp = maxtmp = P[0][1]; + for(int i = 1; i < size_P; ++i) + { + S y_value = P[i][1]; + if(y_value < mintmp) + { + minindex = i; + mintmp = y_value; + } + else if(y_value > maxtmp) + { + maxindex = i; + maxtmp = y_value; + } + } + + S y; + dz = P[minindex][2] - cz; + miny = P[minindex][1] + std::sqrt(std::max(radsqr - dz * dz, 0)); + dz = P[maxindex][2] - cz; + maxy = P[maxindex][1] - std::sqrt(std::max(radsqr - dz * dz, 0)); + + // grow miny + + for(int i = 0; i < size_P; ++i) + { + if(P[i][1] < miny) + { + dz = P[i][2] - cz; + y = P[i][1] + std::sqrt(std::max(radsqr - dz * dz, 0)); + if(y < miny) miny = y; + } + } + + // grow maxy + + for(int i = 0; i < size_P; ++i) + { + if(P[i][1] > maxy) + { + dz = P[i][2] - cz; + y = P[i][1] - std::sqrt(std::max(radsqr - dz * dz, 0)); + if(y > maxy) maxy = y; + } + } + + // corners may have some points which are not covered - grow lengths if necessary + // quite conservative (can be improved) + S dx, dy, u, t; + S a = std::sqrt((S)0.5); + for(int i = 0; i < size_P; ++i) + { + if(P[i][0] > maxx) + { + if(P[i][1] > maxy) + { + dx = P[i][0] - maxx; + dy = P[i][1] - maxy; + u = dx * a + dy * a; + t = (a*u - dx)*(a*u - dx) + + (a*u - dy)*(a*u - dy) + + (cz - P[i][2])*(cz - P[i][2]); + u = u - std::sqrt(std::max(radsqr - t, 0)); + if(u > 0) + { + maxx += u*a; + maxy += u*a; + } + } + else if(P[i][1] < miny) + { + dx = P[i][0] - maxx; + dy = P[i][1] - miny; + u = dx * a - dy * a; + t = (a*u - dx)*(a*u - dx) + + (-a*u - dy)*(-a*u - dy) + + (cz - P[i][2])*(cz - P[i][2]); + u = u - std::sqrt(std::max(radsqr - t, 0)); + if(u > 0) + { + maxx += u*a; + miny -= u*a; + } + } + } + else if(P[i][0] < minx) + { + if(P[i][1] > maxy) + { + dx = P[i][0] - minx; + dy = P[i][1] - maxy; + u = dy * a - dx * a; + t = (-a*u - dx)*(-a*u - dx) + + (a*u - dy)*(a*u - dy) + + (cz - P[i][2])*(cz - P[i][2]); + u = u - std::sqrt(std::max(radsqr - t, 0)); + if(u > 0) + { + minx -= u*a; + maxy += u*a; + } + } + else if(P[i][1] < miny) + { + dx = P[i][0] - minx; + dy = P[i][1] - miny; + u = -dx * a - dy * a; + t = (-a*u - dx)*(-a*u - dx) + + (-a*u - dy)*(-a*u - dy) + + (cz - P[i][2])*(cz - P[i][2]); + u = u - std::sqrt(std::max(radsqr - t, 0)); + if (u > 0) + { + minx -= u*a; + miny -= u*a; + } + } + } + } + + origin = axis.col(0) * minx + axis.col(1) * miny + axis.col(2) * cz; + + l[0] = maxx - minx; + if(l[0] < 0) l[0] = 0; + l[1] = maxy - miny; + if(l[1] < 0) l[1] = 0; + +} + +//============================================================================== +template +void getRadiusAndOriginAndRectangleSize( + Vector3* ps, + Vector3* ps2, + Triangle* ts, + unsigned int* indices, + int n, + Transform3& tf, + S l[2], + S& r) +{ + bool indirect_index = true; + if(!indices) indirect_index = false; + + int size_P = ((ps2) ? 2 : 1) * ((ts) ? 3 : 1) * n; + + std::vector> P(size_P); + + int P_id = 0; + + if(ts) + { + for(int i = 0; i < n; ++i) + { + int index = indirect_index ? indices[i] : i; + const Triangle& t = ts[index]; + + for(int j = 0; j < 3; ++j) + { + int point_id = t[j]; + const Vector3& p = ps[point_id]; + P[P_id][0] = tf.linear().col(0).dot(p); + P[P_id][1] = tf.linear().col(1).dot(p); + P[P_id][2] = tf.linear().col(2).dot(p); + P_id++; + } + + if(ps2) + { + for(int j = 0; j < 3; ++j) + { + int point_id = t[j]; + const Vector3& p = ps2[point_id]; + P[P_id][0] = tf.linear().col(0).dot(p); + P[P_id][1] = tf.linear().col(1).dot(p); + P[P_id][2] = tf.linear().col(2).dot(p); + P_id++; + } + } + } + } + else + { + for(int i = 0; i < n; ++i) + { + int index = indirect_index ? indices[i] : i; + + const Vector3& p = ps[index]; + P[P_id][0] = tf.linear().col(0).dot(p); + P[P_id][1] = tf.linear().col(1).dot(p); + P[P_id][2] = tf.linear().col(2).dot(p); + P_id++; + + if(ps2) + { + P[P_id][0] = tf.linear().col(0).dot(ps2[index]); + P[P_id][1] = tf.linear().col(1).dot(ps2[index]); + P[P_id][2] = tf.linear().col(2).dot(ps2[index]); + P_id++; + } + } + } + + S minx, maxx, miny, maxy, minz, maxz; + + S cz, radsqr; + + minz = maxz = P[0][2]; + + for(int i = 1; i < size_P; ++i) + { + S z_value = P[i][2]; + if(z_value < minz) minz = z_value; + else if(z_value > maxz) maxz = z_value; + } + + r = (S)0.5 * (maxz - minz); + radsqr = r * r; + cz = (S)0.5 * (maxz + minz); + + // compute an initial length of rectangle along x direction + + // find minx and maxx as starting points + + int minindex, maxindex; + minindex = maxindex = 0; + S mintmp, maxtmp; + mintmp = maxtmp = P[0][0]; + + for(int i = 1; i < size_P; ++i) + { + S x_value = P[i][0]; + if(x_value < mintmp) + { + minindex = i; + mintmp = x_value; + } + else if(x_value > maxtmp) + { + maxindex = i; + maxtmp = x_value; + } + } + + S x, dz; + dz = P[minindex][2] - cz; + minx = P[minindex][0] + std::sqrt(std::max(radsqr - dz * dz, 0)); + dz = P[maxindex][2] - cz; + maxx = P[maxindex][0] - std::sqrt(std::max(radsqr - dz * dz, 0)); + + + // grow minx + + for(int i = 0; i < size_P; ++i) + { + if(P[i][0] < minx) + { + dz = P[i][2] - cz; + x = P[i][0] + std::sqrt(std::max(radsqr - dz * dz, 0)); + if(x < minx) minx = x; + } + } + + // grow maxx + + for(int i = 0; i < size_P; ++i) + { + if(P[i][0] > maxx) + { + dz = P[i][2] - cz; + x = P[i][0] - std::sqrt(std::max(radsqr - dz * dz, 0)); + if(x > maxx) maxx = x; + } + } + + // compute an initial length of rectangle along y direction + + // find miny and maxy as starting points + + minindex = maxindex = 0; + mintmp = maxtmp = P[0][1]; + for(int i = 1; i < size_P; ++i) + { + S y_value = P[i][1]; + if(y_value < mintmp) + { + minindex = i; + mintmp = y_value; + } + else if(y_value > maxtmp) + { + maxindex = i; + maxtmp = y_value; + } + } + + S y; + dz = P[minindex][2] - cz; + miny = P[minindex][1] + std::sqrt(std::max(radsqr - dz * dz, 0)); + dz = P[maxindex][2] - cz; + maxy = P[maxindex][1] - std::sqrt(std::max(radsqr - dz * dz, 0)); + + // grow miny + + for(int i = 0; i < size_P; ++i) + { + if(P[i][1] < miny) + { + dz = P[i][2] - cz; + y = P[i][1] + std::sqrt(std::max(radsqr - dz * dz, 0)); + if(y < miny) miny = y; + } + } + + // grow maxy + + for(int i = 0; i < size_P; ++i) + { + if(P[i][1] > maxy) + { + dz = P[i][2] - cz; + y = P[i][1] - std::sqrt(std::max(radsqr - dz * dz, 0)); + if(y > maxy) maxy = y; + } + } + + // corners may have some points which are not covered - grow lengths if necessary + // quite conservative (can be improved) + S dx, dy, u, t; + S a = std::sqrt((S)0.5); + for(int i = 0; i < size_P; ++i) + { + if(P[i][0] > maxx) + { + if(P[i][1] > maxy) + { + dx = P[i][0] - maxx; + dy = P[i][1] - maxy; + u = dx * a + dy * a; + t = (a*u - dx)*(a*u - dx) + + (a*u - dy)*(a*u - dy) + + (cz - P[i][2])*(cz - P[i][2]); + u = u - std::sqrt(std::max(radsqr - t, 0)); + if(u > 0) + { + maxx += u*a; + maxy += u*a; + } + } + else if(P[i][1] < miny) + { + dx = P[i][0] - maxx; + dy = P[i][1] - miny; + u = dx * a - dy * a; + t = (a*u - dx)*(a*u - dx) + + (-a*u - dy)*(-a*u - dy) + + (cz - P[i][2])*(cz - P[i][2]); + u = u - std::sqrt(std::max(radsqr - t, 0)); + if(u > 0) + { + maxx += u*a; + miny -= u*a; + } + } + } + else if(P[i][0] < minx) + { + if(P[i][1] > maxy) + { + dx = P[i][0] - minx; + dy = P[i][1] - maxy; + u = dy * a - dx * a; + t = (-a*u - dx)*(-a*u - dx) + + (a*u - dy)*(a*u - dy) + + (cz - P[i][2])*(cz - P[i][2]); + u = u - std::sqrt(std::max(radsqr - t, 0)); + if(u > 0) + { + minx -= u*a; + maxy += u*a; + } + } + else if(P[i][1] < miny) + { + dx = P[i][0] - minx; + dy = P[i][1] - miny; + u = -dx * a - dy * a; + t = (-a*u - dx)*(-a*u - dx) + + (-a*u - dy)*(-a*u - dy) + + (cz - P[i][2])*(cz - P[i][2]); + u = u - std::sqrt(std::max(radsqr - t, 0)); + if (u > 0) + { + minx -= u*a; + miny -= u*a; + } + } + } + } + + tf.translation().noalias() = tf.linear().col(0) * minx; + tf.translation().noalias() += tf.linear().col(1) * miny; + tf.translation().noalias() += tf.linear().col(2) * cz; + + l[0] = maxx - minx; + if(l[0] < 0) l[0] = 0; + l[1] = maxy - miny; + if(l[1] < 0) l[1] = 0; +} + +//============================================================================== +template +void circumCircleComputation( + const Vector3& a, + const Vector3& b, + const Vector3& c, + Vector3& center, + S& radius) +{ + Vector3 e1 = a - c; + Vector3 e2 = b - c; + S e1_len2 = e1.squaredNorm(); + S e2_len2 = e2.squaredNorm(); + Vector3 e3 = e1.cross(e2); + S e3_len2 = e3.squaredNorm(); + radius = e1_len2 * e2_len2 * (e1 - e2).squaredNorm() / e3_len2; + radius = std::sqrt(radius) * 0.5; + + center = c; + center.noalias() += (e2 * e1_len2 - e1 * e2_len2).cross(e3) * (0.5 * 1 / e3_len2); +} + +//============================================================================== +template +S maximumDistance_mesh(Vector3* ps, Vector3* ps2, Triangle* ts, unsigned int* indices, int n, const Vector3& query) +{ + bool indirect_index = true; + if(!indices) indirect_index = false; + + S maxD = 0; + for(int i = 0; i < n; ++i) + { + unsigned int index = indirect_index ? indices[i] : i; + const Triangle& t = ts[index]; + + for(int j = 0; j < 3; ++j) + { + int point_id = t[j]; + const Vector3& p = ps[point_id]; + + S d = (p - query).squaredNorm(); + if(d > maxD) maxD = d; + } + + if(ps2) + { + for(int j = 0; j < 3; ++j) + { + int point_id = t[j]; + const Vector3& p = ps2[point_id]; + + S d = (p - query).squaredNorm(); + if(d > maxD) maxD = d; + } + } + } + + return std::sqrt(maxD); +} + +//============================================================================== +template +S maximumDistance_pointcloud(Vector3* ps, Vector3* ps2, unsigned int* indices, int n, const Vector3& query) +{ + bool indirect_index = true; + if(!indices) indirect_index = false; + + S maxD = 0; + for(int i = 0; i < n; ++i) + { + int index = indirect_index ? indices[i] : i; + + const Vector3& p = ps[index]; + S d = (p - query).squaredNorm(); + if(d > maxD) maxD = d; + + if(ps2) + { + const Vector3& v = ps2[index]; + S d = (v - query).squaredNorm(); + if(d > maxD) maxD = d; + } + } + + return std::sqrt(maxD); +} + +//============================================================================== +template +S maximumDistance( + Vector3* ps, + Vector3* ps2, + Triangle* ts, + unsigned int* indices, + int n, + const Vector3& query) +{ + if(ts) + return maximumDistance_mesh(ps, ps2, ts, indices, n, query); + else + return maximumDistance_pointcloud(ps, ps2, indices, n, query); } } // namespace fcl diff --git a/include/fcl/math/matrix_3f.h b/include/fcl/math/matrix_3f.h index 138c33f99..dd47f2843 100644 --- a/include/fcl/math/matrix_3f.h +++ b/include/fcl/math/matrix_3f.h @@ -35,8 +35,8 @@ /** \author Jia Pan */ -#ifndef FCL_MATRIX_3F_H -#define FCL_MATRIX_3F_H +#ifndef FCL_MATH_MATRIX3F_H +#define FCL_MATH_MATRIX3F_H #warning "This header has been deprecated in FCL 0.6. "\ "Please include fcl/data_types.h and fcl/math/geometry.h instead." diff --git a/include/fcl/math/rng.h b/include/fcl/math/rng.h new file mode 100644 index 000000000..fc5242533 --- /dev/null +++ b/include/fcl/math/rng.h @@ -0,0 +1,311 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_MATH_RNG_H +#define FCL_MATH_RNG_H + +#include +#include +#include + +#include "fcl/math/constants.h" +#include "fcl/math/detail/seed.h" + +namespace fcl +{ + +/// @brief Random number generation. An instance of this class +/// cannot be used by multiple threads at once (member functions +/// are not const). However, the constructor is thread safe and +/// different instances can be used safely in any number of +/// threads. It is also guaranteed that all created instances will +/// have a different random seed. +template +class RNG +{ +public: + /// @brief Constructor. Always sets a different random seed + RNG(); + + /// @brief Generate a random real between 0 and 1 + S uniform01(); + + /// @brief Generate a random real within given bounds: [\e lower_bound, + /// \e upper_bound) + S uniformReal(S lower_bound, S upper_bound); + + /// @brief Generate a random integer within given bounds: [\e lower_bound, + /// \e upper_bound] + int uniformInt(int lower_bound, int upper_bound); + + /// @brief Generate a random boolean + bool uniformBool(); + + /// @brief Generate a random real using a normal distribution with mean 0 and + /// variance 1 + S gaussian01(); + + /// @brief Generate a random real using a normal distribution with given mean + /// and variance + S gaussian(S mean, S stddev); + + /// @brief Generate a random real using a half-normal distribution. The value + /// is within specified bounds [\e r_min, \e r_max], but with a bias towards + /// \e r_max. The function is implemended using a Gaussian distribution with + /// mean at \e r_max - \e r_min. The distribution is 'folded' around \e r_max + /// axis towards \e r_min. The variance of the distribution is (\e r_max - + /// \e r_min) / \e focus. The higher the focus, the more probable it is that + /// generated numbers are close to \e r_max. + S halfNormalReal(S r_min, S r_max, S focus = 3.0); + + /// @brief Generate a random integer using a half-normal distribution. The + /// value is within specified bounds ([\e r_min, \e r_max]), but with a bias + /// towards \e r_max. The function is implemented on top of halfNormalReal() + int halfNormalInt(int r_min, int r_max, S focus = 3.0); + + /// @brief Uniform random unit quaternion sampling. The computed value has the + /// order (x,y,z,w) + void quaternion(S value[4]); + + /// @brief Uniform random sampling of Euler roll-pitch-yaw angles, each in the + /// range [-pi, pi). The computed value has the order (roll, pitch, yaw) */ + void eulerRPY(S value[3]); + + /// @brief Uniform random sample on a disk with radius from r_min to r_max + void disk(S r_min, S r_max, S& x, S& y); + + /// @brief Uniform random sample in a ball with radius from r_min to r_max + void ball(S r_min, S r_max, S& x, S& y, S& z); + + /// @brief Set the seed for random number generation. Use this function to + /// ensure the same sequence of random numbers is generated. + static void setSeed(std::uint_fast32_t seed); + + /// @brief Get the seed used for random number generation. Passing the + /// returned value to setSeed() at a subsequent execution of the code will + /// ensure deterministic (repeatable) behaviour. Useful for debugging. + static std::uint_fast32_t getSeed(); + +private: + + std::mt19937 generator_; + std::uniform_real_distribution<> uniDist_; + std::normal_distribution<> normalDist_; + +}; + +using RNGf = RNG; +using RNGd = RNG; + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +RNG::RNG() + : generator_(detail::Seed::getNextSeed()), uniDist_(0, 1), normalDist_(0, 1) +{ +} + +//============================================================================== +template +S RNG::uniform01() +{ + return uniDist_(generator_); +} + +//============================================================================== +template +S RNG::uniformReal(S lower_bound, S upper_bound) +{ + assert(lower_bound <= upper_bound); + + return (upper_bound - lower_bound) * uniDist_(generator_) + lower_bound; +} + +//============================================================================== +template +int RNG::uniformInt(int lower_bound, int upper_bound) +{ + int r = (int)floor(uniformReal((S)lower_bound, (S)(upper_bound) + 1.0)); + + return (r > upper_bound) ? upper_bound : r; +} + +//============================================================================== +template +bool RNG::uniformBool() +{ + return uniDist_(generator_) <= 0.5; +} + +//============================================================================== +template +S RNG::gaussian01() +{ + return normalDist_(generator_); +} + +//============================================================================== +template +S RNG::gaussian(S mean, S stddev) +{ + return normalDist_(generator_) * stddev + mean; +} + +//============================================================================== +template +S RNG::halfNormalReal(S r_min, S r_max, S focus) +{ + assert(r_min <= r_max); + + const auto mean = r_max - r_min; + auto v = gaussian(mean, mean / focus); + + if (v > mean) + v = 2.0 * mean - v; + + auto r = v >= 0.0 ? v + r_min : r_min; + + return r > r_max ? r_max : r; +} + +//============================================================================== +template +int RNG::halfNormalInt(int r_min, int r_max, S focus) +{ + int r = (int)std::floor(halfNormalReal( + (S)r_min, (S)(r_max) + 1.0, focus)); + + return (r > r_max) ? r_max : r; +} + +//============================================================================== +template +void RNG::quaternion(S value[]) +{ + auto x0 = uniDist_(generator_); + auto r1 = std::sqrt(1.0 - x0), r2 = std::sqrt(x0); + auto t1 = 2.0 * constants::pi() * uniDist_(generator_); + auto t2 = 2.0 * constants::pi() * uniDist_(generator_); + auto c1 = std::cos(t1); + auto s1 = std::sin(t1); + auto c2 = std::cos(t2); + auto s2 = std::sin(t2); + value[0] = s1 * r1; + value[1] = c1 * r1; + value[2] = s2 * r2; + value[3] = c2 * r2; +} + +//============================================================================== +template +void RNG::eulerRPY(S value[]) +{ + value[0] = constants::pi() * (2.0 * uniDist_(generator_) - 1.0); + value[1] = std::acos(1.0 - 2.0 * uniDist_(generator_)) - constants::pi() / 2.0; + value[2] = constants::pi() * (2.0 * uniDist_(generator_) - 1.0); +} + +//============================================================================== +template +void RNG::disk(S r_min, S r_max, S& x, S& y) +{ + auto a = uniform01(); + auto b = uniform01(); + auto r = std::sqrt(a * r_max * r_max + (1 - a) * r_min * r_min); + auto theta = 2 * constants::pi() * b; + x = r * std::cos(theta); + y = r * std::sin(theta); +} + +//============================================================================== +template +void RNG::ball( + S r_min, S r_max, S& x, S& y, S& z) +{ + auto a = uniform01(); + auto b = uniform01(); + auto c = uniform01(); + auto r = std::pow(a*std::pow(r_max, 3) + (1 - a)*std::pow(r_min, 3), 1/3.0); + auto theta = std::acos(1 - 2 * b); + auto phi = 2 * constants::pi() * c; + + auto costheta = std::cos(theta); + auto sintheta = std::sin(theta); + auto cosphi = std::cos(phi); + auto sinphi = std::sin(phi); + x = r * costheta; + y = r * sintheta * cosphi; + z = r * sintheta * sinphi; +} + +//============================================================================== +template +void RNG::setSeed(uint_fast32_t seed) +{ + if (detail::Seed::isFirstSeedGenerated()) + { + std::cerr << "Random number generation already started. Changing seed now " + << "will not lead to deterministic sampling." << std::endl; + } + + if (seed == 0) + { + std::cerr << "Random generator seed cannot be 0. Using 1 instead." + << std::endl; + detail::Seed::setUserSetSeed(1); + } + else + { + detail::Seed::setUserSetSeed(seed); + } +} + +//============================================================================== +template +uint_fast32_t RNG::getSeed() +{ + return detail::Seed::getFirstSeed(); +} + +} // namespace fcl + +#endif diff --git a/src/broadphase/broadphase_spatialhash.cpp b/include/fcl/math/sampler_base.h similarity index 87% rename from src/broadphase/broadphase_spatialhash.cpp rename to include/fcl/math/sampler_base.h index c994efb32..98883eb71 100644 --- a/src/broadphase/broadphase_spatialhash.cpp +++ b/include/fcl/math/sampler_base.h @@ -1,7 +1,7 @@ /* * Software License Agreement (BSD License) * - * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2013-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * @@ -35,9 +35,21 @@ /** \author Jia Pan */ -#include "fcl/broadphase/broadphase_spatialhash.h" +#ifndef FCL_MATH_SAMPLERBASE_H +#define FCL_MATH_SAMPLERBASE_H + +#include "fcl/math/rng.h" namespace fcl { -} +template +class SamplerBase +{ +public: + mutable RNG rng; +}; + +} // namespace fcl + +#endif diff --git a/include/fcl/math/sampler_r.h b/include/fcl/math/sampler_r.h new file mode 100644 index 000000000..fde0a9920 --- /dev/null +++ b/include/fcl/math/sampler_r.h @@ -0,0 +1,131 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_MATH_SAMPLERR_H +#define FCL_MATH_SAMPLERR_H + +#include +#include "fcl/data_types.h" +#include "fcl/math/sampler_base.h" + +namespace fcl +{ + +template +class SamplerR : public SamplerBase +{ +public: + SamplerR(); + + SamplerR(const VectorN& lower_bound_, + const VectorN& upper_bound_); + + void setBound(const VectorN& lower_bound_, + const VectorN& upper_bound_); + + void getBound(VectorN& lower_bound_, + VectorN& upper_bound_) const; + + VectorN sample() const; + +private: + VectorN lower_bound; + VectorN upper_bound; + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(S, N) + +}; + +template +using SamplerRf = SamplerR; +template +using SamplerRd = SamplerR; + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +SamplerR::SamplerR() +{ + // Do nothing +} + +//============================================================================== +template +SamplerR::SamplerR(const VectorN& lower_bound_, const VectorN& upper_bound_) + : lower_bound(lower_bound_), upper_bound(upper_bound_) +{ +} + +//============================================================================== +template +void SamplerR::setBound(const VectorN& lower_bound_, const VectorN& upper_bound_) +{ + lower_bound = lower_bound_; + upper_bound = upper_bound_; +} + +//============================================================================== +template +void SamplerR::getBound(VectorN& lower_bound_, VectorN& upper_bound_) const +{ + lower_bound_ = lower_bound; + upper_bound_ = upper_bound; +} + +//============================================================================== +template +VectorN SamplerR::sample() const +{ + VectorN q; + + for(std::size_t i = 0; i < N; ++i) + { + q[i] = this->rng.uniformReal(lower_bound[i], upper_bound[i]); + } + + return q; +} + +} // namespace fcl + +#endif diff --git a/include/fcl/math/sampler_se2.h b/include/fcl/math/sampler_se2.h new file mode 100644 index 000000000..ccb2f6ec1 --- /dev/null +++ b/include/fcl/math/sampler_se2.h @@ -0,0 +1,139 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_MATH_SAMPLERSE2_H +#define FCL_MATH_SAMPLERSE2_H + +#include "fcl/data_types.h" +#include "fcl/math/sampler_base.h" + +namespace fcl +{ + +template +class SamplerSE2 : public SamplerBase +{ +public: + SamplerSE2(); + + SamplerSE2(const Vector2& lower_bound_, + const Vector2& upper_bound_); + + SamplerSE2(S x_min, S x_max, + S y_min, S y_max); + + + void setBound(const Vector2& lower_bound_, + const Vector2& upper_bound_); + + void getBound(Vector2& lower_bound_, + Vector2& upper_bound_) const; + + + Vector3 sample() const; + +protected: + Vector2 lower_bound; + Vector2 upper_bound; + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +using SamplerSE2f = SamplerSE2; +using SamplerSE2d = SamplerSE2; + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +SamplerSE2::SamplerSE2() +{ + // Do nothing +} + +//============================================================================== +template +SamplerSE2::SamplerSE2(const Vector2& lower_bound_, const Vector2& upper_bound_) : lower_bound(lower_bound_), + upper_bound(upper_bound_) +{ + // Do nothing +} + +//============================================================================== +template +SamplerSE2::SamplerSE2(S x_min, S x_max, S y_min, S y_max) : lower_bound(Vector2(x_min, y_min)), + upper_bound(Vector2(x_max, y_max)) +{ + // Do nothing +} + +//============================================================================== +template +void SamplerSE2::getBound(Vector2& lower_bound_, Vector2& upper_bound_) const +{ + lower_bound_ = lower_bound; + upper_bound_ = upper_bound; +} + +//============================================================================== +template +void SamplerSE2::setBound(const Vector2& lower_bound_, const Vector2& upper_bound_) +{ + lower_bound = lower_bound_; + upper_bound = upper_bound_; +} + +//============================================================================== +template +Vector3 SamplerSE2::sample() const +{ + Vector3 q; + q[0] = this->rng.uniformReal(lower_bound[0], lower_bound[1]); + q[1] = this->rng.uniformReal(lower_bound[1], lower_bound[2]); + q[2] = this->rng.uniformReal(-constants::pi(), constants::pi()); + + return q; +} + +} // namespace fcl + +#endif diff --git a/include/fcl/math/sampler_se2_disk.h b/include/fcl/math/sampler_se2_disk.h new file mode 100644 index 000000000..53ac5af81 --- /dev/null +++ b/include/fcl/math/sampler_se2_disk.h @@ -0,0 +1,115 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_MATH_SAMPLERSE2DISK_H +#define FCL_MATH_SAMPLERSE2DISK_H + +#include "fcl/data_types.h" +#include "fcl/math/sampler_base.h" + +namespace fcl +{ + +template +class SamplerSE2_disk : public SamplerBase +{ +public: + SamplerSE2_disk(); + + SamplerSE2_disk(S cx, S cy, + S r1, S r2, + S crefx, S crefy); + + void setBound(S cx, S cy, + S r1, S r2, + S crefx, S crefy); + + Vector3 sample() const; + +protected: + S c[2]; + S cref[2]; + S r_min, r_max; +}; + +using SamplerSE2_diskf = SamplerSE2_disk; +using SamplerSE2_diskd = SamplerSE2_disk; + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +SamplerSE2_disk::SamplerSE2_disk() {} + +//============================================================================== +template +SamplerSE2_disk::SamplerSE2_disk(S cx, S cy, S r1, S r2, S crefx, S crefy) +{ + setBound(cx, cy, r1, r2, crefx, crefy); +} + +//============================================================================== +template +void SamplerSE2_disk::setBound(S cx, S cy, S r1, S r2, S crefx, S crefy) +{ + c[0] = cx; c[1] = cy; + cref[0] = crefx; cref[1] = crefy; + r_min = r1; + r_max = r2; +} + +//============================================================================== +template +Vector3 SamplerSE2_disk::sample() const +{ + Vector3 q; + S x, y; + this->rng.disk(r_min, r_max, x, y); + q[0] = x + c[0] - cref[0]; + q[1] = y + c[1] - cref[1]; + q[2] = this->rng.uniformReal(-constants::pi(), constants::pi()); + + return q; +} + +} // namespace fcl + +#endif diff --git a/include/fcl/math/sampler_se3_euler.h b/include/fcl/math/sampler_se3_euler.h new file mode 100644 index 000000000..bb9a5e381 --- /dev/null +++ b/include/fcl/math/sampler_se3_euler.h @@ -0,0 +1,135 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_MATH_SAMPLERSE3EULER_H +#define FCL_MATH_SAMPLERSE3EULER_H + +#include "fcl/data_types.h" +#include "fcl/math/sampler_base.h" + +namespace fcl +{ + +template +class SamplerSE3Euler : public SamplerBase +{ +public: + SamplerSE3Euler(); + + SamplerSE3Euler(const Vector3& lower_bound_, + const Vector3& upper_bound_); + + void setBound(const Vector3& lower_bound_, + const Vector3& upper_bound_); + + void getBound(Vector3& lower_bound_, + Vector3& upper_bound_) const; + + Vector6 sample() const; + +protected: + Vector3 lower_bound; + Vector3 upper_bound; + +}; + +using SamplerSE3Eulerf = SamplerSE3Euler; +using SamplerSE3Eulerd = SamplerSE3Euler; + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +SamplerSE3Euler::SamplerSE3Euler() +{ + // Do nothing +} + +//============================================================================== +template +SamplerSE3Euler::SamplerSE3Euler(const Vector3& lower_bound_, const Vector3& upper_bound_) : lower_bound(lower_bound_), + upper_bound(upper_bound_) +{ + // Do nothing +} + +//============================================================================== +template +Vector6 SamplerSE3Euler::sample() const +{ + Vector6 q; + q[0] = this->rng.uniformReal(lower_bound[0], upper_bound[0]); + q[1] = this->rng.uniformReal(lower_bound[1], upper_bound[1]); + q[2] = this->rng.uniformReal(lower_bound[2], upper_bound[2]); + + S s[4]; + this->rng.quaternion(s); + + Quaternion quat(s[0], s[1], s[2], s[3]); + Vector3 angles = quat.toRotationMatrix().eulerAngles(0, 1, 2); + + q[3] = angles[0]; + q[4] = angles[1]; + q[5] = angles[2]; + + return q; +} + +//============================================================================== +template +void SamplerSE3Euler::getBound(Vector3& lower_bound_, Vector3& upper_bound_) const +{ + lower_bound_ = lower_bound; + upper_bound_ = upper_bound; +} + +//============================================================================== +template +void SamplerSE3Euler::setBound(const Vector3& lower_bound_, const Vector3& upper_bound_) + +{ + lower_bound = lower_bound_; + upper_bound = upper_bound_; +} + +} // namespace fcl + +#endif diff --git a/include/fcl/math/sampler_se3_euler_ball.h b/include/fcl/math/sampler_se3_euler_ball.h new file mode 100644 index 000000000..4689dca88 --- /dev/null +++ b/include/fcl/math/sampler_se3_euler_ball.h @@ -0,0 +1,124 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_MATH_SAMPLERSE3EULERBALL_H +#define FCL_MATH_SAMPLERSE3EULERBALL_H + +#include "fcl/data_types.h" +#include "fcl/math/sampler_base.h" + +namespace fcl +{ + +template +class SamplerSE3Euler_ball : public SamplerBase +{ +public: + SamplerSE3Euler_ball(); + + SamplerSE3Euler_ball(S r_); + + void setBound(const S& r_); + + void getBound(S& r_) const; + + Vector6 sample() const; + +protected: + S r; + +}; + +using SamplerSE3Euler_ballf = SamplerSE3Euler_ball; +using SamplerSE3Euler_balld = SamplerSE3Euler_ball; + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +SamplerSE3Euler_ball::SamplerSE3Euler_ball() {} + +//============================================================================== +template +SamplerSE3Euler_ball::SamplerSE3Euler_ball(S r_) : r(r_) +{ +} + +//============================================================================== +template +void SamplerSE3Euler_ball::setBound(const S& r_) +{ + r = r_; +} + +//============================================================================== +template +void SamplerSE3Euler_ball::getBound(S& r_) const +{ + r_ = r; +} + +//============================================================================== +template +Vector6 SamplerSE3Euler_ball::sample() const +{ + Vector6 q; + S x, y, z; + this->rng.ball(0, r, x, y, z); + q[0] = x; + q[1] = y; + q[2] = z; + + S s[4]; + this->rng.quaternion(s); + + Quaternion quat(s[0], s[1], s[2], s[3]); + Vector3 angles = quat.toRotationMatrix().eulerAngles(0, 1, 2); + q[3] = angles[0]; + q[4] = angles[1]; + q[5] = angles[2]; + + return q; +} + +} // namespace fcl + +#endif diff --git a/include/fcl/math/sampler_se3_quat.h b/include/fcl/math/sampler_se3_quat.h new file mode 100644 index 000000000..cb614d8a2 --- /dev/null +++ b/include/fcl/math/sampler_se3_quat.h @@ -0,0 +1,132 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_MATH_SAMPLERSE3QUAT_H +#define FCL_MATH_SAMPLERSE3QUAT_H + +#include "fcl/data_types.h" +#include "fcl/math/sampler_base.h" + +namespace fcl +{ + +template +class SamplerSE3Quat : public SamplerBase +{ +public: + SamplerSE3Quat(); + + SamplerSE3Quat(const Vector3& lower_bound_, + const Vector3& upper_bound_); + + void setBound(const Vector3& lower_bound_, + const Vector3& upper_bound_); + + void getBound(Vector3& lower_bound_, + Vector3& upper_bound_) const; + + Vector6 sample() const; + +protected: + Vector3 lower_bound; + Vector3 upper_bound; + +}; + +using SamplerSE3Quatf = SamplerSE3Quat; +using SamplerSE3Quatd = SamplerSE3Quat; + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +SamplerSE3Quat::SamplerSE3Quat() +{ + // Do nothing +} + +//============================================================================== +template +SamplerSE3Quat::SamplerSE3Quat(const Vector3& lower_bound_, const Vector3& upper_bound_) : lower_bound(lower_bound_), + upper_bound(upper_bound_) +{ + // Do nothing +} + +//============================================================================== +template +void SamplerSE3Quat::getBound(Vector3& lower_bound_, Vector3& upper_bound_) const +{ + lower_bound_ = lower_bound; + upper_bound_ = upper_bound; +} + +//============================================================================== +template +void SamplerSE3Quat::setBound(const Vector3& lower_bound_, const Vector3& upper_bound_) + +{ + lower_bound = lower_bound_; + upper_bound = upper_bound_; +} + +//============================================================================== +template +Vector6 SamplerSE3Quat::sample() const +{ + Vector6 q; + q[0] = this->rng.uniformReal(lower_bound[0], upper_bound[0]); + q[1] = this->rng.uniformReal(lower_bound[1], upper_bound[1]); + q[2] = this->rng.uniformReal(lower_bound[2], upper_bound[2]); + + S s[4]; + this->rng.quaternion(s); + + q[3] = s[0]; + q[4] = s[1]; + q[5] = s[2]; + q[6] = s[3]; + return q; +} + +} // namespace fcl + +#endif diff --git a/include/fcl/math/sampler_se3_quat_ball.h b/include/fcl/math/sampler_se3_quat_ball.h new file mode 100644 index 000000000..7fa1eb053 --- /dev/null +++ b/include/fcl/math/sampler_se3_quat_ball.h @@ -0,0 +1,126 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_MATH_SAMPLERSE3QUATBALL_H +#define FCL_MATH_SAMPLERSE3QUATBALL_H + +#include "fcl/data_types.h" +#include "fcl/math/sampler_base.h" + + +namespace fcl +{ + +template +class SamplerSE3Quat_ball : public SamplerBase +{ +public: + SamplerSE3Quat_ball(); + + SamplerSE3Quat_ball(S r_); + + void setBound(const S& r_); + + void getBound(S& r_) const; + + Vector7 sample() const; + +protected: + S r; +}; + +using SamplerSE3Quat_ballf = SamplerSE3Quat_ball; +using SamplerSE3Quat_balld = SamplerSE3Quat_ball; + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +SamplerSE3Quat_ball::SamplerSE3Quat_ball() +{ + // Do nothing +} + +//============================================================================== +template +SamplerSE3Quat_ball::SamplerSE3Quat_ball(S r_) : r(r_) +{ + // Do nothing +} + +//============================================================================== +template +void SamplerSE3Quat_ball::setBound(const S& r_) +{ + r = r_; +} + +//============================================================================== +template +void SamplerSE3Quat_ball::getBound(S& r_) const +{ + r_ = r; +} + +//============================================================================== +template +Vector7 SamplerSE3Quat_ball::sample() const +{ + Vector7 q; + S x, y, z; + this->rng.ball(0, r, x, y, z); + q[0] = x; + q[1] = y; + q[2] = z; + + S s[4]; + this->rng.quaternion(s); + + q[3] = s[0]; + q[4] = s[1]; + q[5] = s[2]; + q[6] = s[3]; + return q; +} + +} // namespace fcl + +#endif diff --git a/include/fcl/math/sampling.h b/include/fcl/math/sampling.h index c63736af0..c22adbf89 100644 --- a/include/fcl/math/sampling.h +++ b/include/fcl/math/sampling.h @@ -38,423 +38,15 @@ #ifndef FCL_MATH_SAMPLING_H #define FCL_MATH_SAMPLING_H -#include -#include -#include "fcl/math/constants.h" - -namespace fcl -{ - -/// @brief Random number generation. An instance of this class -/// cannot be used by multiple threads at once (member functions -/// are not const). However, the constructor is thread safe and -/// different instances can be used safely in any number of -/// threads. It is also guaranteed that all created instances will -/// have a different random seed. - -class RNG -{ -public: - - /// @brief Constructor. Always sets a different random seed - RNG(); - - /// @brief Generate a random real between 0 and 1 - double uniform01() - { - return uniDist_(generator_); - } - - /// @brief Generate a random real within given bounds: [\e lower_bound, \e upper_bound) - double uniformReal(double lower_bound, double upper_bound) - { - assert(lower_bound <= upper_bound); - return (upper_bound - lower_bound) * uniDist_(generator_) + lower_bound; - } - - /// @brief Generate a random integer within given bounds: [\e lower_bound, \e upper_bound] - int uniformInt(int lower_bound, int upper_bound) - { - int r = (int)floor(uniformReal((double)lower_bound, (double)(upper_bound) + 1.0)); - return (r > upper_bound) ? upper_bound : r; - } - - /// @brief Generate a random boolean - bool uniformBool() - { - return uniDist_(generator_) <= 0.5; - } - - /// @brief Generate a random real using a normal distribution with mean 0 and variance 1 - double gaussian01() - { - return normalDist_(generator_); - } - - /// @brief Generate a random real using a normal distribution with given mean and variance - double gaussian(double mean, double stddev) - { - return normalDist_(generator_) * stddev + mean; - } - - /// @brief Generate a random real using a half-normal distribution. The value is within specified bounds [\e r_min, \e r_max], but with a bias towards \e r_max. The function is implemended using a Gaussian distribution with mean at \e r_max - \e r_min. The distribution is 'folded' around \e r_max axis towards \e r_min. The variance of the distribution is (\e r_max - \e r_min) / \e focus. The higher the focus, the more probable it is that generated numbers are close to \e r_max. - double halfNormalReal(double r_min, double r_max, double focus = 3.0); - - /// @brief Generate a random integer using a half-normal distribution. The value is within specified bounds ([\e r_min, \e r_max]), but with a bias towards \e r_max. The function is implemented on top of halfNormalReal() - int halfNormalInt(int r_min, int r_max, double focus = 3.0); - - /// @brief Uniform random unit quaternion sampling. The computed value has the order (x,y,z,w) - void quaternion(double value[4]); - - /// @brief Uniform random sampling of Euler roll-pitch-yaw angles, each in the range [-pi, pi). The computed value has the order (roll, pitch, yaw) */ - void eulerRPY(double value[3]); - - /// @brief Uniform random sample on a disk with radius from r_min to r_max - void disk(double r_min, double r_max, double& x, double& y); - - /// @brief Uniform random sample in a ball with radius from r_min to r_max - void ball(double r_min, double r_max, double& x, double& y, double& z); - - /// @brief Set the seed for random number generation. Use this function to ensure the same sequence of random numbers is generated. - static void setSeed(std::uint_fast32_t seed); - - /// @brief Get the seed used for random number generation. Passing the returned value to setSeed() at a subsequent execution of the code will ensure deterministic (repeatable) behaviour. Useful for debugging. - static std::uint_fast32_t getSeed(); - -private: - - std::mt19937 generator_; - std::uniform_real_distribution<> uniDist_; - std::normal_distribution<> normalDist_; - -}; - -class SamplerBase -{ -public: - mutable RNG rng; -}; - -template -class SamplerR : public SamplerBase -{ -public: - SamplerR() {} - - SamplerR(const VectorNd& lower_bound_, - const VectorNd& upper_bound_) : lower_bound(lower_bound_), - upper_bound(upper_bound_) - { - } - - void setBound(const VectorNd& lower_bound_, - const VectorNd& upper_bound_) - { - lower_bound = lower_bound_; - upper_bound = upper_bound_; - } - - void getBound(VectorNd& lower_bound_, - VectorNd& upper_bound_) const - { - lower_bound_ = lower_bound; - upper_bound_ = upper_bound; - } - - VectorNd sample() const - { - VectorNd q; - - for(std::size_t i = 0; i < N; ++i) - { - q[i] = rng.uniformReal(lower_bound[i], upper_bound[i]); - } - - return q; - } - -private: - VectorNd lower_bound; - VectorNd upper_bound; - -}; - - -class SamplerSE2 : public SamplerBase -{ -public: - SamplerSE2() {} - - SamplerSE2(const VectorNd<2>& lower_bound_, - const VectorNd<2>& upper_bound_) : lower_bound(lower_bound_), - upper_bound(upper_bound_) - {} - - SamplerSE2(FCL_REAL x_min, FCL_REAL x_max, - FCL_REAL y_min, FCL_REAL y_max) : lower_bound(VectorNd<2>(x_min, y_min)), - upper_bound(VectorNd<2>(x_max, y_max)) - - {} - - - void setBound(const VectorNd<2>& lower_bound_, - const VectorNd<2>& upper_bound_) - { - lower_bound = lower_bound_; - upper_bound = upper_bound_; - } - - void getBound(VectorNd<2>& lower_bound_, - VectorNd<2>& upper_bound_) const - { - lower_bound_ = lower_bound; - upper_bound_ = upper_bound; - } - - - VectorNd<3> sample() const - { - VectorNd<3> q; - q[0] = rng.uniformReal(lower_bound[0], lower_bound[1]); - q[1] = rng.uniformReal(lower_bound[1], lower_bound[2]); - q[2] = rng.uniformReal(-constants::pi, constants::pi); - - return q; - } - -protected: - VectorNd<2> lower_bound; - VectorNd<2> upper_bound; -}; - - -class SamplerSE2_disk : public SamplerBase -{ -public: - SamplerSE2_disk() {} - - SamplerSE2_disk(FCL_REAL cx, FCL_REAL cy, - FCL_REAL r1, FCL_REAL r2, - FCL_REAL crefx, FCL_REAL crefy) - { - setBound(cx, cy, r1, r2, crefx, crefy); - } - - void setBound(FCL_REAL cx, FCL_REAL cy, - FCL_REAL r1, FCL_REAL r2, - FCL_REAL crefx, FCL_REAL crefy) - { - c[0] = cx; c[1] = cy; - cref[0] = crefx; cref[1] = crefy; - r_min = r1; - r_max = r2; - } - - VectorNd<3> sample() const - { - VectorNd<3> q; - FCL_REAL x, y; - rng.disk(r_min, r_max, x, y); - q[0] = x + c[0] - cref[0]; - q[1] = y + c[1] - cref[1]; - q[2] = rng.uniformReal(-constants::pi, constants::pi); - - return q; - } - -protected: - FCL_REAL c[2]; - FCL_REAL cref[2]; - FCL_REAL r_min, r_max; -}; - -class SamplerSE3Euler : public SamplerBase -{ -public: - SamplerSE3Euler() {} - - SamplerSE3Euler(const VectorNd<3>& lower_bound_, - const VectorNd<3>& upper_bound_) : lower_bound(lower_bound_), - upper_bound(upper_bound_) - {} - - void setBound(const VectorNd<3>& lower_bound_, - const VectorNd<3>& upper_bound_) - - { - lower_bound = lower_bound_; - upper_bound = upper_bound_; - } - - void getBound(VectorNd<3>& lower_bound_, - VectorNd<3>& upper_bound_) const - { - lower_bound_ = lower_bound; - upper_bound_ = upper_bound; - } - - VectorNd<6> sample() const - { - VectorNd<6> q; - q[0] = rng.uniformReal(lower_bound[0], upper_bound[0]); - q[1] = rng.uniformReal(lower_bound[1], upper_bound[1]); - q[2] = rng.uniformReal(lower_bound[2], upper_bound[2]); - - FCL_REAL s[4]; - rng.quaternion(s); - - Quaternion3d quat(s[0], s[1], s[2], s[3]); - Vector3d angles = quat.toRotationMatrix().eulerAngles(0, 1, 2); - - q[3] = angles[0]; - q[4] = angles[1]; - q[5] = angles[2]; - - return q; - } - -protected: - VectorNd<3> lower_bound; - VectorNd<3> upper_bound; - -}; - -class SamplerSE3Quat : public SamplerBase -{ -public: - SamplerSE3Quat() {} - - SamplerSE3Quat(const VectorNd<3>& lower_bound_, - const VectorNd<3>& upper_bound_) : lower_bound(lower_bound_), - upper_bound(upper_bound_) - {} - - void setBound(const VectorNd<3>& lower_bound_, - const VectorNd<3>& upper_bound_) - - { - lower_bound = lower_bound_; - upper_bound = upper_bound_; - } - - void getBound(VectorNd<3>& lower_bound_, - VectorNd<3>& upper_bound_) const - { - lower_bound_ = lower_bound; - upper_bound_ = upper_bound; - } - - VectorNd<7> sample() const - { - VectorNd<7> q; - q[0] = rng.uniformReal(lower_bound[0], upper_bound[0]); - q[1] = rng.uniformReal(lower_bound[1], upper_bound[1]); - q[2] = rng.uniformReal(lower_bound[2], upper_bound[2]); - - FCL_REAL s[4]; - rng.quaternion(s); - - q[3] = s[0]; - q[4] = s[1]; - q[5] = s[2]; - q[6] = s[3]; - return q; - } - -protected: - VectorNd<3> lower_bound; - VectorNd<3> upper_bound; -}; - -class SamplerSE3Euler_ball : public SamplerBase -{ -public: - SamplerSE3Euler_ball() {} - - SamplerSE3Euler_ball(FCL_REAL r_) : r(r_) - { - } - - void setBound(const FCL_REAL& r_) - { - r = r_; - } - - void getBound(FCL_REAL& r_) const - { - r_ = r; - } - - VectorNd<6> sample() const - { - VectorNd<6> q; - FCL_REAL x, y, z; - rng.ball(0, r, x, y, z); - q[0] = x; - q[1] = y; - q[2] = z; - - FCL_REAL s[4]; - rng.quaternion(s); - - Quaternion3d quat(s[0], s[1], s[2], s[3]); - Vector3d angles = quat.toRotationMatrix().eulerAngles(0, 1, 2); - q[3] = angles[0]; - q[4] = angles[1]; - q[5] = angles[2]; - - return q; - } - -protected: - FCL_REAL r; - -}; - - -class SamplerSE3Quat_ball : public SamplerBase -{ -public: - SamplerSE3Quat_ball() {} - - SamplerSE3Quat_ball(FCL_REAL r_) : r(r_) - {} - - void setBound(const FCL_REAL& r_) - { - r = r_; - } - - void getBound(FCL_REAL& r_) const - { - r_ = r; - } - - VectorNd<7> sample() const - { - VectorNd<7> q; - FCL_REAL x, y, z; - rng.ball(0, r, x, y, z); - q[0] = x; - q[1] = y; - q[2] = z; - - FCL_REAL s[4]; - rng.quaternion(s); - - q[3] = s[0]; - q[4] = s[1]; - q[5] = s[2]; - q[6] = s[3]; - return q; - } - -protected: - FCL_REAL r; -}; - - - -} +#warning "This header has been deprecated in FCL 0.6. " + +#include "fcl/math/sampler_base.h" +#include "fcl/math/sampler_r.h" +#include "fcl/math/sampler_se2.h" +#include "fcl/math/sampler_se2_disk.h" +#include "fcl/math/sampler_se3_euler.h" +#include "fcl/math/sampler_se3_euler_ball.h" +#include "fcl/math/sampler_se3_quat.h" +#include "fcl/math/sampler_se3_quat_ball.h" #endif diff --git a/include/fcl/math/transform.h b/include/fcl/math/transform.h index 9d5ffbcbd..5578b8206 100644 --- a/include/fcl/math/transform.h +++ b/include/fcl/math/transform.h @@ -35,9 +35,8 @@ /** \author Jia Pan */ - -#ifndef FCL_TRANSFORM_H -#define FCL_TRANSFORM_H +#ifndef FCL_MATH_TRANSFORM_H +#define FCL_MATH_TRANSFORM_H #warning "This header has been deprecated in FCL 0.6. "\ "Please include fcl/data_types.h and fcl/math/geometry.h instead." diff --git a/include/fcl/math/triangle.h b/include/fcl/math/triangle.h index c720998ed..0ed930171 100644 --- a/include/fcl/math/triangle.h +++ b/include/fcl/math/triangle.h @@ -35,8 +35,8 @@ /** \author Jia Pan */ -#ifndef FCL_TRIANGLE_H -#define FCL_TRIANGLE_H +#ifndef FCL_MATH_TRIANGLE_H +#define FCL_MATH_TRIANGLE_H #include @@ -51,26 +51,56 @@ class Triangle public: /// @brief Default constructor - Triangle() {} + Triangle(); /// @brief Create a triangle with given vertex indices - Triangle(std::size_t p1, std::size_t p2, std::size_t p3) - { - set(p1, p2, p3); - } + Triangle(std::size_t p1, std::size_t p2, std::size_t p3); /// @brief Set the vertex indices of the triangle - inline void set(std::size_t p1, std::size_t p2, std::size_t p3) - { - vids[0] = p1; vids[1] = p2; vids[2] = p3; - } + void set(std::size_t p1, std::size_t p2, std::size_t p3); /// @access the triangle index - inline std::size_t operator[](int i) const { return vids[i]; } + std::size_t operator[](int i) const; - inline std::size_t& operator[](int i) { return vids[i]; } + std::size_t& operator[](int i); }; +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +inline Triangle::Triangle() +{ + // Do nothing +} + +//============================================================================== +inline Triangle::Triangle(std::size_t p1, std::size_t p2, std::size_t p3) +{ + set(p1, p2, p3); +} + +//============================================================================== +inline void Triangle::set(std::size_t p1, std::size_t p2, std::size_t p3) +{ + vids[0] = p1; vids[1] = p2; vids[2] = p3; +} + +//============================================================================== +inline std::size_t Triangle::operator[](int i) const +{ + return vids[i]; +} + +//============================================================================== +inline std::size_t& Triangle::operator[](int i) +{ + return vids[i]; +} + } // namespace fcl #endif diff --git a/include/fcl/math/variance.h b/include/fcl/math/variance.h index a7b0e3e04..06a153807 100644 --- a/include/fcl/math/variance.h +++ b/include/fcl/math/variance.h @@ -35,64 +35,10 @@ /** \author Jia Pan */ -#ifndef FCL_VARIANCE_H -#define FCL_VARIANCE_H +#ifndef FCL_MATH_VARIANCE_H +#define FCL_MATH_VARIANCE_H -#include - -#include "fcl/config.h" -#include "fcl/data_types.h" -#include "fcl/math/geometry.h" - -namespace fcl -{ - -/// @brief Class for variance matrix in 3d -class Variance3f -{ -public: - /// @brief Variation matrix - Matrix3d Sigma; - - /// @brief Variations along the eign axes - Vector3d sigma; - - /// @brief Matrix whose columns are eigenvectors of Sigma - Matrix3d axis; - - Variance3f() {} - - Variance3f(const Matrix3d& S) : Sigma(S) - { - init(); - } - - /// @brief init the Variance - void init() - { - eigen(Sigma, sigma, axis); - } - - /// @brief Compute the sqrt of Sigma matrix based on the eigen decomposition result, this is useful when the uncertainty matrix is initialized as a square variation matrix - Variance3f& sqrt() - { - for(std::size_t i = 0; i < 3; ++i) - { - if(sigma[i] < 0) sigma[i] = 0; - sigma[i] = std::sqrt(sigma[i]); - } - - Sigma.noalias() - = sigma[0] * axis.col(0) * axis.col(0).transpose(); - Sigma.noalias() - += sigma[1] * axis.col(1) * axis.col(1).transpose(); - Sigma.noalias() - += sigma[2] * axis.col(2) * axis.col(2).transpose(); - - return *this; - } -}; - -} // namespace fcl +#warning "This header has been deprecated in FCL 0.6. "\ + "Please include fcl/data_types.h and fcl/math/variance3.h instead." #endif diff --git a/include/fcl/math/variance3.h b/include/fcl/math/variance3.h new file mode 100644 index 000000000..3bc0ba2b4 --- /dev/null +++ b/include/fcl/math/variance3.h @@ -0,0 +1,131 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_MATH_VARIANCE3_H +#define FCL_MATH_VARIANCE3_H + +#include + +#include "fcl/config.h" +#include "fcl/data_types.h" +#include "fcl/math/geometry.h" + +namespace fcl +{ + +/// @brief Class for variance matrix in 3d +template +class Variance3 +{ +public: + /// @brief Variation matrix + Matrix3 Sigma; + + /// @brief Variations along the eign axes + Vector3 sigma; + + /// @brief Matrix whose columns are eigenvectors of Sigma + Matrix3 axis; + + Variance3(); + + Variance3(const Matrix3& sigma); + + /// @brief init the Variance + void init(); + + /// @brief Compute the sqrt of Sigma matrix based on the eigen decomposition + /// result, this is useful when the uncertainty matrix is initialized as a + /// square variation matrix + Variance3& sqrt(); +}; + +using Variance3f = Variance3; +using Variance3d = Variance3; + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +Variance3::Variance3() +{ + // Do nothing +} + +//============================================================================== +template +Variance3::Variance3(const Matrix3& sigma) : Sigma(sigma) +{ + init(); +} + +//============================================================================== +template +void Variance3::init() +{ + eigen_old(Sigma, sigma, axis); +} + +//============================================================================== +template +Variance3& Variance3::sqrt() +{ + for(std::size_t i = 0; i < 3; ++i) + { + if(sigma[i] < 0) + sigma[i] = 0; + + sigma[i] = std::sqrt(sigma[i]); + } + + Sigma.noalias() + = sigma[0] * axis.col(0) * axis.col(0).transpose(); + Sigma.noalias() + += sigma[1] * axis.col(1) * axis.col(1).transpose(); + Sigma.noalias() + += sigma[2] * axis.col(2) * axis.col(2).transpose(); + + return *this; +} + +} // namespace fcl + +#endif diff --git a/include/fcl/math/vec_3f.h b/include/fcl/math/vec_3f.h index c3f923c86..0dcbfdfd0 100644 --- a/include/fcl/math/vec_3f.h +++ b/include/fcl/math/vec_3f.h @@ -35,8 +35,8 @@ /** \author Jia Pan */ -#ifndef FCL_VEC_3F_H -#define FCL_VEC_3F_H +#ifndef FCL_MATH_VEC3F_H +#define FCL_MATH_VEC3F_H #warning "This header has been deprecated in FCL 0.6. "\ "Please include fcl/data_types.h and fcl/math/geometry.h instead." diff --git a/include/fcl/math/vec_nf.h b/include/fcl/math/vec_nf.h index fcc1ede18..076792d32 100644 --- a/include/fcl/math/vec_nf.h +++ b/include/fcl/math/vec_nf.h @@ -35,8 +35,8 @@ /** \author Jia Pan */ -#ifndef FCL_MATH_VEC_NF_H -#define FCL_MATH_VEC_NF_H +#ifndef FCL_MATH_VECNF_H +#define FCL_MATH_VECNF_H #warning "This header has been deprecated in FCL 0.6. "\ "Please include fcl/data_types.h and fcl/math/geometry.h instead." diff --git a/include/fcl/narrowphase/detail/box_box.h b/include/fcl/narrowphase/detail/box_box.h new file mode 100755 index 000000000..951ba7854 --- /dev/null +++ b/include/fcl/narrowphase/detail/box_box.h @@ -0,0 +1,1446 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_NARROWPHASE_DETAIL_BOXBOX_H +#define FCL_NARROWPHASE_DETAIL_BOXBOX_H + +#include +#include "fcl/collision_data.h" + +namespace fcl +{ + +namespace details +{ + +template +void lineClosestApproach(const Vector3& pa, const Vector3& ua, + const Vector3& pb, const Vector3& ub, + S* alpha, S* beta); + +// find all the intersection points between the 2D rectangle with vertices +// at (+/-h[0],+/-h[1]) and the 2D quadrilateral with vertices (p[0],p[1]), +// (p[2],p[3]),(p[4],p[5]),(p[6],p[7]). +// +// the intersection points are returned as x,y pairs in the 'ret' array. +// the number of intersection points is returned by the function (this will +// be in the range 0 to 8). +template +int intersectRectQuad2(S h[2], S p[8], S ret[16]); + +// given n points in the plane (array p, of size 2*n), generate m points that +// best represent the whole set. the definition of 'best' here is not +// predetermined - the idea is to select points that give good box-box +// collision detection behavior. the chosen point indexes are returned in the +// array iret (of size m). 'i0' is always the first entry in the array. +// n must be in the range [1..8]. m must be in the range [1..n]. i0 must be +// in the range [0..n-1]. +template +void cullPoints2(int n, S p[], int m, int i0, int iret[]); + +template +int boxBox2( + const Vector3& side1, + const Eigen::MatrixBase& R1, + const Eigen::MatrixBase& T1, + const Vector3& side2, + const Eigen::MatrixBase& R2, + const Eigen::MatrixBase& T2, + Vector3& normal, + S* depth, + int* return_code, + int maxc, + std::vector>& contacts); + +template +int boxBox2( + const Vector3& side1, + const Transform3& tf1, + const Vector3& side2, + const Transform3& tf2, + Vector3& normal, + S* depth, + int* return_code, + int maxc, + std::vector>& contacts); + +template +bool boxBoxIntersect(const Box& s1, const Transform3& tf1, + const Box& s2, const Transform3& tf2, + std::vector>* contacts_); + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +void lineClosestApproach(const Vector3& pa, const Vector3& ua, + const Vector3& pb, const Vector3& ub, + S* alpha, S* beta) +{ + Vector3 p = pb - pa; + S uaub = ua.dot(ub); + S q1 = ua.dot(p); + S q2 = -ub.dot(p); + S d = 1 - uaub * uaub; + if(d <= (S)(0.0001f)) + { + *alpha = 0; + *beta = 0; + } + else + { + d = 1 / d; + *alpha = (q1 + uaub * q2) * d; + *beta = (uaub * q1 + q2) * d; + } +} + +//============================================================================== +template +int intersectRectQuad2(S h[2], S p[8], S ret[16]) +{ + // q (and r) contain nq (and nr) coordinate points for the current (and + // chopped) polygons + int nq = 4, nr = 0; + S buffer[16]; + S* q = p; + S* r = ret; + for(int dir = 0; dir <= 1; ++dir) + { + // direction notation: xy[0] = x axis, xy[1] = y axis + for(int sign = -1; sign <= 1; sign += 2) + { + // chop q along the line xy[dir] = sign*h[dir] + S* pq = q; + S* pr = r; + nr = 0; + for(int i = nq; i > 0; --i) + { + // go through all points in q and all lines between adjacent points + if(sign * pq[dir] < h[dir]) + { + // this point is inside the chopping line + pr[0] = pq[0]; + pr[1] = pq[1]; + pr += 2; + nr++; + if(nr & 8) + { + q = r; + goto done; + } + } + S* nextq = (i > 1) ? pq+2 : q; + if((sign*pq[dir] < h[dir]) ^ (sign*nextq[dir] < h[dir])) + { + // this line crosses the chopping line + pr[1-dir] = pq[1-dir] + (nextq[1-dir]-pq[1-dir]) / + (nextq[dir]-pq[dir]) * (sign*h[dir]-pq[dir]); + pr[dir] = sign*h[dir]; + pr += 2; + nr++; + if(nr & 8) + { + q = r; + goto done; + } + } + pq += 2; + } + q = r; + r = (q == ret) ? buffer : ret; + nq = nr; + } + } + + done: + if(q != ret) memcpy(ret, q, nr*2*sizeof(S)); + return nr; +} + +//============================================================================== +template +void cullPoints2(int n, S p[], int m, int i0, int iret[]) +{ + // compute the centroid of the polygon in cx,cy + S a, cx, cy, q; + switch(n) + { + case 1: + cx = p[0]; + cy = p[1]; + break; + case 2: + cx = 0.5 * (p[0] + p[2]); + cy = 0.5 * (p[1] + p[3]); + break; + default: + a = 0; + cx = 0; + cy = 0; + for(int i = 0; i < n-1; ++i) + { + q = p[i*2]*p[i*2+3] - p[i*2+2]*p[i*2+1]; + a += q; + cx += q*(p[i*2]+p[i*2+2]); + cy += q*(p[i*2+1]+p[i*2+3]); + } + q = p[n*2-2]*p[1] - p[0]*p[n*2-1]; + if(std::abs(a+q) > std::numeric_limits::epsilon()) + a = 1/(3*(a+q)); + else + a= 1e18f; + + cx = a*(cx + q*(p[n*2-2]+p[0])); + cy = a*(cy + q*(p[n*2-1]+p[1])); + } + + + // compute the angle of each point w.r.t. the centroid + S A[8]; + for(int i = 0; i < n; ++i) + A[i] = atan2(p[i*2+1]-cy,p[i*2]-cx); + + // search for points that have angles closest to A[i0] + i*(2*pi/m). + int avail[8]; + for(int i = 0; i < n; ++i) avail[i] = 1; + avail[i0] = 0; + iret[0] = i0; + iret++; + const S pi = constants::pi(); + for(int j = 1; j < m; ++j) + { + a = j*(2*pi/m) + A[i0]; + if (a > pi) a -= 2*pi; + S maxdiff= 1e9, diff; + + *iret = i0; // iret is not allowed to keep this value, but it sometimes does, when diff=#QNAN0 + for(int i = 0; i < n; ++i) + { + if(avail[i]) + { + diff = std::abs(A[i]-a); + if(diff > pi) diff = 2*pi - diff; + if(diff < maxdiff) + { + maxdiff = diff; + *iret = i; + } + } + } + avail[*iret] = 0; + iret++; + } +} + +//============================================================================== +template +int boxBox2( + const Vector3& side1, + const Eigen::MatrixBase& R1, + const Eigen::MatrixBase& T1, + const Vector3& side2, + const Eigen::MatrixBase& R2, + const Eigen::MatrixBase& T2, + Vector3& normal, + S* depth, + int* return_code, + int maxc, + std::vector>& contacts) +{ + const S fudge_factor = S(1.05); + Vector3 normalC; + S s, s2, l; + int invert_normal, code; + + Vector3 p = T2 - T1; // get vector from centers of box 1 to box 2, relative to box 1 + Vector3 pp = R1.transpose() * p; // get pp = p relative to body 1 + + // get side lengths / 2 + Vector3 A = side1 * 0.5; + Vector3 B = side2 * 0.5; + + // Rij is R1'*R2, i.e. the relative rotation between R1 and R2 + Matrix3 R = R1.transpose() * R2; + Matrix3 Q = R.cwiseAbs(); + + + // for all 15 possible separating axes: + // * see if the axis separates the boxes. if so, return 0. + // * find the depth of the penetration along the separating axis (s2) + // * if this is the largest depth so far, record it. + // the normal vector will be set to the separating axis with the smallest + // depth. note: normalR is set to point to a column of R1 or R2 if that is + // the smallest depth normal so far. otherwise normalR is 0 and normalC is + // set to a vector relative to body 1. invert_normal is 1 if the sign of + // the normal should be flipped. + + int best_col_id = -1; + const Eigen::MatrixBase* normalR = 0; + S tmp = 0; + + s = - std::numeric_limits::max(); + invert_normal = 0; + code = 0; + + // separating axis = u1, u2, u3 + tmp = pp[0]; + s2 = std::abs(tmp) - (Q.row(0).dot(B) + A[0]); + if(s2 > 0) { *return_code = 0; return 0; } + if(s2 > s) + { + s = s2; + best_col_id = 0; + normalR = &R1; + invert_normal = (tmp < 0); + code = 1; + } + + tmp = pp[1]; + s2 = std::abs(tmp) - (Q.row(1).dot(B) + A[1]); + if(s2 > 0) { *return_code = 0; return 0; } + if(s2 > s) + { + s = s2; + best_col_id = 1; + normalR = &R1; + invert_normal = (tmp < 0); + code = 2; + } + + tmp = pp[2]; + s2 = std::abs(tmp) - (Q.row(2).dot(B) + A[2]); + if(s2 > 0) { *return_code = 0; return 0; } + if(s2 > s) + { + s = s2; + best_col_id = 2; + normalR = &R1; + invert_normal = (tmp < 0); + code = 3; + } + + // separating axis = v1, v2, v3 + tmp = R2.col(0).dot(p); + s2 = std::abs(tmp) - (Q.col(0).dot(A) + B[0]); + if(s2 > 0) { *return_code = 0; return 0; } + if(s2 > s) + { + s = s2; + best_col_id = 0; + normalR = &R2; + invert_normal = (tmp < 0); + code = 4; + } + + tmp = R2.col(1).dot(p); + s2 = std::abs(tmp) - (Q.col(1).dot(A) + B[1]); + if(s2 > 0) { *return_code = 0; return 0; } + if(s2 > s) + { + s = s2; + best_col_id = 1; + normalR = &R2; + invert_normal = (tmp < 0); + code = 5; + } + + tmp = R2.col(2).dot(p); + s2 = std::abs(tmp) - (Q.col(2).dot(A) + B[2]); + if(s2 > 0) { *return_code = 0; return 0; } + if(s2 > s) + { + s = s2; + best_col_id = 2; + normalR = &R2; + invert_normal = (tmp < 0); + code = 6; + } + + + S fudge2(1.0e-6); + Q.array() += fudge2; + + Vector3 n; + S eps = std::numeric_limits::epsilon(); + + // separating axis = u1 x (v1,v2,v3) + tmp = pp[2] * R(1, 0) - pp[1] * R(2, 0); + s2 = std::abs(tmp) - (A[1] * Q(2, 0) + A[2] * Q(1, 0) + B[1] * Q(0, 2) + B[2] * Q(0, 1)); + if(s2 > 0) { *return_code = 0; return 0; } + n = Vector3(0, -R(2, 0), R(1, 0)); + l = n.norm(); + if(l > eps) + { + s2 /= l; + if(s2 * fudge_factor > s) + { + s = s2; + best_col_id = -1; + normalC = n / l; + invert_normal = (tmp < 0); + code = 7; + } + } + + tmp = pp[2] * R(1, 1) - pp[1] * R(2, 1); + s2 = std::abs(tmp) - (A[1] * Q(2, 1) + A[2] * Q(1, 1) + B[0] * Q(0, 2) + B[2] * Q(0, 0)); + if(s2 > 0) { *return_code = 0; return 0; } + n = Vector3(0, -R(2, 1), R(1, 1)); + l = n.norm(); + if(l > eps) + { + s2 /= l; + if(s2 * fudge_factor > s) + { + s = s2; + best_col_id = -1; + normalC = n / l; + invert_normal = (tmp < 0); + code = 8; + } + } + + tmp = pp[2] * R(1, 2) - pp[1] * R(2, 2); + s2 = std::abs(tmp) - (A[1] * Q(2, 2) + A[2] * Q(1, 2) + B[0] * Q(0, 1) + B[1] * Q(0, 0)); + if(s2 > 0) { *return_code = 0; return 0; } + n = Vector3(0, -R(2, 2), R(1, 2)); + l = n.norm(); + if(l > eps) + { + s2 /= l; + if(s2 * fudge_factor > s) + { + s = s2; + best_col_id = -1; + normalC = n / l; + invert_normal = (tmp < 0); + code = 9; + } + } + + // separating axis = u2 x (v1,v2,v3) + tmp = pp[0] * R(2, 0) - pp[2] * R(0, 0); + s2 = std::abs(tmp) - (A[0] * Q(2, 0) + A[2] * Q(0, 0) + B[1] * Q(1, 2) + B[2] * Q(1, 1)); + if(s2 > 0) { *return_code = 0; return 0; } + n = Vector3(R(2, 0), 0, -R(0, 0)); + l = n.norm(); + if(l > eps) + { + s2 /= l; + if(s2 * fudge_factor > s) + { + s = s2; + best_col_id = -1; + normalC = n / l; + invert_normal = (tmp < 0); + code = 10; + } + } + + tmp = pp[0] * R(2, 1) - pp[2] * R(0, 1); + s2 = std::abs(tmp) - (A[0] * Q(2, 1) + A[2] * Q(0, 1) + B[0] * Q(1, 2) + B[2] * Q(1, 0)); + if(s2 > 0) { *return_code = 0; return 0; } + n = Vector3(R(2, 1), 0, -R(0, 1)); + l = n.norm(); + if(l > eps) + { + s2 /= l; + if(s2 * fudge_factor > s) + { + s = s2; + best_col_id = -1; + normalC = n / l; + invert_normal = (tmp < 0); + code = 11; + } + } + + tmp = pp[0] * R(2, 2) - pp[2] * R(0, 2); + s2 = std::abs(tmp) - (A[0] * Q(2, 2) + A[2] * Q(0, 2) + B[0] * Q(1, 1) + B[1] * Q(1, 0)); + if(s2 > 0) { *return_code = 0; return 0; } + n = Vector3(R(2, 2), 0, -R(0, 2)); + l = n.norm(); + if(l > eps) + { + s2 /= l; + if(s2 * fudge_factor > s) + { + s = s2; + best_col_id = -1; + normalC = n / l; + invert_normal = (tmp < 0); + code = 12; + } + } + + // separating axis = u3 x (v1,v2,v3) + tmp = pp[1] * R(0, 0) - pp[0] * R(1, 0); + s2 = std::abs(tmp) - (A[0] * Q(1, 0) + A[1] * Q(0, 0) + B[1] * Q(2, 2) + B[2] * Q(2, 1)); + if(s2 > 0) { *return_code = 0; return 0; } + n = Vector3(-R(1, 0), R(0, 0), 0); + l = n.norm(); + if(l > eps) + { + s2 /= l; + if(s2 * fudge_factor > s) + { + s = s2; + best_col_id = -1; + normalC = n / l; + invert_normal = (tmp < 0); + code = 13; + } + } + + tmp = pp[1] * R(0, 1) - pp[0] * R(1, 1); + s2 = std::abs(tmp) - (A[0] * Q(1, 1) + A[1] * Q(0, 1) + B[0] * Q(2, 2) + B[2] * Q(2, 0)); + if(s2 > 0) { *return_code = 0; return 0; } + n = Vector3(-R(1, 1), R(0, 1), 0); + l = n.norm(); + if(l > eps) + { + s2 /= l; + if(s2 * fudge_factor > s) + { + s = s2; + best_col_id = -1; + normalC = n / l; + invert_normal = (tmp < 0); + code = 14; + } + } + + tmp = pp[1] * R(0, 2) - pp[0] * R(1, 2); + s2 = std::abs(tmp) - (A[0] * Q(1, 2) + A[1] * Q(0, 2) + B[0] * Q(2, 1) + B[1] * Q(2, 0)); + if(s2 > 0) { *return_code = 0; return 0; } + n = Vector3(-R(1, 2), R(0, 2), 0); + l = n.norm(); + if(l > eps) + { + s2 /= l; + if(s2 * fudge_factor > s) + { + s = s2; + best_col_id = -1; + normalC = n / l; + invert_normal = (tmp < 0); + code = 15; + } + } + + + + if (!code) { *return_code = code; return 0; } + + // if we get to this point, the boxes interpenetrate. compute the normal + // in global coordinates. + if(best_col_id != -1) + normal = normalR->col(best_col_id); + else + normal = R1 * normalC; + + if(invert_normal) + normal = -normal; + + *depth = -s; // s is negative when the boxes are in collision + + // compute contact point(s) + + if(code > 6) + { + // an edge from box 1 touches an edge from box 2. + // find a point pa on the intersecting edge of box 1 + Vector3 pa(T1); + S sign; + + for(int j = 0; j < 3; ++j) + { + sign = (R1.col(j).dot(normal) > 0) ? 1 : -1; + pa += R1.col(j) * (A[j] * sign); + } + + // find a point pb on the intersecting edge of box 2 + Vector3 pb(T2); + + for(int j = 0; j < 3; ++j) + { + sign = (R2.col(j).dot(normal) > 0) ? -1 : 1; + pb += R2.col(j) * (B[j] * sign); + } + + S alpha, beta; + Vector3 ua(R1.col((code-7)/3)); + Vector3 ub(R2.col((code-7)%3)); + + lineClosestApproach(pa, ua, pb, ub, &alpha, &beta); + pa += ua * alpha; + pb += ub * beta; + + + // Vector3 pointInWorld((pa + pb) * 0.5); + // contacts.push_back(ContactPoint(-normal, pointInWorld, -*depth)); + contacts.emplace_back(normal, pb, -*depth); + *return_code = code; + + return 1; + } + + // okay, we have a face-something intersection (because the separating + // axis is perpendicular to a face). define face 'a' to be the reference + // face (i.e. the normal vector is perpendicular to this) and face 'b' to be + // the incident face (the closest face of the other box). + + const Eigen::MatrixBase *Ra, *Rb; + const Eigen::MatrixBase *pa, *pb; + const Vector3 *Sa, *Sb; + + if(code <= 3) + { + Ra = &R1; + Rb = &R2; + pa = &T1; + pb = &T2; + Sa = &A; + Sb = &B; + } + else + { + Ra = &R2; + Rb = &R1; + pa = &T2; + pb = &T1; + Sa = &B; + Sb = &A; + } + + // nr = normal vector of reference face dotted with axes of incident box. + // anr = absolute values of nr. + Vector3 normal2, nr, anr; + if(code <= 3) + normal2 = normal; + else + normal2 = -normal; + + nr = Rb->transpose() * normal2; + anr = nr.cwiseAbs(); + + // find the largest compontent of anr: this corresponds to the normal + // for the indident face. the other axis numbers of the indicent face + // are stored in a1,a2. + int lanr, a1, a2; + if(anr[1] > anr[0]) + { + if(anr[1] > anr[2]) + { + a1 = 0; + lanr = 1; + a2 = 2; + } + else + { + a1 = 0; + a2 = 1; + lanr = 2; + } + } + else + { + if(anr[0] > anr[2]) + { + lanr = 0; + a1 = 1; + a2 = 2; + } + else + { + a1 = 0; + a2 = 1; + lanr = 2; + } + } + + // compute center point of incident face, in reference-face coordinates + Vector3 center; + if(nr[lanr] < 0) + center = (*pb) - (*pa) + Rb->col(lanr) * ((*Sb)[lanr]); + else + center = (*pb) - (*pa) - Rb->col(lanr) * ((*Sb)[lanr]); + + // find the normal and non-normal axis numbers of the reference box + int codeN, code1, code2; + if(code <= 3) + codeN = code-1; + else codeN = code-4; + + if(codeN == 0) + { + code1 = 1; + code2 = 2; + } + else if(codeN == 1) + { + code1 = 0; + code2 = 2; + } + else + { + code1 = 0; + code2 = 1; + } + + // find the four corners of the incident face, in reference-face coordinates + S quad[8]; // 2D coordinate of incident face (x,y pairs) + S c1, c2, m11, m12, m21, m22; + c1 = Ra->col(code1).dot(center); + c2 = Ra->col(code2).dot(center); + // optimize this? - we have already computed this data above, but it is not + // stored in an easy-to-index format. for now it's quicker just to recompute + // the four dot products. + Vector3 tempRac = Ra->col(code1); + m11 = Rb->col(a1).dot(tempRac); + m12 = Rb->col(a2).dot(tempRac); + tempRac = Ra->col(code2); + m21 = Rb->col(a1).dot(tempRac); + m22 = Rb->col(a2).dot(tempRac); + + S k1 = m11 * (*Sb)[a1]; + S k2 = m21 * (*Sb)[a1]; + S k3 = m12 * (*Sb)[a2]; + S k4 = m22 * (*Sb)[a2]; + quad[0] = c1 - k1 - k3; + quad[1] = c2 - k2 - k4; + quad[2] = c1 - k1 + k3; + quad[3] = c2 - k2 + k4; + quad[4] = c1 + k1 + k3; + quad[5] = c2 + k2 + k4; + quad[6] = c1 + k1 - k3; + quad[7] = c2 + k2 - k4; + + // find the size of the reference face + S rect[2]; + rect[0] = (*Sa)[code1]; + rect[1] = (*Sa)[code2]; + + // intersect the incident and reference faces + S ret[16]; + int n_intersect = intersectRectQuad2(rect, quad, ret); + if(n_intersect < 1) { *return_code = code; return 0; } // this should never happen + + // convert the intersection points into reference-face coordinates, + // and compute the contact position and depth for each point. only keep + // those points that have a positive (penetrating) depth. delete points in + // the 'ret' array as necessary so that 'point' and 'ret' correspond. + Vector3 points[8]; // penetrating contact points + S dep[8]; // depths for those points + S det1 = 1.f/(m11*m22 - m12*m21); + m11 *= det1; + m12 *= det1; + m21 *= det1; + m22 *= det1; + int cnum = 0; // number of penetrating contact points found + for(int j = 0; j < n_intersect; ++j) + { + S k1 = m22*(ret[j*2]-c1) - m12*(ret[j*2+1]-c2); + S k2 = -m21*(ret[j*2]-c1) + m11*(ret[j*2+1]-c2); + points[cnum] = center + Rb->col(a1) * k1 + Rb->col(a2) * k2; + dep[cnum] = (*Sa)[codeN] - normal2.dot(points[cnum]); + if(dep[cnum] >= 0) + { + ret[cnum*2] = ret[j*2]; + ret[cnum*2+1] = ret[j*2+1]; + cnum++; + } + } + if(cnum < 1) { *return_code = code; return 0; } // this should never happen + + // we can't generate more contacts than we actually have + if(maxc > cnum) maxc = cnum; + if(maxc < 1) maxc = 1; + + if(cnum <= maxc) + { + if(code<4) + { + // we have less contacts than we need, so we use them all + for(int j = 0; j < cnum; ++j) + { + Vector3 pointInWorld = points[j] + (*pa); + contacts.emplace_back(normal, pointInWorld, -dep[j]); + } + } + else + { + // we have less contacts than we need, so we use them all + for(int j = 0; j < cnum; ++j) + { + Vector3 pointInWorld = points[j] + (*pa) - normal * dep[j]; + contacts.emplace_back(normal, pointInWorld, -dep[j]); + } + } + } + else + { + // we have more contacts than are wanted, some of them must be culled. + // find the deepest point, it is always the first contact. + int i1 = 0; + S maxdepth = dep[0]; + for(int i = 1; i < cnum; ++i) + { + if(dep[i] > maxdepth) + { + maxdepth = dep[i]; + i1 = i; + } + } + + int iret[8]; + cullPoints2(cnum, ret, maxc, i1, iret); + + for(int j = 0; j < maxc; ++j) + { + Vector3 posInWorld = points[iret[j]] + (*pa); + if(code < 4) + contacts.emplace_back(normal, posInWorld, -dep[iret[j]]); + else + contacts.emplace_back(normal, posInWorld - normal * dep[iret[j]], -dep[iret[j]]); + } + cnum = maxc; + } + + *return_code = code; + return cnum; +} + +//============================================================================== +template +int boxBox2( + const Vector3& side1, + const Transform3& tf1, + const Vector3& side2, + const Transform3& tf2, + Vector3& normal, + S* depth, + int* return_code, + int maxc, + std::vector>& contacts) +{ + const S fudge_factor = S(1.05); + Vector3 normalC; + + const Vector3 p = tf2.translation() - tf1.translation(); // get vector from centers of box 1 to box 2, relative to box 1 + const Vector3 pp = tf1.linear().transpose() * p; // get pp = p relative to body 1 + + // get side lengths / 2 + const Vector3 A = side1 * 0.5; + const Vector3 B = side2 * 0.5; + + // Rij is R1'*R2, i.e. the relative rotation between R1 and R2 + const Matrix3 R = tf1.linear().transpose() * tf2.linear(); + Matrix3 Q = R.cwiseAbs(); + + // for all 15 possible separating axes: + // * see if the axis separates the boxes. if so, return 0. + // * find the depth of the penetration along the separating axis (s2) + // * if this is the largest depth so far, record it. + // the normal vector will be set to the separating axis with the smallest + // depth. note: normalR is set to point to a column of R1 or R2 if that is + // the smallest depth normal so far. otherwise normalR is 0 and normalC is + // set to a vector relative to body 1. invert_normal is 1 if the sign of + // the normal should be flipped. + + int best_col_id = -1; + const Transform3* normalT = nullptr; + + S s = - std::numeric_limits::max(); + int invert_normal = 0; + int code = 0; + + // separating axis = u1, u2, u3 + S tmp = pp[0]; + S s2 = std::abs(tmp) - (Q.row(0).dot(B) + A[0]); + if(s2 > 0) { *return_code = 0; return 0; } + if(s2 > s) + { + s = s2; + best_col_id = 0; + normalT = &tf1; + invert_normal = (tmp < 0); + code = 1; + } + + tmp = pp[1]; + s2 = std::abs(tmp) - (Q.row(1).dot(B) + A[1]); + if(s2 > 0) { *return_code = 0; return 0; } + if(s2 > s) + { + s = s2; + best_col_id = 1; + normalT = &tf1; + invert_normal = (tmp < 0); + code = 2; + } + + tmp = pp[2]; + s2 = std::abs(tmp) - (Q.row(2).dot(B) + A[2]); + if(s2 > 0) { *return_code = 0; return 0; } + if(s2 > s) + { + s = s2; + best_col_id = 2; + normalT = &tf1; + invert_normal = (tmp < 0); + code = 3; + } + + // separating axis = v1, v2, v3 + tmp = tf2.linear().col(0).dot(p); + s2 = std::abs(tmp) - (Q.col(0).dot(A) + B[0]); + if(s2 > 0) { *return_code = 0; return 0; } + if(s2 > s) + { + s = s2; + best_col_id = 0; + normalT = &tf2; + invert_normal = (tmp < 0); + code = 4; + } + + tmp = tf2.linear().col(1).dot(p); + s2 = std::abs(tmp) - (Q.col(1).dot(A) + B[1]); + if(s2 > 0) { *return_code = 0; return 0; } + if(s2 > s) + { + s = s2; + best_col_id = 1; + normalT = &tf2; + invert_normal = (tmp < 0); + code = 5; + } + + tmp = tf2.linear().col(2).dot(p); + s2 = std::abs(tmp) - (Q.col(2).dot(A) + B[2]); + if(s2 > 0) { *return_code = 0; return 0; } + if(s2 > s) + { + s = s2; + best_col_id = 2; + normalT = &tf2; + invert_normal = (tmp < 0); + code = 6; + } + + + S fudge2(1.0e-6); + Q.array() += fudge2; + + Vector3 n; + S eps = std::numeric_limits::epsilon(); + + // separating axis = u1 x (v1,v2,v3) + tmp = pp[2] * R(1, 0) - pp[1] * R(2, 0); + s2 = std::abs(tmp) - (A[1] * Q(2, 0) + A[2] * Q(1, 0) + B[1] * Q(0, 2) + B[2] * Q(0, 1)); + if(s2 > 0) { *return_code = 0; return 0; } + n = Vector3(0, -R(2, 0), R(1, 0)); + S l = n.norm(); + if(l > eps) + { + s2 /= l; + if(s2 * fudge_factor > s) + { + s = s2; + best_col_id = -1; + normalC = n / l; + invert_normal = (tmp < 0); + code = 7; + } + } + + tmp = pp[2] * R(1, 1) - pp[1] * R(2, 1); + s2 = std::abs(tmp) - (A[1] * Q(2, 1) + A[2] * Q(1, 1) + B[0] * Q(0, 2) + B[2] * Q(0, 0)); + if(s2 > 0) { *return_code = 0; return 0; } + n = Vector3(0, -R(2, 1), R(1, 1)); + l = n.norm(); + if(l > eps) + { + s2 /= l; + if(s2 * fudge_factor > s) + { + s = s2; + best_col_id = -1; + normalC = n / l; + invert_normal = (tmp < 0); + code = 8; + } + } + + tmp = pp[2] * R(1, 2) - pp[1] * R(2, 2); + s2 = std::abs(tmp) - (A[1] * Q(2, 2) + A[2] * Q(1, 2) + B[0] * Q(0, 1) + B[1] * Q(0, 0)); + if(s2 > 0) { *return_code = 0; return 0; } + n = Vector3(0, -R(2, 2), R(1, 2)); + l = n.norm(); + if(l > eps) + { + s2 /= l; + if(s2 * fudge_factor > s) + { + s = s2; + best_col_id = -1; + normalC = n / l; + invert_normal = (tmp < 0); + code = 9; + } + } + + // separating axis = u2 x (v1,v2,v3) + tmp = pp[0] * R(2, 0) - pp[2] * R(0, 0); + s2 = std::abs(tmp) - (A[0] * Q(2, 0) + A[2] * Q(0, 0) + B[1] * Q(1, 2) + B[2] * Q(1, 1)); + if(s2 > 0) { *return_code = 0; return 0; } + n = Vector3(R(2, 0), 0, -R(0, 0)); + l = n.norm(); + if(l > eps) + { + s2 /= l; + if(s2 * fudge_factor > s) + { + s = s2; + best_col_id = -1; + normalC = n / l; + invert_normal = (tmp < 0); + code = 10; + } + } + + tmp = pp[0] * R(2, 1) - pp[2] * R(0, 1); + s2 = std::abs(tmp) - (A[0] * Q(2, 1) + A[2] * Q(0, 1) + B[0] * Q(1, 2) + B[2] * Q(1, 0)); + if(s2 > 0) { *return_code = 0; return 0; } + n = Vector3(R(2, 1), 0, -R(0, 1)); + l = n.norm(); + if(l > eps) + { + s2 /= l; + if(s2 * fudge_factor > s) + { + s = s2; + best_col_id = -1; + normalC = n / l; + invert_normal = (tmp < 0); + code = 11; + } + } + + tmp = pp[0] * R(2, 2) - pp[2] * R(0, 2); + s2 = std::abs(tmp) - (A[0] * Q(2, 2) + A[2] * Q(0, 2) + B[0] * Q(1, 1) + B[1] * Q(1, 0)); + if(s2 > 0) { *return_code = 0; return 0; } + n = Vector3(R(2, 2), 0, -R(0, 2)); + l = n.norm(); + if(l > eps) + { + s2 /= l; + if(s2 * fudge_factor > s) + { + s = s2; + best_col_id = -1; + normalC = n / l; + invert_normal = (tmp < 0); + code = 12; + } + } + + // separating axis = u3 x (v1,v2,v3) + tmp = pp[1] * R(0, 0) - pp[0] * R(1, 0); + s2 = std::abs(tmp) - (A[0] * Q(1, 0) + A[1] * Q(0, 0) + B[1] * Q(2, 2) + B[2] * Q(2, 1)); + if(s2 > 0) { *return_code = 0; return 0; } + n = Vector3(-R(1, 0), R(0, 0), 0); + l = n.norm(); + if(l > eps) + { + s2 /= l; + if(s2 * fudge_factor > s) + { + s = s2; + best_col_id = -1; + normalC = n / l; + invert_normal = (tmp < 0); + code = 13; + } + } + + tmp = pp[1] * R(0, 1) - pp[0] * R(1, 1); + s2 = std::abs(tmp) - (A[0] * Q(1, 1) + A[1] * Q(0, 1) + B[0] * Q(2, 2) + B[2] * Q(2, 0)); + if(s2 > 0) { *return_code = 0; return 0; } + n = Vector3(-R(1, 1), R(0, 1), 0); + l = n.norm(); + if(l > eps) + { + s2 /= l; + if(s2 * fudge_factor > s) + { + s = s2; + best_col_id = -1; + normalC = n / l; + invert_normal = (tmp < 0); + code = 14; + } + } + + tmp = pp[1] * R(0, 2) - pp[0] * R(1, 2); + s2 = std::abs(tmp) - (A[0] * Q(1, 2) + A[1] * Q(0, 2) + B[0] * Q(2, 1) + B[1] * Q(2, 0)); + if(s2 > 0) { *return_code = 0; return 0; } + n = Vector3(-R(1, 2), R(0, 2), 0); + l = n.norm(); + if(l > eps) + { + s2 /= l; + if(s2 * fudge_factor > s) + { + s = s2; + best_col_id = -1; + normalC = n / l; + invert_normal = (tmp < 0); + code = 15; + } + } + + + + if (!code) { *return_code = code; return 0; } + + // if we get to this point, the boxes interpenetrate. compute the normal + // in global coordinates. + if(best_col_id != -1) + normal = normalT->linear().col(best_col_id); + else + normal = tf1.linear() * normalC; + + if(invert_normal) + normal = -normal; + + *depth = -s; // s is negative when the boxes are in collision + + // compute contact point(s) + + if(code > 6) + { + // an edge from box 1 touches an edge from box 2. + // find a point pa on the intersecting edge of box 1 + Vector3 pa(tf1.translation()); + S sign; + + for(int j = 0; j < 3; ++j) + { + sign = (tf1.linear().col(j).dot(normal) > 0) ? 1 : -1; + pa += tf1.linear().col(j) * (A[j] * sign); + } + + // find a point pb on the intersecting edge of box 2 + Vector3 pb(tf2.translation()); + + for(int j = 0; j < 3; ++j) + { + sign = (tf2.linear().col(j).dot(normal) > 0) ? -1 : 1; + pb += tf2.linear().col(j) * (B[j] * sign); + } + + S alpha, beta; + Vector3 ua(tf1.linear().col((code-7)/3)); + Vector3 ub(tf2.linear().col((code-7)%3)); + + lineClosestApproach(pa, ua, pb, ub, &alpha, &beta); + pa += ua * alpha; + pb += ub * beta; + + + // Vector3 pointInWorld((pa + pb) * 0.5); + // contacts.push_back(ContactPoint(-normal, pointInWorld, -*depth)); + contacts.emplace_back(normal, pb, -*depth); + *return_code = code; + + return 1; + } + + // okay, we have a face-something intersection (because the separating + // axis is perpendicular to a face). define face 'a' to be the reference + // face (i.e. the normal vector is perpendicular to this) and face 'b' to be + // the incident face (the closest face of the other box). + + const Transform3 *Ta, *Tb; + const Vector3 *Sa, *Sb; + + if(code <= 3) + { + Ta = &tf1; + Tb = &tf2; + Sa = &A; + Sb = &B; + } + else + { + Ta = &tf2; + Tb = &tf1; + Sa = &B; + Sb = &A; + } + + // nr = normal vector of reference face dotted with axes of incident box. + // anr = absolute values of nr. + Vector3 normal2, nr, anr; + if(code <= 3) + normal2 = normal; + else + normal2 = -normal; + + nr = Tb->linear().transpose() * normal2; + anr = nr.cwiseAbs(); + + // find the largest compontent of anr: this corresponds to the normal + // for the indident face. the other axis numbers of the indicent face + // are stored in a1,a2. + int lanr, a1, a2; + if(anr[1] > anr[0]) + { + if(anr[1] > anr[2]) + { + a1 = 0; + lanr = 1; + a2 = 2; + } + else + { + a1 = 0; + a2 = 1; + lanr = 2; + } + } + else + { + if(anr[0] > anr[2]) + { + lanr = 0; + a1 = 1; + a2 = 2; + } + else + { + a1 = 0; + a2 = 1; + lanr = 2; + } + } + + // compute center point of incident face, in reference-face coordinates + Vector3 center; + if(nr[lanr] < 0) + center = Tb->translation() - Ta->translation() + Tb->linear().col(lanr) * ((*Sb)[lanr]); + else + center = Tb->translation() - Ta->translation() - Tb->linear().col(lanr) * ((*Sb)[lanr]); + + // find the normal and non-normal axis numbers of the reference box + int codeN, code1, code2; + if(code <= 3) + codeN = code-1; + else codeN = code-4; + + if(codeN == 0) + { + code1 = 1; + code2 = 2; + } + else if(codeN == 1) + { + code1 = 0; + code2 = 2; + } + else + { + code1 = 0; + code2 = 1; + } + + // find the four corners of the incident face, in reference-face coordinates + S quad[8]; // 2D coordinate of incident face (x,y pairs) + S c1, c2, m11, m12, m21, m22; + c1 = Ta->linear().col(code1).dot(center); + c2 = Ta->linear().col(code2).dot(center); + // optimize this? - we have already computed this data above, but it is not + // stored in an easy-to-index format. for now it's quicker just to recompute + // the four dot products. + Vector3 tempRac = Ta->linear().col(code1); + m11 = Tb->linear().col(a1).dot(tempRac); + m12 = Tb->linear().col(a2).dot(tempRac); + tempRac = Ta->linear().col(code2); + m21 = Tb->linear().col(a1).dot(tempRac); + m22 = Tb->linear().col(a2).dot(tempRac); + + S k1 = m11 * (*Sb)[a1]; + S k2 = m21 * (*Sb)[a1]; + S k3 = m12 * (*Sb)[a2]; + S k4 = m22 * (*Sb)[a2]; + quad[0] = c1 - k1 - k3; + quad[1] = c2 - k2 - k4; + quad[2] = c1 - k1 + k3; + quad[3] = c2 - k2 + k4; + quad[4] = c1 + k1 + k3; + quad[5] = c2 + k2 + k4; + quad[6] = c1 + k1 - k3; + quad[7] = c2 + k2 - k4; + + // find the size of the reference face + S rect[2]; + rect[0] = (*Sa)[code1]; + rect[1] = (*Sa)[code2]; + + // intersect the incident and reference faces + S ret[16]; + int n_intersect = intersectRectQuad2(rect, quad, ret); + if(n_intersect < 1) { *return_code = code; return 0; } // this should never happen + + // convert the intersection points into reference-face coordinates, + // and compute the contact position and depth for each point. only keep + // those points that have a positive (penetrating) depth. delete points in + // the 'ret' array as necessary so that 'point' and 'ret' correspond. + Vector3 points[8]; // penetrating contact points + S dep[8]; // depths for those points + S det1 = 1.f/(m11*m22 - m12*m21); + m11 *= det1; + m12 *= det1; + m21 *= det1; + m22 *= det1; + int cnum = 0; // number of penetrating contact points found + for(int j = 0; j < n_intersect; ++j) + { + S k1 = m22*(ret[j*2]-c1) - m12*(ret[j*2+1]-c2); + S k2 = -m21*(ret[j*2]-c1) + m11*(ret[j*2+1]-c2); + points[cnum] = center + Tb->linear().col(a1) * k1 + Tb->linear().col(a2) * k2; + dep[cnum] = (*Sa)[codeN] - normal2.dot(points[cnum]); + if(dep[cnum] >= 0) + { + ret[cnum*2] = ret[j*2]; + ret[cnum*2+1] = ret[j*2+1]; + cnum++; + } + } + if(cnum < 1) { *return_code = code; return 0; } // this should never happen + + // we can't generate more contacts than we actually have + if(maxc > cnum) maxc = cnum; + if(maxc < 1) maxc = 1; + + if(cnum <= maxc) + { + if(code<4) + { + // we have less contacts than we need, so we use them all + for(int j = 0; j < cnum; ++j) + { + Vector3 pointInWorld = points[j] + Ta->translation(); + contacts.emplace_back(normal, pointInWorld, -dep[j]); + } + } + else + { + // we have less contacts than we need, so we use them all + for(int j = 0; j < cnum; ++j) + { + Vector3 pointInWorld = points[j] + Ta->translation() - normal * dep[j]; + contacts.emplace_back(normal, pointInWorld, -dep[j]); + } + } + } + else + { + // we have more contacts than are wanted, some of them must be culled. + // find the deepest point, it is always the first contact. + int i1 = 0; + S maxdepth = dep[0]; + for(int i = 1; i < cnum; ++i) + { + if(dep[i] > maxdepth) + { + maxdepth = dep[i]; + i1 = i; + } + } + + int iret[8]; + cullPoints2(cnum, ret, maxc, i1, iret); + + for(int j = 0; j < maxc; ++j) + { + Vector3 posInWorld = points[iret[j]] + Ta->translation(); + if(code < 4) + contacts.emplace_back(normal, posInWorld, -dep[iret[j]]); + else + contacts.emplace_back(normal, posInWorld - normal * dep[iret[j]], -dep[iret[j]]); + } + cnum = maxc; + } + + *return_code = code; + return cnum; +} + +//============================================================================== +template +bool boxBoxIntersect(const Box& s1, const Transform3& tf1, + const Box& s2, const Transform3& tf2, + std::vector>* contacts_) +{ + std::vector> contacts; + int return_code; + Vector3 normal; + S depth; + /* int cnum = */ boxBox2(s1.side, tf1, + s2.side, tf2, + normal, &depth, &return_code, + 4, contacts); + + if(contacts_) + *contacts_ = contacts; + + return return_code != 0; +} + +} // namespace details + +} // namespace fcl + +#endif diff --git a/include/fcl/narrowphase/detail/capsule_capsule.h b/include/fcl/narrowphase/detail/capsule_capsule.h new file mode 100755 index 000000000..aa008fc69 --- /dev/null +++ b/include/fcl/narrowphase/detail/capsule_capsule.h @@ -0,0 +1,198 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_NARROWPHASE_DETAIL_CAPSULECAPSULE_H +#define FCL_NARROWPHASE_DETAIL_CAPSULECAPSULE_H + +#include "fcl/collision_data.h" + +namespace fcl +{ + +namespace details +{ + +// Clamp n to lie within the range [min, max] +template +S clamp(S n, S min, S max); + +// Computes closest points C1 and C2 of S1(s)=P1+s*(Q1-P1) and +// S2(t)=P2+t*(Q2-P2), returning s and t. Function result is squared +// distance between between S1(s) and S2(t) +template +S closestPtSegmentSegment( + Vector3 p1, Vector3 q1, Vector3 p2, Vector3 q2, + S &s, S &t, Vector3 &c1, Vector3 &c2); + +// Computes closest points C1 and C2 of S1(s)=P1+s*(Q1-P1) and +// S2(t)=P2+t*(Q2-P2), returning s and t. Function result is squared +// distance between between S1(s) and S2(t) +template +bool capsuleCapsuleDistance(const Capsule& s1, const Transform3& tf1, + const Capsule& s2, const Transform3& tf2, + S* dist, Vector3* p1_res, Vector3* p2_res); + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +S clamp(S n, S min, S max) +{ + if (n < min) return min; + if (n > max) return max; + return n; +} + +//============================================================================== +template +S closestPtSegmentSegment( + Vector3 p1, Vector3 q1, Vector3 p2, Vector3 q2, + S &s, S &t, Vector3 &c1, Vector3 &c2) +{ + const S EPSILON = 0.001; + Vector3 d1 = q1 - p1; // Direction vector of segment S1 + Vector3 d2 = q2 - p2; // Direction vector of segment S2 + Vector3 r = p1 - p2; + S a = d1.dot(d1); // Squared length of segment S1, always nonnegative + + S e = d2.dot(d2); // Squared length of segment S2, always nonnegative + S f = d2.dot(r); + // Check if either or both segments degenerate into points + if (a <= EPSILON && e <= EPSILON) { + // Both segments degenerate into points + s = t = 0.0; + c1 = p1; + c2 = p2; + Vector3 diff = c1-c2; + S res = diff.dot(diff); + return res; + } + if (a <= EPSILON) { + // First segment degenerates into a point + s = 0.0; + t = f / e; // s = 0 => t = (b*s + f) / e = f / e + t = clamp(t, (S)0.0, (S)1.0); + } else { + S c = d1.dot(r); + if (e <= EPSILON) { + // Second segment degenerates into a point + t = 0.0; + s = clamp(-c / a, (S)0.0, (S)1.0); // t = 0 => s = (b*t - c) / a = -c / a + } else { + // The general nondegenerate case starts here + S b = d1.dot(d2); + S denom = a*e-b*b; // Always nonnegative + // If segments not parallel, compute closest point on L1 to L2 and + // clamp to segment S1. Else pick arbitrary s (here 0) + if (denom != 0.0) { + std::cerr << "denominator equals zero, using 0 as reference" << std::endl; + s = clamp((b*f - c*e) / denom, (S)0.0, (S)1.0); + } else s = 0.0; + // Compute point on L2 closest to S1(s) using + // t = Dot((P1 + D1*s) - P2,D2) / Dot(D2,D2) = (b*s + f) / e + t = (b*s + f) / e; + + // + //If t in [0,1] done. Else clamp t, recompute s for the new value + //of t using s = Dot((P2 + D2*t) - P1,D1) / Dot(D1,D1)= (t*b - c) / a + //and clamp s to [0, 1] + if(t < 0.0) { + t = 0.0; + s = clamp(-c / a, (S)0.0, (S)1.0); + } else if (t > 1.0) { + t = 1.0; + s = clamp((b - c) / a, (S)0.0, (S)1.0); + } + } + } + c1 = p1 + d1 * s; + c2 = p2 + d2 * t; + Vector3 diff = c1-c2; + S res = diff.dot(diff); + return res; +} + +//============================================================================== +template +bool capsuleCapsuleDistance(const Capsule& s1, const Transform3& tf1, + const Capsule& s2, const Transform3& tf2, + S* dist, Vector3* p1_res, Vector3* p2_res) +{ + + Vector3 p1(tf1.translation()); + Vector3 p2(tf2.translation()); + + // line segment composes two points. First point is given by the origin, second point is computed by the origin transformed along z. + // extension along z-axis means transformation with identity matrix and translation vector z pos + Transform3 transformQ1 = tf1 * Translation3(Vector3(0,0,s1.lz)); + Vector3 q1 = transformQ1.translation(); + + Transform3 transformQ2 = tf2 * Translation3(Vector3(0,0,s2.lz)); + Vector3 q2 = transformQ2.translation(); + + // s and t correspont to the length of the line segment + S s, t; + Vector3 c1, c2; + + S result = closestPtSegmentSegment(p1, q1, p2, q2, s, t, c1, c2); + *dist = sqrt(result)-s1.radius-s2.radius; + + // getting directional unit vector + Vector3 distVec = c2 -c1; + distVec.normalize(); + + // extend the point to be border of the capsule. + // Done by following the directional unit vector for the length of the capsule radius + *p1_res = c1 + distVec*s1.radius; + + distVec = c1-c2; + distVec.normalize(); + + *p2_res = c2 + distVec*s2.radius; + + return true; +} + +} // namespace details + +} // namespace fcl + +#endif diff --git a/include/fcl/narrowphase/detail/gjk.h b/include/fcl/narrowphase/detail/gjk.h new file mode 100644 index 000000000..25b339156 --- /dev/null +++ b/include/fcl/narrowphase/detail/gjk.h @@ -0,0 +1,1006 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_NARROWPHASE_DETAIL_GJK_H +#define FCL_NARROWPHASE_DETAIL_GJK_H + +#include "fcl/shape/geometric_shapes.h" +#include "fcl/intersect.h" + +namespace fcl +{ + +namespace details +{ + +/// @brief the support function for shape +template +Vector3 getSupport( + const ShapeBase* shape, + const Eigen::MatrixBase& dir); + +/// @brief Minkowski difference class of two shapes +template +struct MinkowskiDiff +{ + /// @brief points to two shapes + const ShapeBase* shapes[2]; + + /// @brief rotation from shape0 to shape1 + Matrix3 toshape1; + + /// @brief transform from shape1 to shape0 + Transform3 toshape0; + + MinkowskiDiff() { } + + /// @brief support function for shape0 + inline Vector3 support0(const Vector3& d) const + { + return getSupport(shapes[0], d); + } + + /// @brief support function for shape1 + inline Vector3 support1(const Vector3& d) const + { + return toshape0 * getSupport(shapes[1], toshape1 * d); + } + + /// @brief support function for the pair of shapes + inline Vector3 support(const Vector3& d) const + { + return support0(d) - support1(-d); + } + + /// @brief support function for the d-th shape (d = 0 or 1) + inline Vector3 support(const Vector3& d, size_t index) const + { + if(index) + return support1(d); + else + return support0(d); + } + + /// @brief support function for translating shape0, which is translating at velocity v + inline Vector3 support0(const Vector3& d, const Vector3& v) const + { + if(d.dot(v) <= 0) + return getSupport(shapes[0], d); + else + return getSupport(shapes[0], d) + v; + } + + /// @brief support function for the pair of shapes, where shape0 is translating at velocity v + inline Vector3 support(const Vector3& d, const Vector3& v) const + { + return support0(d, v) - support1(-d); + } + + /// @brief support function for the d-th shape (d = 0 or 1), where shape0 is translating at velocity v + inline Vector3 support(const Vector3& d, const Vector3& v, size_t index) const + { + if(index) + return support1(d); + else + return support0(d, v); + } + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +using MinkowskiDifff = MinkowskiDiff; +using MinkowskiDiffd = MinkowskiDiff; + +/// @brief class for GJK algorithm +template +struct GJK +{ + struct SimplexV + { + /// @brief support direction + Vector3 d; + /// @brieg support vector (i.e., the furthest point on the shape along the support direction) + Vector3 w; + }; + + struct Simplex + { + /// @brief simplex vertex + SimplexV* c[4]; + /// @brief weight + S p[4]; + /// @brief size of simplex (number of vertices) + size_t rank; + + Simplex() : rank(0) {} + }; + + enum Status {Valid, Inside, Failed}; + + MinkowskiDiff shape; + Vector3 ray; + S distance; + Simplex simplices[2]; + + + GJK(unsigned int max_iterations_, S tolerance_) : max_iterations(max_iterations_), + tolerance(tolerance_) + { + initialize(); + } + + void initialize(); + + /// @brief GJK algorithm, given the initial value guess + Status evaluate(const MinkowskiDiff& shape_, const Vector3& guess); + + /// @brief apply the support function along a direction, the result is return in sv + void getSupport(const Vector3& d, SimplexV& sv) const; + + /// @brief apply the support function along a direction, the result is return is sv, here shape0 is translating at velocity v + void getSupport(const Vector3& d, const Vector3& v, SimplexV& sv) const; + + /// @brief discard one vertex from the simplex + void removeVertex(Simplex& simplex); + + /// @brief append one vertex to the simplex + void appendVertex(Simplex& simplex, const Vector3& v); + + /// @brief whether the simplex enclose the origin + bool encloseOrigin(); + + /// @brief get the underlying simplex using in GJK, can be used for cache in next iteration + inline Simplex* getSimplex() const + { + return simplex; + } + + /// @brief get the guess from current simplex + Vector3 getGuessFromSimplex() const; + +private: + SimplexV store_v[4]; + SimplexV* free_v[4]; + size_t nfree; + size_t current; + Simplex* simplex; + Status status; + + unsigned int max_iterations; + S tolerance; + +}; + +using GJKf = GJK; +using GJKd = GJK; + +//static const size_t EPA_MAX_FACES = 128; +//static const size_t EPA_MAX_VERTICES = 64; +//static const S EPA_EPS = 0.000001; +//static const size_t EPA_MAX_ITERATIONS = 255; +// TODO(JS): remove? + +/// @brief class for EPA algorithm +template +struct EPA +{ +private: + using SimplexV = typename GJK::SimplexV; + + struct SimplexF + { + Vector3 n; + S d; + SimplexV* c[3]; // a face has three vertices + SimplexF* f[3]; // a face has three adjacent faces + SimplexF* l[2]; // the pre and post faces in the list + size_t e[3]; + size_t pass; + }; + + struct SimplexList + { + SimplexF* root; + size_t count; + SimplexList() : root(nullptr), count(0) {} + void append(SimplexF* face) + { + face->l[0] = nullptr; + face->l[1] = root; + if(root) root->l[0] = face; + root = face; + ++count; + } + + void remove(SimplexF* face) + { + if(face->l[1]) face->l[1]->l[0] = face->l[0]; + if(face->l[0]) face->l[0]->l[1] = face->l[1]; + if(face == root) root = face->l[1]; + --count; + } + }; + + static inline void bind(SimplexF* fa, size_t ea, SimplexF* fb, size_t eb) + { + fa->e[ea] = eb; fa->f[ea] = fb; + fb->e[eb] = ea; fb->f[eb] = fa; + } + + struct SimplexHorizon + { + SimplexF* cf; // current face in the horizon + SimplexF* ff; // first face in the horizon + size_t nf; // number of faces in the horizon + SimplexHorizon() : cf(nullptr), ff(nullptr), nf(0) {} + }; + +private: + unsigned int max_face_num; + unsigned int max_vertex_num; + unsigned int max_iterations; + S tolerance; + +public: + + enum Status {Valid, Touching, Degenerated, NonConvex, InvalidHull, OutOfFaces, OutOfVertices, AccuracyReached, FallBack, Failed}; + + Status status; + typename GJK::Simplex result; + Vector3 normal; + S depth; + SimplexV* sv_store; + SimplexF* fc_store; + size_t nextsv; + SimplexList hull, stock; + + EPA(unsigned int max_face_num_, unsigned int max_vertex_num_, unsigned int max_iterations_, S tolerance_) : max_face_num(max_face_num_), + max_vertex_num(max_vertex_num_), + max_iterations(max_iterations_), + tolerance(tolerance_) + { + initialize(); + } + + ~EPA() + { + delete [] sv_store; + delete [] fc_store; + } + + void initialize(); + + bool getEdgeDist(SimplexF* face, SimplexV* a, SimplexV* b, S& dist); + + SimplexF* newFace(SimplexV* a, SimplexV* b, SimplexV* c, bool forced); + + /// @brief Find the best polytope face to split + SimplexF* findBest(); + + Status evaluate(GJK& gjk, const Vector3& guess); + + /// @brief the goal is to add a face connecting vertex w and face edge f[e] + bool expand(size_t pass, SimplexV* w, SimplexF* f, size_t e, SimplexHorizon& horizon); +}; + +using EPAf = EPA; +using EPAd = EPA; + +} // namespace details + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +namespace details +{ + +template +Vector3 getSupport( + const ShapeBase* shape, + const Eigen::MatrixBase& dir) +{ + // Check the number of rows is 6 at compile time + EIGEN_STATIC_ASSERT( + Derived::RowsAtCompileTime == 3 + && Derived::ColsAtCompileTime == 1, + THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE); + + switch(shape->getNodeType()) + { + case GEOM_TRIANGLE: + { + const auto* triangle = static_cast*>(shape); + S dota = dir.dot(triangle->a); + S dotb = dir.dot(triangle->b); + S dotc = dir.dot(triangle->c); + if(dota > dotb) + { + if(dotc > dota) + return triangle->c; + else + return triangle->a; + } + else + { + if(dotc > dotb) + return triangle->c; + else + return triangle->b; + } + } + break; + case GEOM_BOX: + { + const Box* box = static_cast*>(shape); + return Vector3((dir[0]>0)?(box->side[0]/2):(-box->side[0]/2), + (dir[1]>0)?(box->side[1]/2):(-box->side[1]/2), + (dir[2]>0)?(box->side[2]/2):(-box->side[2]/2)); + } + break; + case GEOM_SPHERE: + { + const Sphere* sphere = static_cast*>(shape); + return dir * sphere->radius; + } + break; + case GEOM_ELLIPSOID: + { + const Ellipsoid* ellipsoid = static_cast*>(shape); + + const S a2 = ellipsoid->radii[0] * ellipsoid->radii[0]; + const S b2 = ellipsoid->radii[1] * ellipsoid->radii[1]; + const S c2 = ellipsoid->radii[2] * ellipsoid->radii[2]; + + const Vector3 v(a2 * dir[0], b2 * dir[1], c2 * dir[2]); + const S d = std::sqrt(v.dot(dir)); + + return v / d; + } + break; + case GEOM_CAPSULE: + { + const Capsule* capsule = static_cast*>(shape); + S half_h = capsule->lz * 0.5; + Vector3 pos1(0, 0, half_h); + Vector3 pos2(0, 0, -half_h); + Vector3 v = dir * capsule->radius; + pos1 += v; + pos2 += v; + if(dir.dot(pos1) > dir.dot(pos2)) + return pos1; + else return pos2; + } + break; + case GEOM_CONE: + { + const Cone* cone = static_cast*>(shape); + S zdist = dir[0] * dir[0] + dir[1] * dir[1]; + S len = zdist + dir[2] * dir[2]; + zdist = std::sqrt(zdist); + len = std::sqrt(len); + S half_h = cone->lz * 0.5; + S radius = cone->radius; + + S sin_a = radius / std::sqrt(radius * radius + 4 * half_h * half_h); + + if(dir[2] > len * sin_a) + return Vector3(0, 0, half_h); + else if(zdist > 0) + { + S rad = radius / zdist; + return Vector3(rad * dir[0], rad * dir[1], -half_h); + } + else + return Vector3(0, 0, -half_h); + } + break; + case GEOM_CYLINDER: + { + const Cylinder* cylinder = static_cast*>(shape); + S zdist = std::sqrt(dir[0] * dir[0] + dir[1] * dir[1]); + S half_h = cylinder->lz * 0.5; + if(zdist == 0.0) + { + return Vector3(0, 0, (dir[2]>0)? half_h:-half_h); + } + else + { + S d = cylinder->radius / zdist; + return Vector3(d * dir[0], d * dir[1], (dir[2]>0)?half_h:-half_h); + } + } + break; + case GEOM_CONVEX: + { + 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) + { + S dot = dir.dot(*curp); + if(dot > maxdot) + { + bestv = *curp; + maxdot = dot; + } + } + return bestv; + } + break; + case GEOM_PLANE: + break; + default: + ; // nothing + } + + return Vector3::Zero(); +} + +//============================================================================== +template +void GJK::initialize() +{ + ray.setZero(); + nfree = 0; + status = Failed; + current = 0; + distance = 0.0; + simplex = nullptr; +} + +//============================================================================== +template +Vector3 GJK::getGuessFromSimplex() const +{ + return ray; +} + + +//============================================================================== +template +typename GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vector3& guess) +{ + size_t iterations = 0; + S alpha = 0; + Vector3 lastw[4]; + size_t clastw = 0; + + free_v[0] = &store_v[0]; + free_v[1] = &store_v[1]; + free_v[2] = &store_v[2]; + free_v[3] = &store_v[3]; + + nfree = 4; + current = 0; + status = Valid; + shape = shape_; + distance = 0.0; + simplices[0].rank = 0; + ray = guess; + + appendVertex(simplices[0], (ray.squaredNorm() > 0) ? (-ray).eval() : Vector3::UnitX()); + simplices[0].p[0] = 1; + ray = simplices[0].c[0]->w; + lastw[0] = lastw[1] = lastw[2] = lastw[3] = ray; // cache previous support points, the new support point will compare with it to avoid too close support points + + do + { + size_t next = 1 - current; + Simplex& curr_simplex = simplices[current]; + Simplex& next_simplex = simplices[next]; + + // check A: when origin is near the existing simplex, stop + S rl = ray.norm(); + if(rl < tolerance) // mean origin is near the face of original simplex, return touch + { + status = Inside; + break; + } + + appendVertex(curr_simplex, -ray); // see below, ray points away from origin + + // check B: when the new support point is close to previous support points, stop (as the new simplex is degenerated) + Vector3& w = curr_simplex.c[curr_simplex.rank - 1]->w; + bool found = false; + for(size_t i = 0; i < 4; ++i) + { + if((w - lastw[i]).squaredNorm() < tolerance) + { + found = true; break; + } + } + + if(found) + { + removeVertex(simplices[current]); + break; + } + else + { + lastw[clastw = (clastw+1)&3] = w; + } + + // check C: when the new support point is close to the sub-simplex where the ray point lies, stop (as the new simplex again is degenerated) + S omega = ray.dot(w) / rl; + alpha = std::max(alpha, omega); + if((rl - alpha) - tolerance * rl <= 0) + { + removeVertex(simplices[current]); + break; + } + + typename Project::ProjectResult project_res; + switch(curr_simplex.rank) + { + case 2: + project_res = Project::projectLineOrigin(curr_simplex.c[0]->w, curr_simplex.c[1]->w); break; + case 3: + project_res = Project::projectTriangleOrigin(curr_simplex.c[0]->w, curr_simplex.c[1]->w, curr_simplex.c[2]->w); break; + case 4: + project_res = Project::projectTetrahedraOrigin(curr_simplex.c[0]->w, curr_simplex.c[1]->w, curr_simplex.c[2]->w, curr_simplex.c[3]->w); break; + } + + if(project_res.sqr_distance >= 0) + { + next_simplex.rank = 0; + ray.setZero(); + current = next; + for(size_t i = 0; i < curr_simplex.rank; ++i) + { + if(project_res.encode & (1 << i)) + { + next_simplex.c[next_simplex.rank] = curr_simplex.c[i]; + next_simplex.p[next_simplex.rank++] = project_res.parameterization[i]; // weights[i]; + ray += curr_simplex.c[i]->w * project_res.parameterization[i]; // weights[i]; + } + else + free_v[nfree++] = curr_simplex.c[i]; + } + if(project_res.encode == 15) status = Inside; // the origin is within the 4-simplex, collision + } + else + { + removeVertex(simplices[current]); + break; + } + + status = ((++iterations) < max_iterations) ? status : Failed; + + } while(status == Valid); + + simplex = &simplices[current]; + switch(status) + { + case Valid: distance = ray.norm(); break; + case Inside: distance = 0; break; + default: break; + } + return status; +} + +//============================================================================== +template +void GJK::getSupport(const Vector3& d, SimplexV& sv) const +{ + sv.d = d.normalized(); + sv.w = shape.support(sv.d); +} + +//============================================================================== +template +void GJK::getSupport(const Vector3& d, const Vector3& v, SimplexV& sv) const +{ + sv.d = d.normalized(); + sv.w = shape.support(sv.d, v); +} + +//============================================================================== +template +void GJK::removeVertex(Simplex& simplex) +{ + free_v[nfree++] = simplex.c[--simplex.rank]; +} + +//============================================================================== +template +void GJK::appendVertex(Simplex& simplex, const Vector3& v) +{ + simplex.p[simplex.rank] = 0; // initial weight 0 + simplex.c[simplex.rank] = free_v[--nfree]; // set the memory + getSupport(v, *simplex.c[simplex.rank++]); +} + +//============================================================================== +template +bool GJK::encloseOrigin() +{ + switch(simplex->rank) + { + case 1: + { + for(size_t i = 0; i < 3; ++i) + { + Vector3 axis = Vector3::Zero(); + axis[i] = 1; + appendVertex(*simplex, axis); + if(encloseOrigin()) return true; + removeVertex(*simplex); + appendVertex(*simplex, -axis); + if(encloseOrigin()) return true; + removeVertex(*simplex); + } + } + break; + case 2: + { + Vector3 d = simplex->c[1]->w - simplex->c[0]->w; + for(size_t i = 0; i < 3; ++i) + { + Vector3 axis = Vector3::Zero(); + axis[i] = 1; + Vector3 p = d.cross(axis); + if(p.squaredNorm() > 0) + { + appendVertex(*simplex, p); + if(encloseOrigin()) return true; + removeVertex(*simplex); + appendVertex(*simplex, -p); + if(encloseOrigin()) return true; + removeVertex(*simplex); + } + } + } + break; + case 3: + { + Vector3 n = (simplex->c[1]->w - simplex->c[0]->w).cross(simplex->c[2]->w - simplex->c[0]->w); + if(n.squaredNorm() > 0) + { + appendVertex(*simplex, n); + if(encloseOrigin()) return true; + removeVertex(*simplex); + appendVertex(*simplex, -n); + if(encloseOrigin()) return true; + removeVertex(*simplex); + } + } + break; + case 4: + { + if(std::abs(triple(simplex->c[0]->w - simplex->c[3]->w, simplex->c[1]->w - simplex->c[3]->w, simplex->c[2]->w - simplex->c[3]->w)) > 0) + return true; + } + break; + } + + return false; +} + +//============================================================================== +template +void EPA::initialize() +{ + sv_store = new SimplexV[max_vertex_num]; + fc_store = new SimplexF[max_face_num]; + status = Failed; + normal = Vector3(0, 0, 0); + depth = 0; + nextsv = 0; + for(size_t i = 0; i < max_face_num; ++i) + stock.append(&fc_store[max_face_num-i-1]); +} + +//============================================================================== +template +bool EPA::getEdgeDist(SimplexF* face, SimplexV* a, SimplexV* b, S& dist) +{ + Vector3 ba = b->w - a->w; + Vector3 n_ab = ba.cross(face->n); + S a_dot_nab = a->w.dot(n_ab); + + if(a_dot_nab < 0) // the origin is on the outside part of ab + { + // following is similar to projectOrigin for two points + // however, as we dont need to compute the parameterization, dont need to compute 0 or 1 + S a_dot_ba = a->w.dot(ba); + S b_dot_ba = b->w.dot(ba); + + if(a_dot_ba > 0) + dist = a->w.norm(); + else if(b_dot_ba < 0) + dist = b->w.norm(); + else + { + S a_dot_b = a->w.dot(b->w); + dist = std::sqrt(std::max(a->w.squaredNorm() * b->w.squaredNorm() - a_dot_b * a_dot_b, (S)0)); + } + + return true; + } + + return false; +} + +//============================================================================== +template +typename EPA::SimplexF* EPA::newFace( + typename GJK::SimplexV* a, + typename GJK::SimplexV* b, + typename GJK::SimplexV* c, + bool forced) +{ + if(stock.root) + { + SimplexF* face = stock.root; + stock.remove(face); + hull.append(face); + face->pass = 0; + face->c[0] = a; + face->c[1] = b; + face->c[2] = c; + face->n.noalias() = (b->w - a->w).cross(c->w - a->w); + S l = face->n.norm(); + + if(l > tolerance) + { + if(!(getEdgeDist(face, a, b, face->d) || + getEdgeDist(face, b, c, face->d) || + getEdgeDist(face, c, a, face->d))) + { + face->d = a->w.dot(face->n) / l; + } + + face->n /= l; + if(forced || face->d >= -tolerance) + return face; + else + status = NonConvex; + } + else + status = Degenerated; + + hull.remove(face); + stock.append(face); + return nullptr; + } + + status = stock.root ? OutOfVertices : OutOfFaces; + return nullptr; +} + +//============================================================================== +/** \brief Find the best polytope face to split */ +template +typename EPA::SimplexF* EPA::findBest() +{ + SimplexF* minf = hull.root; + S mind = minf->d * minf->d; + for(SimplexF* f = minf->l[1]; f; f = f->l[1]) + { + S sqd = f->d * f->d; + if(sqd < mind) + { + minf = f; + mind = sqd; + } + } + return minf; +} + +//============================================================================== +template +typename EPA::Status EPA::evaluate(GJK& gjk, const Vector3& guess) +{ + typename GJK::Simplex& simplex = *gjk.getSimplex(); + if((simplex.rank > 1) && gjk.encloseOrigin()) + { + while(hull.root) + { + SimplexF* f = hull.root; + hull.remove(f); + stock.append(f); + } + + status = Valid; + nextsv = 0; + + if((simplex.c[0]->w - simplex.c[3]->w).dot((simplex.c[1]->w - simplex.c[3]->w).cross(simplex.c[2]->w - simplex.c[3]->w)) < 0) + { + SimplexV* tmp = simplex.c[0]; + simplex.c[0] = simplex.c[1]; + simplex.c[1] = tmp; + + S tmpv = simplex.p[0]; + simplex.p[0] = simplex.p[1]; + simplex.p[1] = tmpv; + } + + SimplexF* tetrahedron[] = {newFace(simplex.c[0], simplex.c[1], simplex.c[2], true), + newFace(simplex.c[1], simplex.c[0], simplex.c[3], true), + newFace(simplex.c[2], simplex.c[1], simplex.c[3], true), + newFace(simplex.c[0], simplex.c[2], simplex.c[3], true) }; + + if(hull.count == 4) + { + SimplexF* best = findBest(); // find the best face (the face with the minimum distance to origin) to split + SimplexF outer = *best; + size_t pass = 0; + size_t iterations = 0; + + // set the face connectivity + bind(tetrahedron[0], 0, tetrahedron[1], 0); + bind(tetrahedron[0], 1, tetrahedron[2], 0); + bind(tetrahedron[0], 2, tetrahedron[3], 0); + bind(tetrahedron[1], 1, tetrahedron[3], 2); + bind(tetrahedron[1], 2, tetrahedron[2], 1); + bind(tetrahedron[2], 2, tetrahedron[3], 1); + + status = Valid; + for(; iterations < max_iterations; ++iterations) + { + if(nextsv < max_vertex_num) + { + SimplexHorizon horizon; + SimplexV* w = &sv_store[nextsv++]; + bool valid = true; + best->pass = ++pass; + gjk.getSupport(best->n, *w); + S wdist = best->n.dot(w->w) - best->d; + if(wdist > tolerance) + { + for(size_t j = 0; (j < 3) && valid; ++j) + { + valid &= expand(pass, w, best->f[j], best->e[j], horizon); + } + + + if(valid && horizon.nf >= 3) + { + // need to add the edge connectivity between first and last faces + bind(horizon.ff, 2, horizon.cf, 1); + hull.remove(best); + stock.append(best); + best = findBest(); + outer = *best; + } + else + { + status = InvalidHull; break; + } + } + else + { + status = AccuracyReached; break; + } + } + else + { + status = OutOfVertices; break; + } + } + + Vector3 projection = outer.n * outer.d; + normal = outer.n; + depth = outer.d; + result.rank = 3; + result.c[0] = outer.c[0]; + result.c[1] = outer.c[1]; + result.c[2] = outer.c[2]; + result.p[0] = ((outer.c[1]->w - projection).cross(outer.c[2]->w - projection)).norm(); + result.p[1] = ((outer.c[2]->w - projection).cross(outer.c[0]->w - projection)).norm(); + result.p[2] = ((outer.c[0]->w - projection).cross(outer.c[1]->w - projection)).norm(); + + S sum = result.p[0] + result.p[1] + result.p[2]; + result.p[0] /= sum; + result.p[1] /= sum; + result.p[2] /= sum; + return status; + } + } + + status = FallBack; + normal = -guess; + S nl = normal.norm(); + if(nl > 0) normal /= nl; + else normal = Vector3(1, 0, 0); + depth = 0; + result.rank = 1; + result.c[0] = simplex.c[0]; + result.p[0] = 1; + return status; +} + +//============================================================================== +/** \brief the goal is to add a face connecting vertex w and face edge f[e] */ +template +bool EPA::expand(size_t pass, SimplexV* w, SimplexF* f, size_t e, SimplexHorizon& horizon) +{ + static const size_t nexti[] = {1, 2, 0}; + static const size_t previ[] = {2, 0, 1}; + + if(f->pass != pass) + { + const size_t e1 = nexti[e]; + + // case 1: the new face is not degenerated, i.e., the new face is not coplanar with the old face f. + if(f->n.dot(w->w) - f->d < -tolerance) + { + SimplexF* nf = newFace(f->c[e1], f->c[e], w, false); + if(nf) + { + // add face-face connectivity + bind(nf, 0, f, e); + + // if there is last face in the horizon, then need to add another connectivity, i.e. the edge connecting the current new add edge and the last new add edge. + // This does not finish all the connectivities because the final face need to connect with the first face, this will be handled in the evaluate function. + // Notice the face is anti-clockwise, so the edges are 0 (bottom), 1 (right), 2 (left) + if(horizon.cf) + bind(nf, 2, horizon.cf, 1); + else + horizon.ff = nf; + + horizon.cf = nf; + ++horizon.nf; + return true; + } + } + else // case 2: the new face is coplanar with the old face f. We need to add two faces and delete the old face + { + const size_t e2 = previ[e]; + f->pass = pass; + if(expand(pass, w, f->f[e1], f->e[e1], horizon) && expand(pass, w, f->f[e2], f->e[e2], horizon)) + { + hull.remove(f); + stock.append(f); + return true; + } + } + } + + return false; +} + +} // details + +} // namespace fcl + +#endif diff --git a/src/narrowphase/gjk_libccd.cpp b/include/fcl/narrowphase/detail/gjk_libccd.h similarity index 68% rename from src/narrowphase/gjk_libccd.cpp rename to include/fcl/narrowphase/detail/gjk_libccd.h index 2c16b6002..27ffc09d9 100644 --- a/src/narrowphase/gjk_libccd.cpp +++ b/include/fcl/narrowphase/detail/gjk_libccd.h @@ -35,11 +35,16 @@ /** \author Jia Pan */ +#ifndef FCL_NARROWPHASE_DETAIL_GJKLIBCCD_H +#define FCL_NARROWPHASE_DETAIL_GJKLIBCCD_H + +#include "fcl/shape/geometric_shapes.h" -#include "fcl/narrowphase/gjk_libccd.h" -#include "fcl/ccd/simplex.h" -#include #include +#include +#include +#include "fcl/narrowphase/detail/gjk_libccd.h" +#include "fcl/ccd/simplex.h" namespace fcl { @@ -47,6 +52,155 @@ namespace fcl namespace details { +/// @brief callback function used by GJK algorithm + +using GJKSupportFunction = void (*)(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v); +using GJKCenterFunction = void (*)(const void* obj, ccd_vec3_t* c); + +/// @brief initialize GJK stuffs +template +class GJKInitializer +{ +public: + /// @brief Get GJK support function + static GJKSupportFunction getSupportFunction() { return nullptr; } + + /// @brief Get GJK center function + static GJKCenterFunction getCenterFunction() { return nullptr; } + + /// @brief Get GJK object from a shape + /// Notice that only local transformation is applied. + /// Gloal transformation are considered later + static void* createGJKObject(const T& /* s */, const Transform3& /*tf*/) { return nullptr; } + + /// @brief Delete GJK object + static void deleteGJKObject(void* o) {} +}; + +/// @brief initialize GJK Cylinder +template +class GJKInitializer> +{ +public: + static GJKSupportFunction getSupportFunction(); + static GJKCenterFunction getCenterFunction(); + static void* createGJKObject(const Cylinder& s, const Transform3& tf); + static void deleteGJKObject(void* o); +}; + +/// @brief initialize GJK Sphere +template +class GJKInitializer> +{ +public: + static GJKSupportFunction getSupportFunction(); + static GJKCenterFunction getCenterFunction(); + static void* createGJKObject(const Sphere& s, const Transform3& tf); + static void deleteGJKObject(void* o); +}; + +/// @brief initialize GJK Ellipsoid +template +class GJKInitializer> +{ +public: + static GJKSupportFunction getSupportFunction(); + static GJKCenterFunction getCenterFunction(); + static void* createGJKObject(const Ellipsoid& s, const Transform3& tf); + static void deleteGJKObject(void* o); +}; + +/// @brief initialize GJK Box +template +class GJKInitializer> +{ +public: + static GJKSupportFunction getSupportFunction(); + static GJKCenterFunction getCenterFunction(); + static void* createGJKObject(const Box& s, const Transform3& tf); + static void deleteGJKObject(void* o); +}; + +/// @brief initialize GJK Capsule +template +class GJKInitializer> +{ +public: + static GJKSupportFunction getSupportFunction(); + static GJKCenterFunction getCenterFunction(); + static void* createGJKObject(const Capsule& s, const Transform3& tf); + static void deleteGJKObject(void* o); +}; + +/// @brief initialize GJK Cone +template +class GJKInitializer> +{ +public: + static GJKSupportFunction getSupportFunction(); + static GJKCenterFunction getCenterFunction(); + static void* createGJKObject(const Cone& s, const Transform3& tf); + static void deleteGJKObject(void* o); +}; + +/// @brief initialize GJK Convex +template +class GJKInitializer> +{ +public: + static GJKSupportFunction getSupportFunction(); + static GJKCenterFunction getCenterFunction(); + static void* createGJKObject(const Convex& s, const Transform3& tf); + static void deleteGJKObject(void* o); +}; + +/// @brief initialize GJK Triangle +GJKSupportFunction triGetSupportFunction(); + +GJKCenterFunction triGetCenterFunction(); + +template +void* triCreateGJKObject(const Vector3& P1, const Vector3& P2, const Vector3& P3); + +template +void* triCreateGJKObject(const Vector3& P1, const Vector3& P2, const Vector3& P3, const Transform3& tf); + +void triDeleteGJKObject(void* o); + +/// @brief GJK collision algorithm +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); + +template +bool GJKDistance(void* obj1, ccd_support_fn supp1, + void* obj2, ccd_support_fn supp2, + unsigned int max_iterations, S tolerance, + S* dist, Vector3* p1, Vector3* p2); + + +} // details + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +namespace details +{ + struct ccd_obj_t { ccd_vec3_t pos; @@ -83,9 +237,10 @@ struct ccd_ellipsoid_t : public ccd_obj_t ccd_real_t radii[3]; }; +template struct ccd_convex_t : public ccd_obj_t { - const Convex* convex; + const Convex* convex; }; struct ccd_triangle_t : public ccd_obj_t @@ -195,7 +350,7 @@ 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, NULL); + dist = ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &B->v, &C->v, nullptr); if (ccdIsZero(dist)){ return 1; } @@ -280,23 +435,23 @@ 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, NULL); + dist = ccdVec3PointTriDist2(&A->v, &B->v, &C->v, &D->v, nullptr); if (ccdIsZero(dist)){ 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, NULL); + 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, NULL); + 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, NULL); + 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, NULL); + dist = ccdVec3PointTriDist2(ccd_vec3_origin, &B->v, &C->v, &D->v, nullptr); if (ccdIsZero(dist)) return 1; @@ -422,7 +577,7 @@ static int __ccdGJK(const void *obj1, const void *obj2, } /// change the libccd distance to add two closest points -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) { unsigned long iterations; ccd_simplex_t simplex; @@ -509,16 +664,18 @@ ccd_real_t ccdGJKDist2(const void *obj1, const void *obj2, const ccd_t *ccd, ccd /** Basic shape to ccd shape */ -static void shapeToGJK(const ShapeBase& s, const Transform3d& tf, ccd_obj_t* o) +template +static void shapeToGJK(const ShapeBase& s, const Transform3& tf, ccd_obj_t* o) { - const Quaternion3d q(tf.linear()); - const Vector3d& T = tf.translation(); + const Quaternion q(tf.linear()); + const Vector3& T = tf.translation(); ccdVec3Set(&o->pos, T[0], T[1], T[2]); ccdQuatSet(&o->rot, q.x(), q.y(), q.z(), q.w()); ccdQuatInvert2(&o->rot_inv, &o->rot); } -static void boxToGJK(const Box& s, const Transform3d& tf, ccd_box_t* box) +template +static void boxToGJK(const Box& s, const Transform3& tf, ccd_box_t* box) { shapeToGJK(s, tf, box); box->dim[0] = s.side[0] / 2.0; @@ -526,34 +683,39 @@ static void boxToGJK(const Box& s, const Transform3d& tf, ccd_box_t* box) box->dim[2] = s.side[2] / 2.0; } -static void capToGJK(const Capsule& s, const Transform3d& tf, ccd_cap_t* cap) +template +static void capToGJK(const Capsule& s, const Transform3& tf, ccd_cap_t* cap) { shapeToGJK(s, tf, cap); cap->radius = s.radius; cap->height = s.lz / 2; } -static void cylToGJK(const Cylinder& s, const Transform3d& tf, ccd_cyl_t* cyl) +template +static void cylToGJK(const Cylinder& s, const Transform3& tf, ccd_cyl_t* cyl) { shapeToGJK(s, tf, cyl); cyl->radius = s.radius; cyl->height = s.lz / 2; } -static void coneToGJK(const Cone& s, const Transform3d& tf, ccd_cone_t* cone) +template +static void coneToGJK(const Cone& s, const Transform3& tf, ccd_cone_t* cone) { shapeToGJK(s, tf, cone); cone->radius = s.radius; cone->height = s.lz / 2; } -static void sphereToGJK(const Sphere& s, const Transform3d& tf, ccd_sphere_t* sph) +template +static void sphereToGJK(const Sphere& s, const Transform3& tf, ccd_sphere_t* sph) { shapeToGJK(s, tf, sph); sph->radius = s.radius; } -static void ellipsoidToGJK(const Ellipsoid& s, const Transform3d& tf, ccd_ellipsoid_t* ellipsoid) +template +static void ellipsoidToGJK(const Ellipsoid& s, const Transform3& tf, ccd_ellipsoid_t* ellipsoid) { shapeToGJK(s, tf, ellipsoid); ellipsoid->radii[0] = s.radii[0]; @@ -561,14 +723,15 @@ static void ellipsoidToGJK(const Ellipsoid& s, const Transform3d& tf, ccd_ellips ellipsoid->radii[2] = s.radii[2]; } -static void convexToGJK(const Convex& s, const Transform3d& tf, ccd_convex_t* conv) +template +static void convexToGJK(const Convex& s, const Transform3& tf, ccd_convex_t* conv) { shapeToGJK(s, tf, conv); conv->convex = &s; } /** Support functions */ -static 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) { const ccd_box_t* o = static_cast(obj); ccd_vec3_t dir; @@ -581,7 +744,7 @@ static void supportBox(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v) ccdVec3Add(v, &o->pos); } -static 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; @@ -608,7 +771,7 @@ static void supportCap(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v) ccdVec3Add(v, &o->pos); } -static 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; @@ -635,7 +798,7 @@ static void supportCyl(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v) ccdVec3Add(v, &cyl->pos); } -static 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; @@ -666,7 +829,7 @@ static void supportCone(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v) ccdVec3Add(v, &cone->pos); } -static 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; @@ -683,7 +846,7 @@ static void supportSphere(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v ccdVec3Add(v, &s->pos); } -static 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; @@ -707,14 +870,15 @@ static void supportEllipsoid(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t ccdVec3Add(v, &s->pos); } +template static void supportConvex(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v) { - const ccd_convex_t* c = (const ccd_convex_t*)obj; + const auto* c = (const ccd_convex_t*)obj; ccd_vec3_t dir, p; ccd_real_t maxdot, dot; int i; - Vector3d* curp; - const Vector3d& center = c->convex->center; + Vector3* curp; + const auto& center = c->convex->center; ccdVec3Copy(&dir, dir_); ccdQuatRotVec(&dir, &c->rot_inv); @@ -766,15 +930,16 @@ static void supportTriangle(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* ccdVec3Add(v, &tri->pos); } -static void centerShape(const void* obj, ccd_vec3_t* c) +static inline void centerShape(const void* obj, ccd_vec3_t* c) { const ccd_obj_t *o = static_cast(obj); ccdVec3Copy(c, &o->pos); } +template static void centerConvex(const void* obj, ccd_vec3_t* c) { - const ccd_convex_t *o = static_cast(obj); + const auto *o = static_cast*>(obj); ccdVec3Set(c, o->convex->center[0], o->convex->center[1], o->convex->center[2]); ccdQuatRotVec(c, &o->rot); ccdVec3Add(c, &o->pos); @@ -788,10 +953,11 @@ static void centerTriangle(const void* obj, ccd_vec3_t* c) ccdVec3Add(c, &o->pos); } +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, FCL_REAL tolerance, - Vector3d* contact_points, FCL_REAL* penetration_depth, Vector3d* normal) + unsigned int max_iterations, S tolerance, + Vector3* contact_points, S* penetration_depth, Vector3* normal) { ccd_t ccd; int res; @@ -829,21 +995,29 @@ bool GJKCollide(void* obj1, ccd_support_fn supp1, ccd_center_fn cen1, /// p1 and p2 are in global coordinate, so needs transform in the narrowphase.h functions +template bool GJKDistance(void* obj1, ccd_support_fn supp1, void* obj2, ccd_support_fn supp2, - unsigned int max_iterations, FCL_REAL tolerance, - FCL_REAL* res, Vector3d* p1, Vector3d* p2) + unsigned int max_iterations, S tolerance, + S* res, Vector3* p1, Vector3* p2) { ccd_t ccd; ccd_real_t dist; CCD_INIT(&ccd); ccd.support1 = supp1; ccd.support2 = supp2; - + ccd.max_iterations = max_iterations; ccd.dist_tolerance = tolerance; ccd_vec3_t p1_, p2_; + // NOTE(JS): p1_ and p2_ are set to zeros in order to suppress uninitialized + // warning. It seems the warnings occur since libccd_extension::ccdGJKDist2 + // conditionally set p1_ and p2_. If this wasn't intentional then please + // remove the initialization of p1_ and p2_, and change the function + // libccd_extension::ccdGJKDist2(...) to always set p1_ and p2_. + ccdVec3Set(&p1_, 0.0, 0.0, 0.0); + ccdVec3Set(&p2_, 0.0, 0.0, 0.0); dist = libccd_extension::ccdGJKDist2(obj1, obj2, &ccd, &p1_, &p2_); if(p1) *p1 << ccdVec3X(&p1_), ccdVec3Y(&p1_), ccdVec3Z(&p1_); if(p2) *p2 << ccdVec3X(&p2_), ccdVec3Y(&p2_), ccdVec3Z(&p2_); @@ -852,206 +1026,210 @@ bool GJKDistance(void* obj1, ccd_support_fn supp1, else return true; } - -GJKSupportFunction GJKInitializer::getSupportFunction() +template +GJKSupportFunction GJKInitializer>::getSupportFunction() { return &supportCyl; } - -GJKCenterFunction GJKInitializer::getCenterFunction() +template +GJKCenterFunction GJKInitializer>::getCenterFunction() { return ¢erShape; } - -void* GJKInitializer::createGJKObject(const Cylinder& s, const Transform3d& tf) +template +void* GJKInitializer>::createGJKObject(const Cylinder& s, const Transform3& tf) { ccd_cyl_t* o = new ccd_cyl_t; cylToGJK(s, tf, o); return o; } - -void GJKInitializer::deleteGJKObject(void* o_) +template +void GJKInitializer>::deleteGJKObject(void* o_) { ccd_cyl_t* o = static_cast(o_); delete o; } - -GJKSupportFunction GJKInitializer::getSupportFunction() +template +GJKSupportFunction GJKInitializer>::getSupportFunction() { return &supportSphere; } - -GJKCenterFunction GJKInitializer::getCenterFunction() +template +GJKCenterFunction GJKInitializer>::getCenterFunction() { return ¢erShape; } - -void* GJKInitializer::createGJKObject(const Sphere& s, const Transform3d& tf) +template +void* GJKInitializer>::createGJKObject(const Sphere& s, const Transform3& tf) { ccd_sphere_t* o = new ccd_sphere_t; sphereToGJK(s, tf, o); return o; } -void GJKInitializer::deleteGJKObject(void* o_) +template +void GJKInitializer>::deleteGJKObject(void* o_) { ccd_sphere_t* o = static_cast(o_); delete o; } -GJKSupportFunction GJKInitializer::getSupportFunction() +template +GJKSupportFunction GJKInitializer>::getSupportFunction() { return &supportEllipsoid; } -GJKCenterFunction GJKInitializer::getCenterFunction() +template +GJKCenterFunction GJKInitializer>::getCenterFunction() { return ¢erShape; } -void* GJKInitializer::createGJKObject(const Ellipsoid& s, const Transform3d& tf) +template +void* GJKInitializer>::createGJKObject(const Ellipsoid& s, const Transform3& tf) { ccd_ellipsoid_t* o = new ccd_ellipsoid_t; ellipsoidToGJK(s, tf, o); return o; } -void GJKInitializer::deleteGJKObject(void* o_) +template +void GJKInitializer>::deleteGJKObject(void* o_) { ccd_ellipsoid_t* o = static_cast(o_); delete o; } -GJKSupportFunction GJKInitializer::getSupportFunction() +template +GJKSupportFunction GJKInitializer>::getSupportFunction() { return &supportBox; } - -GJKCenterFunction GJKInitializer::getCenterFunction() +template +GJKCenterFunction GJKInitializer>::getCenterFunction() { return ¢erShape; } - -void* GJKInitializer::createGJKObject(const Box& s, const Transform3d& tf) +template +void* GJKInitializer>::createGJKObject(const Box& s, const Transform3& tf) { ccd_box_t* o = new ccd_box_t; boxToGJK(s, tf, o); return o; } - -void GJKInitializer::deleteGJKObject(void* o_) +template +void GJKInitializer>::deleteGJKObject(void* o_) { ccd_box_t* o = static_cast(o_); delete o; } - -GJKSupportFunction GJKInitializer::getSupportFunction() +template +GJKSupportFunction GJKInitializer>::getSupportFunction() { return &supportCap; } - -GJKCenterFunction GJKInitializer::getCenterFunction() +template +GJKCenterFunction GJKInitializer>::getCenterFunction() { return ¢erShape; } - -void* GJKInitializer::createGJKObject(const Capsule& s, const Transform3d& tf) +template +void* GJKInitializer>::createGJKObject(const Capsule& s, const Transform3& tf) { ccd_cap_t* o = new ccd_cap_t; capToGJK(s, tf, o); return o; } - -void GJKInitializer::deleteGJKObject(void* o_) +template +void GJKInitializer>::deleteGJKObject(void* o_) { ccd_cap_t* o = static_cast(o_); delete o; } - -GJKSupportFunction GJKInitializer::getSupportFunction() +template +GJKSupportFunction GJKInitializer>::getSupportFunction() { return &supportCone; } - -GJKCenterFunction GJKInitializer::getCenterFunction() +template +GJKCenterFunction GJKInitializer>::getCenterFunction() { return ¢erShape; } - -void* GJKInitializer::createGJKObject(const Cone& s, const Transform3d& tf) +template +void* GJKInitializer>::createGJKObject(const Cone& s, const Transform3& tf) { ccd_cone_t* o = new ccd_cone_t; coneToGJK(s, tf, o); return o; } - -void GJKInitializer::deleteGJKObject(void* o_) +template +void GJKInitializer>::deleteGJKObject(void* o_) { ccd_cone_t* o = static_cast(o_); delete o; } - -GJKSupportFunction GJKInitializer::getSupportFunction() +template +GJKSupportFunction GJKInitializer>::getSupportFunction() { - return &supportConvex; + return &supportConvex; } - -GJKCenterFunction GJKInitializer::getCenterFunction() +template +GJKCenterFunction GJKInitializer>::getCenterFunction() { - return ¢erConvex; + return ¢erConvex; } - -void* GJKInitializer::createGJKObject(const Convex& s, const Transform3d& tf) +template +void* GJKInitializer>::createGJKObject(const Convex& s, const Transform3& tf) { - ccd_convex_t* o = new ccd_convex_t; + auto* o = new ccd_convex_t; convexToGJK(s, tf, o); return o; } - -void GJKInitializer::deleteGJKObject(void* o_) +template +void GJKInitializer>::deleteGJKObject(void* o_) { - ccd_convex_t* o = static_cast(o_); + auto* o = static_cast*>(o_); delete o; } - -GJKSupportFunction triGetSupportFunction() +inline GJKSupportFunction triGetSupportFunction() { return &supportTriangle; } - -GJKCenterFunction triGetCenterFunction() +inline GJKCenterFunction triGetCenterFunction() { return ¢erTriangle; } - -void* triCreateGJKObject(const Vector3d& P1, const Vector3d& P2, const Vector3d& P3) +template +void* triCreateGJKObject(const Vector3& P1, const Vector3& P2, const Vector3& P3) { ccd_triangle_t* o = new ccd_triangle_t; - Vector3d 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]); @@ -1064,17 +1242,18 @@ void* triCreateGJKObject(const Vector3d& P1, const Vector3d& P2, const Vector3d& return o; } -void* triCreateGJKObject(const Vector3d& P1, const Vector3d& P2, const Vector3d& P3, const Transform3d& tf) +template +void* triCreateGJKObject(const Vector3& P1, const Vector3& P2, const Vector3& P3, const Transform3& tf) { ccd_triangle_t* o = new ccd_triangle_t; - Vector3d 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]); ccdVec3Set(&o->p[2], P3[0], P3[1], P3[2]); ccdVec3Set(&o->c, center[0], center[1], center[2]); - const Quaternion3d q(tf.linear()); - const Vector3d& T = tf.translation(); + const Quaternion q(tf.linear()); + const Vector3& T = tf.translation(); ccdVec3Set(&o->pos, T[0], T[1], T[2]); ccdQuatSet(&o->rot, q.x(), q.y(), q.z(), q.w()); ccdQuatInvert2(&o->rot_inv, &o->rot); @@ -1082,7 +1261,7 @@ void* triCreateGJKObject(const Vector3d& P1, const Vector3d& P2, const Vector3d& return o; } -void triDeleteGJKObject(void* o_) +inline void triDeleteGJKObject(void* o_) { ccd_triangle_t* o = static_cast(o_); delete o; @@ -1090,4 +1269,6 @@ void triDeleteGJKObject(void* o_) } // details -} // fcl +} // namespace fcl + +#endif diff --git a/include/fcl/narrowphase/detail/halfspace.h b/include/fcl/narrowphase/detail/halfspace.h new file mode 100755 index 000000000..35807fafa --- /dev/null +++ b/include/fcl/narrowphase/detail/halfspace.h @@ -0,0 +1,701 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_NARROWPHASE_DETAIL_HALFSPACE_H +#define FCL_NARROWPHASE_DETAIL_HALFSPACE_H + +#include "fcl/collision_data.h" + +namespace fcl +{ + +namespace details +{ + +template +S halfspaceIntersectTolerance(); + +template <> +float halfspaceIntersectTolerance(); + +template <> +double halfspaceIntersectTolerance(); + +template +bool sphereHalfspaceIntersect(const Sphere& s1, const Transform3& tf1, + const Halfspace& s2, const Transform3& tf2, + std::vector>* contacts); + +template +bool ellipsoidHalfspaceIntersect(const Ellipsoid& s1, const Transform3& tf1, + const Halfspace& s2, const Transform3& tf2, + std::vector>* contacts); + +/// @brief box half space, a, b, c = +/- edge size +/// n^T * (R(o + a v1 + b v2 + c v3) + T) <= d +/// so (R^T n) (a v1 + b v2 + c v3) + n * T <= d +/// check whether d - n * T - (R^T n) (a v1 + b v2 + c v3) >= 0 for some a, b, c +/// the max value of left side is d - n * T + |(R^T n) (a v1 + b v2 + c v3)|, check that is enough +template +bool boxHalfspaceIntersect(const Box& s1, const Transform3& tf1, + const Halfspace& s2, const Transform3& tf2); + +template +bool boxHalfspaceIntersect(const Box& s1, const Transform3& tf1, + const Halfspace& s2, const Transform3& tf2, + std::vector>* contacts); + +template +bool capsuleHalfspaceIntersect(const Capsule& s1, const Transform3& tf1, + const Halfspace& s2, const Transform3& tf2, + std::vector>* contacts); + +template +bool cylinderHalfspaceIntersect(const Cylinder& s1, const Transform3& tf1, + const Halfspace& s2, const Transform3& tf2, + std::vector>* contacts); + +template +bool coneHalfspaceIntersect(const Cone& s1, const Transform3& tf1, + const Halfspace& s2, const Transform3& tf2, + std::vector>* contacts); + +template +bool convexHalfspaceIntersect(const Convex& s1, const Transform3& tf1, + const Halfspace& s2, const Transform3& tf2, + Vector3* contact_points, S* penetration_depth, Vector3* normal); + +template +bool halfspaceTriangleIntersect(const Halfspace& s1, const Transform3& tf1, + const Vector3& P1, const Vector3& P2, const Vector3& P3, const Transform3& tf2, + Vector3* contact_points, S* penetration_depth, Vector3* normal); + +/// @brief return whether plane collides with halfspace +/// if the separation plane of the halfspace is parallel with the plane +/// return code 1, if the plane's normal is the same with halfspace's normal and plane is inside halfspace, also return plane in pl +/// return code 2, if the plane's normal is oppositie to the halfspace's normal and plane is inside halfspace, also return plane in pl +/// plane is outside halfspace, collision-free +/// if not parallel +/// return the intersection ray, return code 3. ray origin is p and direction is d +template +bool planeHalfspaceIntersect(const Plane& s1, const Transform3& tf1, + const Halfspace& s2, const Transform3& tf2, + Plane& pl, + Vector3& p, Vector3& d, + S& penetration_depth, + int& ret); + +/// @brief return whether two halfspace intersect +/// if the separation planes of the two halfspaces are parallel +/// return code 1, if two halfspaces' normal are same and s1 is in s2, also return s1 in s; +/// return code 2, if two halfspaces' normal are same and s2 is in s1, also return s2 in s; +/// return code 3, if two halfspaces' normal are opposite and s1 and s2 are into each other; +/// collision free, if two halfspaces' are separate; +/// if the separation planes of the two halfspaces are not parallel, return intersection ray, return code 4. ray origin is p and direction is d +/// collision free return code 0 +template +bool halfspaceIntersect(const Halfspace& s1, const Transform3& tf1, + const Halfspace& s2, const Transform3& tf2, + Vector3& p, Vector3& d, + Halfspace& s, + S& penetration_depth, + int& ret); + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +S halfspaceIntersectTolerance() +{ + return 0; +} + +//============================================================================== +template <> +inline float halfspaceIntersectTolerance() +{ + return 0.0001f; +} + +//============================================================================== +template <> +inline double halfspaceIntersectTolerance() +{ + return 0.0000001; +} + +//============================================================================== +template +bool sphereHalfspaceIntersect(const Sphere& s1, const Transform3& tf1, + const Halfspace& s2, const Transform3& tf2, + std::vector>* contacts) +{ + const Halfspace new_s2 = transform(s2, tf2); + const Vector3& center = tf1.translation(); + const S depth = s1.radius - new_s2.signedDistance(center); + + if (depth >= 0) + { + if (contacts) + { + const Vector3 normal = -new_s2.n; // pointing from s1 to s2 + const Vector3 point = center - new_s2.n * s1.radius + new_s2.n * (depth * 0.5); + const S penetration_depth = depth; + + contacts->emplace_back(normal, point, penetration_depth); + } + + return true; + } + else + { + return false; + } +} + +//============================================================================== +template +bool ellipsoidHalfspaceIntersect(const Ellipsoid& s1, const Transform3& tf1, + const Halfspace& s2, const Transform3& tf2, + std::vector>* contacts) +{ + // We first compute a single contact in the ellipsoid coordinates, tf1, then + // will transform it to the world frame. So we use a new halfspace that is + // expressed in the ellipsoid coordinates. + const Halfspace& new_s2 = transform(s2, tf1.inverse(Eigen::Isometry) * tf2); + + // Compute distance between the ellipsoid's center and a contact plane, whose + // normal is equal to the halfspace's normal. + const Vector3 normal2(std::pow(new_s2.n[0], 2), std::pow(new_s2.n[1], 2), std::pow(new_s2.n[2], 2)); + const Vector3 radii2(std::pow(s1.radii[0], 2), std::pow(s1.radii[1], 2), std::pow(s1.radii[2], 2)); + const S center_to_contact_plane = std::sqrt(normal2.dot(radii2)); + + // Depth is the distance between the contact plane and the halfspace. + const S depth = center_to_contact_plane + new_s2.d; + + if (depth >= 0) + { + if (contacts) + { + // Transform the results to the world coordinates. + const Vector3 normal = tf1.linear() * -new_s2.n; // pointing from s1 to s2 + const Vector3 support_vector = (1.0/center_to_contact_plane) * Vector3(radii2[0]*new_s2.n[0], radii2[1]*new_s2.n[1], radii2[2]*new_s2.n[2]); + const Vector3 point_in_halfspace_coords = support_vector * (0.5 * depth / new_s2.n.dot(support_vector) - 1.0); + const Vector3 point = tf1 * point_in_halfspace_coords; // roughly speaking, a middle point of the intersecting volume + const S penetration_depth = depth; + + contacts->emplace_back(normal, point, penetration_depth); + } + + return true; + } + else + { + return false; + } +} + +//============================================================================== +template +bool boxHalfspaceIntersect(const Box& s1, const Transform3& tf1, + const Halfspace& s2, const Transform3& tf2) +{ + Halfspace new_s2 = transform(s2, tf2); + + const Matrix3& R = tf1.linear(); + const Vector3& T = tf1.translation(); + + Vector3 Q = R.transpose() * new_s2.n; + Vector3 A(Q[0] * s1.side[0], Q[1] * s1.side[1], Q[2] * s1.side[2]); + Vector3 B = A.cwiseAbs(); + + S depth = 0.5 * (B[0] + B[1] + B[2]) - new_s2.signedDistance(T); + return (depth >= 0); +} + +//============================================================================== +template +bool boxHalfspaceIntersect(const Box& s1, const Transform3& tf1, + const Halfspace& s2, const Transform3& tf2, + std::vector>* contacts) +{ + if(!contacts) + { + return boxHalfspaceIntersect(s1, tf1, s2, tf2); + } + else + { + const Halfspace new_s2 = transform(s2, tf2); + + const Matrix3& R = tf1.linear(); + const Vector3& T = tf1.translation(); + + Vector3 Q = R.transpose() * new_s2.n; + Vector3 A(Q[0] * s1.side[0], Q[1] * s1.side[1], Q[2] * s1.side[2]); + Vector3 B = A.cwiseAbs(); + + S depth = 0.5 * (B[0] + B[1] + B[2]) - new_s2.signedDistance(T); + if(depth < 0) return false; + + Vector3 axis[3]; + axis[0] = R.col(0); + axis[1] = R.col(1); + axis[2] = R.col(2); + + /// find deepest point + Vector3 p(T); + int sign = 0; + + if(std::abs(Q[0] - 1) < halfspaceIntersectTolerance() || std::abs(Q[0] + 1) < halfspaceIntersectTolerance()) + { + sign = (A[0] > 0) ? -1 : 1; + p += axis[0] * (0.5 * s1.side[0] * sign); + } + else if(std::abs(Q[1] - 1) < halfspaceIntersectTolerance() || std::abs(Q[1] + 1) < halfspaceIntersectTolerance()) + { + sign = (A[1] > 0) ? -1 : 1; + p += axis[1] * (0.5 * s1.side[1] * sign); + } + else if(std::abs(Q[2] - 1) < halfspaceIntersectTolerance() || std::abs(Q[2] + 1) < halfspaceIntersectTolerance()) + { + sign = (A[2] > 0) ? -1 : 1; + p += axis[2] * (0.5 * s1.side[2] * sign); + } + else + { + for(std::size_t i = 0; i < 3; ++i) + { + sign = (A[i] > 0) ? -1 : 1; + p += axis[i] * (0.5 * s1.side[i] * sign); + } + } + + /// compute the contact point from the deepest point + if (contacts) + { + const Vector3 normal = -new_s2.n; + const Vector3 point = p + new_s2.n * (depth * 0.5); + const S penetration_depth = depth; + + contacts->emplace_back(normal, point, penetration_depth); + } + + return true; + } +} + +//============================================================================== +template +bool capsuleHalfspaceIntersect(const Capsule& s1, const Transform3& tf1, + const Halfspace& s2, const Transform3& tf2, + std::vector>* contacts) +{ + Halfspace new_s2 = transform(s2, tf2); + + const Matrix3& R = tf1.linear(); + const Vector3& T = tf1.translation(); + + Vector3 dir_z = R.col(2); + + S cosa = dir_z.dot(new_s2.n); + if(std::abs(cosa) < halfspaceIntersectTolerance()) + { + S signed_dist = new_s2.signedDistance(T); + S depth = s1.radius - signed_dist; + if(depth < 0) return false; + + if (contacts) + { + const Vector3 normal = -new_s2.n; + const Vector3 point = T + new_s2.n * (0.5 * depth - s1.radius); + const S penetration_depth = depth; + + contacts->emplace_back(normal, point, penetration_depth); + } + + return true; + } + else + { + int sign = (cosa > 0) ? -1 : 1; + Vector3 p = T + dir_z * (s1.lz * 0.5 * sign); + + S signed_dist = new_s2.signedDistance(p); + S depth = s1.radius - signed_dist; + if(depth < 0) return false; + + if (contacts) + { + const Vector3 normal = -new_s2.n; + const Vector3 point = p - new_s2.n * s1.radius + new_s2.n * (0.5 * depth); // deepest point + const S penetration_depth = depth; + + contacts->emplace_back(normal, point, penetration_depth); + } + + return true; + } +} + +//============================================================================== +template +bool cylinderHalfspaceIntersect(const Cylinder& s1, const Transform3& tf1, + const Halfspace& s2, const Transform3& tf2, + std::vector>* contacts) +{ + Halfspace new_s2 = transform(s2, tf2); + + const Matrix3& R = tf1.linear(); + const Vector3& T = tf1.translation(); + + Vector3 dir_z = R.col(2); + S cosa = dir_z.dot(new_s2.n); + + if(cosa < halfspaceIntersectTolerance()) + { + S signed_dist = new_s2.signedDistance(T); + S depth = s1.radius - signed_dist; + if(depth < 0) return false; + + if (contacts) + { + const Vector3 normal = -new_s2.n; + const Vector3 point = T + new_s2.n * (0.5 * depth - s1.radius); + const S penetration_depth = depth; + + contacts->emplace_back(normal, point, penetration_depth); + } + + return true; + } + else + { + Vector3 C = dir_z * cosa - new_s2.n; + if(std::abs(cosa + 1) < halfspaceIntersectTolerance() || std::abs(cosa - 1) < halfspaceIntersectTolerance()) + C = Vector3(0, 0, 0); + else + { + S s = C.norm(); + s = s1.radius / s; + C *= s; + } + + int sign = (cosa > 0) ? -1 : 1; + // deepest point + Vector3 p = T + dir_z * (s1.lz * 0.5 * sign) + C; + S depth = -new_s2.signedDistance(p); + if(depth < 0) return false; + else + { + if (contacts) + { + const Vector3 normal = -new_s2.n; + const Vector3 point = p + new_s2.n * (0.5 * depth); + const S penetration_depth = depth; + + contacts->emplace_back(normal, point, penetration_depth); + } + + return true; + } + } +} + +//============================================================================== +template +bool coneHalfspaceIntersect(const Cone& s1, const Transform3& tf1, + const Halfspace& s2, const Transform3& tf2, + std::vector>* contacts) +{ + Halfspace new_s2 = transform(s2, tf2); + + const Matrix3& R = tf1.linear(); + const Vector3& T = tf1.translation(); + + Vector3 dir_z = R.col(2); + S cosa = dir_z.dot(new_s2.n); + + if(cosa < halfspaceIntersectTolerance()) + { + S signed_dist = new_s2.signedDistance(T); + S depth = s1.radius - signed_dist; + if(depth < 0) return false; + else + { + if (contacts) + { + const Vector3 normal = -new_s2.n; + const Vector3 point = T - dir_z * (s1.lz * 0.5) + new_s2.n * (0.5 * depth - s1.radius); + const S penetration_depth = depth; + + contacts->emplace_back(normal, point, penetration_depth); + } + + return true; + } + } + else + { + Vector3 C = dir_z * cosa - new_s2.n; + if(std::abs(cosa + 1) < halfspaceIntersectTolerance() || std::abs(cosa - 1) < halfspaceIntersectTolerance()) + C = Vector3(0, 0, 0); + else + { + S s = C.norm(); + s = s1.radius / s; + C *= s; + } + + Vector3 p1 = T + dir_z * (0.5 * s1.lz); + Vector3 p2 = T - dir_z * (0.5 * s1.lz) + C; + + S d1 = new_s2.signedDistance(p1); + S d2 = new_s2.signedDistance(p2); + + if(d1 > 0 && d2 > 0) return false; + else + { + if (contacts) + { + const S penetration_depth = -std::min(d1, d2); + const Vector3 normal = -new_s2.n; + const Vector3 point = ((d1 < d2) ? p1 : p2) + new_s2.n * (0.5 * penetration_depth); + + contacts->emplace_back(normal, point, penetration_depth); + } + + return true; + } + } +} + +//============================================================================== +template +bool convexHalfspaceIntersect(const Convex& s1, const Transform3& tf1, + const Halfspace& s2, const Transform3& tf2, + Vector3* contact_points, S* penetration_depth, Vector3* normal) +{ + Halfspace new_s2 = transform(s2, tf2); + + Vector3 v; + S depth = std::numeric_limits::max(); + + for(int i = 0; i < s1.num_points; ++i) + { + Vector3 p = tf1 * s1.points[i]; + + S d = new_s2.signedDistance(p); + if(d < depth) + { + depth = d; + v = p; + } + } + + if(depth <= 0) + { + if(contact_points) *contact_points = v - new_s2.n * (0.5 * depth); + if(penetration_depth) *penetration_depth = depth; + if(normal) *normal = -new_s2.n; + return true; + } + else + return false; +} + +//============================================================================== +template +bool halfspaceTriangleIntersect(const Halfspace& s1, const Transform3& tf1, + const Vector3& P1, const Vector3& P2, const Vector3& P3, const Transform3& tf2, + Vector3* contact_points, S* penetration_depth, Vector3* normal) +{ + Halfspace new_s1 = transform(s1, tf1); + + Vector3 v = tf2 * P1; + S depth = new_s1.signedDistance(v); + + Vector3 p = tf2 * P2; + S d = new_s1.signedDistance(p); + if(d < depth) + { + depth = d; + v = p; + } + + p = tf2 * P3; + d = new_s1.signedDistance(p); + if(d < depth) + { + depth = d; + v = p; + } + + if(depth <= 0) + { + if(penetration_depth) *penetration_depth = -depth; + if(normal) *normal = new_s1.n; + if(contact_points) *contact_points = v - new_s1.n * (0.5 * depth); + return true; + } + else + return false; +} + +//============================================================================== +template +bool planeHalfspaceIntersect(const Plane& s1, const Transform3& tf1, + const Halfspace& s2, const Transform3& tf2, + Plane& pl, + Vector3& p, Vector3& d, + S& penetration_depth, + int& ret) +{ + Plane new_s1 = transform(s1, tf1); + Halfspace new_s2 = transform(s2, tf2); + + ret = 0; + + Vector3 dir = (new_s1.n).cross(new_s2.n); + S dir_norm = dir.squaredNorm(); + if(dir_norm < std::numeric_limits::epsilon()) // parallel + { + if((new_s1.n).dot(new_s2.n) > 0) + { + if(new_s1.d < new_s2.d) + { + penetration_depth = new_s2.d - new_s1.d; + ret = 1; + pl = new_s1; + return true; + } + else + return false; + } + else + { + if(new_s1.d + new_s2.d > 0) + return false; + else + { + penetration_depth = -(new_s1.d + new_s2.d); + ret = 2; + pl = new_s1; + return true; + } + } + } + + Vector3 n = new_s2.n * new_s1.d - new_s1.n * new_s2.d; + Vector3 origin = n.cross(dir); + origin *= (1.0 / dir_norm); + + p = origin; + d = dir; + ret = 3; + penetration_depth = std::numeric_limits::max(); + + return true; +} + +//============================================================================== +template +bool halfspaceIntersect(const Halfspace& s1, const Transform3& tf1, + const Halfspace& s2, const Transform3& tf2, + Vector3& p, Vector3& d, + Halfspace& s, + S& penetration_depth, + int& ret) +{ + Halfspace new_s1 = transform(s1, tf1); + Halfspace new_s2 = transform(s2, tf2); + + ret = 0; + + Vector3 dir = (new_s1.n).cross(new_s2.n); + S dir_norm = dir.squaredNorm(); + if(dir_norm < std::numeric_limits::epsilon()) // parallel + { + if((new_s1.n).dot(new_s2.n) > 0) + { + if(new_s1.d < new_s2.d) // s1 is inside s2 + { + ret = 1; + penetration_depth = std::numeric_limits::max(); + s = new_s1; + } + else // s2 is inside s1 + { + ret = 2; + penetration_depth = std::numeric_limits::max(); + s = new_s2; + } + return true; + } + else + { + if(new_s1.d + new_s2.d > 0) // not collision + return false; + else // in each other + { + ret = 3; + penetration_depth = -(new_s1.d + new_s2.d); + return true; + } + } + } + + Vector3 n = new_s2.n * new_s1.d - new_s1.n * new_s2.d; + Vector3 origin = n.cross(dir); + origin *= (1.0 / dir_norm); + + p = origin; + d = dir; + ret = 4; + penetration_depth = std::numeric_limits::max(); + + return true; +} + +} // namespace details + +} // namespace fcl + +#endif diff --git a/include/fcl/narrowphase/detail/plane.h b/include/fcl/narrowphase/detail/plane.h new file mode 100755 index 000000000..7de0bc36f --- /dev/null +++ b/include/fcl/narrowphase/detail/plane.h @@ -0,0 +1,830 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_NARROWPHASE_DETAIL_PLANE_H +#define FCL_NARROWPHASE_DETAIL_PLANE_H + +#include "fcl/collision_data.h" + +namespace fcl +{ + +namespace details +{ + +template +S planeIntersectTolerance(); + +template <> +double planeIntersectTolerance(); + +template <> +float planeIntersectTolerance(); + +template +bool spherePlaneIntersect(const Sphere& s1, const Transform3& tf1, + const Plane& s2, const Transform3& tf2, + std::vector>* contacts); + +template +bool ellipsoidPlaneIntersect(const Ellipsoid& s1, const Transform3& tf1, + const Plane& s2, const Transform3& tf2, + std::vector>* contacts); + +/// @brief box half space, a, b, c = +/- edge size +/// n^T * (R(o + a v1 + b v2 + c v3) + T) ~ d +/// so (R^T n) (a v1 + b v2 + c v3) + n * T ~ d +/// check whether d - n * T - (R^T n) (a v1 + b v2 + c v3) >= 0 for some a, b, c and <=0 for some a, b, c +/// so need to check whether |d - n * T| <= |(R^T n)(a v1 + b v2 + c v3)|, the reason is as follows: +/// (R^T n) (a v1 + b v2 + c v3) can get |(R^T n) (a v1 + b v2 + c v3)| for one a, b, c. +/// if |d - n * T| <= |(R^T n)(a v1 + b v2 + c v3)| then can get both positive and negative value on the right side. +template +bool boxPlaneIntersect(const Box& s1, const Transform3& tf1, + const Plane& s2, const Transform3& tf2, + std::vector>* contacts); + +template +bool capsulePlaneIntersect(const Capsule& s1, const Transform3& tf1, + const Plane& s2, const Transform3& tf2); + +template +bool capsulePlaneIntersect(const Capsule& s1, const Transform3& tf1, + const Plane& s2, const Transform3& tf2, + std::vector>* contacts); + +/// @brief cylinder-plane intersect +/// n^T (R (r * cosa * v1 + r * sina * v2 + h * v3) + T) ~ d +/// need one point to be positive and one to be negative +/// (n^T * v3) * h + n * T -d + r * (cosa * (n^T * R * v1) + sina * (n^T * R * v2)) ~ 0 +/// (n^T * v3) * h + r * (cosa * (n^T * R * v1) + sina * (n^T * R * v2)) + n * T - d ~ 0 +template +bool cylinderPlaneIntersect(const Cylinder& s1, const Transform3& tf1, + const Plane& s2, const Transform3& tf2); + +template +bool cylinderPlaneIntersect(const Cylinder& s1, const Transform3& tf1, + const Plane& s2, const Transform3& tf2, + std::vector>* contacts); + +template +bool conePlaneIntersect(const Cone& s1, const Transform3& tf1, + const Plane& s2, const Transform3& tf2, + std::vector>* contacts); + +template +bool convexPlaneIntersect(const Convex& s1, const Transform3& tf1, + const Plane& s2, const Transform3& tf2, + Vector3* contact_points, S* penetration_depth, Vector3* normal); + +template +bool planeTriangleIntersect(const Plane& s1, const Transform3& tf1, + const Vector3& P1, const Vector3& P2, const Vector3& P3, const Transform3& tf2, + Vector3* contact_points, S* penetration_depth, Vector3* normal); + +template +bool halfspacePlaneIntersect(const Halfspace& s1, const Transform3& tf1, + const Plane& s2, const Transform3& tf2, + Plane& pl, Vector3& p, Vector3& d, + S& penetration_depth, + int& ret); + +template +bool planeIntersect(const Plane& s1, const Transform3& tf1, + const Plane& s2, const Transform3& tf2, + std::vector>* contacts); + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +S planeIntersectTolerance() +{ + return 0; +} + +//============================================================================== +template <> +inline double planeIntersectTolerance() +{ + return 0.0000001; +} + +//============================================================================== +template <> +inline float planeIntersectTolerance() +{ + return 0.0001; +} + +//============================================================================== +template +bool spherePlaneIntersect(const Sphere& s1, const Transform3& tf1, + const Plane& s2, const Transform3& tf2, + std::vector>* contacts) +{ + const Plane new_s2 = transform(s2, tf2); + + const Vector3& center = tf1.translation(); + const S signed_dist = new_s2.signedDistance(center); + const S depth = - std::abs(signed_dist) + s1.radius; + + if(depth >= 0) + { + if (contacts) + { + const Vector3 normal = (signed_dist > 0) ? (-new_s2.n).eval() : new_s2.n; + const Vector3 point = center - new_s2.n * signed_dist; + const S penetration_depth = depth; + + contacts->emplace_back(normal, point, penetration_depth); + } + + return true; + } + else + { + return false; + } +} + +//============================================================================== +template +bool ellipsoidPlaneIntersect(const Ellipsoid& s1, const Transform3& tf1, + const Plane& s2, const Transform3& tf2, + std::vector>* contacts) +{ + // We first compute a single contact in the ellipsoid coordinates, tf1, then + // will transform it to the world frame. So we use a new plane that is + // expressed in the ellipsoid coordinates. + const Plane& new_s2 = transform(s2, tf1.inverse(Eigen::Isometry) * tf2); + + // Compute distance between the ellipsoid's center and a contact plane, whose + // normal is equal to the plane's normal. + const Vector3 normal2(std::pow(new_s2.n[0], 2), std::pow(new_s2.n[1], 2), std::pow(new_s2.n[2], 2)); + const Vector3 radii2(std::pow(s1.radii[0], 2), std::pow(s1.radii[1], 2), std::pow(s1.radii[2], 2)); + const S center_to_contact_plane = std::sqrt(normal2.dot(radii2)); + + const S signed_dist = -new_s2.d; + + // Depth is the distance between the contact plane and the given plane. + const S depth = center_to_contact_plane - std::abs(signed_dist); + + if (depth >= 0) + { + if (contacts) + { + // Transform the results to the world coordinates. + const Vector3 normal = (signed_dist > 0) ? (tf1.linear() * -new_s2.n).eval() : (tf1.linear() * new_s2.n).eval(); // pointing from the ellipsoid's center to the plane + const Vector3 support_vector = (1.0/center_to_contact_plane) * Vector3(radii2[0]*new_s2.n[0], radii2[1]*new_s2.n[1], radii2[2]*new_s2.n[2]); + const Vector3 point_in_plane_coords = support_vector * (depth / new_s2.n.dot(support_vector) - 1.0); + const Vector3 point = (signed_dist > 0) ? tf1 * point_in_plane_coords : tf1 * -point_in_plane_coords; // a middle point of the intersecting volume + const S penetration_depth = depth; + + contacts->emplace_back(normal, point, penetration_depth); + } + + return true; + } + else + { + return false; + } +} + +//============================================================================== +template +bool boxPlaneIntersect(const Box& s1, const Transform3& tf1, + const Plane& s2, const Transform3& tf2, + std::vector>* contacts) +{ + Plane new_s2 = transform(s2, tf2); + + const Matrix3& R = tf1.linear(); + const Vector3& T = tf1.translation(); + + Vector3 Q = R.transpose() * new_s2.n; + Vector3 A(Q[0] * s1.side[0], Q[1] * s1.side[1], Q[2] * s1.side[2]); + Vector3 B = A.cwiseAbs(); + + S signed_dist = new_s2.signedDistance(T); + S depth = 0.5 * (B[0] + B[1] + B[2]) - std::abs(signed_dist); + if(depth < 0) return false; + + Vector3 axis[3]; + axis[0] = R.col(0); + axis[1] = R.col(1); + axis[2] = R.col(2); + + // find the deepest point + Vector3 p = T; + + // when center is on the positive side of the plane, use a, b, c make (R^T n) (a v1 + b v2 + c v3) the minimum + // otherwise, use a, b, c make (R^T n) (a v1 + b v2 + c v3) the maximum + int sign = (signed_dist > 0) ? 1 : -1; + + if(std::abs(Q[0] - 1) < planeIntersectTolerance() || std::abs(Q[0] + 1) < planeIntersectTolerance()) + { + int sign2 = (A[0] > 0) ? -1 : 1; + sign2 *= sign; + p += axis[0] * (0.5 * s1.side[0] * sign2); + } + else if(std::abs(Q[1] - 1) < planeIntersectTolerance() || std::abs(Q[1] + 1) < planeIntersectTolerance()) + { + int sign2 = (A[1] > 0) ? -1 : 1; + sign2 *= sign; + p += axis[1] * (0.5 * s1.side[1] * sign2); + } + else if(std::abs(Q[2] - 1) < planeIntersectTolerance() || std::abs(Q[2] + 1) < planeIntersectTolerance()) + { + int sign2 = (A[2] > 0) ? -1 : 1; + sign2 *= sign; + p += axis[2] * (0.5 * s1.side[2] * sign2); + } + else + { + for(std::size_t i = 0; i < 3; ++i) + { + int sign2 = (A[i] > 0) ? -1 : 1; + sign2 *= sign; + p += axis[i] * (0.5 * s1.side[i] * sign2); + } + } + + // compute the contact point by project the deepest point onto the plane + if (contacts) + { + const Vector3 normal = (signed_dist > 0) ? (-new_s2.n).eval() : new_s2.n; + const Vector3 point = p - new_s2.n * new_s2.signedDistance(p); + const S penetration_depth = depth; + + contacts->emplace_back(normal, point, penetration_depth); + } + + return true; +} + +//============================================================================== +template +bool capsulePlaneIntersect(const Capsule& s1, const Transform3& tf1, + const Plane& s2, const Transform3& tf2) +{ + Plane new_s2 = transform(s2, tf2); + + const Matrix3& R = tf1.linear(); + const Vector3& T = tf1.translation(); + + Vector3 dir_z = R.col(2); + Vector3 p1 = T + dir_z * (0.5 * s1.lz); + Vector3 p2 = T - dir_z * (0.5 * s1.lz); + + S d1 = new_s2.signedDistance(p1); + S d2 = new_s2.signedDistance(p2); + + // two end points on different side of the plane + if(d1 * d2 <= 0) + return true; + + // two end points on the same side of the plane, but the end point spheres might intersect the plane + return (std::abs(d1) <= s1.radius) || (std::abs(d2) <= s1.radius); +} + +//============================================================================== +template +bool capsulePlaneIntersect(const Capsule& s1, const Transform3& tf1, + const Plane& s2, const Transform3& tf2, + std::vector>* contacts) +{ + if(!contacts) + { + return capsulePlaneIntersect(s1, tf1, s2, tf2); + } + else + { + Plane new_s2 = transform(s2, tf2); + + const Matrix3& R = tf1.linear(); + const Vector3& T = tf1.translation(); + + Vector3 dir_z = R.col(2); + + + Vector3 p1 = T + dir_z * (0.5 * s1.lz); + Vector3 p2 = T - dir_z * (0.5 * s1.lz); + + S d1 = new_s2.signedDistance(p1); + S d2 = new_s2.signedDistance(p2); + + S abs_d1 = std::abs(d1); + S abs_d2 = std::abs(d2); + + // two end points on different side of the plane + // the contact point is the intersect of axis with the plane + // the normal is the direction to avoid intersection + // the depth is the minimum distance to resolve the collision + if(d1 * d2 < -planeIntersectTolerance()) + { + if(abs_d1 < abs_d2) + { + if (contacts) + { + const Vector3 normal = (d1 < 0) ? (-new_s2.n).eval() : new_s2.n; + const Vector3 point = p1 * (abs_d2 / (abs_d1 + abs_d2)) + p2 * (abs_d1 / (abs_d1 + abs_d2)); + const S penetration_depth = abs_d1 + s1.radius; + + contacts->emplace_back(normal, point, penetration_depth); + } + } + else + { + if (contacts) + { + const Vector3 normal = (d2 < 0) ? (-new_s2.n).eval() : new_s2.n; + const Vector3 point = p1 * (abs_d2 / (abs_d1 + abs_d2)) + p2 * (abs_d1 / (abs_d1 + abs_d2)); + const S penetration_depth = abs_d2 + s1.radius; + + contacts->emplace_back(normal, point, penetration_depth); + } + } + return true; + } + + if(abs_d1 > s1.radius && abs_d2 > s1.radius) + { + return false; + } + else + { + if (contacts) + { + const Vector3 normal = (d1 < 0) ? new_s2.n : (-new_s2.n).eval(); + const S penetration_depth = s1.radius - std::min(abs_d1, abs_d2); + Vector3 point; + if(abs_d1 <= s1.radius && abs_d2 <= s1.radius) + { + const Vector3 c1 = p1 - new_s2.n * d2; + const Vector3 c2 = p2 - new_s2.n * d1; + point = (c1 + c2) * 0.5; + } + else if(abs_d1 <= s1.radius) + { + const Vector3 c = p1 - new_s2.n * d1; + point = c; + } + else // (abs_d2 <= s1.radius) + { + assert(abs_d2 <= s1.radius); + + const Vector3 c = p2 - new_s2.n * d2; + point = c; + } + + contacts->emplace_back(normal, point, penetration_depth); + } + + return true; + } + } +} + +//============================================================================== +template +bool cylinderPlaneIntersect(const Cylinder& s1, const Transform3& tf1, + const Plane& s2, const Transform3& tf2) +{ + Plane new_s2 = transform(s2, tf2); + + const Matrix3& R = tf1.linear(); + const Vector3& T = tf1.translation(); + + Vector3 Q = R.transpose() * new_s2.n; + + S term = std::abs(Q[2]) * s1.lz + s1.radius * std::sqrt(Q[0] * Q[0] + Q[1] * Q[1]); + S dist = new_s2.distance(T); + S depth = term - dist; + + if(depth < 0) + return false; + else + return true; +} + +//============================================================================== +template +bool cylinderPlaneIntersect(const Cylinder& s1, const Transform3& tf1, + const Plane& s2, const Transform3& tf2, + std::vector>* contacts) +{ + if(!contacts) + { + return cylinderPlaneIntersect(s1, tf1, s2, tf2); + } + else + { + Plane new_s2 = transform(s2, tf2); + + const Matrix3& R = tf1.linear(); + const Vector3& T = tf1.translation(); + + Vector3 dir_z = R.col(2); + S cosa = dir_z.dot(new_s2.n); + + if(std::abs(cosa) < planeIntersectTolerance()) + { + S d = new_s2.signedDistance(T); + S depth = s1.radius - std::abs(d); + if(depth < 0) return false; + else + { + if (contacts) + { + const Vector3 normal = (d < 0) ? new_s2.n : (-new_s2.n).eval(); + const Vector3 point = T - new_s2.n * d; + const S penetration_depth = depth; + + contacts->emplace_back(normal, point, penetration_depth); + } + return true; + } + } + else + { + Vector3 C = dir_z * cosa - new_s2.n; + if(std::abs(cosa + 1) < planeIntersectTolerance() || std::abs(cosa - 1) < planeIntersectTolerance()) + C = Vector3(0, 0, 0); + else + { + S s = C.norm(); + s = s1.radius / s; + C *= s; + } + + Vector3 p1 = T + dir_z * (0.5 * s1.lz); + Vector3 p2 = T - dir_z * (0.5 * s1.lz); + + Vector3 c1, c2; + if(cosa > 0) + { + c1 = p1 - C; + c2 = p2 + C; + } + else + { + c1 = p1 + C; + c2 = p2 - C; + } + + S d1 = new_s2.signedDistance(c1); + S d2 = new_s2.signedDistance(c2); + + if(d1 * d2 <= 0) + { + S abs_d1 = std::abs(d1); + S abs_d2 = std::abs(d2); + + if(abs_d1 > abs_d2) + { + if (contacts) + { + const Vector3 normal = (d2 < 0) ? (-new_s2.n).eval() : new_s2.n; + const Vector3 point = c2 - new_s2.n * d2; + const S penetration_depth = abs_d2; + + contacts->emplace_back(normal, point, penetration_depth); + } + } + else + { + if (contacts) + { + const Vector3 normal = (d1 < 0) ? (-new_s2.n).eval() : new_s2.n; + const Vector3 point = c1 - new_s2.n * d1; + const S penetration_depth = abs_d1; + + contacts->emplace_back(normal, point, penetration_depth); + } + } + return true; + } + else + { + return false; + } + } + } +} + +//============================================================================== +template +bool conePlaneIntersect(const Cone& s1, const Transform3& tf1, + const Plane& s2, const Transform3& tf2, + std::vector>* contacts) +{ + Plane new_s2 = transform(s2, tf2); + + const Matrix3& R = tf1.linear(); + const Vector3& T = tf1.translation(); + + Vector3 dir_z = R.col(2); + S cosa = dir_z.dot(new_s2.n); + + if(std::abs(cosa) < planeIntersectTolerance()) + { + S d = new_s2.signedDistance(T); + S depth = s1.radius - std::abs(d); + if(depth < 0) return false; + else + { + if (contacts) + { + const Vector3 normal = (d < 0) ? new_s2.n : (-new_s2.n).eval(); + const Vector3 point = T - dir_z * (0.5 * s1.lz) + dir_z * (0.5 * depth / s1.radius * s1.lz) - new_s2.n * d; + const S penetration_depth = depth; + + contacts->emplace_back(normal, point, penetration_depth); + } + + return true; + } + } + else + { + Vector3 C = dir_z * cosa - new_s2.n; + if(std::abs(cosa + 1) < planeIntersectTolerance() || std::abs(cosa - 1) < planeIntersectTolerance()) + C = Vector3(0, 0, 0); + else + { + S s = C.norm(); + s = s1.radius / s; + C *= s; + } + + Vector3 c[3]; + c[0] = T + dir_z * (0.5 * s1.lz); + c[1] = T - dir_z * (0.5 * s1.lz) + C; + c[2] = T - dir_z * (0.5 * s1.lz) - C; + + S d[3]; + d[0] = new_s2.signedDistance(c[0]); + d[1] = new_s2.signedDistance(c[1]); + d[2] = new_s2.signedDistance(c[2]); + + if((d[0] >= 0 && d[1] >= 0 && d[2] >= 0) || (d[0] <= 0 && d[1] <= 0 && d[2] <= 0)) + return false; + else + { + bool positive[3]; + for(std::size_t i = 0; i < 3; ++i) + positive[i] = (d[i] >= 0); + + int n_positive = 0; + S d_positive = 0, d_negative = 0; + for(std::size_t i = 0; i < 3; ++i) + { + if(positive[i]) + { + n_positive++; + if(d_positive <= d[i]) d_positive = d[i]; + } + else + { + if(d_negative <= -d[i]) d_negative = -d[i]; + } + } + + if (contacts) + { + const Vector3 normal = (d_positive > d_negative) ? (-new_s2.n).eval() : new_s2.n; + const S penetration_depth = std::min(d_positive, d_negative); + + Vector3 point; + Vector3 p[2] { Vector3::Zero(), Vector3::Zero() }; + Vector3 q = Vector3::Zero(); + + S p_d[2]; + S q_d(0); + + if(n_positive == 2) + { + for(std::size_t i = 0, j = 0; i < 3; ++i) + { + if(positive[i]) { p[j] = c[i]; p_d[j] = d[i]; j++; } + else { q = c[i]; q_d = d[i]; } + } + + const Vector3 t1 = (-p[0] * q_d + q * p_d[0]) / (-q_d + p_d[0]); + const Vector3 t2 = (-p[1] * q_d + q * p_d[1]) / (-q_d + p_d[1]); + point = (t1 + t2) * 0.5; + } + else + { + for(std::size_t i = 0, j = 0; i < 3; ++i) + { + if(!positive[i]) { p[j] = c[i]; p_d[j] = d[i]; j++; } + else { q = c[i]; q_d = d[i]; } + } + + const Vector3 t1 = (p[0] * q_d - q * p_d[0]) / (q_d - p_d[0]); + const Vector3 t2 = (p[1] * q_d - q * p_d[1]) / (q_d - p_d[1]); + point = (t1 + t2) * 0.5; + } + + contacts->emplace_back(normal, point, penetration_depth); + } + + return true; + } + } +} + +//============================================================================== +template +bool convexPlaneIntersect(const Convex& s1, const Transform3& tf1, + const Plane& s2, const Transform3& tf2, + Vector3* contact_points, S* penetration_depth, Vector3* normal) +{ + Plane new_s2 = transform(s2, tf2); + + 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) + { + Vector3 p = tf1 * s1.points[i]; + + S d = new_s2.signedDistance(p); + + if(d < d_min) { d_min = d; v_min = p; } + if(d > d_max) { d_max = d; v_max = p; } + } + + if(d_min * d_max > 0) return false; + else + { + if(d_min + d_max > 0) + { + if(penetration_depth) *penetration_depth = -d_min; + if(normal) *normal = -new_s2.n; + if(contact_points) *contact_points = v_min - new_s2.n * d_min; + } + else + { + if(penetration_depth) *penetration_depth = d_max; + if(normal) *normal = new_s2.n; + if(contact_points) *contact_points = v_max - new_s2.n * d_max; + } + return true; + } +} + +//============================================================================== +template +bool planeTriangleIntersect(const Plane& s1, const Transform3& tf1, + const Vector3& P1, const Vector3& P2, const Vector3& P3, const Transform3& tf2, + Vector3* contact_points, S* penetration_depth, Vector3* normal) +{ + Plane new_s1 = transform(s1, tf1); + + Vector3 c[3]; + c[0] = tf2 * P1; + c[1] = tf2 * P2; + c[2] = tf2 * P3; + + S d[3]; + d[0] = new_s1.signedDistance(c[0]); + d[1] = new_s1.signedDistance(c[1]); + d[2] = new_s1.signedDistance(c[2]); + + if((d[0] >= 0 && d[1] >= 0 && d[2] >= 0) || (d[0] <= 0 && d[1] <= 0 && d[2] <= 0)) + return false; + else + { + bool positive[3]; + for(std::size_t i = 0; i < 3; ++i) + positive[i] = (d[i] > 0); + + int n_positive = 0; + S d_positive = 0, d_negative = 0; + for(std::size_t i = 0; i < 3; ++i) + { + if(positive[i]) + { + n_positive++; + if(d_positive <= d[i]) d_positive = d[i]; + } + else + { + if(d_negative <= -d[i]) d_negative = -d[i]; + } + } + + if(penetration_depth) *penetration_depth = std::min(d_positive, d_negative); + if(normal) *normal = (d_positive > d_negative) ? new_s1.n : (-new_s1.n).eval(); + if(contact_points) + { + Vector3 p[2] = {Vector3::Zero(), Vector3::Zero()}; + Vector3 q = Vector3::Zero(); + + S p_d[2]; + S q_d(0); + + if(n_positive == 2) + { + for(std::size_t i = 0, j = 0; i < 3; ++i) + { + if(positive[i]) { p[j] = c[i]; p_d[j] = d[i]; j++; } + else { q = c[i]; q_d = d[i]; } + } + + Vector3 t1 = (-p[0] * q_d + q * p_d[0]) / (-q_d + p_d[0]); + Vector3 t2 = (-p[1] * q_d + q * p_d[1]) / (-q_d + p_d[1]); + *contact_points = (t1 + t2) * 0.5; + } + else + { + for(std::size_t i = 0, j = 0; i < 3; ++i) + { + if(!positive[i]) { p[j] = c[i]; p_d[j] = d[i]; j++; } + else { q = c[i]; q_d = d[i]; } + } + + Vector3 t1 = (p[0] * q_d - q * p_d[0]) / (q_d - p_d[0]); + Vector3 t2 = (p[1] * q_d - q * p_d[1]) / (q_d - p_d[1]); + *contact_points = (t1 + t2) * 0.5; + } + } + return true; + } +} + +//============================================================================== +template +bool halfspacePlaneIntersect(const Halfspace& s1, const Transform3& tf1, + const Plane& s2, const Transform3& tf2, + Plane& pl, Vector3& p, Vector3& d, + S& penetration_depth, + int& ret) +{ + return planeHalfspaceIntersect(s2, tf2, s1, tf1, pl, p, d, penetration_depth, ret); +} + +//============================================================================== +template +bool planeIntersect(const Plane& s1, const Transform3& tf1, + const Plane& s2, const Transform3& tf2, + std::vector>* /*contacts*/) +{ + Plane new_s1 = transform(s1, tf1); + Plane new_s2 = transform(s2, tf2); + + S a = (new_s1.n).dot(new_s2.n); + if(a == 1 && new_s1.d != new_s2.d) + return false; + if(a == -1 && new_s1.d != -new_s2.d) + return false; + + return true; +} + +} // namespace details + +} // namespace fcl + +#endif diff --git a/src/collision_data.cpp b/include/fcl/narrowphase/detail/primitive_shape_algorithms.h old mode 100644 new mode 100755 similarity index 79% rename from src/collision_data.cpp rename to include/fcl/narrowphase/detail/primitive_shape_algorithms.h index 9f62cfa71..95e86bfd2 --- a/src/collision_data.cpp +++ b/include/fcl/narrowphase/detail/primitive_shape_algorithms.h @@ -35,24 +35,15 @@ /** \author Jia Pan */ -#include "fcl/collision_data.h" +#ifndef FCL_NARROWPHASE_DETAIL_PRIMITIVESHAPEALGORITHMS_H +#define FCL_NARROWPHASE_DETAIL_PRIMITIVESHAPEALGORITHMS_H -namespace fcl -{ +#include "fcl/narrowphase/detail/capsule_capsule.h" +#include "fcl/narrowphase/detail/sphere_capsule.h" +#include "fcl/narrowphase/detail/sphere_sphere.h" +#include "fcl/narrowphase/detail/sphere_triangle.h" +#include "fcl/narrowphase/detail/box_box.h" +#include "fcl/narrowphase/detail/halfspace.h" +#include "fcl/narrowphase/detail/plane.h" -bool comparePenDepth(const ContactPoint& _cp1, const ContactPoint& _cp2) -{ - return _cp1.penetration_depth < _cp2.penetration_depth; -} - -bool CollisionRequest::isSatisfied(const CollisionResult& result) const -{ - return (!enable_cost) && result.isCollision() && (num_max_contacts <= result.numContacts()); -} - -bool DistanceRequest::isSatisfied(const DistanceResult& result) const -{ - return (result.min_distance <= 0); -} - -} +#endif diff --git a/include/fcl/narrowphase/detail/sphere_capsule.h b/include/fcl/narrowphase/detail/sphere_capsule.h new file mode 100755 index 000000000..55aa7f7c5 --- /dev/null +++ b/include/fcl/narrowphase/detail/sphere_capsule.h @@ -0,0 +1,164 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_NARROWPHASE_DETAIL_SPHERECAPSULE_H +#define FCL_NARROWPHASE_DETAIL_SPHERECAPSULE_H + +#include "fcl/collision_data.h" + +namespace fcl +{ + +namespace details +{ + +// Compute the point on a line segment that is the closest point on the +// segment to to another point. The code is inspired by the explanation +// given by Dan Sunday's page: +// http://geomalgorithms.com/a02-_lines.html +template +void lineSegmentPointClosestToPoint (const Vector3 &p, const Vector3 &s1, const Vector3 &s2, Vector3 &sp); + +template +bool sphereCapsuleIntersect(const Sphere& s1, const Transform3& tf1, + const Capsule& s2, const Transform3& tf2, + std::vector>* contacts); + +template +bool sphereCapsuleDistance(const Sphere& s1, const Transform3& tf1, + const Capsule& s2, const Transform3& tf2, + S* dist, Vector3* p1, Vector3* p2); + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +void lineSegmentPointClosestToPoint (const Vector3 &p, const Vector3 &s1, const Vector3 &s2, Vector3 &sp) { + Vector3 v = s2 - s1; + Vector3 w = p - s1; + + S c1 = w.dot(v); + S c2 = v.dot(v); + + if (c1 <= 0) { + sp = s1; + } else if (c2 <= c1) { + sp = s2; + } else { + S b = c1/c2; + Vector3 Pb = s1 + v * b; + sp = Pb; + } +} + +//============================================================================== +template +bool sphereCapsuleIntersect(const Sphere& s1, const Transform3& tf1, + const Capsule& s2, const Transform3& tf2, + std::vector>* contacts) +{ + const Vector3 pos1(0., 0., 0.5 * s2.lz); + const Vector3 pos2(0., 0., -0.5 * s2.lz); + const Vector3 s_c = tf2.inverse(Eigen::Isometry) * tf1.translation(); + + Vector3 segment_point; + + lineSegmentPointClosestToPoint (s_c, pos1, pos2, segment_point); + Vector3 diff = s_c - segment_point; + + const S distance = diff.norm() - s1.radius - s2.radius; + + if (distance > 0) + return false; + + const Vector3 local_normal = -diff.normalized(); + + if (contacts) + { + const Vector3 normal = tf2.linear() * local_normal; + const Vector3 point = tf2 * (segment_point + local_normal * distance); + const S penetration_depth = -distance; + + contacts->emplace_back(normal, point, penetration_depth); + } + + return true; +} + +//============================================================================== +template +bool sphereCapsuleDistance(const Sphere& s1, const Transform3& tf1, + const Capsule& s2, const Transform3& tf2, + S* dist, Vector3* p1, Vector3* p2) +{ + Vector3 pos1(0., 0., 0.5 * s2.lz); + Vector3 pos2(0., 0., -0.5 * s2.lz); + Vector3 s_c = tf2.inverse(Eigen::Isometry) * tf1.translation(); + + Vector3 segment_point; + + lineSegmentPointClosestToPoint (s_c, pos1, pos2, segment_point); + Vector3 diff = s_c - segment_point; + + S distance = diff.norm() - s1.radius - s2.radius; + + if(distance <= 0) + return false; + + if(dist) *dist = distance; + + if(p1 || p2) diff.normalize(); + if(p1) + { + *p1 = s_c - diff * s1.radius; + *p1 = tf1.inverse(Eigen::Isometry) * tf2 * (*p1); + } + + if(p2) *p2 = segment_point + diff * s1.radius; + + return true; +} + +} // namespace details + +} // namespace fcl + +#endif diff --git a/include/fcl/narrowphase/detail/sphere_sphere.h b/include/fcl/narrowphase/detail/sphere_sphere.h new file mode 100755 index 000000000..40c5faa23 --- /dev/null +++ b/include/fcl/narrowphase/detail/sphere_sphere.h @@ -0,0 +1,115 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_NARROWPHASE_DETAIL_SPHERESPHERE_H +#define FCL_NARROWPHASE_DETAIL_SPHERESPHERE_H + +#include "fcl/collision_data.h" + +namespace fcl +{ + +namespace details +{ + +template +bool sphereSphereIntersect(const Sphere& s1, const Transform3& tf1, + const Sphere& s2, const Transform3& tf2, + std::vector>* contacts); + +template +bool sphereSphereDistance(const Sphere& s1, const Transform3& tf1, + const Sphere& s2, const Transform3& tf2, + S* dist, Vector3* p1, Vector3* p2); + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +bool sphereSphereIntersect(const Sphere& s1, const Transform3& tf1, + const Sphere& s2, const Transform3& tf2, + std::vector>* contacts) +{ + Vector3 diff = tf2.translation() - tf1.translation(); + S len = diff.norm(); + if(len > s1.radius + s2.radius) + return false; + + if(contacts) + { + // If the centers of two sphere are at the same position, the normal is (0, 0, 0). + // Otherwise, normal is pointing from center of object 1 to center of object 2 + const Vector3 normal = len > 0 ? (diff / len).eval() : diff; + const Vector3 point = tf1.translation() + diff * s1.radius / (s1.radius + s2.radius); + const S penetration_depth = s1.radius + s2.radius - len; + contacts->emplace_back(normal, point, penetration_depth); + } + + return true; +} + +//============================================================================== +template +bool sphereSphereDistance(const Sphere& s1, const Transform3& tf1, + const Sphere& s2, const Transform3& tf2, + S* dist, Vector3* p1, Vector3* p2) +{ + Vector3 o1 = tf1.translation(); + Vector3 o2 = tf2.translation(); + Vector3 diff = o1 - o2; + S len = diff.norm(); + 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)); + return true; + } + + if(dist) *dist = -1; + return false; +} + +} // namespace details + +} // namespace fcl + +#endif diff --git a/include/fcl/narrowphase/detail/sphere_triangle.h b/include/fcl/narrowphase/detail/sphere_triangle.h new file mode 100755 index 000000000..5e7cccc23 --- /dev/null +++ b/include/fcl/narrowphase/detail/sphere_triangle.h @@ -0,0 +1,512 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_NARROWPHASE_DETAIL_SPHERETRIANGLE_H +#define FCL_NARROWPHASE_DETAIL_SPHERETRIANGLE_H + +#include "fcl/collision_data.h" + +namespace fcl +{ + +namespace details +{ + +/** \brief the minimum distance from a point to a line */ +template +S segmentSqrDistance(const Vector3& from, const Vector3& to,const Vector3& p, Vector3& nearest); + +/// @brief Whether a point's projection is in a triangle +template +bool projectInTriangle(const Vector3& p1, const Vector3& p2, const Vector3& p3, const Vector3& normal, const Vector3& p); + +template +bool sphereTriangleIntersect(const Sphere& s, const Transform3& tf, + const Vector3& P1, const Vector3& P2, const Vector3& P3, Vector3* contact_points, S* penetration_depth, Vector3* normal_); + +template +bool sphereTriangleDistance(const Sphere& sp, const Transform3& tf, + const Vector3& P1, const Vector3& P2, const Vector3& P3, + S* dist); + +template +bool sphereTriangleDistance(const Sphere& sp, const Transform3& tf, + const Vector3& P1, const Vector3& P2, const Vector3& P3, + S* dist, Vector3* p1, Vector3* p2); + +template +bool sphereTriangleDistance(const Sphere& sp, const Transform3& tf1, + const Vector3& P1, const Vector3& P2, const Vector3& P3, const Transform3& tf2, + S* dist, Vector3* p1, Vector3* p2); + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +S segmentSqrDistance(const Vector3& from, const Vector3& to,const Vector3& p, Vector3& nearest) +{ + Vector3 diff = p - from; + Vector3 v = to - from; + S t = v.dot(diff); + + if(t > 0) + { + S dotVV = v.dot(v); + if(t < dotVV) + { + t /= dotVV; + diff -= v * t; + } + else + { + t = 1; + diff -= v; + } + } + else + t = 0; + + nearest = from + v * t; + return diff.dot(diff); +} + +//============================================================================== +template +bool projectInTriangle(const Vector3& p1, const Vector3& p2, const Vector3& p3, const Vector3& normal, const Vector3& p) +{ + Vector3 edge1(p2 - p1); + Vector3 edge2(p3 - p2); + Vector3 edge3(p1 - p3); + + Vector3 p1_to_p(p - p1); + Vector3 p2_to_p(p - p2); + Vector3 p3_to_p(p - p3); + + Vector3 edge1_normal(edge1.cross(normal)); + Vector3 edge2_normal(edge2.cross(normal)); + Vector3 edge3_normal(edge3.cross(normal)); + + S r1, r2, r3; + r1 = edge1_normal.dot(p1_to_p); + r2 = edge2_normal.dot(p2_to_p); + r3 = edge3_normal.dot(p3_to_p); + if ( ( r1 > 0 && r2 > 0 && r3 > 0 ) || + ( r1 <= 0 && r2 <= 0 && r3 <= 0 ) ) + return true; + return false; +} + +//============================================================================== +template +bool sphereTriangleIntersect(const Sphere& s, const Transform3& tf, + const Vector3& P1, const Vector3& P2, const Vector3& P3, Vector3* contact_points, S* penetration_depth, Vector3* normal_) +{ + Vector3 normal = (P2 - P1).cross(P3 - P1); + normal.normalize(); + const Vector3& center = tf.translation(); + const S& radius = s.radius; + S radius_with_threshold = radius + std::numeric_limits::epsilon(); + Vector3 p1_to_center = center - P1; + S distance_from_plane = p1_to_center.dot(normal); + + if(distance_from_plane < 0) + { + distance_from_plane *= -1; + normal *= -1; + } + + bool is_inside_contact_plane = (distance_from_plane < radius_with_threshold); + + bool has_contact = false; + Vector3 contact_point; + if(is_inside_contact_plane) + { + if(projectInTriangle(P1, P2, P3, normal, center)) + { + has_contact = true; + contact_point = center - normal * distance_from_plane; + } + else + { + S contact_capsule_radius_sqr = radius_with_threshold * radius_with_threshold; + Vector3 nearest_on_edge; + S distance_sqr; + distance_sqr = segmentSqrDistance(P1, P2, center, nearest_on_edge); + if(distance_sqr < contact_capsule_radius_sqr) + { + has_contact = true; + contact_point = nearest_on_edge; + } + + distance_sqr = segmentSqrDistance(P2, P3, center, nearest_on_edge); + if(distance_sqr < contact_capsule_radius_sqr) + { + has_contact = true; + contact_point = nearest_on_edge; + } + + distance_sqr = segmentSqrDistance(P3, P1, center, nearest_on_edge); + if(distance_sqr < contact_capsule_radius_sqr) + { + has_contact = true; + contact_point = nearest_on_edge; + } + } + } + + if(has_contact) + { + Vector3 contact_to_center = contact_point - center; + S distance_sqr = contact_to_center.squaredNorm(); + + if(distance_sqr < radius_with_threshold * radius_with_threshold) + { + if(distance_sqr > 0) + { + S distance = std::sqrt(distance_sqr); + if(normal_) *normal_ = contact_to_center.normalized(); + if(contact_points) *contact_points = contact_point; + if(penetration_depth) *penetration_depth = -(radius - distance); + } + else + { + if(normal_) *normal_ = -normal; + if(contact_points) *contact_points = contact_point; + if(penetration_depth) *penetration_depth = -radius; + } + + return true; + } + } + + return false; +} + +//============================================================================== +template +bool sphereTriangleDistance(const Sphere& sp, const Transform3& tf, + const Vector3& P1, const Vector3& P2, const Vector3& P3, + S* dist) +{ + // from geometric tools, very different from the collision code. + + const Vector3& center = tf.translation(); + S radius = sp.radius; + Vector3 diff = P1 - center; + Vector3 edge0 = P2 - P1; + Vector3 edge1 = P3 - P1; + S a00 = edge0.squaredNorm(); + S a01 = edge0.dot(edge1); + S a11 = edge1.squaredNorm(); + S b0 = diff.dot(edge0); + S b1 = diff.dot(edge1); + S c = diff.squaredNorm(); + S det = fabs(a00*a11 - a01*a01); + S s = a01*b1 - a11*b0; + S t = a01*b0 - a00*b1; + + S sqr_dist; + + if(s + t <= det) + { + if(s < 0) + { + if(t < 0) // region 4 + { + if(b0 < 0) + { + t = 0; + if(-b0 >= a00) + { + s = 1; + sqr_dist = a00 + 2*b0 + c; + } + else + { + s = -b0/a00; + sqr_dist = b0*s + c; + } + } + else + { + s = 0; + if(b1 >= 0) + { + t = 0; + sqr_dist = c; + } + else if(-b1 >= a11) + { + t = 1; + sqr_dist = a11 + 2*b1 + c; + } + else + { + t = -b1/a11; + sqr_dist = b1*t + c; + } + } + } + else // region 3 + { + s = 0; + if(b1 >= 0) + { + t = 0; + sqr_dist = c; + } + else if(-b1 >= a11) + { + t = 1; + sqr_dist = a11 + 2*b1 + c; + } + else + { + t = -b1/a11; + sqr_dist = b1*t + c; + } + } + } + else if(t < 0) // region 5 + { + t = 0; + if(b0 >= 0) + { + s = 0; + sqr_dist = c; + } + else if(-b0 >= a00) + { + s = 1; + sqr_dist = a00 + 2*b0 + c; + } + else + { + s = -b0/a00; + sqr_dist = b0*s + c; + } + } + else // region 0 + { + // minimum at interior point + S inv_det = (1)/det; + s *= inv_det; + t *= inv_det; + sqr_dist = s*(a00*s + a01*t + 2*b0) + t*(a01*s + a11*t + 2*b1) + c; + } + } + else + { + S tmp0, tmp1, numer, denom; + + if(s < 0) // region 2 + { + tmp0 = a01 + b0; + tmp1 = a11 + b1; + if(tmp1 > tmp0) + { + numer = tmp1 - tmp0; + denom = a00 - 2*a01 + a11; + if(numer >= denom) + { + s = 1; + t = 0; + sqr_dist = a00 + 2*b0 + c; + } + else + { + s = numer/denom; + t = 1 - s; + sqr_dist = s*(a00*s + a01*t + 2*b0) + t*(a01*s + a11*t + 2*b1) + c; + } + } + else + { + s = 0; + if(tmp1 <= 0) + { + t = 1; + sqr_dist = a11 + 2*b1 + c; + } + else if(b1 >= 0) + { + t = 0; + sqr_dist = c; + } + else + { + t = -b1/a11; + sqr_dist = b1*t + c; + } + } + } + else if(t < 0) // region 6 + { + tmp0 = a01 + b1; + tmp1 = a00 + b0; + if(tmp1 > tmp0) + { + numer = tmp1 - tmp0; + denom = a00 - 2*a01 + a11; + if(numer >= denom) + { + t = 1; + s = 0; + sqr_dist = a11 + 2*b1 + c; + } + else + { + t = numer/denom; + s = 1 - t; + sqr_dist = s*(a00*s + a01*t + 2*b0) + t*(a01*s + a11*t + 2*b1) + c; + } + } + else + { + t = 0; + if(tmp1 <= 0) + { + s = 1; + sqr_dist = a00 + 2*b0 + c; + } + else if(b0 >= 0) + { + s = 0; + sqr_dist = c; + } + else + { + s = -b0/a00; + sqr_dist = b0*s + c; + } + } + } + else // region 1 + { + numer = a11 + b1 - a01 - b0; + if(numer <= 0) + { + s = 0; + t = 1; + sqr_dist = a11 + 2*b1 + c; + } + else + { + denom = a00 - 2*a01 + a11; + if(numer >= denom) + { + s = 1; + t = 0; + sqr_dist = a00 + 2*b0 + c; + } + else + { + s = numer/denom; + t = 1 - s; + sqr_dist = s*(a00*s + a01*t + 2*b0) + t*(a01*s + a11*t + 2*b1) + c; + } + } + } + } + + // Account for numerical round-off error. + if(sqr_dist < 0) + sqr_dist = 0; + + if(sqr_dist > radius * radius) + { + if(dist) *dist = std::sqrt(sqr_dist) - radius; + return true; + } + else + { + if(dist) *dist = -1; + return false; + } +} + +//============================================================================== +template +bool sphereTriangleDistance(const Sphere& sp, const Transform3& tf, + const Vector3& P1, const Vector3& P2, const Vector3& P3, + S* dist, Vector3* p1, Vector3* p2) +{ + if(p1 || p2) + { + Vector3 o = tf.translation(); + typename Project::ProjectResult result; + result = Project::projectTriangle(P1, P2, P3, o); + if(result.sqr_distance > sp.radius * sp.radius) + { + if(dist) *dist = std::sqrt(result.sqr_distance) - sp.radius; + Vector3 project_p = P1 * result.parameterization[0] + P2 * result.parameterization[1] + P3 * result.parameterization[2]; + Vector3 dir = o - project_p; + dir.normalize(); + if(p1) { *p1 = o - dir * sp.radius; *p1 = tf.inverse(Eigen::Isometry) * (*p1); } + if(p2) *p2 = project_p; + return true; + } + else + return false; + } + else + { + return sphereTriangleDistance(sp, tf, P1, P2, P3, dist); + } +} + +//============================================================================== +template +bool sphereTriangleDistance(const Sphere& sp, const Transform3& tf1, + const Vector3& P1, const Vector3& P2, const Vector3& P3, const Transform3& tf2, + S* dist, Vector3* p1, Vector3* p2) +{ + bool res = details::sphereTriangleDistance(sp, tf1, tf2 * P1, tf2 * P2, tf2 * P3, dist, p1, p2); + if(p2) *p2 = tf2.inverse(Eigen::Isometry) * (*p2); + + return res; +} + +} // namespace details + +} // namespace fcl + +#endif diff --git a/include/fcl/narrowphase/gjk.h b/include/fcl/narrowphase/gjk.h deleted file mode 100644 index c06fd4591..000000000 --- a/include/fcl/narrowphase/gjk.h +++ /dev/null @@ -1,313 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#ifndef FCL_GJK_H -#define FCL_GJK_H - -#include "fcl/shape/geometric_shapes.h" - -namespace fcl -{ - -namespace details -{ - -/// @brief the support function for shape -Vector3d getSupport(const ShapeBase* shape, const Vector3d& dir); - -/// @brief Minkowski difference class of two shapes -struct MinkowskiDiff -{ - /// @brief points to two shapes - const ShapeBase* shapes[2]; - - /// @brief rotation from shape0 to shape1 - Matrix3d toshape1; - - /// @brief transform from shape1 to shape0 - Transform3d toshape0; - - MinkowskiDiff() { } - - /// @brief support function for shape0 - inline Vector3d support0(const Vector3d& d) const - { - return getSupport(shapes[0], d); - } - - /// @brief support function for shape1 - inline Vector3d support1(const Vector3d& d) const - { - return toshape0 * getSupport(shapes[1], toshape1 * d); - } - - /// @brief support function for the pair of shapes - inline Vector3d support(const Vector3d& d) const - { - return support0(d) - support1(-d); - } - - /// @brief support function for the d-th shape (d = 0 or 1) - inline Vector3d support(const Vector3d& d, size_t index) const - { - if(index) - return support1(d); - else - return support0(d); - } - - /// @brief support function for translating shape0, which is translating at velocity v - inline Vector3d support0(const Vector3d& d, const Vector3d& v) const - { - if(d.dot(v) <= 0) - return getSupport(shapes[0], d); - else - return getSupport(shapes[0], d) + v; - } - - /// @brief support function for the pair of shapes, where shape0 is translating at velocity v - inline Vector3d support(const Vector3d& d, const Vector3d& v) const - { - return support0(d, v) - support1(-d); - } - - /// @brief support function for the d-th shape (d = 0 or 1), where shape0 is translating at velocity v - inline Vector3d support(const Vector3d& d, const Vector3d& v, size_t index) const - { - if(index) - return support1(d); - else - return support0(d, v); - } -}; - - -/// @brief class for GJK algorithm -struct GJK -{ - struct SimplexV - { - /// @brief support direction - Vector3d d; - /// @brieg support vector (i.e., the furthest point on the shape along the support direction) - Vector3d w; - }; - - struct Simplex - { - /// @brief simplex vertex - SimplexV* c[4]; - /// @brief weight - FCL_REAL p[4]; - /// @brief size of simplex (number of vertices) - size_t rank; - - Simplex() : rank(0) {} - }; - - enum Status {Valid, Inside, Failed}; - - MinkowskiDiff shape; - Vector3d ray; - FCL_REAL distance; - Simplex simplices[2]; - - - GJK(unsigned int max_iterations_, FCL_REAL tolerance_) : max_iterations(max_iterations_), - tolerance(tolerance_) - { - initialize(); - } - - void initialize(); - - /// @brief GJK algorithm, given the initial value guess - Status evaluate(const MinkowskiDiff& shape_, const Vector3d& guess); - - /// @brief apply the support function along a direction, the result is return in sv - void getSupport(const Vector3d& d, SimplexV& sv) const; - - /// @brief apply the support function along a direction, the result is return is sv, here shape0 is translating at velocity v - void getSupport(const Vector3d& d, const Vector3d& v, SimplexV& sv) const; - - /// @brief discard one vertex from the simplex - void removeVertex(Simplex& simplex); - - /// @brief append one vertex to the simplex - void appendVertex(Simplex& simplex, const Vector3d& v); - - /// @brief whether the simplex enclose the origin - bool encloseOrigin(); - - /// @brief get the underlying simplex using in GJK, can be used for cache in next iteration - inline Simplex* getSimplex() const - { - return simplex; - } - - /// @brief get the guess from current simplex - Vector3d getGuessFromSimplex() const; - -private: - SimplexV store_v[4]; - SimplexV* free_v[4]; - size_t nfree; - size_t current; - Simplex* simplex; - Status status; - - unsigned int max_iterations; - FCL_REAL tolerance; - -}; - - -static const size_t EPA_MAX_FACES = 128; -static const size_t EPA_MAX_VERTICES = 64; -static const FCL_REAL EPA_EPS = 0.000001; -static const size_t EPA_MAX_ITERATIONS = 255; - -/// @brief class for EPA algorithm -struct EPA -{ -private: - typedef GJK::SimplexV SimplexV; - struct SimplexF - { - Vector3d n; - FCL_REAL d; - SimplexV* c[3]; // a face has three vertices - SimplexF* f[3]; // a face has three adjacent faces - SimplexF* l[2]; // the pre and post faces in the list - size_t e[3]; - size_t pass; - }; - - struct SimplexList - { - SimplexF* root; - size_t count; - SimplexList() : root(NULL), count(0) {} - void append(SimplexF* face) - { - face->l[0] = NULL; - face->l[1] = root; - if(root) root->l[0] = face; - root = face; - ++count; - } - - void remove(SimplexF* face) - { - if(face->l[1]) face->l[1]->l[0] = face->l[0]; - if(face->l[0]) face->l[0]->l[1] = face->l[1]; - if(face == root) root = face->l[1]; - --count; - } - }; - - static inline void bind(SimplexF* fa, size_t ea, SimplexF* fb, size_t eb) - { - fa->e[ea] = eb; fa->f[ea] = fb; - fb->e[eb] = ea; fb->f[eb] = fa; - } - - struct SimplexHorizon - { - SimplexF* cf; // current face in the horizon - SimplexF* ff; // first face in the horizon - size_t nf; // number of faces in the horizon - SimplexHorizon() : cf(NULL), ff(NULL), nf(0) {} - }; - -private: - unsigned int max_face_num; - unsigned int max_vertex_num; - unsigned int max_iterations; - FCL_REAL tolerance; - -public: - - enum Status {Valid, Touching, Degenerated, NonConvex, InvalidHull, OutOfFaces, OutOfVertices, AccuracyReached, FallBack, Failed}; - - Status status; - GJK::Simplex result; - Vector3d normal; - FCL_REAL depth; - SimplexV* sv_store; - SimplexF* fc_store; - size_t nextsv; - SimplexList hull, stock; - - EPA(unsigned int max_face_num_, unsigned int max_vertex_num_, unsigned int max_iterations_, FCL_REAL tolerance_) : max_face_num(max_face_num_), - max_vertex_num(max_vertex_num_), - max_iterations(max_iterations_), - tolerance(tolerance_) - { - initialize(); - } - - ~EPA() - { - delete [] sv_store; - delete [] fc_store; - } - - void initialize(); - - bool getEdgeDist(SimplexF* face, SimplexV* a, SimplexV* b, FCL_REAL& dist); - - SimplexF* newFace(SimplexV* a, SimplexV* b, SimplexV* c, bool forced); - - /// @brief Find the best polytope face to split - SimplexF* findBest(); - - Status evaluate(GJK& gjk, const Vector3d& guess); - - /// @brief the goal is to add a face connecting vertex w and face edge f[e] - bool expand(size_t pass, SimplexV* w, SimplexF* f, size_t e, SimplexHorizon& horizon); -}; - - -} // details - - - -} - - -#endif diff --git a/include/fcl/narrowphase/gjk_libccd.h b/include/fcl/narrowphase/gjk_libccd.h deleted file mode 100644 index 87e5f9db4..000000000 --- a/include/fcl/narrowphase/gjk_libccd.h +++ /dev/null @@ -1,182 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - - -#ifndef FCL_GJK_LIBCCD_H -#define FCL_GJK_LIBCCD_H - -#include "fcl/shape/geometric_shapes.h" - -#include -#include - -namespace fcl -{ - -namespace details -{ - -/// @brief callback function used by GJK algorithm -typedef void (*GJKSupportFunction)(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v); -typedef void (*GJKCenterFunction)(const void* obj, ccd_vec3_t* c); - -/// @brief initialize GJK stuffs -template -class GJKInitializer -{ -public: - /// @brief Get GJK support function - static GJKSupportFunction getSupportFunction() { return NULL; } - - /// @brief Get GJK center function - static GJKCenterFunction getCenterFunction() { return NULL; } - - /// @brief Get GJK object from a shape - /// Notice that only local transformation is applied. - /// Gloal transformation are considered later - static void* createGJKObject(const T& /* s */, const Transform3d& /*tf*/) { return NULL; } - - /// @brief Delete GJK object - static void deleteGJKObject(void* o) {} -}; - -/// @brief initialize GJK Cylinder -template<> -class GJKInitializer -{ -public: - static GJKSupportFunction getSupportFunction(); - static GJKCenterFunction getCenterFunction(); - static void* createGJKObject(const Cylinder& s, const Transform3d& tf); - static void deleteGJKObject(void* o); -}; - -/// @brief initialize GJK Sphere -template<> -class GJKInitializer -{ -public: - static GJKSupportFunction getSupportFunction(); - static GJKCenterFunction getCenterFunction(); - static void* createGJKObject(const Sphere& s, const Transform3d& tf); - static void deleteGJKObject(void* o); -}; - -/// @brief initialize GJK Ellipsoid -template<> -class GJKInitializer -{ -public: - static GJKSupportFunction getSupportFunction(); - static GJKCenterFunction getCenterFunction(); - static void* createGJKObject(const Ellipsoid& s, const Transform3d& tf); - static void deleteGJKObject(void* o); -}; - -/// @brief initialize GJK Box -template<> -class GJKInitializer -{ -public: - static GJKSupportFunction getSupportFunction(); - static GJKCenterFunction getCenterFunction(); - static void* createGJKObject(const Box& s, const Transform3d& tf); - static void deleteGJKObject(void* o); -}; - -/// @brief initialize GJK Capsule -template<> -class GJKInitializer -{ -public: - static GJKSupportFunction getSupportFunction(); - static GJKCenterFunction getCenterFunction(); - static void* createGJKObject(const Capsule& s, const Transform3d& tf); - static void deleteGJKObject(void* o); -}; - -/// @brief initialize GJK Cone -template<> -class GJKInitializer -{ -public: - static GJKSupportFunction getSupportFunction(); - static GJKCenterFunction getCenterFunction(); - static void* createGJKObject(const Cone& s, const Transform3d& tf); - static void deleteGJKObject(void* o); -}; - -/// @brief initialize GJK Convex -template<> -class GJKInitializer -{ -public: - static GJKSupportFunction getSupportFunction(); - static GJKCenterFunction getCenterFunction(); - static void* createGJKObject(const Convex& s, const Transform3d& tf); - static void deleteGJKObject(void* o); -}; - -/// @brief initialize GJK Triangle -GJKSupportFunction triGetSupportFunction(); - -GJKCenterFunction triGetCenterFunction(); - -void* triCreateGJKObject(const Vector3d& P1, const Vector3d& P2, const Vector3d& P3); - -void* triCreateGJKObject(const Vector3d& P1, const Vector3d& P2, const Vector3d& P3, const Transform3d& tf); - -void triDeleteGJKObject(void* o); - -/// @brief GJK collision algorithm -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, FCL_REAL tolerance, - Vector3d* contact_points, FCL_REAL* penetration_depth, Vector3d* normal); - -bool GJKDistance(void* obj1, ccd_support_fn supp1, - void* obj2, ccd_support_fn supp2, - unsigned int max_iterations, FCL_REAL tolerance, - FCL_REAL* dist, Vector3d* p1, Vector3d* p2); - - -} // details - - -} - -#endif diff --git a/include/fcl/narrowphase/gjk_solver_indep.h b/include/fcl/narrowphase/gjk_solver_indep.h new file mode 100755 index 000000000..fc9d0c6ba --- /dev/null +++ b/include/fcl/narrowphase/gjk_solver_indep.h @@ -0,0 +1,1071 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_NARROWPHASE_GJKSOLVERINDEP_H +#define FCL_NARROWPHASE_GJKSOLVERINDEP_H + +#include + +#include "fcl/collision_data.h" +#include "fcl/narrowphase/detail/gjk.h" +#include "fcl/narrowphase/detail/primitive_shape_algorithms.h" + +namespace fcl +{ + +/// @brief collision and distance solver based on GJK algorithm implemented in fcl (rewritten the code from the GJK in bullet) +template +struct GJKSolver_indep +{ + using S = S_; + + /// @brief intersection checking between two shapes + /// @deprecated use shapeIntersect(const Shape1&, const Transform3&, const Shape2&, const Transform3&, std::vector>*) const + template + FCL_DEPRECATED + bool shapeIntersect( + const Shape1& s1, + const Transform3& tf1, + const Shape2& s2, + const Transform3& tf2, + Vector3* contact_points, + S* penetration_depth, + Vector3* normal) const; + + /// @brief intersection checking between two shapes + template + bool shapeIntersect( + const Shape1& s1, + const Transform3& tf1, + const Shape2& s2, + const Transform3& tf2, + std::vector>* contacts = nullptr) const; + + /// @brief intersection checking between one shape and a triangle + template + bool shapeTriangleIntersect( + const Shape& s, + const Transform3& tf, + const Vector3& P1, + const Vector3& P2, + const Vector3& P3, + Vector3* contact_points = nullptr, + S* penetration_depth = nullptr, + Vector3* normal = nullptr) const; + + //// @brief intersection checking between one shape and a triangle with transformation + template + bool shapeTriangleIntersect( + const Shape& s, + const Transform3& tf1, + const Vector3& P1, + const Vector3& P2, + const Vector3& P3, + const Transform3& tf2, + Vector3* contact_points = nullptr, + S* penetration_depth = nullptr, + Vector3* normal = nullptr) const; + + /// @brief distance computation between two shapes + template + bool shapeDistance( + const Shape1& s1, + const Transform3& tf1, + const Shape2& s2, + const Transform3& tf2, + S* distance = nullptr, + Vector3* p1 = nullptr, + Vector3* p2 = nullptr) const; + + /// @brief distance computation between one shape and a triangle + template + bool shapeTriangleDistance( + const Shape& s, + const Transform3& tf, + const Vector3& P1, + const Vector3& P2, + const Vector3& P3, + S* distance = nullptr, + Vector3* p1 = nullptr, + Vector3* p2 = nullptr) const; + + /// @brief distance computation between one shape and a triangle with transformation + template + bool shapeTriangleDistance( + const Shape& s, + const Transform3& tf1, + const Vector3& P1, + const Vector3& P2, + const Vector3& P3, + const Transform3& tf2, + S* distance = nullptr, + Vector3* p1 = nullptr, + Vector3* p2 = nullptr) const; + + /// @brief default setting for GJK algorithm + GJKSolver_indep(); + + void enableCachedGuess(bool if_enable) const; + + void setCachedGuess(const Vector3& guess) const; + + Vector3 getCachedGuess() const; + + /// @brief maximum number of simplex face used in EPA algorithm + unsigned int epa_max_face_num; + + /// @brief maximum number of simplex vertex used in EPA algorithm + unsigned int epa_max_vertex_num; + + /// @brief maximum number of iterations used for EPA iterations + unsigned int epa_max_iterations; + + /// @brief the threshold used in EPA to stop iteration + S epa_tolerance; + + /// @brief the threshold used in GJK to stop iteration + S gjk_tolerance; + + /// @brief maximum number of iterations used for GJK iterations + S gjk_max_iterations; + + /// @brief Whether smart guess can be provided + mutable bool enable_cached_guess; + + /// @brief smart guess + mutable Vector3 cached_guess; +}; + +using GJKSolver_indepf = GJKSolver_indep; +using GJKSolver_indepd = GJKSolver_indep; + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +template +bool GJKSolver_indep::shapeIntersect(const Shape1& s1, const Transform3& tf1, + const Shape2& s2, const Transform3& tf2, + Vector3* contact_points, S* penetration_depth, Vector3* normal) const +{ + bool res; + + if (contact_points || penetration_depth || normal) + { + std::vector> contacts; + + res = shapeIntersect(s1, tf1, s2, tf2, &contacts); + + if (!contacts.empty()) + { + // Get the deepest contact point + const ContactPoint& maxDepthContact = *std::max_element(contacts.begin(), contacts.end(), comparePenDepth); + + if (contact_points) + *contact_points = maxDepthContact.pos; + + if (penetration_depth) + *penetration_depth = maxDepthContact.penetration_depth; + + if (normal) + *normal = maxDepthContact.normal; + } + } + else + { + res = shapeIntersect(s1, tf1, s2, tf2, nullptr); + } + + return res; +} + +//============================================================================== +template +struct ShapeIntersectIndepImpl +{ + static bool run( + const GJKSolver_indep& gjkSolver, + const Shape1& s1, + const Transform3& tf1, + const Shape2& s2, + const Transform3& tf2, + std::vector>* contacts) + { + Vector3 guess(1, 0, 0); + if(gjkSolver.enable_cached_guess) guess = gjkSolver.cached_guess; + + details::MinkowskiDiff shape; + shape.shapes[0] = &s1; + shape.shapes[1] = &s2; + shape.toshape1.noalias() = tf2.linear().transpose() * tf1.linear(); + shape.toshape0 = tf1.inverse(Eigen::Isometry) * tf2; + + details::GJK gjk(gjkSolver.gjk_max_iterations, gjkSolver.gjk_tolerance); + typename details::GJK::Status gjk_status = gjk.evaluate(shape, -guess); + if(gjkSolver.enable_cached_guess) gjkSolver.cached_guess = gjk.getGuessFromSimplex(); + + switch(gjk_status) + { + case details::GJK::Inside: + { + details::EPA epa(gjkSolver.epa_max_face_num, gjkSolver.epa_max_vertex_num, gjkSolver.epa_max_iterations, gjkSolver.epa_tolerance); + typename details::EPA::Status epa_status = epa.evaluate(gjk, -guess); + if(epa_status != details::EPA::Failed) + { + Vector3 w0 = Vector3::Zero(); + for(size_t i = 0; i < epa.result.rank; ++i) + { + w0.noalias() += shape.support(epa.result.c[i]->d, 0) * epa.result.p[i]; + } + if(contacts) + { + Vector3 normal = epa.normal; + Vector3 point = tf1 * (w0 - epa.normal*(epa.depth *0.5)); + S depth = -epa.depth; + contacts->emplace_back(normal, point, depth); + } + return true; + } + else return false; + } + break; + default: + ; + } + + return false; + } +}; + +//============================================================================== +template +template +bool GJKSolver_indep::shapeIntersect( + const Shape1& s1, + const Transform3& tf1, + const Shape2& s2, + const Transform3& tf2, + std::vector>* contacts) const +{ + return ShapeIntersectIndepImpl::run( + *this, s1, tf1, s2, tf2, contacts); +} + +// Shape intersect algorithms not using built-in GJK algorithm +// +// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ +// | | box | sphere | ellipsoid | capsule | cone | cylinder | plane | half-space | triangle | +// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ +// | box | O | | | | | | O | O | | +// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ +// | sphere |/////| O | | O | | | O | O | O | +// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ +// | ellipsoid |/////|////////| | | | | O | O | | +// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ +// | capsule |/////|////////|///////////| | | | O | O | | +// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ +// | cone |/////|////////|///////////|/////////| | | O | O | | +// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ +// | cylinder |/////|////////|///////////|/////////|//////| | O | O | | +// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ +// | plane |/////|////////|///////////|/////////|//////|//////////| O | O | O | +// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ +// | half-space |/////|////////|///////////|/////////|//////|//////////|///////| O | O | +// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ +// | triangle |/////|////////|///////////|/////////|//////|//////////|///////|////////////| | +// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ + +#define FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT_REG(SHAPE1, SHAPE2, ALG)\ + template \ + struct ShapeIntersectIndepImpl, SHAPE2>\ + {\ + static bool run(\ + const GJKSolver_indep& /*gjkSolver*/,\ + const SHAPE1& s1,\ + const Transform3& tf1,\ + const SHAPE2& s2,\ + const Transform3& tf2,\ + std::vector>* contacts)\ + {\ + return ALG(s1, tf1, s2, tf2, contacts);\ + }\ + }; + +#define FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT_INV(SHAPE1, SHAPE2, ALG)\ + template \ + struct ShapeIntersectIndepImpl, SHAPE1>\ + {\ + static bool run(\ + const GJKSolver_indep& /*gjkSolver*/,\ + const SHAPE2& s1,\ + const Transform3& tf1,\ + const SHAPE1& s2,\ + const Transform3& tf2,\ + std::vector>* contacts)\ + {\ + const bool res = ALG(s2, tf2, s1, tf1, contacts);\ + if (contacts) flipNormal(*contacts);\ + return res;\ + }\ + }; + +#define FCL_GJK_INDEP_SHAPE_INTERSECT(SHAPE, ALG)\ + FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT_REG(SHAPE, SHAPE, ALG) + +#define FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(SHAPE1, SHAPE2, ALG)\ + FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT_REG(SHAPE1, SHAPE2, ALG)\ + FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT_INV(SHAPE1, SHAPE2, ALG) + +FCL_GJK_INDEP_SHAPE_INTERSECT(Sphere, details::sphereSphereIntersect) +FCL_GJK_INDEP_SHAPE_INTERSECT(Box, details::boxBoxIntersect) + +FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Sphere, Capsule, details::sphereCapsuleIntersect) + +FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Sphere, Halfspace, details::sphereHalfspaceIntersect) +FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Ellipsoid, Halfspace, details::ellipsoidHalfspaceIntersect) +FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Box, Halfspace, details::boxHalfspaceIntersect) +FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Capsule, Halfspace, details::capsuleHalfspaceIntersect) +FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Cylinder, Halfspace, details::cylinderHalfspaceIntersect) +FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Cone, Halfspace, details::coneHalfspaceIntersect) + +FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Sphere, Plane, details::spherePlaneIntersect) +FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Ellipsoid, Plane, details::ellipsoidPlaneIntersect) +FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Box, Plane, details::boxPlaneIntersect) +FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Capsule, Plane, details::capsulePlaneIntersect) +FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Cylinder, Plane, details::cylinderPlaneIntersect) +FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Cone, Plane, details::conePlaneIntersect) + +template +struct ShapeIntersectIndepImpl, Halfspace> +{ + static bool run( + const GJKSolver_indep& /*gjkSolver*/, + const Halfspace& s1, + const Transform3& tf1, + const Halfspace& s2, + const Transform3& tf2, + std::vector>* contacts) + { + Halfspace s; + Vector3 p, d; + S depth; + int ret; + return details::halfspaceIntersect(s1, tf1, s2, tf2, p, d, s, depth, ret); + } +}; + +template +struct ShapeIntersectIndepImpl, Plane> +{ + static bool run( + const GJKSolver_indep& /*gjkSolver*/, + const Plane& s1, + const Transform3& tf1, + const Plane& s2, + const Transform3& tf2, + std::vector>* contacts) + { + return details::planeIntersect(s1, tf1, s2, tf2, contacts); + } +}; + +template +struct ShapeIntersectIndepImpl, Halfspace> +{ + static bool run( + const GJKSolver_indep& /*gjkSolver*/, + const Plane& s1, + const Transform3& tf1, + const Halfspace& s2, + const Transform3& tf2, + std::vector>* contacts) + { + Plane pl; + Vector3 p, d; + S depth; + int ret; + return details::planeHalfspaceIntersect(s1, tf1, s2, tf2, pl, p, d, depth, ret); + } +}; + +template +struct ShapeIntersectIndepImpl, Plane> +{ + static bool run( + const GJKSolver_indep& /*gjkSolver*/, + const Halfspace& s1, + const Transform3& tf1, + const Plane& s2, + const Transform3& tf2, + std::vector>* contacts) + { + Plane pl; + Vector3 p, d; + S depth; + int ret; + return details::halfspacePlaneIntersect(s1, tf1, s2, tf2, pl, p, d, depth, ret); + } +}; + +//============================================================================== +template +struct ShapeTriangleIntersectIndepImpl +{ + static bool run( + const GJKSolver_indep& gjkSolver, + const Shape& s, + const Transform3& tf, + const Vector3& P1, + const Vector3& P2, + const Vector3& P3, + Vector3* contact_points, + S* penetration_depth, + Vector3* normal) + { + TriangleP tri(P1, P2, P3); + + Vector3 guess(1, 0, 0); + if(gjkSolver.enable_cached_guess) guess = gjkSolver.cached_guess; + + details::MinkowskiDiff shape; + shape.shapes[0] = &s; + shape.shapes[1] = &tri; + shape.toshape1 = tf.linear(); + shape.toshape0 = tf.inverse(Eigen::Isometry); + + details::GJK gjk(gjkSolver.gjk_max_iterations, gjkSolver.gjk_tolerance); + typename details::GJK::Status gjk_status = gjk.evaluate(shape, -guess); + if(gjkSolver.enable_cached_guess) gjkSolver.cached_guess = gjk.getGuessFromSimplex(); + + switch(gjk_status) + { + case details::GJK::Inside: + { + details::EPA epa(gjkSolver.epa_max_face_num, gjkSolver.epa_max_vertex_num, gjkSolver.epa_max_iterations, gjkSolver.epa_tolerance); + typename details::EPA::Status epa_status = epa.evaluate(gjk, -guess); + if(epa_status != details::EPA::Failed) + { + Vector3 w0 = Vector3::Zero(); + for(size_t i = 0; i < epa.result.rank; ++i) + { + w0.noalias() += shape.support(epa.result.c[i]->d, 0) * epa.result.p[i]; + } + if(penetration_depth) *penetration_depth = -epa.depth; + if(normal) *normal = -epa.normal; + if(contact_points) (*contact_points).noalias() = tf * (w0 - epa.normal*(epa.depth *0.5)); + return true; + } + else return false; + } + break; + default: + ; + } + + return false; + } +}; + +template +template +bool GJKSolver_indep::shapeTriangleIntersect( + const Shape& s, + const Transform3& tf, + const Vector3& P1, + const Vector3& P2, + const Vector3& P3, + Vector3* contact_points, + S* penetration_depth, + Vector3* normal) const +{ + return ShapeTriangleIntersectIndepImpl::run( + *this, s, tf, P1, P2, P3, contact_points, penetration_depth, normal); +} + +//============================================================================== +template +struct ShapeTriangleIntersectIndepImpl> +{ + static bool run( + const GJKSolver_indep& /*gjkSolver*/, + const Sphere& s, + const Transform3& tf, + const Vector3& P1, + const Vector3& P2, + const Vector3& P3, + Vector3* contact_points, + S* penetration_depth, + Vector3* normal) + { + return details::sphereTriangleIntersect( + s, tf, P1, P2, P3, contact_points, penetration_depth, normal); + } +}; + + +//============================================================================== +template +struct ShapeTransformedTriangleIntersectIndepImpl +{ + static bool run( + const GJKSolver_indep& gjkSolver, + const Shape& s, + const Transform3& tf1, + const Vector3& P1, + const Vector3& P2, + const Vector3& P3, + const Transform3& tf2, + Vector3* contact_points, + S* penetration_depth, + Vector3* normal) + { + TriangleP tri(P1, P2, P3); + + Vector3 guess(1, 0, 0); + if(gjkSolver.enable_cached_guess) guess = gjkSolver.cached_guess; + + details::MinkowskiDiff shape; + shape.shapes[0] = &s; + shape.shapes[1] = &tri; + shape.toshape1.noalias() = tf2.linear().transpose() * tf1.linear(); + shape.toshape0 = tf1.inverse(Eigen::Isometry) * tf2; + + details::GJK gjk(gjkSolver.gjk_max_iterations, gjkSolver.gjk_tolerance); + typename details::GJK::Status gjk_status = gjk.evaluate(shape, -guess); + if(gjkSolver.enable_cached_guess) gjkSolver.cached_guess = gjk.getGuessFromSimplex(); + + switch(gjk_status) + { + case details::GJK::Inside: + { + details::EPA epa(gjkSolver.epa_max_face_num, gjkSolver.epa_max_vertex_num, gjkSolver.epa_max_iterations, gjkSolver.epa_tolerance); + typename details::EPA::Status epa_status = epa.evaluate(gjk, -guess); + if(epa_status != details::EPA::Failed) + { + Vector3 w0 = Vector3::Zero(); + for(size_t i = 0; i < epa.result.rank; ++i) + { + w0.noalias() += shape.support(epa.result.c[i]->d, 0) * epa.result.p[i]; + } + if(penetration_depth) *penetration_depth = -epa.depth; + if(normal) *normal = -epa.normal; + if(contact_points) (*contact_points).noalias() = tf1 * (w0 - epa.normal*(epa.depth *0.5)); + return true; + } + else return false; + } + break; + default: + ; + } + + return false; + } +}; + +template +template +bool GJKSolver_indep::shapeTriangleIntersect( + const Shape& s, + const Transform3& tf1, + const Vector3& P1, + const Vector3& P2, + const Vector3& P3, + const Transform3& tf2, + Vector3* contact_points, + S* penetration_depth, + Vector3* normal) const +{ + return ShapeTransformedTriangleIntersectIndepImpl::run( + *this, s, tf1, P1, P2, P3, tf2, + contact_points, penetration_depth, normal); +} + +//============================================================================== +template +struct ShapeTransformedTriangleIntersectIndepImpl> +{ + static bool run( + const GJKSolver_indep& /*gjkSolver*/, + const Sphere& s, + const Transform3& tf1, + const Vector3& P1, + const Vector3& P2, + const Vector3& P3, + const Transform3& tf2, + Vector3* contact_points, + S* penetration_depth, + Vector3* normal) + { + return details::sphereTriangleIntersect( + s, tf1, tf2 * P1, tf2 * P2, tf2 * P3, + contact_points, penetration_depth, normal); + } +}; + +//============================================================================== +template +struct ShapeTransformedTriangleIntersectIndepImpl> +{ + static bool run( + const GJKSolver_indep& /*gjkSolver*/, + const Halfspace& s, + const Transform3& tf1, + const Vector3& P1, + const Vector3& P2, + const Vector3& P3, + const Transform3& tf2, + Vector3* contact_points, + S* penetration_depth, + Vector3* normal) + { + return details::halfspaceTriangleIntersect( + s, tf1, P1, P2, P3, tf2, + contact_points, penetration_depth, normal); + } +}; + +//============================================================================== +template +struct ShapeTransformedTriangleIntersectIndepImpl> +{ + static bool run( + const GJKSolver_indep& /*gjkSolver*/, + const Plane& s, + const Transform3& tf1, + const Vector3& P1, + const Vector3& P2, + const Vector3& P3, + const Transform3& tf2, + Vector3* contact_points, + S* penetration_depth, + Vector3* normal) + { + return details::planeTriangleIntersect( + s, tf1, P1, P2, P3, tf2, + contact_points, penetration_depth, normal); + } +}; + +//============================================================================== +template +struct ShapeDistanceIndepImpl +{ + static bool run( + const GJKSolver_indep& gjkSolver, + const Shape1& s1, + const Transform3& tf1, + const Shape2& s2, + const Transform3& tf2, + S* distance, + Vector3* p1, + Vector3* p2) + { + Vector3 guess(1, 0, 0); + if(gjkSolver.enable_cached_guess) guess = gjkSolver.cached_guess; + + details::MinkowskiDiff shape; + shape.shapes[0] = &s1; + shape.shapes[1] = &s2; + shape.toshape1.noalias() = tf2.linear().transpose() * tf1.linear(); + shape.toshape0 = tf1.inverse(Eigen::Isometry) * tf2; + + details::GJK gjk(gjkSolver.gjk_max_iterations, gjkSolver.gjk_tolerance); + typename details::GJK::Status gjk_status = gjk.evaluate(shape, -guess); + if(gjkSolver.enable_cached_guess) gjkSolver.cached_guess = gjk.getGuessFromSimplex(); + + if(gjk_status == details::GJK::Valid) + { + Vector3 w0 = Vector3::Zero(); + Vector3 w1 = Vector3::Zero(); + for(size_t i = 0; i < gjk.getSimplex()->rank; ++i) + { + S p = gjk.getSimplex()->p[i]; + w0.noalias() += shape.support(gjk.getSimplex()->c[i]->d, 0) * p; + w1.noalias() += shape.support(-gjk.getSimplex()->c[i]->d, 1) * p; + } + + if(distance) *distance = (w0 - w1).norm(); + + if(p1) *p1 = w0; + if(p2) (*p2).noalias() = shape.toshape0 * w1; + + return true; + } + else + { + if(distance) *distance = -1; + return false; + } + } +}; + +template +template +bool GJKSolver_indep::shapeDistance( + const Shape1& s1, + const Transform3& tf1, + const Shape2& s2, + const Transform3& tf2, + S* dist, + Vector3* p1, + Vector3* p2) const +{ + return ShapeDistanceIndepImpl::run( + *this, s1, tf1, s2, tf2, dist, p1, p2); +} + +// Shape distance algorithms not using built-in GJK algorithm +// +// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ +// | | box | sphere | ellipsoid | capsule | cone | cylinder | plane | half-space | triangle | +// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ +// | box | | | | | | | | | | +// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ +// | sphere |/////| O | | O | | | | | O | +// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ +// | ellipsoid |/////|////////| | | | | | | | +// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ +// | capsule |/////|////////|///////////| O | | | | | | +// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ +// | cone |/////|////////|///////////|/////////| | | | | | +// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ +// | cylinder |/////|////////|///////////|/////////|//////| | | | | +// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ +// | plane |/////|////////|///////////|/////////|//////|//////////| | | | +// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ +// | half-space |/////|////////|///////////|/////////|//////|//////////|///////| | | +// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ +// | triangle |/////|////////|///////////|/////////|//////|//////////|///////|////////////| | +// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ + +//============================================================================== +template +struct ShapeDistanceIndepImpl, Capsule> +{ + static bool run( + const GJKSolver_indep& /*gjkSolver*/, + const Sphere& s1, + const Transform3& tf1, + const Capsule& s2, + const Transform3& tf2, + S* dist, + Vector3* p1, + Vector3* p2) + { + return details::sphereCapsuleDistance(s1, tf1, s2, tf2, dist, p1, p2); + } +}; + +//============================================================================== +template +struct ShapeDistanceIndepImpl, Sphere> +{ + static bool run( + const GJKSolver_indep& /*gjkSolver*/, + const Capsule& s1, + const Transform3& tf1, + const Sphere& s2, + const Transform3& tf2, + S* dist, + Vector3* p1, + Vector3* p2) + { + return details::sphereCapsuleDistance(s2, tf2, s1, tf1, dist, p2, p1); + } +}; + +//============================================================================== +template +struct ShapeDistanceIndepImpl, Sphere> +{ + static bool run( + const GJKSolver_indep& /*gjkSolver*/, + const Sphere& s1, + const Transform3& tf1, + const Sphere& s2, + const Transform3& tf2, + S* dist, + Vector3* p1, + Vector3* p2) + { + return details::sphereSphereDistance(s1, tf1, s2, tf2, dist, p1, p2); + } +}; + +//============================================================================== +template +struct ShapeDistanceIndepImpl, Capsule> +{ + static bool run( + const GJKSolver_indep& /*gjkSolver*/, + const Capsule& s1, + const Transform3& tf1, + const Capsule& s2, + const Transform3& tf2, + S* dist, + Vector3* p1, + Vector3* p2) + { + return details::capsuleCapsuleDistance(s1, tf1, s2, tf2, dist, p1, p2); + } +}; + +//============================================================================== +template +struct ShapeTriangleDistanceIndepImpl +{ + static bool run( + const GJKSolver_indep& gjkSolver, + const Shape& s, + const Transform3& tf, + const Vector3& P1, + const Vector3& P2, + const Vector3& P3, + S* distance, + Vector3* p1, + Vector3* p2) + { + TriangleP tri(P1, P2, P3); + Vector3 guess(1, 0, 0); + if(gjkSolver.enable_cached_guess) guess = gjkSolver.cached_guess; + + details::MinkowskiDiff shape; + shape.shapes[0] = &s; + shape.shapes[1] = &tri; + shape.toshape1 = tf.linear(); + shape.toshape0 = tf.inverse(Eigen::Isometry); + + details::GJK gjk(gjkSolver.gjk_max_iterations, gjkSolver.gjk_tolerance); + typename details::GJK::Status gjk_status = gjk.evaluate(shape, -guess); + if(gjkSolver.enable_cached_guess) gjkSolver.cached_guess = gjk.getGuessFromSimplex(); + + if(gjk_status == details::GJK::Valid) + { + Vector3 w0 = Vector3::Zero(); + Vector3 w1 = Vector3::Zero(); + for(size_t i = 0; i < gjk.getSimplex()->rank; ++i) + { + S p = gjk.getSimplex()->p[i]; + w0.noalias() += shape.support(gjk.getSimplex()->c[i]->d, 0) * p; + w1.noalias() += shape.support(-gjk.getSimplex()->c[i]->d, 1) * p; + } + + if(distance) *distance = (w0 - w1).norm(); + if(p1) *p1 = w0; + if(p2) (*p2).noalias() = shape.toshape0 * w1; + return true; + } + else + { + if(distance) *distance = -1; + return false; + } + } +}; + +//============================================================================== +template +template +bool GJKSolver_indep::shapeTriangleDistance( + const Shape& s, + const Transform3& tf, + const Vector3& P1, + const Vector3& P2, + const Vector3& P3, + S* dist, + Vector3* p1, + Vector3* p2) const +{ + return ShapeTriangleDistanceIndepImpl::run( + *this, s, tf, P1, P2, P3, dist, p1, p2); +} + +//============================================================================== +template +struct ShapeTriangleDistanceIndepImpl> +{ + static bool run( + const GJKSolver_indep& /*gjkSolver*/, + const Sphere& s, + const Transform3& tf, + const Vector3& P1, + const Vector3& P2, + const Vector3& P3, + S* dist, + Vector3* p1, + Vector3* p2) + { + return details::sphereTriangleDistance(s, tf, P1, P2, P3, dist, p1, p2); + } +}; + +//============================================================================== +template +struct ShapeTransformedTriangleDistanceIndepImpl +{ + static bool run( + const GJKSolver_indep& gjkSolver, + const Shape& s, + const Transform3& tf1, + const Vector3& P1, + const Vector3& P2, + const Vector3& P3, + const Transform3& tf2, + S* distance, + Vector3* p1, + Vector3* p2) + { + TriangleP tri(P1, P2, P3); + Vector3 guess(1, 0, 0); + if(gjkSolver.enable_cached_guess) guess = gjkSolver.cached_guess; + + details::MinkowskiDiff shape; + shape.shapes[0] = &s; + shape.shapes[1] = &tri; + shape.toshape1.noalias() = tf2.linear().transpose() * tf1.linear(); + shape.toshape0 = tf1.inverse(Eigen::Isometry) * tf2; + + details::GJK gjk(gjkSolver.gjk_max_iterations, gjkSolver.gjk_tolerance); + typename details::GJK::Status gjk_status = gjk.evaluate(shape, -guess); + if(gjkSolver.enable_cached_guess) gjkSolver.cached_guess = gjk.getGuessFromSimplex(); + + if(gjk_status == details::GJK::Valid) + { + Vector3 w0 = Vector3::Zero(); + Vector3 w1 = Vector3::Zero(); + for(size_t i = 0; i < gjk.getSimplex()->rank; ++i) + { + S p = gjk.getSimplex()->p[i]; + w0.noalias() += shape.support(gjk.getSimplex()->c[i]->d, 0) * p; + w1.noalias() += shape.support(-gjk.getSimplex()->c[i]->d, 1) * p; + } + + if(distance) *distance = (w0 - w1).norm(); + if(p1) *p1 = w0; + if(p2) (*p2).noalias() = shape.toshape0 * w1; + return true; + } + else + { + if(distance) *distance = -1; + return false; + } + } +}; + +//============================================================================== +template +template +bool GJKSolver_indep::shapeTriangleDistance( + const Shape& s, + const Transform3& tf1, + const Vector3& P1, + const Vector3& P2, + const Vector3& P3, + const Transform3& tf2, + S* dist, + Vector3* p1, + Vector3* p2) const +{ + return ShapeTransformedTriangleDistanceIndepImpl::run( + *this, s, tf1, P1, P2, P3, tf2, dist, p1, p2); +} + +//============================================================================== +template +struct ShapeTransformedTriangleDistanceIndepImpl> +{ + static bool run( + const GJKSolver_indep& /*gjkSolver*/, + const Sphere& s, + const Transform3& tf1, + const Vector3& P1, + const Vector3& P2, + const Vector3& P3, + const Transform3& tf2, + S* dist, + Vector3* p1, + Vector3* p2) + { + return details::sphereTriangleDistance( + s, tf1, P1, P2, P3, tf2, dist, p1, p2); + } +}; + +//============================================================================== +template +GJKSolver_indep::GJKSolver_indep() +{ + gjk_max_iterations = 128; + gjk_tolerance = 1e-6; + epa_max_face_num = 128; + epa_max_vertex_num = 64; + epa_max_iterations = 255; + epa_tolerance = 1e-6; + enable_cached_guess = false; + cached_guess = Vector3(1, 0, 0); +} + +//============================================================================== +template +void GJKSolver_indep::enableCachedGuess(bool if_enable) const +{ + enable_cached_guess = if_enable; +} + +//============================================================================== +template +void GJKSolver_indep::setCachedGuess(const Vector3& guess) const +{ + cached_guess = guess; +} + +//============================================================================== +template +Vector3 GJKSolver_indep::getCachedGuess() const +{ + return cached_guess; +} + +} // namespace fcl + +#endif diff --git a/include/fcl/narrowphase/gjk_solver_libccd.h b/include/fcl/narrowphase/gjk_solver_libccd.h new file mode 100755 index 000000000..2951c4eba --- /dev/null +++ b/include/fcl/narrowphase/gjk_solver_libccd.h @@ -0,0 +1,973 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_NARROWPHASE_GJKSOLVERLIBCCD_H +#define FCL_NARROWPHASE_GJKSOLVERLIBCCD_H + +#include + +#include "fcl/collision_data.h" +#include "fcl/narrowphase/detail/gjk_libccd.h" +#include "fcl/narrowphase/detail/primitive_shape_algorithms.h" + +namespace fcl +{ + +/// @brief collision and distance solver based on libccd library. +template +struct GJKSolver_libccd +{ + using S = S_; + + /// @brief intersection checking between two shapes + /// @deprecated use shapeIntersect(const Shape1&, const Transform3&, const Shape2&, const Transform3&, std::vector>*) const + template + FCL_DEPRECATED + bool shapeIntersect( + const Shape1& s1, + const Transform3& tf1, + const Shape2& s2, + const Transform3& tf2, + Vector3* contact_points, + S* penetration_depth, + Vector3* normal) const; + + /// @brief intersection checking between two shapes + template + bool shapeIntersect( + const Shape1& s1, + const Transform3& tf1, + const Shape2& s2, + const Transform3& tf2, + std::vector>* contacts = nullptr) const; + + /// @brief intersection checking between one shape and a triangle + template + bool shapeTriangleIntersect( + const Shape& s, + const Transform3& tf, + const Vector3& P1, + const Vector3& P2, + const Vector3& P3, + Vector3* contact_points = nullptr, + S* penetration_depth = nullptr, + Vector3* normal = nullptr) const; + + //// @brief intersection checking between one shape and a triangle with transformation + template + bool shapeTriangleIntersect( + const Shape& s, + const Transform3& tf1, + const Vector3& P1, + const Vector3& P2, + const Vector3& P3, + const Transform3& tf2, + Vector3* contact_points = nullptr, + S* penetration_depth = nullptr, + Vector3* normal = nullptr) const; + + /// @brief distance computation between two shapes + template + bool shapeDistance( + const Shape1& s1, + const Transform3& tf1, + const Shape2& s2, + const Transform3& tf2, + S* dist = nullptr, + Vector3* p1 = nullptr, + Vector3* p2 = nullptr) const; + + /// @brief distance computation between one shape and a triangle + template + bool shapeTriangleDistance( + const Shape& s, + const Transform3& tf, + const Vector3& P1, + const Vector3& P2, + const Vector3& P3, + S* dist = nullptr, + Vector3* p1 = nullptr, + Vector3* p2 = nullptr) const; + + /// @brief distance computation between one shape and a triangle with transformation + template + bool shapeTriangleDistance( + const Shape& s, + const Transform3& tf1, + const Vector3& P1, + const Vector3& P2, + const Vector3& P3, + const Transform3& tf2, + S* dist = nullptr, + Vector3* p1 = nullptr, + Vector3* p2 = nullptr) const; + + /// @brief default setting for GJK algorithm + GJKSolver_libccd(); + + void enableCachedGuess(bool if_enable) const; + + void setCachedGuess(const Vector3& guess) const; + + Vector3 getCachedGuess() const; + + /// @brief maximum number of iterations used in GJK algorithm for collision + unsigned int max_collision_iterations; + + /// @brief maximum number of iterations used in GJK algorithm for distance + unsigned int max_distance_iterations; + + /// @brief the threshold used in GJK algorithm to stop collision iteration + S collision_tolerance; + + /// @brief the threshold used in GJK algorithm to stop distance iteration + S distance_tolerance; + +}; + +using GJKSolver_libccdf = GJKSolver_libccd; +using GJKSolver_libccdd = GJKSolver_libccd; + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +template +bool GJKSolver_libccd::shapeIntersect( + const Shape1& s1, const Transform3& tf1, + const Shape2& s2, const Transform3& tf2, + Vector3* contact_points, + S* penetration_depth, + Vector3* normal) const +{ + bool res; + + if (contact_points || penetration_depth || normal) + { + std::vector> contacts; + + res = shapeIntersect(s1, tf1, s2, tf2, &contacts); + + if (!contacts.empty()) + { + // Get the deepest contact point + const ContactPoint& maxDepthContact = *std::max_element(contacts.begin(), contacts.end(), comparePenDepth); + + if (contact_points) + *contact_points = maxDepthContact.pos; + + if (penetration_depth) + *penetration_depth = maxDepthContact.penetration_depth; + + if (normal) + *normal = maxDepthContact.normal; + } + } + else + { + res = shapeIntersect(s1, tf1, s2, tf2, nullptr); + } + + return res; +} + +//============================================================================== +template +struct ShapeIntersectLibccdImpl +{ + static bool run( + const GJKSolver_libccd& gjkSolver, + const Shape1& s1, const Transform3& tf1, + const Shape2& s2, const Transform3& tf2, + std::vector>* contacts) + { + void* o1 = details::GJKInitializer::createGJKObject(s1, tf1); + void* o2 = details::GJKInitializer::createGJKObject(s2, tf2); + + bool res; + + if(contacts) + { + Vector3 normal; + Vector3 point; + S depth; + res = details::GJKCollide( + o1, + details::GJKInitializer::getSupportFunction(), + details::GJKInitializer::getCenterFunction(), + o2, details::GJKInitializer::getSupportFunction(), + details::GJKInitializer::getCenterFunction(), + gjkSolver.max_collision_iterations, + gjkSolver.collision_tolerance, + &point, + &depth, + &normal); + contacts->emplace_back(normal, point, depth); + } + else + { + res = details::GJKCollide( + o1, + details::GJKInitializer::getSupportFunction(), + details::GJKInitializer::getCenterFunction(), + o2, + details::GJKInitializer::getSupportFunction(), + details::GJKInitializer::getCenterFunction(), + gjkSolver.max_collision_iterations, + gjkSolver.collision_tolerance, + nullptr, + nullptr, + nullptr); + } + + details::GJKInitializer::deleteGJKObject(o1); + details::GJKInitializer::deleteGJKObject(o2); + + return res; + } +}; + +//============================================================================== +template +template +bool GJKSolver_libccd::shapeIntersect( + const Shape1& s1, const Transform3& tf1, + const Shape2& s2, const Transform3& tf2, + std::vector>* contacts) const +{ + return ShapeIntersectLibccdImpl::run( + *this, s1, tf1, s2, tf2, contacts); +} + +// Shape intersect algorithms not using libccd +// +// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ +// | | box | sphere | ellipsoid | capsule | cone | cylinder | plane | half-space | triangle | +// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ +// | box | O | | | | | | O | O | | +// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ +// | sphere |/////| O | | O | | | O | O | O | +// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ +// | ellipsoid |/////|////////| | | | | O | O | TODO | +// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ +// | capsule |/////|////////|///////////| | | | O | O | | +// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ +// | cone |/////|////////|///////////|/////////| | | O | O | | +// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ +// | cylinder |/////|////////|///////////|/////////|//////| | O | O | | +// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ +// | plane |/////|////////|///////////|/////////|//////|//////////| O | O | O | +// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ +// | half-space |/////|////////|///////////|/////////|//////|//////////|///////| O | O | +// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ +// | triangle |/////|////////|///////////|/////////|//////|//////////|///////|////////////| | +// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ + +#define FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT_REG(SHAPE1, SHAPE2, ALG)\ + template \ + struct ShapeIntersectLibccdImpl, SHAPE2>\ + {\ + static bool run(\ + const GJKSolver_libccd& /*gjkSolver*/,\ + const SHAPE1& s1,\ + const Transform3& tf1,\ + const SHAPE2& s2,\ + const Transform3& tf2,\ + std::vector>* contacts)\ + {\ + return ALG(s1, tf1, s2, tf2, contacts);\ + }\ + }; + +#define FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT_INV(SHAPE1, SHAPE2, ALG)\ + template \ + struct ShapeIntersectLibccdImpl, SHAPE1>\ + {\ + static bool run(\ + const GJKSolver_libccd& /*gjkSolver*/,\ + const SHAPE2& s1,\ + const Transform3& tf1,\ + const SHAPE1& s2,\ + const Transform3& tf2,\ + std::vector>* contacts)\ + {\ + const bool res = ALG(s2, tf2, s1, tf1, contacts);\ + if (contacts) flipNormal(*contacts);\ + return res;\ + }\ + }; + +#define FCL_GJK_LIBCCD_SHAPE_INTERSECT(SHAPE, ALG)\ + FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT_REG(SHAPE, SHAPE, ALG) + +#define FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(SHAPE1, SHAPE2, ALG)\ + FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT_REG(SHAPE1, SHAPE2, ALG)\ + FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT_INV(SHAPE1, SHAPE2, ALG) + +FCL_GJK_LIBCCD_SHAPE_INTERSECT(Sphere, details::sphereSphereIntersect) +FCL_GJK_LIBCCD_SHAPE_INTERSECT(Box, details::boxBoxIntersect) + +FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Sphere, Capsule, details::sphereCapsuleIntersect) + +FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Sphere, Halfspace, details::sphereHalfspaceIntersect) +FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Ellipsoid, Halfspace, details::ellipsoidHalfspaceIntersect) +FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Box, Halfspace, details::boxHalfspaceIntersect) +FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Capsule, Halfspace, details::capsuleHalfspaceIntersect) +FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Cylinder, Halfspace, details::cylinderHalfspaceIntersect) +FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Cone, Halfspace, details::coneHalfspaceIntersect) + +FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Sphere, Plane, details::spherePlaneIntersect) +FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Ellipsoid, Plane, details::ellipsoidPlaneIntersect) +FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Box, Plane, details::boxPlaneIntersect) +FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Capsule, Plane, details::capsulePlaneIntersect) +FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Cylinder, Plane, details::cylinderPlaneIntersect) +FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Cone, Plane, details::conePlaneIntersect) + +template +struct ShapeIntersectLibccdImpl, Halfspace> +{ + static bool run( + const GJKSolver_libccd& /*gjkSolver*/, + const Halfspace& s1, + const Transform3& tf1, + const Halfspace& s2, + const Transform3& tf2, + std::vector>* contacts) + { + Halfspace s; + Vector3 p, d; + S depth; + int ret; + return details::halfspaceIntersect(s1, tf1, s2, tf2, p, d, s, depth, ret); + } +}; + +template +struct ShapeIntersectLibccdImpl, Plane> +{ + static bool run( + const GJKSolver_libccd& /*gjkSolver*/, + const Plane& s1, + const Transform3& tf1, + const Plane& s2, + const Transform3& tf2, + std::vector>* contacts) + { + return details::planeIntersect(s1, tf1, s2, tf2, contacts); + } +}; + +template +struct ShapeIntersectLibccdImpl, Halfspace> +{ + static bool run( + const GJKSolver_libccd& /*gjkSolver*/, + const Plane& s1, + const Transform3& tf1, + const Halfspace& s2, + const Transform3& tf2, + std::vector>* contacts) + { + Plane pl; + Vector3 p, d; + S depth; + int ret; + return details::planeHalfspaceIntersect(s1, tf1, s2, tf2, pl, p, d, depth, ret); + } +}; + +template +struct ShapeIntersectLibccdImpl, Plane> +{ + static bool run( + const GJKSolver_libccd& /*gjkSolver*/, + const Halfspace& s1, + const Transform3& tf1, + const Plane& s2, + const Transform3& tf2, + std::vector>* contacts) + { + Plane pl; + Vector3 p, d; + S depth; + int ret; + return details::halfspacePlaneIntersect(s1, tf1, s2, tf2, pl, p, d, depth, ret); + } +}; + +//============================================================================== +template +struct ShapeTriangleIntersectLibccdImpl +{ + static bool run( + const GJKSolver_libccd& gjkSolver, + const Shape& s, + const Transform3& tf, + const Vector3& P1, + const Vector3& P2, + const Vector3& P3, + Vector3* contact_points, + S* penetration_depth, + Vector3* normal) + { + void* o1 = details::GJKInitializer::createGJKObject(s, tf); + void* o2 = details::triCreateGJKObject(P1, P2, P3); + + bool res = details::GJKCollide( + o1, + details::GJKInitializer::getSupportFunction(), + details::GJKInitializer::getCenterFunction(), + o2, + details::triGetSupportFunction(), + details::triGetCenterFunction(), + gjkSolver.max_collision_iterations, + gjkSolver.collision_tolerance, + contact_points, + penetration_depth, + normal); + + details::GJKInitializer::deleteGJKObject(o1); + details::triDeleteGJKObject(o2); + + return res; + } +}; + +template +template +bool GJKSolver_libccd::shapeTriangleIntersect( + const Shape& s, + const Transform3& tf, + const Vector3& P1, + const Vector3& P2, + const Vector3& P3, + Vector3* contact_points, + S* penetration_depth, + Vector3* normal) const +{ + return ShapeTriangleIntersectLibccdImpl::run( + *this, s, tf, P1, P2, P3, contact_points, penetration_depth, normal); +} + +//============================================================================== +template +struct ShapeTriangleIntersectLibccdImpl> +{ + static bool run( + const GJKSolver_libccd& /*gjkSolver*/, + const Sphere& s, + const Transform3& tf, + const Vector3& P1, + const Vector3& P2, + const Vector3& P3, + Vector3* contact_points, + S* penetration_depth, + Vector3* normal) + { + return details::sphereTriangleIntersect( + s, tf, P1, P2, P3, contact_points, penetration_depth, normal); + } +}; + +//============================================================================== +template +struct ShapeTransformedTriangleIntersectLibccdImpl +{ + static bool run( + const GJKSolver_libccd& gjkSolver, + const Shape& s, + const Transform3& tf1, + const Vector3& P1, + const Vector3& P2, + const Vector3& P3, + const Transform3& tf2, + Vector3* contact_points, + S* penetration_depth, + Vector3* normal) + { + void* o1 = details::GJKInitializer::createGJKObject(s, tf1); + void* o2 = details::triCreateGJKObject(P1, P2, P3, tf2); + + bool res = details::GJKCollide( + o1, + details::GJKInitializer::getSupportFunction(), + details::GJKInitializer::getCenterFunction(), + o2, + details::triGetSupportFunction(), + details::triGetCenterFunction(), + gjkSolver.max_collision_iterations, + gjkSolver.collision_tolerance, + contact_points, + penetration_depth, + normal); + + details::GJKInitializer::deleteGJKObject(o1); + details::triDeleteGJKObject(o2); + + return res; + } +}; + +template +template +bool GJKSolver_libccd::shapeTriangleIntersect( + const Shape& s, + const Transform3& tf1, + const Vector3& P1, + const Vector3& P2, + const Vector3& P3, + const Transform3& tf2, + Vector3* contact_points, + S* penetration_depth, + Vector3* normal) const +{ + return ShapeTransformedTriangleIntersectLibccdImpl::run( + *this, s, tf1, P1, P2, P3, tf2, + contact_points, penetration_depth, normal); +} + +//============================================================================== +template +struct ShapeTransformedTriangleIntersectLibccdImpl> +{ + static bool run( + const GJKSolver_libccd& /*gjkSolver*/, + const Sphere& s, + const Transform3& tf1, + const Vector3& P1, + const Vector3& P2, + const Vector3& P3, + const Transform3& tf2, + Vector3* contact_points, + S* penetration_depth, + Vector3* normal) + { + return details::sphereTriangleIntersect( + s, tf1, tf2 * P1, tf2 * P2, tf2 * P3, + contact_points, penetration_depth, normal); + } +}; + +//============================================================================== +template +struct ShapeTransformedTriangleIntersectLibccdImpl> +{ + static bool run( + const GJKSolver_libccd& /*gjkSolver*/, + const Halfspace& s, + const Transform3& tf1, + const Vector3& P1, + const Vector3& P2, + const Vector3& P3, + const Transform3& tf2, + Vector3* contact_points, + S* penetration_depth, + Vector3* normal) + { + return details::halfspaceTriangleIntersect( + s, tf1, P1, P2, P3, tf2, + contact_points, penetration_depth, normal); + } +}; + +//============================================================================== +template +struct ShapeTransformedTriangleIntersectLibccdImpl> +{ + static bool run( + const GJKSolver_libccd& /*gjkSolver*/, + const Plane& s, + const Transform3& tf1, + const Vector3& P1, + const Vector3& P2, + const Vector3& P3, + const Transform3& tf2, + Vector3* contact_points, + S* penetration_depth, + Vector3* normal) + { + return details::planeTriangleIntersect( + s, tf1, P1, P2, P3, tf2, + contact_points, penetration_depth, normal); + } +}; + +//============================================================================== +template +struct ShapeDistanceLibccdImpl +{ + static bool run( + const GJKSolver_libccd& gjkSolver, + const Shape1& s1, + const Transform3& tf1, + const Shape2& s2, + const Transform3& tf2, + S* dist, + Vector3* p1, + Vector3* p2) + { + void* o1 = details::GJKInitializer::createGJKObject(s1, tf1); + void* o2 = details::GJKInitializer::createGJKObject(s2, tf2); + + bool res = details::GJKDistance( + o1, + details::GJKInitializer::getSupportFunction(), + o2, + details::GJKInitializer::getSupportFunction(), + gjkSolver.max_distance_iterations, + gjkSolver.distance_tolerance, + dist, + p1, + p2); + + if (p1) + (*p1).noalias() = tf1.inverse(Eigen::Isometry) * *p1; + + if (p2) + (*p2).noalias() = tf2.inverse(Eigen::Isometry) * *p2; + + details::GJKInitializer::deleteGJKObject(o1); + details::GJKInitializer::deleteGJKObject(o2); + + return res; + } +}; + +template +template +bool GJKSolver_libccd::shapeDistance( + const Shape1& s1, + const Transform3& tf1, + const Shape2& s2, + const Transform3& tf2, + S* dist, + Vector3* p1, + Vector3* p2) const +{ + return ShapeDistanceLibccdImpl::run( + *this, s1, tf1, s2, tf2, dist, p1, p2); +} + +// Shape distance algorithms not using libccd +// +// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ +// | | box | sphere | ellipsoid | capsule | cone | cylinder | plane | half-space | triangle | +// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ +// | box | | | | | | | | | | +// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ +// | sphere |/////| O | | O | | | | | O | +// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ +// | ellipsoid |/////|////////| | | | | | | | +// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ +// | capsule |/////|////////|///////////| O | | | | | | +// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ +// | cone |/////|////////|///////////|/////////| | | | | | +// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ +// | cylinder |/////|////////|///////////|/////////|//////| | | | | +// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ +// | plane |/////|////////|///////////|/////////|//////|//////////| | | | +// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ +// | half-space |/////|////////|///////////|/////////|//////|//////////|///////| | | +// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ +// | triangle |/////|////////|///////////|/////////|//////|//////////|///////|////////////| | +// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ + +//============================================================================== +template +struct ShapeDistanceLibccdImpl, Capsule> +{ + static bool run( + const GJKSolver_libccd& /*gjkSolver*/, + const Sphere& s1, + const Transform3& tf1, + const Capsule& s2, + const Transform3& tf2, + S* dist, + Vector3* p1, + Vector3* p2) + { + return details::sphereCapsuleDistance(s1, tf1, s2, tf2, dist, p1, p2); + } +}; + +//============================================================================== +template +struct ShapeDistanceLibccdImpl, Sphere> +{ + static bool run( + const GJKSolver_libccd& /*gjkSolver*/, + const Capsule& s1, + const Transform3& tf1, + const Sphere& s2, + const Transform3& tf2, + S* dist, + Vector3* p1, + Vector3* p2) + { + return details::sphereCapsuleDistance(s2, tf2, s1, tf1, dist, p2, p1); + } +}; + +//============================================================================== +template +struct ShapeDistanceLibccdImpl, Sphere> +{ + static bool run( + const GJKSolver_libccd& /*gjkSolver*/, + const Sphere& s1, + const Transform3& tf1, + const Sphere& s2, + const Transform3& tf2, + S* dist, + Vector3* p1, + Vector3* p2) + { + return details::sphereSphereDistance(s1, tf1, s2, tf2, dist, p1, p2); + } +}; + +//============================================================================== +template +struct ShapeDistanceLibccdImpl, Capsule> +{ + static bool run( + const GJKSolver_libccd& /*gjkSolver*/, + const Capsule& s1, + const Transform3& tf1, + const Capsule& s2, + const Transform3& tf2, + S* dist, + Vector3* p1, + Vector3* p2) + { + return details::capsuleCapsuleDistance(s1, tf1, s2, tf2, dist, p1, p2); + } +}; + +//============================================================================== +template +struct ShapeTriangleDistanceLibccdImpl +{ + static bool run( + const GJKSolver_libccd& gjkSolver, + const Shape& s, + const Transform3& tf, + const Vector3& P1, + const Vector3& P2, + const Vector3& P3, + S* dist, + Vector3* p1, + Vector3* p2) + { + void* o1 = details::GJKInitializer::createGJKObject(s, tf); + void* o2 = details::triCreateGJKObject(P1, P2, P3); + + bool res = details::GJKDistance( + o1, + details::GJKInitializer::getSupportFunction(), + o2, + details::triGetSupportFunction(), + gjkSolver.max_distance_iterations, + gjkSolver.distance_tolerance, + dist, + p1, + p2); + if(p1) + (*p1).noalias() = tf.inverse(Eigen::Isometry) * *p1; + + details::GJKInitializer::deleteGJKObject(o1); + details::triDeleteGJKObject(o2); + + return res; + } +}; + +//============================================================================== +template +template +bool GJKSolver_libccd::shapeTriangleDistance( + const Shape& s, + const Transform3& tf, + const Vector3& P1, + const Vector3& P2, + const Vector3& P3, + S* dist, + Vector3* p1, + Vector3* p2) const +{ + return ShapeTriangleDistanceLibccdImpl::run( + *this, s, tf, P1, P2, P3, dist, p1, p2); +} + +//============================================================================== +template +struct ShapeTriangleDistanceLibccdImpl> +{ + static bool run( + const GJKSolver_libccd& /*gjkSolver*/, + const Sphere& s, + const Transform3& tf, + const Vector3& P1, + const Vector3& P2, + const Vector3& P3, + S* dist, + Vector3* p1, + Vector3* p2) + { + return details::sphereTriangleDistance(s, tf, P1, P2, P3, dist, p1, p2); + } +}; + +//============================================================================== +template +struct ShapeTransformedTriangleDistanceLibccdImpl +{ + static bool run( + const GJKSolver_libccd& gjkSolver, + const Shape& s, + const Transform3& tf1, + const Vector3& P1, + const Vector3& P2, + const Vector3& P3, + const Transform3& tf2, + S* dist, + Vector3* p1, + Vector3* p2) + { + void* o1 = details::GJKInitializer::createGJKObject(s, tf1); + void* o2 = details::triCreateGJKObject(P1, P2, P3, tf2); + + bool res = details::GJKDistance( + o1, + details::GJKInitializer::getSupportFunction(), + o2, + details::triGetSupportFunction(), + gjkSolver.max_distance_iterations, + gjkSolver.distance_tolerance, + dist, + p1, + p2); + if(p1) + (*p1).noalias() = tf1.inverse(Eigen::Isometry) * *p1; + if(p2) + (*p2).noalias() = tf2.inverse(Eigen::Isometry) * *p2; + + details::GJKInitializer::deleteGJKObject(o1); + details::triDeleteGJKObject(o2); + + return res; + } +}; + +//============================================================================== +template +template +bool GJKSolver_libccd::shapeTriangleDistance( + const Shape& s, + const Transform3& tf1, + const Vector3& P1, + const Vector3& P2, + const Vector3& P3, + const Transform3& tf2, + S* dist, + Vector3* p1, + Vector3* p2) const +{ + return ShapeTransformedTriangleDistanceLibccdImpl::run( + *this, s, tf1, P1, P2, P3, tf2, dist, p1, p2); +} + +//============================================================================== +template +struct ShapeTransformedTriangleDistanceLibccdImpl> +{ + static bool run( + const GJKSolver_libccd& /*gjkSolver*/, + const Sphere& s, + const Transform3& tf1, + const Vector3& P1, + const Vector3& P2, + const Vector3& P3, + const Transform3& tf2, + S* dist, + Vector3* p1, + Vector3* p2) + { + return details::sphereTriangleDistance( + s, tf1, P1, P2, P3, tf2, dist, p1, p2); + } +}; + +//============================================================================== +template +GJKSolver_libccd::GJKSolver_libccd() +{ + max_collision_iterations = 500; + max_distance_iterations = 1000; + collision_tolerance = 1e-6; + distance_tolerance = 1e-6; +} + +//============================================================================== +template +void GJKSolver_libccd::enableCachedGuess(bool if_enable) const +{ + // TODO: need change libccd to exploit spatial coherence +} + +//============================================================================== +template +void GJKSolver_libccd::setCachedGuess( + const Vector3& guess) const +{ + // TODO: need change libccd to exploit spatial coherence +} + +//============================================================================== +template +Vector3 GJKSolver_libccd::getCachedGuess() const +{ + return Vector3(-1, 0, 0); +} + +} // namespace fcl + +#endif diff --git a/include/fcl/narrowphase/narrowphase.h b/include/fcl/narrowphase/narrowphase.h index c4b4655de..9820a2543 100755 --- a/include/fcl/narrowphase/narrowphase.h +++ b/include/fcl/narrowphase/narrowphase.h @@ -38,1061 +38,6 @@ #ifndef FCL_NARROWPHASE_H #define FCL_NARROWPHASE_H -#include - -#include "fcl/collision_data.h" -#include "fcl/narrowphase/gjk.h" -#include "fcl/narrowphase/gjk_libccd.h" - -namespace fcl -{ -/// @brief collision and distance solver based on libccd library. -struct GJKSolver_libccd -{ - /// @brief intersection checking between two shapes - /// @deprecated use shapeIntersect(const S1&, const Transform3d&, const S2&, const Transform3d&, std::vector*) const - template - FCL_DEPRECATED - bool shapeIntersect(const S1& s1, const Transform3d& tf1, - const S2& s2, const Transform3d& tf2, - Vector3d* contact_points, FCL_REAL* penetration_depth, Vector3d* normal) const; - - /// @brief intersection checking between two shapes - template - bool shapeIntersect(const S1& s1, const Transform3d& tf1, - const S2& s2, const Transform3d& tf2, - std::vector* contacts = NULL) const - { - void* o1 = details::GJKInitializer::createGJKObject(s1, tf1); - void* o2 = details::GJKInitializer::createGJKObject(s2, tf2); - - bool res; - - if(contacts) - { - Vector3d normal; - Vector3d point; - FCL_REAL depth; - res = details::GJKCollide(o1, details::GJKInitializer::getSupportFunction(), details::GJKInitializer::getCenterFunction(), - o2, details::GJKInitializer::getSupportFunction(), details::GJKInitializer::getCenterFunction(), - max_collision_iterations, collision_tolerance, - &point, &depth, &normal); - contacts->push_back(ContactPoint(normal, point, depth)); - } - else - { - res = details::GJKCollide(o1, details::GJKInitializer::getSupportFunction(), details::GJKInitializer::getCenterFunction(), - o2, details::GJKInitializer::getSupportFunction(), details::GJKInitializer::getCenterFunction(), - max_collision_iterations, collision_tolerance, - NULL, NULL, NULL); - } - - details::GJKInitializer::deleteGJKObject(o1); - details::GJKInitializer::deleteGJKObject(o2); - - return res; - } - - /// @brief intersection checking between one shape and a triangle - template - bool shapeTriangleIntersect(const S& s, const Transform3d& tf, - const Vector3d& P1, const Vector3d& P2, const Vector3d& P3, Vector3d* contact_points = NULL, FCL_REAL* penetration_depth = NULL, Vector3d* normal = NULL) const - { - void* o1 = details::GJKInitializer::createGJKObject(s, tf); - void* o2 = details::triCreateGJKObject(P1, P2, P3); - - bool res = details::GJKCollide(o1, details::GJKInitializer::getSupportFunction(), details::GJKInitializer::getCenterFunction(), - o2, details::triGetSupportFunction(), details::triGetCenterFunction(), - max_collision_iterations, collision_tolerance, - contact_points, penetration_depth, normal); - - details::GJKInitializer::deleteGJKObject(o1); - details::triDeleteGJKObject(o2); - - return res; - } - - //// @brief intersection checking between one shape and a triangle with transformation - template - bool shapeTriangleIntersect(const S& s, const Transform3d& tf1, - const Vector3d& P1, const Vector3d& P2, const Vector3d& P3, const Transform3d& tf2, - Vector3d* contact_points = NULL, FCL_REAL* penetration_depth = NULL, Vector3d* normal = NULL) const - { - void* o1 = details::GJKInitializer::createGJKObject(s, tf1); - void* o2 = details::triCreateGJKObject(P1, P2, P3, tf2); - - bool res = details::GJKCollide(o1, details::GJKInitializer::getSupportFunction(), details::GJKInitializer::getCenterFunction(), - o2, details::triGetSupportFunction(), details::triGetCenterFunction(), - max_collision_iterations, collision_tolerance, - contact_points, penetration_depth, normal); - - details::GJKInitializer::deleteGJKObject(o1); - details::triDeleteGJKObject(o2); - - return res; - } - - - /// @brief distance computation between two shapes - template - bool shapeDistance(const S1& s1, const Transform3d& tf1, - const S2& s2, const Transform3d& tf2, - FCL_REAL* dist = NULL, Vector3d* p1 = NULL, Vector3d* p2 = NULL) const - { - void* o1 = details::GJKInitializer::createGJKObject(s1, tf1); - void* o2 = details::GJKInitializer::createGJKObject(s2, tf2); - - bool res = details::GJKDistance(o1, details::GJKInitializer::getSupportFunction(), - o2, details::GJKInitializer::getSupportFunction(), - max_distance_iterations, distance_tolerance, - dist, p1, p2); - - if (p1) - *p1 = tf1.inverse() * *p1; - - if (p2) - *p2 = tf2.inverse() * *p2; - - details::GJKInitializer::deleteGJKObject(o1); - details::GJKInitializer::deleteGJKObject(o2); - - return res; - } - - - /// @brief distance computation between one shape and a triangle - template - bool shapeTriangleDistance(const S& s, const Transform3d& tf, - const Vector3d& P1, const Vector3d& P2, const Vector3d& P3, - FCL_REAL* dist = NULL, Vector3d* p1 = NULL, Vector3d* p2 = NULL) const - { - void* o1 = details::GJKInitializer::createGJKObject(s, tf); - void* o2 = details::triCreateGJKObject(P1, P2, P3); - - bool res = details::GJKDistance(o1, details::GJKInitializer::getSupportFunction(), - o2, details::triGetSupportFunction(), - max_distance_iterations, distance_tolerance, - dist, p1, p2); - if(p1) *p1 = tf.inverse() * *p1; - - details::GJKInitializer::deleteGJKObject(o1); - details::triDeleteGJKObject(o2); - - return res; - } - - /// @brief distance computation between one shape and a triangle with transformation - template - bool shapeTriangleDistance(const S& s, const Transform3d& tf1, - const Vector3d& P1, const Vector3d& P2, const Vector3d& P3, const Transform3d& tf2, - FCL_REAL* dist = NULL, Vector3d* p1 = NULL, Vector3d* p2 = NULL) const - { - void* o1 = details::GJKInitializer::createGJKObject(s, tf1); - void* o2 = details::triCreateGJKObject(P1, P2, P3, tf2); - - bool res = details::GJKDistance(o1, details::GJKInitializer::getSupportFunction(), - o2, details::triGetSupportFunction(), - max_distance_iterations, distance_tolerance, - dist, p1, p2); - if(p1) *p1 = tf1.inverse() * *p1; - if(p2) *p2 = tf2.inverse() * *p2; - - details::GJKInitializer::deleteGJKObject(o1); - details::triDeleteGJKObject(o2); - - return res; - } - - /// @brief default setting for GJK algorithm - GJKSolver_libccd() - { - max_collision_iterations = 500; - max_distance_iterations = 1000; - collision_tolerance = 1e-6; - distance_tolerance = 1e-6; - } - - - void enableCachedGuess(bool if_enable) const - { - // TODO: need change libccd to exploit spatial coherence - } - - void setCachedGuess(const Vector3d& guess) const - { - // TODO: need change libccd to exploit spatial coherence - } - - Vector3d getCachedGuess() const - { - return Vector3d(-1, 0, 0); - } - - - /// @brief maximum number of iterations used in GJK algorithm for collision - unsigned int max_collision_iterations; - - /// @brief maximum number of iterations used in GJK algorithm for distance - unsigned int max_distance_iterations; - - /// @brief the threshold used in GJK algorithm to stop collision iteration - FCL_REAL collision_tolerance; - - /// @brief the threshold used in GJK algorithm to stop distance iteration - FCL_REAL distance_tolerance; - - -}; - -template -bool GJKSolver_libccd::shapeIntersect(const S1& s1, const Transform3d& tf1, - const S2& s2, const Transform3d& tf2, - Vector3d* contact_points, FCL_REAL* penetration_depth, Vector3d* normal) const -{ - bool res; - - if (contact_points || penetration_depth || normal) - { - std::vector contacts; - - res = shapeIntersect(s1, tf1, s2, tf2, &contacts); - - if (!contacts.empty()) - { - // Get the deepest contact point - const ContactPoint& maxDepthContact = *std::max_element(contacts.begin(), contacts.end(), comparePenDepth); - - if (contact_points) - *contact_points = maxDepthContact.pos; - - if (penetration_depth) - *penetration_depth = maxDepthContact.penetration_depth; - - if (normal) - *normal = maxDepthContact.normal; - } - } - else - { - res = shapeIntersect(s1, tf1, s2, tf2, NULL); - } - - return res; -} - -/// @brief Fast implementation for sphere-capsule collision -template<> -bool GJKSolver_libccd::shapeIntersect(const Sphere& s1, const Transform3d& tf1, - const Capsule& s2, const Transform3d& tf2, - std::vector* contacts) const; - -template<> -bool GJKSolver_libccd::shapeIntersect(const Capsule &s1, const Transform3d& tf1, - const Sphere &s2, const Transform3d& tf2, - std::vector* contacts) const; - -/// @brief Fast implementation for sphere-sphere collision -template<> -bool GJKSolver_libccd::shapeIntersect(const Sphere& s1, const Transform3d& tf1, - const Sphere& s2, const Transform3d& tf2, - std::vector* contacts) const; - -/// @brief Fast implementation for box-box collision -template<> -bool GJKSolver_libccd::shapeIntersect(const Box& s1, const Transform3d& tf1, - const Box& s2, const Transform3d& tf2, - std::vector* contacts) const; - -template<> -bool GJKSolver_libccd::shapeIntersect(const Sphere& s1, const Transform3d& tf1, - const Halfspace& s2, const Transform3d& tf2, - std::vector* contacts) const; - -template<> -bool GJKSolver_libccd::shapeIntersect(const Halfspace& s1, const Transform3d& tf1, - const Sphere& s2, const Transform3d& tf2, - std::vector* contacts) const; - -template<> -bool GJKSolver_libccd::shapeIntersect(const Ellipsoid& s1, const Transform3d& tf1, - const Halfspace& s2, const Transform3d& tf2, - std::vector* contacts) const; - -template<> -bool GJKSolver_libccd::shapeIntersect(const Halfspace& s1, const Transform3d& tf1, - const Ellipsoid& s2, const Transform3d& tf2, - std::vector* contacts) const; - -template<> -bool GJKSolver_libccd::shapeIntersect(const Box& s1, const Transform3d& tf1, - const Halfspace& s2, const Transform3d& tf2, - std::vector* contacts) const; - -template<> -bool GJKSolver_libccd::shapeIntersect(const Halfspace& s1, const Transform3d& tf1, - const Box& s2, const Transform3d& tf2, - std::vector* contacts) const; - -template<> -bool GJKSolver_libccd::shapeIntersect(const Capsule& s1, const Transform3d& tf1, - const Halfspace& s2, const Transform3d& tf2, - std::vector* contacts) const; - -template<> -bool GJKSolver_libccd::shapeIntersect(const Halfspace& s1, const Transform3d& tf1, - const Capsule& s2, const Transform3d& tf2, - std::vector* contacts) const; - -template<> -bool GJKSolver_libccd::shapeIntersect(const Cylinder& s1, const Transform3d& tf1, - const Halfspace& s2, const Transform3d& tf2, - std::vector* contacts) const; - -template<> -bool GJKSolver_libccd::shapeIntersect(const Halfspace& s1, const Transform3d& tf1, - const Cylinder& s2, const Transform3d& tf2, - std::vector* contacts) const; - -template<> -bool GJKSolver_libccd::shapeIntersect(const Cone& s1, const Transform3d& tf1, - const Halfspace& s2, const Transform3d& tf2, - std::vector* contacts) const; - -template<> -bool GJKSolver_libccd::shapeIntersect(const Halfspace& s1, const Transform3d& tf1, - const Cone& s2, const Transform3d& tf2, - std::vector* contacts) const; - -template<> -bool GJKSolver_libccd::shapeIntersect(const Halfspace& s1, const Transform3d& tf1, - const Halfspace& s2, const Transform3d& tf2, - std::vector* contacts) const; - -template<> -bool GJKSolver_libccd::shapeIntersect(const Plane& s1, const Transform3d& tf1, - const Halfspace& s2, const Transform3d& tf2, - std::vector* contacts) const; - -template<> -bool GJKSolver_libccd::shapeIntersect(const Halfspace& s1, const Transform3d& tf1, - const Plane& s2, const Transform3d& tf2, - std::vector* contacts) const; - -template<> -bool GJKSolver_libccd::shapeIntersect(const Sphere& s1, const Transform3d& tf1, - const Plane& s2, const Transform3d& tf2, - std::vector* contacts) const; - -template<> -bool GJKSolver_libccd::shapeIntersect(const Plane& s1, const Transform3d& tf1, - const Sphere& s2, const Transform3d& tf2, - std::vector* contacts) const; - -template<> -bool GJKSolver_libccd::shapeIntersect(const Ellipsoid& s1, const Transform3d& tf1, - const Plane& s2, const Transform3d& tf2, - std::vector* contacts) const; - -template<> -bool GJKSolver_libccd::shapeIntersect(const Plane& s1, const Transform3d& tf1, - const Ellipsoid& s2, const Transform3d& tf2, - std::vector* contacts) const; - -template<> -bool GJKSolver_libccd::shapeIntersect(const Box& s1, const Transform3d& tf1, - const Plane& s2, const Transform3d& tf2, - std::vector* contacts) const; - -template<> -bool GJKSolver_libccd::shapeIntersect(const Plane& s1, const Transform3d& tf1, - const Box& s2, const Transform3d& tf2, - std::vector* contacts) const; - -template<> -bool GJKSolver_libccd::shapeIntersect(const Capsule& s1, const Transform3d& tf1, - const Plane& s2, const Transform3d& tf2, - std::vector* contacts) const; - -template<> -bool GJKSolver_libccd::shapeIntersect(const Plane& s1, const Transform3d& tf1, - const Capsule& s2, const Transform3d& tf2, - std::vector* contacts) const; - -template<> -bool GJKSolver_libccd::shapeIntersect(const Cylinder& s1, const Transform3d& tf1, - const Plane& s2, const Transform3d& tf2, - std::vector* contacts) const; - -template<> -bool GJKSolver_libccd::shapeIntersect(const Plane& s1, const Transform3d& tf1, - const Cylinder& s2, const Transform3d& tf2, - std::vector* contacts) const; - -template<> -bool GJKSolver_libccd::shapeIntersect(const Cone& s1, const Transform3d& tf1, - const Plane& s2, const Transform3d& tf2, - std::vector* contacts) const; - -template<> -bool GJKSolver_libccd::shapeIntersect(const Plane& s1, const Transform3d& tf1, - const Cone& s2, const Transform3d& tf2, - std::vector* contacts) const; - -template<> -bool GJKSolver_libccd::shapeIntersect(const Plane& s1, const Transform3d& tf1, - const Plane& s2, const Transform3d& tf2, - std::vector* contacts) const; - -/// @brief Fast implementation for sphere-triangle collision -template<> -bool GJKSolver_libccd::shapeTriangleIntersect(const Sphere& s, const Transform3d& tf, - const Vector3d& P1, const Vector3d& P2, const Vector3d& P3, Vector3d* contact_points, FCL_REAL* penetration_depth, Vector3d* normal) const; - -/// @brief Fast implementation for sphere-triangle collision -template<> -bool GJKSolver_libccd::shapeTriangleIntersect(const Sphere& s, const Transform3d& tf1, - const Vector3d& P1, const Vector3d& P2, const Vector3d& P3, const Transform3d& tf2, Vector3d* contact_points, FCL_REAL* penetration_depth, Vector3d* normal) const; - - -template<> -bool GJKSolver_libccd::shapeTriangleIntersect(const Halfspace& s, const Transform3d& tf1, - const Vector3d& P1, const Vector3d& P2, const Vector3d& P3, const Transform3d& tf2, Vector3d* contact_points, FCL_REAL* penetration_depth, Vector3d* normal) const; - -template<> -bool GJKSolver_libccd::shapeTriangleIntersect(const Plane& s, const Transform3d& tf1, - const Vector3d& P1, const Vector3d& P2, const Vector3d& P3, const Transform3d& tf2, Vector3d* contact_points, FCL_REAL* penetration_depth, Vector3d* normal) const; - -/// @brief Fast implementation for sphere-capsule distance -template<> -bool GJKSolver_libccd::shapeDistance(const Sphere& s1, const Transform3d& tf1, - const Capsule& s2, const Transform3d& tf2, - FCL_REAL* dist, Vector3d* p1, Vector3d* p2) const; - -template<> -bool GJKSolver_libccd::shapeDistance(const Capsule& s1, const Transform3d& tf1, - const Sphere& s2, const Transform3d& tf2, - FCL_REAL* dist, Vector3d* p1, Vector3d* p2) const; - -/// @brief Fast implementation for sphere-sphere distance -template<> -bool GJKSolver_libccd::shapeDistance(const Sphere& s1, const Transform3d& tf1, - const Sphere& s2, const Transform3d& tf2, - FCL_REAL* dist, Vector3d* p1, Vector3d* p2) const; - -// @brief Computation of the distance result for capsule capsule. Closest points are based on two line-segments. -template<> -bool GJKSolver_libccd::shapeDistance(const Capsule& s1, const Transform3d& tf1, - const Capsule& s2, const Transform3d& tf2, - FCL_REAL* dist, Vector3d* p1, Vector3d* p2) const; - -/// @brief Fast implementation for sphere-triangle distance -template<> -bool GJKSolver_libccd::shapeTriangleDistance(const Sphere& s, const Transform3d& tf, - const Vector3d& P1, const Vector3d& P2, const Vector3d& P3, - FCL_REAL* dist, Vector3d* p1, Vector3d* p2) const; - -/// @brief Fast implementation for sphere-triangle distance -template<> -bool GJKSolver_libccd::shapeTriangleDistance(const Sphere& s, const Transform3d& tf1, - const Vector3d& P1, const Vector3d& P2, const Vector3d& P3, const Transform3d& tf2, - FCL_REAL* dist, Vector3d* p1, Vector3d* p2) const; - -/// @brief collision and distance solver based on GJK algorithm implemented in fcl (rewritten the code from the GJK in bullet) -struct GJKSolver_indep -{ - /// @brief intersection checking between two shapes - /// @deprecated use shapeIntersect(const S1&, const Transform3d&, const S2&, const Transform3d&, std::vector*) const - template - FCL_DEPRECATED - bool shapeIntersect(const S1& s1, const Transform3d& tf1, - const S2& s2, const Transform3d& tf2, - Vector3d* contact_points, FCL_REAL* penetration_depth, Vector3d* normal) const; - - /// @brief intersection checking between two shapes - template - bool shapeIntersect(const S1& s1, const Transform3d& tf1, - const S2& s2, const Transform3d& tf2, - std::vector* contacts = NULL) const - { - Vector3d guess(1, 0, 0); - if(enable_cached_guess) guess = cached_guess; - - details::MinkowskiDiff shape; - shape.shapes[0] = &s1; - shape.shapes[1] = &s2; - shape.toshape1 = tf2.linear().transpose() * tf1.linear(); - shape.toshape0 = tf1.inverse() * tf2; - - details::GJK gjk(gjk_max_iterations, gjk_tolerance); - details::GJK::Status gjk_status = gjk.evaluate(shape, -guess); - if(enable_cached_guess) cached_guess = gjk.getGuessFromSimplex(); - - switch(gjk_status) - { - case details::GJK::Inside: - { - details::EPA epa(epa_max_face_num, epa_max_vertex_num, epa_max_iterations, epa_tolerance); - details::EPA::Status epa_status = epa.evaluate(gjk, -guess); - if(epa_status != details::EPA::Failed) - { - Vector3d w0 = Vector3d::Zero(); - for(size_t i = 0; i < epa.result.rank; ++i) - { - w0 += shape.support(epa.result.c[i]->d, 0) * epa.result.p[i]; - } - if(contacts) - { - Vector3d normal = epa.normal; - Vector3d point = tf1 * (w0 - epa.normal*(epa.depth *0.5)); - FCL_REAL depth = -epa.depth; - contacts->push_back(ContactPoint(normal, point, depth)); - } - return true; - } - else return false; - } - break; - default: - ; - } - - return false; - } - - /// @brief intersection checking between one shape and a triangle - template - bool shapeTriangleIntersect(const S& s, const Transform3d& tf, - const Vector3d& P1, const Vector3d& P2, const Vector3d& P3, - Vector3d* contact_points = NULL, FCL_REAL* penetration_depth = NULL, Vector3d* normal = NULL) const - { - TriangleP tri(P1, P2, P3); - - Vector3d guess(1, 0, 0); - if(enable_cached_guess) guess = cached_guess; - - details::MinkowskiDiff shape; - shape.shapes[0] = &s; - shape.shapes[1] = &tri; - shape.toshape1 = tf.linear(); - shape.toshape0 = tf.inverse(); - - details::GJK gjk(gjk_max_iterations, gjk_tolerance); - details::GJK::Status gjk_status = gjk.evaluate(shape, -guess); - if(enable_cached_guess) cached_guess = gjk.getGuessFromSimplex(); - - switch(gjk_status) - { - case details::GJK::Inside: - { - details::EPA epa(epa_max_face_num, epa_max_vertex_num, epa_max_iterations, epa_tolerance); - details::EPA::Status epa_status = epa.evaluate(gjk, -guess); - if(epa_status != details::EPA::Failed) - { - Vector3d w0 = Vector3d::Zero(); - for(size_t i = 0; i < epa.result.rank; ++i) - { - w0 += shape.support(epa.result.c[i]->d, 0) * epa.result.p[i]; - } - if(penetration_depth) *penetration_depth = -epa.depth; - if(normal) *normal = -epa.normal; - if(contact_points) *contact_points = tf * (w0 - epa.normal*(epa.depth *0.5)); - return true; - } - else return false; - } - break; - default: - ; - } - - return false; - } - - //// @brief intersection checking between one shape and a triangle with transformation - template - bool shapeTriangleIntersect(const S& s, const Transform3d& tf1, - const Vector3d& P1, const Vector3d& P2, const Vector3d& P3, const Transform3d& tf2, - Vector3d* contact_points = NULL, FCL_REAL* penetration_depth = NULL, Vector3d* normal = NULL) const - { - TriangleP tri(P1, P2, P3); - - Vector3d guess(1, 0, 0); - if(enable_cached_guess) guess = cached_guess; - - details::MinkowskiDiff shape; - shape.shapes[0] = &s; - shape.shapes[1] = &tri; - shape.toshape1 = tf2.linear().transpose() * tf1.linear(); - shape.toshape0 = tf1.inverse() * tf2; - - details::GJK gjk(gjk_max_iterations, gjk_tolerance); - details::GJK::Status gjk_status = gjk.evaluate(shape, -guess); - if(enable_cached_guess) cached_guess = gjk.getGuessFromSimplex(); - - switch(gjk_status) - { - case details::GJK::Inside: - { - details::EPA epa(epa_max_face_num, epa_max_vertex_num, epa_max_iterations, epa_tolerance); - details::EPA::Status epa_status = epa.evaluate(gjk, -guess); - if(epa_status != details::EPA::Failed) - { - Vector3d w0 = Vector3d::Zero(); - for(size_t i = 0; i < epa.result.rank; ++i) - { - w0 += shape.support(epa.result.c[i]->d, 0) * epa.result.p[i]; - } - if(penetration_depth) *penetration_depth = -epa.depth; - if(normal) *normal = -epa.normal; - if(contact_points) *contact_points = tf1 * (w0 - epa.normal*(epa.depth *0.5)); - return true; - } - else return false; - } - break; - default: - ; - } - - return false; - } - - /// @brief distance computation between two shapes - template - bool shapeDistance(const S1& s1, const Transform3d& tf1, - const S2& s2, const Transform3d& tf2, - FCL_REAL* distance = NULL, Vector3d* p1 = NULL, Vector3d* p2 = NULL) const - { - Vector3d guess(1, 0, 0); - if(enable_cached_guess) guess = cached_guess; - - details::MinkowskiDiff shape; - shape.shapes[0] = &s1; - shape.shapes[1] = &s2; - shape.toshape1 = tf2.linear().transpose() * tf1.linear(); - shape.toshape0 = tf1.inverse() * tf2; - - details::GJK gjk(gjk_max_iterations, gjk_tolerance); - details::GJK::Status gjk_status = gjk.evaluate(shape, -guess); - if(enable_cached_guess) cached_guess = gjk.getGuessFromSimplex(); - - if(gjk_status == details::GJK::Valid) - { - Vector3d w0 = Vector3d::Zero(); - Vector3d w1 = Vector3d::Zero(); - for(size_t i = 0; i < gjk.getSimplex()->rank; ++i) - { - FCL_REAL p = gjk.getSimplex()->p[i]; - w0 += shape.support(gjk.getSimplex()->c[i]->d, 0) * p; - w1 += shape.support(-gjk.getSimplex()->c[i]->d, 1) * p; - } - - if(distance) *distance = (w0 - w1).norm(); - - if(p1) *p1 = w0; - if(p2) *p2 = shape.toshape0 * w1; - - return true; - } - else - { - if(distance) *distance = -1; - return false; - } - } - - /// @brief distance computation between one shape and a triangle - template - bool shapeTriangleDistance(const S& s, const Transform3d& tf, - const Vector3d& P1, const Vector3d& P2, const Vector3d& P3, - FCL_REAL* distance = NULL, Vector3d* p1 = NULL, Vector3d* p2 = NULL) const - { - TriangleP tri(P1, P2, P3); - Vector3d guess(1, 0, 0); - if(enable_cached_guess) guess = cached_guess; - - details::MinkowskiDiff shape; - shape.shapes[0] = &s; - shape.shapes[1] = &tri; - shape.toshape1 = tf.linear(); - shape.toshape0 = tf.inverse(); - - details::GJK gjk(gjk_max_iterations, gjk_tolerance); - details::GJK::Status gjk_status = gjk.evaluate(shape, -guess); - if(enable_cached_guess) cached_guess = gjk.getGuessFromSimplex(); - - if(gjk_status == details::GJK::Valid) - { - Vector3d w0 = Vector3d::Zero(); - Vector3d w1 = Vector3d::Zero(); - for(size_t i = 0; i < gjk.getSimplex()->rank; ++i) - { - FCL_REAL p = gjk.getSimplex()->p[i]; - w0 += shape.support(gjk.getSimplex()->c[i]->d, 0) * p; - w1 += shape.support(-gjk.getSimplex()->c[i]->d, 1) * p; - } - - if(distance) *distance = (w0 - w1).norm(); - if(p1) *p1 = w0; - if(p2) *p2 = shape.toshape0 * w1; - return true; - } - else - { - if(distance) *distance = -1; - return false; - } - } - - /// @brief distance computation between one shape and a triangle with transformation - template - bool shapeTriangleDistance(const S& s, const Transform3d& tf1, - const Vector3d& P1, const Vector3d& P2, const Vector3d& P3, const Transform3d& tf2, - FCL_REAL* distance = NULL, Vector3d* p1 = NULL, Vector3d* p2 = NULL) const - { - TriangleP tri(P1, P2, P3); - Vector3d guess(1, 0, 0); - if(enable_cached_guess) guess = cached_guess; - - details::MinkowskiDiff shape; - shape.shapes[0] = &s; - shape.shapes[1] = &tri; - shape.toshape1 = tf2.linear().transpose() * tf1.linear(); - shape.toshape0 = tf1.inverse() * tf2; - - details::GJK gjk(gjk_max_iterations, gjk_tolerance); - details::GJK::Status gjk_status = gjk.evaluate(shape, -guess); - if(enable_cached_guess) cached_guess = gjk.getGuessFromSimplex(); - - if(gjk_status == details::GJK::Valid) - { - Vector3d w0 = Vector3d::Zero(); - Vector3d w1 = Vector3d::Zero(); - for(size_t i = 0; i < gjk.getSimplex()->rank; ++i) - { - FCL_REAL p = gjk.getSimplex()->p[i]; - w0 += shape.support(gjk.getSimplex()->c[i]->d, 0) * p; - w1 += shape.support(-gjk.getSimplex()->c[i]->d, 1) * p; - } - - if(distance) *distance = (w0 - w1).norm(); - if(p1) *p1 = w0; - if(p2) *p2 = shape.toshape0 * w1; - return true; - } - else - { - if(distance) *distance = -1; - return false; - } - } - - /// @brief default setting for GJK algorithm - GJKSolver_indep() - { - gjk_max_iterations = 128; - gjk_tolerance = 1e-6; - epa_max_face_num = 128; - epa_max_vertex_num = 64; - epa_max_iterations = 255; - epa_tolerance = 1e-6; - enable_cached_guess = false; - cached_guess = Vector3d(1, 0, 0); - } - - void enableCachedGuess(bool if_enable) const - { - enable_cached_guess = if_enable; - } - - void setCachedGuess(const Vector3d& guess) const - { - cached_guess = guess; - } - - Vector3d getCachedGuess() const - { - return cached_guess; - } - - /// @brief maximum number of simplex face used in EPA algorithm - unsigned int epa_max_face_num; - - /// @brief maximum number of simplex vertex used in EPA algorithm - unsigned int epa_max_vertex_num; - - /// @brief maximum number of iterations used for EPA iterations - unsigned int epa_max_iterations; - - /// @brief the threshold used in EPA to stop iteration - FCL_REAL epa_tolerance; - - /// @brief the threshold used in GJK to stop iteration - FCL_REAL gjk_tolerance; - - /// @brief maximum number of iterations used for GJK iterations - FCL_REAL gjk_max_iterations; - - /// @brief Whether smart guess can be provided - mutable bool enable_cached_guess; - - /// @brief smart guess - mutable Vector3d cached_guess; -}; - -template -bool GJKSolver_indep::shapeIntersect(const S1& s1, const Transform3d& tf1, - const S2& s2, const Transform3d& tf2, - Vector3d* contact_points, FCL_REAL* penetration_depth, Vector3d* normal) const -{ - bool res; - - if (contact_points || penetration_depth || normal) - { - std::vector contacts; - - res = shapeIntersect(s1, tf1, s2, tf2, &contacts); - - if (!contacts.empty()) - { - // Get the deepest contact point - const ContactPoint& maxDepthContact = *std::max_element(contacts.begin(), contacts.end(), comparePenDepth); - - if (contact_points) - *contact_points = maxDepthContact.pos; - - if (penetration_depth) - *penetration_depth = maxDepthContact.penetration_depth; - - if (normal) - *normal = maxDepthContact.normal; - } - } - else - { - res = shapeIntersect(s1, tf1, s2, tf2, NULL); - } - - return res; -} - -/// @brief Fast implementation for sphere-capsule collision -template<> -bool GJKSolver_indep::shapeIntersect(const Sphere &s1, const Transform3d& tf1, - const Capsule &s2, const Transform3d& tf2, - std::vector* contacts) const; - -template<> -bool GJKSolver_indep::shapeIntersect(const Capsule &s1, const Transform3d& tf1, - const Sphere &s2, const Transform3d& tf2, - std::vector* contacts) const; - -/// @brief Fast implementation for sphere-sphere collision -template<> -bool GJKSolver_indep::shapeIntersect(const Sphere& s1, const Transform3d& tf1, - const Sphere& s2, const Transform3d& tf2, - std::vector* contacts) const; - -/// @brief Fast implementation for box-box collision -template<> -bool GJKSolver_indep::shapeIntersect(const Box& s1, const Transform3d& tf1, - const Box& s2, const Transform3d& tf2, - std::vector* contacts) const; - -template<> -bool GJKSolver_indep::shapeIntersect(const Sphere& s1, const Transform3d& tf1, - const Halfspace& s2, const Transform3d& tf2, - std::vector* contacts) const; - -template<> -bool GJKSolver_indep::shapeIntersect(const Halfspace& s1, const Transform3d& tf1, - const Sphere& s2, const Transform3d& tf2, - std::vector* contacts) const; - -template<> -bool GJKSolver_indep::shapeIntersect(const Ellipsoid& s1, const Transform3d& tf1, - const Halfspace& s2, const Transform3d& tf2, - std::vector* contacts) const; - -template<> -bool GJKSolver_indep::shapeIntersect(const Halfspace& s1, const Transform3d& tf1, - const Ellipsoid& s2, const Transform3d& tf2, - std::vector* contacts) const; - -template<> -bool GJKSolver_indep::shapeIntersect(const Box& s1, const Transform3d& tf1, - const Halfspace& s2, const Transform3d& tf2, - std::vector* contacts) const; - -template<> -bool GJKSolver_indep::shapeIntersect(const Halfspace& s1, const Transform3d& tf1, - const Box& s2, const Transform3d& tf2, - std::vector* contacts) const; - -template<> -bool GJKSolver_indep::shapeIntersect(const Capsule& s1, const Transform3d& tf1, - const Halfspace& s2, const Transform3d& tf2, - std::vector* contacts) const; - -template<> -bool GJKSolver_indep::shapeIntersect(const Halfspace& s1, const Transform3d& tf1, - const Capsule& s2, const Transform3d& tf2, - std::vector* contacts) const; - -template<> -bool GJKSolver_indep::shapeIntersect(const Cylinder& s1, const Transform3d& tf1, - const Halfspace& s2, const Transform3d& tf2, - std::vector* contacts) const; - -template<> -bool GJKSolver_indep::shapeIntersect(const Halfspace& s1, const Transform3d& tf1, - const Cylinder& s2, const Transform3d& tf2, - std::vector* contacts) const; - -template<> -bool GJKSolver_indep::shapeIntersect(const Cone& s1, const Transform3d& tf1, - const Halfspace& s2, const Transform3d& tf2, - std::vector* contacts) const; - -template<> -bool GJKSolver_indep::shapeIntersect(const Halfspace& s1, const Transform3d& tf1, - const Cone& s2, const Transform3d& tf2, - std::vector* contacts) const; - -template<> -bool GJKSolver_indep::shapeIntersect(const Halfspace& s1, const Transform3d& tf1, - const Halfspace& s2, const Transform3d& tf2, - std::vector* contacts) const; - -template<> -bool GJKSolver_indep::shapeIntersect(const Plane& s1, const Transform3d& tf1, - const Halfspace& s2, const Transform3d& tf2, - std::vector* contacts) const; - -template<> -bool GJKSolver_indep::shapeIntersect(const Halfspace& s1, const Transform3d& tf1, - const Plane& s2, const Transform3d& tf2, - std::vector* contacts) const; - -template<> -bool GJKSolver_indep::shapeIntersect(const Sphere& s1, const Transform3d& tf1, - const Plane& s2, const Transform3d& tf2, - std::vector* contacts) const; - -template<> -bool GJKSolver_indep::shapeIntersect(const Plane& s1, const Transform3d& tf1, - const Sphere& s2, const Transform3d& tf2, - std::vector* contacts) const; - -template<> -bool GJKSolver_indep::shapeIntersect(const Ellipsoid& s1, const Transform3d& tf1, - const Plane& s2, const Transform3d& tf2, - std::vector* contacts) const; - -template<> -bool GJKSolver_indep::shapeIntersect(const Plane& s1, const Transform3d& tf1, - const Ellipsoid& s2, const Transform3d& tf2, - std::vector* contacts) const; - -template<> -bool GJKSolver_indep::shapeIntersect(const Box& s1, const Transform3d& tf1, - const Plane& s2, const Transform3d& tf2, - std::vector* contacts) const; - -template<> -bool GJKSolver_indep::shapeIntersect(const Plane& s1, const Transform3d& tf1, - const Box& s2, const Transform3d& tf2, - std::vector* contacts) const; - -template<> -bool GJKSolver_indep::shapeIntersect(const Capsule& s1, const Transform3d& tf1, - const Plane& s2, const Transform3d& tf2, - std::vector* contacts) const; - -template<> -bool GJKSolver_indep::shapeIntersect(const Plane& s1, const Transform3d& tf1, - const Capsule& s2, const Transform3d& tf2, - std::vector* contacts) const; - -template<> -bool GJKSolver_indep::shapeIntersect(const Cylinder& s1, const Transform3d& tf1, - const Plane& s2, const Transform3d& tf2, - std::vector* contacts) const; - -template<> -bool GJKSolver_indep::shapeIntersect(const Plane& s1, const Transform3d& tf1, - const Cylinder& s2, const Transform3d& tf2, - std::vector* contacts) const; - -template<> -bool GJKSolver_indep::shapeIntersect(const Cone& s1, const Transform3d& tf1, - const Plane& s2, const Transform3d& tf2, - std::vector* contacts) const; - -template<> -bool GJKSolver_indep::shapeIntersect(const Plane& s1, const Transform3d& tf1, - const Cone& s2, const Transform3d& tf2, - std::vector* contacts) const; - -template<> -bool GJKSolver_indep::shapeIntersect(const Plane& s1, const Transform3d& tf1, - const Plane& s2, const Transform3d& tf2, - std::vector* contacts) const; - -/// @brief Fast implementation for sphere-triangle collision -template<> -bool GJKSolver_indep::shapeTriangleIntersect(const Sphere& s, const Transform3d& tf, - const Vector3d& P1, const Vector3d& P2, const Vector3d& P3, Vector3d* contact_points, FCL_REAL* penetration_depth, Vector3d* normal) const; - -/// @brief Fast implementation for sphere-triangle collision -template<> -bool GJKSolver_indep::shapeTriangleIntersect(const Sphere& s, const Transform3d& tf1, - const Vector3d& P1, const Vector3d& P2, const Vector3d& P3, const Transform3d& tf2, Vector3d* contact_points, FCL_REAL* penetration_depth, Vector3d* normal) const; - - -template<> -bool GJKSolver_indep::shapeTriangleIntersect(const Halfspace& s, const Transform3d& tf1, - const Vector3d& P1, const Vector3d& P2, const Vector3d& P3, const Transform3d& tf2, Vector3d* contact_points, FCL_REAL* penetration_depth, Vector3d* normal) const; - -template<> -bool GJKSolver_indep::shapeTriangleIntersect(const Plane& s, const Transform3d& tf1, - const Vector3d& P1, const Vector3d& P2, const Vector3d& P3, const Transform3d& tf2, Vector3d* contact_points, FCL_REAL* penetration_depth, Vector3d* normal) const; - -/// @brief Fast implementation for sphere-capsule distance -template<> -bool GJKSolver_indep::shapeDistance(const Sphere& s1, const Transform3d& tf1, - const Capsule& s2, const Transform3d& tf2, - FCL_REAL* dist, Vector3d* p1, Vector3d* p2) const; - -template<> -bool GJKSolver_indep::shapeDistance(const Capsule& s1, const Transform3d& tf1, - const Sphere& s2, const Transform3d& tf2, - FCL_REAL* dist, Vector3d* p1, Vector3d* p2) const; - -/// @brief Fast implementation for sphere-sphere distance -template<> -bool GJKSolver_indep::shapeDistance(const Sphere& s1, const Transform3d& tf1, - const Sphere& s2, const Transform3d& tf2, - FCL_REAL* dist, Vector3d* p1, Vector3d* p2) const; - -// @brief Computation of the distance result for capsule capsule. Closest points are based on two line-segments. -template<> -bool GJKSolver_indep::shapeDistance(const Capsule& s1, const Transform3d& tf1, - const Capsule& s2, const Transform3d& tf2, - FCL_REAL* dist, Vector3d* p1, Vector3d* p2) const; - -/// @brief Fast implementation for sphere-triangle distance -template<> -bool GJKSolver_indep::shapeTriangleDistance(const Sphere& s, const Transform3d& tf, - const Vector3d& P1, const Vector3d& P2, const Vector3d& P3, - FCL_REAL* dist, Vector3d* p1, Vector3d* p2) const; - -/// @brief Fast implementation for sphere-triangle distance -template<> -bool GJKSolver_indep::shapeTriangleDistance(const Sphere& s, const Transform3d& tf1, - const Vector3d& P1, const Vector3d& P2, const Vector3d& P3, const Transform3d& tf2, - FCL_REAL* dist, Vector3d* p1, Vector3d* p2) const; - -} +#error "This header has been deprecated in FCL 0.6. " #endif diff --git a/include/fcl/octree.h b/include/fcl/octree.h index 4e5d3386b..3e149d0a6 100644 --- a/include/fcl/octree.h +++ b/include/fcl/octree.h @@ -54,23 +54,25 @@ namespace fcl { -/// @brief Octree is one type of collision geometry which can encode uncertainty information in the sensor data. -class OcTree : public CollisionGeometry +/// @brief Octree is one type of collision geometry which can encode uncertainty +/// information in the sensor data. +template +class OcTree : public CollisionGeometry { private: std::shared_ptr tree; - FCL_REAL default_occupancy; + S default_occupancy; - FCL_REAL occupancy_threshold; - FCL_REAL free_threshold; + S occupancy_threshold; + S free_threshold; public: typedef octomap::OcTreeNode OcTreeNode; /// @brief construct octree with a given resolution - OcTree(FCL_REAL resolution) : tree(std::shared_ptr(new octomap::OcTree(resolution))) + OcTree(S resolution) : tree(std::shared_ptr(new octomap::OcTree(resolution))) { default_occupancy = tree->getOccupancyThres(); @@ -89,21 +91,21 @@ class OcTree : public CollisionGeometry free_threshold = 0; } - /// @brief compute the AABB for the octree in its local coordinate system + /// @brief compute the AABB for the octree in its local coordinate system void computeLocalAABB() { - aabb_local = getRootBV(); - aabb_center = aabb_local.center(); - aabb_radius = (aabb_local.min_ - aabb_center).norm(); + this->aabb_local = getRootBV(); + this->aabb_center = this->aabb_local.center(); + this->aabb_radius = (this->aabb_local.min_ - this->aabb_center).norm(); } /// @brief get the bounding volume for the root - AABB getRootBV() const + AABB getRootBV() const { - FCL_REAL delta = (1 << tree->getTreeDepth()) * tree->getResolution() / 2; + S delta = (1 << tree->getTreeDepth()) * tree->getResolution() / 2; // std::cout << "octree size " << delta << std::endl; - return AABB(Vector3d(-delta, -delta, -delta), Vector3d(delta, delta, delta)); + return AABB(Vector3(-delta, -delta, -delta), Vector3(delta, delta, delta)); } /// @brief get the root node of the octree @@ -134,9 +136,9 @@ class OcTree : public CollisionGeometry /// @brief transform the octree into a bunch of boxes; uncertainty information is kept in the boxes. However, we /// only keep the occupied boxes (i.e., the boxes whose occupied probability is higher enough). - std::vector > toBoxes() const + std::vector > toBoxes() const { - std::vector > boxes; + std::vector > boxes; boxes.reserve(tree->size() / 2); for(octomap::OcTree::iterator it = tree->begin(tree->getTreeDepth()), end = tree->end(); it != end; @@ -145,14 +147,14 @@ class OcTree : public CollisionGeometry // if(tree->isNodeOccupied(*it)) if(isNodeOccupied(&*it)) { - FCL_REAL size = it.getSize(); - FCL_REAL x = it.getX(); - FCL_REAL y = it.getY(); - FCL_REAL z = it.getZ(); - FCL_REAL c = (*it).getOccupancy(); - FCL_REAL t = tree->getOccupancyThres(); - - std::array box = {{x, y, z, size, c, t}}; + S size = it.getSize(); + S x = it.getX(); + S y = it.getY(); + S z = it.getZ(); + S c = (*it).getOccupancy(); + S t = tree->getOccupancyThres(); + + std::array box = {{x, y, z, size, c, t}}; boxes.push_back(box); } } @@ -160,33 +162,33 @@ class OcTree : public CollisionGeometry } /// @brief the threshold used to decide whether one node is occupied, this is NOT the octree occupied_thresold - FCL_REAL getOccupancyThres() const + S getOccupancyThres() const { return occupancy_threshold; } /// @brief the threshold used to decide whether one node is occupied, this is NOT the octree free_threshold - FCL_REAL getFreeThres() const + S getFreeThres() const { return free_threshold; } - FCL_REAL getDefaultOccupancy() const + S getDefaultOccupancy() const { return default_occupancy; } - void setCellDefaultOccupancy(FCL_REAL d) + void setCellDefaultOccupancy(S d) { default_occupancy = d; } - void setOccupancyThres(FCL_REAL d) + void setOccupancyThres(S d) { occupancy_threshold = d; } - void setFreeThres(FCL_REAL d) + void setFreeThres(S d) { free_threshold = d; } @@ -239,7 +241,8 @@ class OcTree : public CollisionGeometry }; /// @brief compute the bounding volume of an octree node's i-th child -static inline void computeChildBV(const AABB& root_bv, unsigned int i, AABB& child_bv) +template +void computeChildBV(const AABB& root_bv, unsigned int i, AABB& child_bv) { if(i&1) { @@ -275,8 +278,9 @@ static inline void computeChildBV(const AABB& root_bv, unsigned int i, AABB& chi } } +using OcTreef = OcTree; +using OcTreed = OcTree; - -} +} // namespace fcl #endif diff --git a/include/fcl/profile.h b/include/fcl/profile.h deleted file mode 100644 index c9a08d1d8..000000000 --- a/include/fcl/profile.h +++ /dev/null @@ -1,479 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2008-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - - -/* Author Ioan Sucan */ - -#ifndef FCL_UTIL_PROFILER -#define FCL_UTIL_PROFILER - -#define ENABLE_PROFILING 1 - -#ifndef ENABLE_PROFILING - -/// The ENABLE_PROFILING macro can be set externally. If it is not, profiling is enabled by default, unless NDEBUG is defined. - -# ifdef NDEBUG -# define ENABLE_PROFILING 0 -# else -# define ENABLE_PROFILING 1 -# endif - -#endif - -#if ENABLE_PROFILING - -#include -#include -#include -#include -#include -#include - -namespace fcl -{ - -/// @brief Namespace containing time datatypes and time operations -namespace time -{ - -/// @brief Representation of a point in time -typedef std::chrono::system_clock::time_point point; - -/// @brief Representation of a time duration -typedef std::chrono::system_clock::duration duration; - -/// @brief Get the current time point -inline point now(void) -{ - return std::chrono::system_clock::now(); -} - -/// @brief Return the time duration representing a given number of seconds -inline duration seconds(double sec) -{ - long s = (long)sec; - long us = (long)((sec - s) * 1000000); - return std::chrono::seconds(s) + std::chrono::microseconds(us); -} - -/// @brief Return the number of seconds that a time duration represents -inline double seconds(const duration &d) -{ - return std::chrono::duration(d).count(); -} - -} - -namespace tools -{ - -/// @brief This is a simple thread-safe tool for counting time -/// spent in various chunks of code. This is different from -/// external profiling tools in that it allows the user to count -/// time spent in various bits of code (sub-function granularity) -/// or count how many times certain pieces of code are executed. -class Profiler -{ -public: - // non-copyable - Profiler(const Profiler&) = delete; - Profiler& operator=(const Profiler&) = delete; - - /// @brief This instance will call Profiler::begin() when constructed and Profiler::end() when it goes out of scope. - class ScopedBlock - { - public: - /// @brief Start counting time for the block named \e name of the profiler \e prof - ScopedBlock(const std::string &name, Profiler &prof = Profiler::Instance()) : name_(name), prof_(prof) - { - prof_.begin(name); - } - - ~ScopedBlock(void) - { - prof_.end(name_); - } - - private: - - std::string name_; - Profiler &prof_; - }; - - /// @brief This instance will call Profiler::start() when constructed and Profiler::stop() when it goes out of scope. - /// If the profiler was already started, this block's constructor and destructor take no action - class ScopedStart - { - public: - - /// @brief Take as argument the profiler instance to operate on (\e prof) - ScopedStart(Profiler &prof = Profiler::Instance()) : prof_(prof), wasRunning_(prof_.running()) - { - if (!wasRunning_) - prof_.start(); - } - - ~ScopedStart(void) - { - if (!wasRunning_) - prof_.stop(); - } - - private: - - Profiler &prof_; - bool wasRunning_; - }; - - /// @brief Return an instance of the class - static Profiler& Instance(void); - - /// @brief Constructor. It is allowed to separately instantiate this - /// class (not only as a singleton) - Profiler(bool printOnDestroy = false, bool autoStart = false) : running_(false), printOnDestroy_(printOnDestroy) - { - if (autoStart) - start(); - } - - /// @brief Destructor - ~Profiler(void) - { - if (printOnDestroy_ && !data_.empty()) - status(); - } - - /// @brief Start counting time - static void Start(void) - { - Instance().start(); - } - - /// @brief Stop counting time - static void Stop(void) - { - Instance().stop(); - } - - /// @brief Clear counted time and events - static void Clear(void) - { - Instance().clear(); - } - - /// @brief Start counting time - void start(void); - - /// @brief Stop counting time - void stop(void); - - /// @brief Clear counted time and events - void clear(void); - - /// @brief Count a specific event for a number of times - static void Event(const std::string& name, const unsigned int times = 1) - { - Instance().event(name, times); - } - - /// @brief Count a specific event for a number of times - void event(const std::string &name, const unsigned int times = 1); - - /// @brief Maintain the average of a specific value - static void Average(const std::string& name, const double value) - { - Instance().average(name, value); - } - - /// @brief Maintain the average of a specific value - void average(const std::string &name, const double value); - - /// @brief Begin counting time for a specific chunk of code - static void Begin(const std::string &name) - { - Instance().begin(name); - } - - /// @brief Stop counting time for a specific chunk of code - static void End(const std::string &name) - { - Instance().end(name); - } - - /// @brief Begin counting time for a specific chunk of code - void begin(const std::string &name); - - /// @brief Stop counting time for a specific chunk of code - void end(const std::string &name); - - /// @brief Print the status of the profiled code chunks and - /// events. Optionally, computation done by different threads - /// can be printed separately. - static void Status(std::ostream &out = std::cout, bool merge = true) - { - Instance().status(out, merge); - } - - /// @brief Print the status of the profiled code chunks and - /// events. Optionally, computation done by different threads - /// can be printed separately. - void status(std::ostream &out = std::cout, bool merge = true); - - /// @brief Check if the profiler is counting time or not - bool running(void) const - { - return running_; - } - - /// @brief Check if the profiler is counting time or not - static bool Running(void) - { - return Instance().running(); - } - -private: - - /// @brief Information about time spent in a section of the code - struct TimeInfo - { - TimeInfo(void) : total(time::seconds(0.)), shortest(time::duration::max()), longest(time::duration::min()), parts(0) - { - } - - /// @brief Total time counted. - time::duration total; - - /// @brief The shortest counted time interval - time::duration shortest; - - /// @brief The longest counted time interval - time::duration longest; - - /// @brief Number of times a chunk of time was added to this structure - unsigned long int parts; - - /// @brief The point in time when counting time started - time::point start; - - /// @brief Begin counting time - void set(void) - { - start = time::now(); - } - - /// @brief Add the counted time to the total time - void update(void) - { - const time::duration &dt = time::now() - start; - if (dt > longest) - longest = dt; - if (dt < shortest) - shortest = dt; - total = total + dt; - ++parts; - } - }; - - /// @brief Information maintained about averaged values - struct AvgInfo - { - /// @brief The sum of the values to average - double total; - - /// @brief The sub of squares of the values to average - double totalSqr; - - /// @brief Number of times a value was added to this structure - unsigned long int parts; - }; - - /// @brief Information to be maintained for each thread - struct PerThread - { - /// @brief The stored events - std::map events; - - /// @brief The stored averages - std::map avg; - - /// @brief The amount of time spent in various places - std::map time; - }; - - void printThreadInfo(std::ostream &out, const PerThread &data); - - std::mutex lock_; - std::map data_; - TimeInfo tinfo_; - bool running_; - bool printOnDestroy_; - -}; -} -} - -#else - -#include -#include - -/// If profiling is disabled, provide empty implementations for the public functions -namespace fcl -{ - -namespace tools -{ - -class Profiler -{ -public: - - class ScopedBlock - { - public: - - ScopedBlock(const std::string &, Profiler & = Profiler::Instance()) - { - } - - ~ScopedBlock(void) - { - } - }; - - class ScopedStart - { - public: - - ScopedStart(Profiler & = Profiler::Instance()) - { - } - - ~ScopedStart(void) - { - } - }; - - static Profiler& Instance(void); - - Profiler(bool = true, bool = true) - { - } - - ~Profiler(void) - { - } - - static void Start(void) - { - } - - static void Stop(void) - { - } - - static void Clear(void) - { - } - - void start(void) - { - } - - void stop(void) - { - } - - void clear(void) - { - } - - static void Event(const std::string&, const unsigned int = 1) - { - } - - void event(const std::string &, const unsigned int = 1) - { - } - - static void Average(const std::string&, const double) - { - } - - void average(const std::string &, const double) - { - } - - static void Begin(const std::string &) - { - } - - static void End(const std::string &) - { - } - - void begin(const std::string &) - { - } - - void end(const std::string &) - { - } - - static void Status(std::ostream & = std::cout, bool = true) - { - } - - void status(std::ostream & = std::cout, bool = true) - { - } - - bool running(void) const - { - return false; - } - - static bool Running(void) - { - return false; - } -}; -} -} - -#endif - -#endif diff --git a/include/fcl/shape/box.h b/include/fcl/shape/box.h new file mode 100644 index 000000000..e0615551a --- /dev/null +++ b/include/fcl/shape/box.h @@ -0,0 +1,181 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_SHAPE_BOX_H +#define FCL_SHAPE_BOX_H + +#include "fcl/shape/shape_base.h" +#include "fcl/shape/detail/bv_computer.h" + +namespace fcl +{ + +/// @brief Center at zero point, axis aligned box +template +class Box : public ShapeBase +{ +public: + + using S = S_; + + /// @brief Constructor + Box(S x, S y, S z); + + /// @brief Constructor + Box(const Vector3& side); + + /// @brief Constructor + Box(); + + /// @brief box side length + Vector3 side; + + /// @brief Compute AABBd + void computeLocalAABB() override; + + /// @brief Get node type: a box + NODE_TYPE getNodeType() const override; + + // Documentation inherited + S computeVolume() const override; + + // Documentation inherited + Matrix3 computeMomentofInertia() const override; + + /// @brief get the vertices of some convex shape which can bound this shape in + /// a specific configuration + std::vector> getBoundVertices( + const Transform3& tf) const; +}; + +using Boxf = Box; +using Boxd = Box; + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +Box::Box(S x, S y, S z) + : ShapeBase(), side(x, y, z) +{ + // Do nothing +} + +//============================================================================== +template +Box::Box(const Vector3& side_) + : ShapeBase(), side(side_) +{ + // Do nothing +} + +//============================================================================== +template +Box::Box() + : ShapeBase(), side(Vector3::Zero()) +{ + // Do nothing +} + +//============================================================================== +template +void Box::computeLocalAABB() +{ + computeBV(*this, Transform3::Identity(), this->aabb_local); + this->aabb_center = this->aabb_local.center(); + this->aabb_radius = (this->aabb_local.min_ - this->aabb_center).norm(); +} + +//============================================================================== +template +NODE_TYPE Box::getNodeType() const +{ + return GEOM_BOX; +} + +//============================================================================== +template +S Box::computeVolume() const +{ + return side.prod(); +} + +//============================================================================== +template +Matrix3 Box::computeMomentofInertia() const +{ + S V = computeVolume(); + + S a2 = side[0] * side[0] * V; + S b2 = side[1] * side[1] * V; + S c2 = side[2] * side[2] * V; + + Vector3 I((b2 + c2) / 12, (a2 + c2) / 12, (a2 + b2) / 12); + + return I.asDiagonal(); +} + +//============================================================================== +template +std::vector> Box::getBoundVertices( + const Transform3& tf) const +{ + std::vector> result(8); + auto a = side[0] / 2; + auto b = side[1] / 2; + auto c = side[2] / 2; + result[0] = tf * Vector3(a, b, c); + result[1] = tf * Vector3(a, b, -c); + result[2] = tf * Vector3(a, -b, c); + result[3] = tf * Vector3(a, -b, -c); + result[4] = tf * Vector3(-a, b, c); + result[5] = tf * Vector3(-a, b, -c); + result[6] = tf * Vector3(-a, -b, c); + result[7] = tf * Vector3(-a, -b, -c); + + return result; +} + +} // namespace fcl + +#include "fcl/shape/detail/bv_computer_box.h" + +#endif diff --git a/include/fcl/shape/capsule.h b/include/fcl/shape/capsule.h new file mode 100644 index 000000000..6eedaf4d6 --- /dev/null +++ b/include/fcl/shape/capsule.h @@ -0,0 +1,199 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_SHAPE_CAPSULE_H +#define FCL_SHAPE_CAPSULE_H + +#include "fcl/shape/shape_base.h" +#include "fcl/shape/compute_bv.h" +#include "fcl/BV/OBB.h" + +namespace fcl +{ + +/// @brief Center at zero point capsule +template +class Capsule : public ShapeBase +{ +public: + + using S = S_; + + /// @brief Constructor + Capsule(S radius, S lz); + + /// @brief Radius of capsule + S radius; + + /// @brief Length along z axis + S lz; + + /// @brief Compute AABB + void computeLocalAABB() override; + + /// @brief Get node type: a capsule + NODE_TYPE getNodeType() const override; + + // Documentation inherited + S computeVolume() const override; + + // Documentation inherited + Matrix3 computeMomentofInertia() const override; + + /// @brief get the vertices of some convex shape which can bound this shape in + /// a specific configuration + std::vector> getBoundVertices( + const Transform3& tf) const; +}; + +using Capsulef = Capsule; +using Capsuled = Capsule; + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +Capsule::Capsule(S radius, S lz) + : ShapeBase(), radius(radius), lz(lz) +{ + // Do nothing +} + +//============================================================================== +template +void Capsule::computeLocalAABB() +{ + computeBV(*this, Transform3::Identity(), this->aabb_local); + this->aabb_center = this->aabb_local.center(); + this->aabb_radius = (this->aabb_local.min_ - this->aabb_center).norm(); +} + +//============================================================================== +template +NODE_TYPE Capsule::getNodeType() const +{ + return GEOM_CAPSULE; +} + +//============================================================================== +template +S Capsule::computeVolume() const +{ + return constants::pi() * radius * radius *(lz + radius * 4/3.0); +} + +//============================================================================== +template +Matrix3 Capsule::computeMomentofInertia() const +{ + S v_cyl = radius * radius * lz * constants::pi(); + S v_sph = radius * radius * radius * constants::pi() * 4 / 3.0; + + S ix = v_cyl * lz * lz / 12.0 + 0.25 * v_cyl * radius + 0.4 * v_sph * radius * radius + 0.25 * v_sph * lz * lz; + S iz = (0.5 * v_cyl + 0.4 * v_sph) * radius * radius; + + return Vector3(ix, ix, iz).asDiagonal(); +} + +//============================================================================== +template +std::vector> Capsule::getBoundVertices( + const Transform3& tf) const +{ + std::vector> result(36); + const auto m = (1 + std::sqrt(5.0)) / 2.0; + + auto hl = lz * 0.5; + auto edge_size = radius * 6 / (std::sqrt(27.0) + std::sqrt(15.0)); + auto a = edge_size; + auto b = m * edge_size; + auto r2 = radius * 2 / std::sqrt(3.0); + + result[0] = tf * Vector3(0, a, b + hl); + result[1] = tf * Vector3(0, -a, b + hl); + result[2] = tf * Vector3(0, a, -b + hl); + result[3] = tf * Vector3(0, -a, -b + hl); + result[4] = tf * Vector3(a, b, hl); + result[5] = tf * Vector3(-a, b, hl); + result[6] = tf * Vector3(a, -b, hl); + result[7] = tf * Vector3(-a, -b, hl); + result[8] = tf * Vector3(b, 0, a + hl); + result[9] = tf * Vector3(b, 0, -a + hl); + result[10] = tf * Vector3(-b, 0, a + hl); + result[11] = tf * Vector3(-b, 0, -a + hl); + + result[12] = tf * Vector3(0, a, b - hl); + result[13] = tf * Vector3(0, -a, b - hl); + result[14] = tf * Vector3(0, a, -b - hl); + result[15] = tf * Vector3(0, -a, -b - hl); + result[16] = tf * Vector3(a, b, -hl); + result[17] = tf * Vector3(-a, b, -hl); + result[18] = tf * Vector3(a, -b, -hl); + result[19] = tf * Vector3(-a, -b, -hl); + result[20] = tf * Vector3(b, 0, a - hl); + result[21] = tf * Vector3(b, 0, -a - hl); + result[22] = tf * Vector3(-b, 0, a - hl); + result[23] = tf * Vector3(-b, 0, -a - hl); + + auto c = 0.5 * r2; + auto d = radius; + result[24] = tf * Vector3(r2, 0, hl); + result[25] = tf * Vector3(c, d, hl); + result[26] = tf * Vector3(-c, d, hl); + result[27] = tf * Vector3(-r2, 0, hl); + result[28] = tf * Vector3(-c, -d, hl); + result[29] = tf * Vector3(c, -d, hl); + + result[30] = tf * Vector3(r2, 0, -hl); + result[31] = tf * Vector3(c, d, -hl); + result[32] = tf * Vector3(-c, d, -hl); + result[33] = tf * Vector3(-r2, 0, -hl); + result[34] = tf * Vector3(-c, -d, -hl); + result[35] = tf * Vector3(c, -d, -hl); + + return result; +} + +} // namespace fcl + +#include "fcl/shape/detail/bv_computer_capsule.h" + +#endif diff --git a/include/fcl/shape/compute_bv.h b/include/fcl/shape/compute_bv.h new file mode 100644 index 000000000..d5a038b6d --- /dev/null +++ b/include/fcl/shape/compute_bv.h @@ -0,0 +1,68 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_SHAPE_COMPUTEBV_H +#define FCL_SHAPE_COMPUTEBV_H + +#include "fcl/data_types.h" +#include "fcl/shape/detail/bv_computer.h" + +namespace fcl +{ + +/// @brief calculate a bounding volume for a shape in a specific configuration +template +void computeBV(const Shape& s, const Transform3& tf, BV& bv); + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +void computeBV(const Shape& s, const Transform3& tf, BV& bv) +{ + using S = typename BV::S; + + detail::BVComputer::compute(s, tf, bv); +} + +} // namespace fcl + +#endif diff --git a/include/fcl/shape/cone.h b/include/fcl/shape/cone.h new file mode 100644 index 000000000..220cb93a7 --- /dev/null +++ b/include/fcl/shape/cone.h @@ -0,0 +1,171 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_SHAPE_CONE_H +#define FCL_SHAPE_CONE_H + +#include "fcl/shape/shape_base.h" +#include "fcl/shape/compute_bv.h" +#include "fcl/BV/OBB.h" + +namespace fcl +{ + +/// @brief Center at zero cone +template +class Cone : public ShapeBase +{ +public: + + using S = S_; + + Cone(S radius, S lz); + + /// @brief Radius of the cone + S radius; + + /// @brief Length along z axis + S lz; + + /// @brief Compute AABB + void computeLocalAABB() override; + + /// @brief Get node type: a cone + NODE_TYPE getNodeType() const override; + + // Documentation inherited + S computeVolume() const override; + + // Documentation inherited + Matrix3 computeMomentofInertia() const override; + + // Documentation inherited + Vector3 computeCOM() const override; + + /// @brief get the vertices of some convex shape which can bound this shape in + /// a specific configuration + std::vector> getBoundVertices( + const Transform3& tf) const; +}; + +using Conef = Cone; +using Coned = Cone; + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +Cone::Cone(S radius, S lz) + : ShapeBase(), radius(radius), lz(lz) +{ + // Do nothing +} + +//============================================================================== +template +void Cone::computeLocalAABB() +{ + computeBV(*this, Transform3::Identity(), this->aabb_local); + this->aabb_center = this->aabb_local.center(); + this->aabb_radius = (this->aabb_local.min_ - this->aabb_center).norm(); +} + +//============================================================================== +template +NODE_TYPE Cone::getNodeType() const +{ + return GEOM_CONE; +} + +//============================================================================== +template +S Cone::computeVolume() const +{ + return constants::pi() * radius * radius * lz / 3; +} + +//============================================================================== +template +Matrix3 Cone::computeMomentofInertia() const +{ + S V = computeVolume(); + S ix = V * (0.1 * lz * lz + 3 * radius * radius / 20); + S iz = 0.3 * V * radius * radius; + + return Vector3(ix, ix, iz).asDiagonal(); +} + +//============================================================================== +template +Vector3 Cone::computeCOM() const +{ + return Vector3(0, 0, -0.25 * lz); +} + +//============================================================================== +template +std::vector> Cone::getBoundVertices( + const Transform3& tf) const +{ + std::vector> result(7); + + auto hl = lz * 0.5; + auto r2 = radius * 2 / std::sqrt(3.0); + auto a = 0.5 * r2; + auto b = radius; + + result[0] = tf * Vector3(r2, 0, -hl); + result[1] = tf * Vector3(a, b, -hl); + result[2] = tf * Vector3(-a, b, -hl); + result[3] = tf * Vector3(-r2, 0, -hl); + result[4] = tf * Vector3(-a, -b, -hl); + result[5] = tf * Vector3(a, -b, -hl); + + result[6] = tf * Vector3(0, 0, hl); + + return result; +} + +} // namespace fcl + +#include "fcl/shape/detail/bv_computer_cone.h" + +#endif diff --git a/include/fcl/shape/construct_box.h b/include/fcl/shape/construct_box.h new file mode 100644 index 000000000..25544e815 --- /dev/null +++ b/include/fcl/shape/construct_box.h @@ -0,0 +1,249 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + + +#ifndef FCL_SHAPE_CONSTRUCT_BOX_H +#define FCL_SHAPE_CONSTRUCT_BOX_H + +#include + +#include "fcl/BV/convert_bv.h" +#include "fcl/shape/box.h" + +namespace fcl +{ + +/// @brief construct a box shape (with a configuration) from a given bounding volume +template +void constructBox(const AABB& bv, Box& box, Transform3& tf); + +template +void constructBox(const OBB& bv, Box& box, Transform3& tf); + +template +void constructBox(const OBBRSS& bv, Box& box, Transform3& tf); + +template +void constructBox(const kIOS& bv, Box& box, Transform3& tf); + +template +void constructBox(const RSS& bv, Box& box, Transform3& tf); + +template +void constructBox(const KDOP& bv, Box& box, Transform3& tf); + +template +void constructBox(const KDOP& bv, Box& box, Transform3& tf); + +template +void constructBox(const KDOP& bv, Box& box, Transform3& tf); + +template +void constructBox(const AABB& bv, const Transform3& tf_bv, Box& box, Transform3& tf); + +template +void constructBox(const OBB& bv, const Transform3& tf_bv, Box& box, Transform3& tf); + +template +void constructBox(const OBBRSS& bv, const Transform3& tf_bv, Box& box, Transform3& tf); + +template +void constructBox(const kIOS& bv, const Transform3& tf_bv, Box& box, Transform3& tf); + +template +void constructBox(const RSS& bv, const Transform3& tf_bv, Box& box, Transform3& tf); + +template +void constructBox(const KDOP& bv, const Transform3& tf_bv, Box& box, Transform3& tf); + +template +void constructBox(const KDOP& bv, const Transform3& tf_bv, Box& box, Transform3& tf); + +template +void constructBox(const KDOP& bv, const Transform3& tf_bv, Box& box, Transform3& tf); + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +void constructBox(const AABB& bv, Box& box, Transform3& tf) +{ + box = Box(bv.max_ - bv.min_); + tf.linear().setIdentity(); + tf.translation() = bv.center(); +} + +//============================================================================== +template +void constructBox(const OBB& bv, Box& box, Transform3& tf) +{ + box = Box(bv.extent * 2); + tf.linear() = bv.axis; + tf.translation() = bv.To; +} + +//============================================================================== +template +void constructBox(const OBBRSS& bv, Box& box, Transform3& tf) +{ + box = Box(bv.obb.extent * 2); + tf.linear() = bv.obb.axis; + tf.translation() = bv.obb.To; +} + +//============================================================================== +template +void constructBox(const kIOS& bv, Box& box, Transform3& tf) +{ + box = Box(bv.obb.extent * 2); + tf.linear() = bv.obb.axis; + tf.translation() = bv.obb.To; +} + +//============================================================================== +template +void constructBox(const RSS& bv, Box& box, Transform3& tf) +{ + box = Box(bv.width(), bv.height(), bv.depth()); + tf.linear() = bv.axis; + tf.translation() = bv.To; +} + +//============================================================================== +template +void constructBox(const KDOP& bv, Box& box, Transform3& tf) +{ + box = Box(bv.width(), bv.height(), bv.depth()); + tf.linear().setIdentity(); + tf.translation() = bv.center(); +} + +//============================================================================== +template +void constructBox(const KDOP& bv, Box& box, Transform3& tf) +{ + box = Box(bv.width(), bv.height(), bv.depth()); + tf.linear().setIdentity(); + tf.translation() = bv.center(); +} + +//============================================================================== +template +void constructBox(const KDOP& bv, Box& box, Transform3& tf) +{ + box = Box(bv.width(), bv.height(), bv.depth()); + tf.linear().setIdentity(); + tf.translation() = bv.center(); +} + +//============================================================================== +template +void constructBox(const AABB& bv, const Transform3& tf_bv, Box& box, Transform3& tf) +{ + box = Box(bv.max_ - bv.min_); + tf = tf_bv * Translation3(bv.center()); +} + +//============================================================================== +template +void constructBox(const OBB& bv, const Transform3& tf_bv, Box& box, Transform3& tf) +{ + box = Box(bv.extent * 2); + tf.linear() = bv.axis; + tf.translation() = bv.To; +} + +//============================================================================== +template +void constructBox(const OBBRSS& bv, const Transform3& tf_bv, Box& box, Transform3& tf) +{ + box = Box(bv.obb.extent * 2); + tf.linear() = bv.obb.axis; + tf.translation() = bv.obb.To; + tf = tf_bv * tf; +} + +//============================================================================== +template +void constructBox(const kIOS& bv, const Transform3& tf_bv, Box& box, Transform3& tf) +{ + box = Box(bv.obb.extent * 2); + tf.linear() = bv.obb.axis; + tf.translation() = bv.obb.To; +} + +//============================================================================== +template +void constructBox(const RSS& bv, const Transform3& tf_bv, Box& box, Transform3& tf) +{ + box = Box(bv.width(), bv.height(), bv.depth()); + tf.linear() = bv.axis; + tf.translation() = bv.To; + tf = tf_bv * tf; +} + +//============================================================================== +template +void constructBox(const KDOP& bv, const Transform3& tf_bv, Box& box, Transform3& tf) +{ + box = Box(bv.width(), bv.height(), bv.depth()); + tf = tf_bv * Translation3(bv.center()); +} + +//============================================================================== +template +void constructBox(const KDOP& bv, const Transform3& tf_bv, Box& box, Transform3& tf) +{ + box = Box(bv.width(), bv.height(), bv.depth()); + tf = tf_bv * Translation3(bv.center()); +} + +//============================================================================== +template +void constructBox(const KDOP& bv, const Transform3& tf_bv, Box& box, Transform3& tf) +{ + box = Box(bv.width(), bv.height(), bv.depth()); + tf = tf_bv * Translation3(bv.center()); +} + +} // namespace fcl + +#endif diff --git a/include/fcl/shape/convex.h b/include/fcl/shape/convex.h new file mode 100644 index 000000000..8ee322eae --- /dev/null +++ b/include/fcl/shape/convex.h @@ -0,0 +1,391 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + + +#ifndef FCL_SHAPE_CONVEX_H +#define FCL_SHAPE_CONVEX_H + +#include "fcl/shape/shape_base.h" +#include "fcl/shape/compute_bv.h" +#include "fcl/BV/OBB.h" + +namespace fcl +{ + +/// @brief Convex polytope +template +class Convex : public ShapeBase +{ +public: + + 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); + + /// @brief Copy constructor + Convex(const Convex& other); + + ~Convex(); + + /// @brief Compute AABB + void computeLocalAABB() override; + + /// @brief Get node type: a conex 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 + Matrix3 computeMomentofInertia() const override; + + // Documentation inherited + Vector3 computeCOM() const override; + + // Documentation inherited + S computeVolume() const override; + + /// @brief get 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(); +}; + +using Convexf = Convex; +using Convexd = Convex; + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +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; + + Vector3 sum = Vector3::Zero(); + for(int i = 0; i < num_points; ++i) + { + sum += points[i]; + } + + 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); +} + +//============================================================================== +template +Convex::~Convex() +{ + delete [] edges; +} + +//============================================================================== +template +void Convex::computeLocalAABB() +{ + computeBV(*this, Transform3::Identity(), this->aabb_local); + this->aabb_center = this->aabb_local.center(); + this->aabb_radius = (this->aabb_local.min_ - this->aabb_center).norm(); +} + +//============================================================================== +template +NODE_TYPE Convex::getNodeType() const +{ + return GEOM_CONVEX; +} + +//============================================================================== +template +Matrix3 Convex::computeMomentofInertia() const +{ + Matrix3 C = Matrix3::Zero(); + + Matrix3 C_canonical; + C_canonical << 1/ 60.0, 1/120.0, 1/120.0, + 1/120.0, 1/ 60.0, 1/120.0, + 1/120.0, 1/120.0, 1/ 60.0; + + 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 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 + } + + points_in_poly += (*points_in_poly + 1); + index = points_in_poly + 1; + } + + S trace_C = C(0, 0) + C(1, 1) + C(2, 2); + + Matrix3 m; + m << trace_C - C(0, 0), -C(0, 1), -C(0, 2), + -C(1, 0), trace_C - C(1, 1), -C(1, 2), + -C(2, 0), -C(2, 1), trace_C - C(2, 2); + + return m; +} + +//============================================================================== +template +Vector3 Convex::computeCOM() const +{ + 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]; + 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; + } + + points_in_poly += (*points_in_poly + 1); + index = points_in_poly + 1; + } + + return com / (vol * 4); // here we choose zero as the reference +} + +//============================================================================== +template +S Convex::computeVolume() const +{ + 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]; + 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; + } + + 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]; + } + + return result; +} + +} // namespace fcl + +#include "fcl/shape/detail/bv_computer_convex.h" + +#endif diff --git a/include/fcl/shape/cylinder.h b/include/fcl/shape/cylinder.h new file mode 100644 index 000000000..e16e437c9 --- /dev/null +++ b/include/fcl/shape/cylinder.h @@ -0,0 +1,167 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_SHAPE_CYLINDER_H +#define FCL_SHAPE_CYLINDER_H + +#include "fcl/shape/shape_base.h" +#include "fcl/shape/compute_bv.h" +#include "fcl/BV/OBB.h" + +namespace fcl +{ + +/// @brief Center at zero cylinder +template +class Cylinder : public ShapeBase +{ +public: + + using S = S_; + + /// @brief Constructor + Cylinder(S radius, S lz); + + /// @brief Radius of the cylinder + S radius; + + /// @brief Length along z axis + S lz; + + /// @brief Compute AABBd + void computeLocalAABB() override; + + /// @brief Get node type: a cylinder + NODE_TYPE getNodeType() const override; + + // Documentation inherited + S computeVolume() const override; + + // Documentation inherited + Matrix3 computeMomentofInertia() const override; + + /// @brief get the vertices of some convex shape which can bound this shape in + /// a specific configuration + std::vector> getBoundVertices( + const Transform3& tf) const; +}; + +using Cylinderf = Cylinder; +using Cylinderd = Cylinder; + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +Cylinder::Cylinder(S radius, S lz) + : ShapeBase(), radius(radius), lz(lz) +{ + // Do nothing +} + +//============================================================================== +template +void Cylinder::computeLocalAABB() +{ + computeBV(*this, Transform3::Identity(), this->aabb_local); + this->aabb_center = this->aabb_local.center(); + this->aabb_radius = (this->aabb_local.min_ - this->aabb_center).norm(); +} + +//============================================================================== +template +NODE_TYPE Cylinder::getNodeType() const +{ + return GEOM_CYLINDER; +} + +//============================================================================== +template +S Cylinder::computeVolume() const +{ + return constants::pi() * radius * radius * lz; +} + +//============================================================================== +template +Matrix3 Cylinder::computeMomentofInertia() const +{ + S V = computeVolume(); + S ix = V * (3 * radius * radius + lz * lz) / 12; + S iz = V * radius * radius / 2; + + return Vector3(ix, ix, iz).asDiagonal(); +} + +//============================================================================== +template +std::vector> Cylinder::getBoundVertices( + const Transform3& tf) const +{ + std::vector> result(12); + + auto hl = lz * 0.5; + auto r2 = radius * 2 / std::sqrt(3.0); + auto a = 0.5 * r2; + auto b = radius; + + result[0] = tf * Vector3(r2, 0, -hl); + result[1] = tf * Vector3(a, b, -hl); + result[2] = tf * Vector3(-a, b, -hl); + result[3] = tf * Vector3(-r2, 0, -hl); + result[4] = tf * Vector3(-a, -b, -hl); + result[5] = tf * Vector3(a, -b, -hl); + + result[6] = tf * Vector3(r2, 0, hl); + result[7] = tf * Vector3(a, b, hl); + result[8] = tf * Vector3(-a, b, hl); + result[9] = tf * Vector3(-r2, 0, hl); + result[10] = tf * Vector3(-a, -b, hl); + result[11] = tf * Vector3(a, -b, hl); + + return result; +} + +} // namespace fcl + +#include "fcl/shape/detail/bv_computer_cylinder.h" + +#endif diff --git a/src/BV/OBBRSS.cpp b/include/fcl/shape/detail/bv_computer.h similarity index 77% rename from src/BV/OBBRSS.cpp rename to include/fcl/shape/detail/bv_computer.h index 4fd72943a..add2964a1 100644 --- a/src/BV/OBBRSS.cpp +++ b/include/fcl/shape/detail/bv_computer.h @@ -35,28 +35,29 @@ /** \author Jia Pan */ -#include "fcl/BV/OBBRSS.h" +#ifndef FCL_SHAPE_DETAIL_BVCOMPUTER_H +#define FCL_SHAPE_DETAIL_BVCOMPUTER_H -namespace fcl -{ +#include +#include "fcl/data_types.h" +#include "fcl/BV/fit.h" -bool overlap(const Matrix3d& R0, const Vector3d& T0, const OBBRSS& b1, const OBBRSS& b2) +namespace fcl { - return overlap(R0, T0, b1.obb, b2.obb); -} - -FCL_REAL distance(const Matrix3d& R0, const Vector3d& T0, const OBBRSS& b1, const OBBRSS& b2, Vector3d* P, Vector3d* Q) +namespace detail { - return distance(R0, T0, b1.rss, b2.rss, P, Q); -} -OBBRSS translate(const OBBRSS& bv, const Vector3d& t) +template +struct BVComputer { - OBBRSS res(bv); - res.obb.To += t; - res.rss.Tr += t; - return res; -} + static void compute(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); + } +}; +} // namespace detail +} // namespace fcl -} +#endif diff --git a/src/collision_node.cpp b/include/fcl/shape/detail/bv_computer_box.h similarity index 51% rename from src/collision_node.cpp rename to include/fcl/shape/detail/bv_computer_box.h index 182399bbe..71bbc3689 100644 --- a/src/collision_node.cpp +++ b/include/fcl/shape/detail/bv_computer_box.h @@ -35,82 +35,60 @@ /** \author Jia Pan */ +#ifndef FCL_SHAPE_DETAIL_BVCOMPUTERBOX_H +#define FCL_SHAPE_DETAIL_BVCOMPUTERBOX_H -#include "fcl/collision_node.h" -#include "fcl/traversal/traversal_recurse.h" +#include "fcl/BV/AABB.h" namespace fcl { - -void collide(CollisionTraversalNodeBase* node, BVHFrontList* front_list) +namespace detail { - if(front_list && front_list->size() > 0) - { - propagateBVHFrontListCollisionRecurse(node, front_list); - } - else - { - collisionRecurse(node, 0, 0, front_list); - } -} -void collide2(MeshCollisionTraversalNodeOBB* node, BVHFrontList* front_list) -{ - if(front_list && front_list->size() > 0) - { - propagateBVHFrontListCollisionRecurse(node, front_list); - } - else - { - Matrix3d Rtemp, R; - Vector3d Ttemp, T; - Rtemp = node->R * node->model2->getBV(0).getOrientation(); - R = node->model1->getBV(0).getOrientation().transpose() * Rtemp; - Ttemp = node->R * node->model2->getBV(0).getCenter() + node->T; - Ttemp -= node->model1->getBV(0).getCenter(); - T = node->model1->getBV(0).getOrientation().transpose() * Ttemp; +template +struct BVComputer, Box>; - collisionRecurse(node, 0, 0, R, T, front_list); - } -} +template +struct BVComputer, Box>; -void collide2(MeshCollisionTraversalNodeRSS* node, BVHFrontList* front_list) +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +struct BVComputer, Box> { - if(front_list && front_list->size() > 0) - { - propagateBVHFrontListCollisionRecurse(node, front_list); - } - else + static void compute(const Box& s, const Transform3& tf, AABB& bv) { - collisionRecurse(node, 0, 0, node->R, node->T, front_list); - } -} + const Matrix3& R = tf.linear(); + const Vector3& T = tf.translation(); + S x_range = 0.5 * (fabs(R(0, 0) * s.side[0]) + fabs(R(0, 1) * s.side[1]) + fabs(R(0, 2) * s.side[2])); + S y_range = 0.5 * (fabs(R(1, 0) * s.side[0]) + fabs(R(1, 1) * s.side[1]) + fabs(R(1, 2) * s.side[2])); + S z_range = 0.5 * (fabs(R(2, 0) * s.side[0]) + fabs(R(2, 1) * s.side[1]) + fabs(R(2, 2) * s.side[2])); + Vector3 v_delta(x_range, y_range, z_range); + bv.max_ = T + v_delta; + bv.min_ = T - v_delta; + } +}; -void selfCollide(CollisionTraversalNodeBase* node, BVHFrontList* front_list) +//============================================================================== +template +struct BVComputer, Box> { - - if(front_list && front_list->size() > 0) - { - propagateBVHFrontListCollisionRecurse(node, front_list); - } - else + static void compute(const Box& s, const Transform3& tf, OBB& bv) { - selfCollisionRecurse(node, 0, front_list); + bv.axis = tf.linear(); + bv.To = tf.translation(); + bv.extent = s.side * (S)0.5; } -} - -void distance(DistanceTraversalNodeBase* node, BVHFrontList* front_list, int qsize) -{ - node->preprocess(); - - if(qsize <= 2) - distanceRecurse(node, 0, 0, front_list); - else - distanceQueueRecurse(node, 0, 0, front_list, qsize); +}; - node->postprocess(); -} +} // namespace detail +} // namespace fcl -} +#endif diff --git a/src/BV/AABB.cpp b/include/fcl/shape/detail/bv_computer_capsule.h similarity index 51% rename from src/BV/AABB.cpp rename to include/fcl/shape/detail/bv_computer_capsule.h index f5cae0ebb..6676e23d5 100644 --- a/src/BV/AABB.cpp +++ b/include/fcl/shape/detail/bv_computer_capsule.h @@ -35,95 +35,61 @@ /** \author Jia Pan */ -#include "fcl/BV/AABB.h" +#ifndef FCL_SHAPE_DETAIL_BVCOMPUTERCAPSULE_H +#define FCL_SHAPE_DETAIL_BVCOMPUTERCAPSULE_H -#include -#include +#include "fcl/BV/AABB.h" +#include "fcl/BV/OBB.h" namespace fcl { - -AABB::AABB() : min_(Vector3d::Constant(std::numeric_limits::max())), - max_(Vector3d::Constant(-std::numeric_limits::max())) +namespace detail { -} -FCL_REAL AABB::distance(const AABB& other, Vector3d* P, Vector3d* Q) const +template +struct BVComputer, Capsule>; + +template +struct BVComputer, Capsule>; + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +struct BVComputer, Capsule> { - FCL_REAL result = 0; - for(std::size_t i = 0; i < 3; ++i) + static void compute(const Capsule& s, const Transform3& tf, AABB& bv) { - const FCL_REAL& amin = min_[i]; - const FCL_REAL& amax = max_[i]; - const FCL_REAL& bmin = other.min_[i]; - const FCL_REAL& bmax = other.max_[i]; - - if(amin > bmax) - { - FCL_REAL delta = bmax - amin; - result += delta * delta; - if(P && Q) - { - (*P)[i] = amin; - (*Q)[i] = bmax; - } - } - else if(bmin > amax) - { - FCL_REAL delta = amax - bmin; - result += delta * delta; - if(P && Q) - { - (*P)[i] = amax; - (*Q)[i] = bmin; - } - } - else - { - if(P && Q) - { - if(bmin >= amin) - { - FCL_REAL t = 0.5 * (amax + bmin); - (*P)[i] = t; - (*Q)[i] = t; - } - else - { - FCL_REAL t = 0.5 * (amin + bmax); - (*P)[i] = t; - (*Q)[i] = t; - } - } - } - } + const Matrix3& R = tf.linear(); + const Vector3& T = tf.translation(); - return std::sqrt(result); -} + S x_range = 0.5 * fabs(R(0, 2) * s.lz) + s.radius; + S y_range = 0.5 * fabs(R(1, 2) * s.lz) + s.radius; + S z_range = 0.5 * fabs(R(2, 2) * s.lz) + s.radius; + + Vector3 v_delta(x_range, y_range, z_range); + bv.max_ = T + v_delta; + bv.min_ = T - v_delta; + } +}; -FCL_REAL AABB::distance(const AABB& other) const +//============================================================================== +template +struct BVComputer, Capsule> { - FCL_REAL result = 0; - for(std::size_t i = 0; i < 3; ++i) + static void compute(const Capsule& s, const Transform3& tf, OBB& bv) { - const FCL_REAL& amin = min_[i]; - const FCL_REAL& amax = max_[i]; - const FCL_REAL& bmin = other.min_[i]; - const FCL_REAL& bmax = other.max_[i]; - - if(amin > bmax) - { - FCL_REAL delta = bmax - amin; - result += delta * delta; - } - else if(bmin > amax) - { - FCL_REAL delta = amax - bmin; - result += delta * delta; - } + bv.axis = tf.linear(); + bv.To = tf.translation(); + bv.extent << s.radius, s.radius, s.lz / 2 + s.radius; } +}; - return std::sqrt(result); -} +} // namespace detail +} // namespace fcl -} +#endif diff --git a/include/fcl/shape/detail/bv_computer_cone.h b/include/fcl/shape/detail/bv_computer_cone.h new file mode 100644 index 000000000..bec19d1cf --- /dev/null +++ b/include/fcl/shape/detail/bv_computer_cone.h @@ -0,0 +1,95 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_SHAPE_DETAIL_BVCOMPUTERCONE_H +#define FCL_SHAPE_DETAIL_BVCOMPUTERCONE_H + +#include "fcl/BV/AABB.h" +#include "fcl/BV/OBB.h" + +namespace fcl +{ +namespace detail +{ + +template +struct BVComputer, Cone>; + +template +struct BVComputer, Cone>; + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +struct BVComputer, Cone> +{ + static void compute(const Cone& s, const Transform3& tf, AABB& bv) + { + const Matrix3& R = tf.linear(); + const Vector3& T = tf.translation(); + + S x_range = fabs(R(0, 0) * s.radius) + fabs(R(0, 1) * s.radius) + 0.5 * fabs(R(0, 2) * s.lz); + S y_range = fabs(R(1, 0) * s.radius) + fabs(R(1, 1) * s.radius) + 0.5 * fabs(R(1, 2) * s.lz); + S z_range = fabs(R(2, 0) * s.radius) + fabs(R(2, 1) * s.radius) + 0.5 * fabs(R(2, 2) * s.lz); + + Vector3 v_delta(x_range, y_range, z_range); + bv.max_ = T + v_delta; + bv.min_ = T - v_delta; + } +}; + +//============================================================================== +template +struct BVComputer, Cone> +{ + static void compute(const Cone& s, const Transform3& tf, OBB& bv) + { + bv.axis = tf.linear(); + bv.To = tf.translation(); + bv.extent << s.radius, s.radius, s.lz / 2; + } +}; + +} // namespace detail +} // namespace fcl + +#endif diff --git a/include/fcl/shape/detail/bv_computer_convex.h b/include/fcl/shape/detail/bv_computer_convex.h new file mode 100644 index 000000000..abaea6867 --- /dev/null +++ b/include/fcl/shape/detail/bv_computer_convex.h @@ -0,0 +1,97 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_SHAPE_DETAIL_BVCOMPUTERCONVEX_H +#define FCL_SHAPE_DETAIL_BVCOMPUTERCONVEX_H + +#include "fcl/BV/AABB.h" +#include "fcl/BV/OBB.h" + +namespace fcl +{ +namespace detail +{ + +template +struct BVComputer, Convex>; + +template +struct BVComputer, Convex>; + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +struct BVComputer, Convex> +{ + static void compute(const Convex& s, const Transform3& tf, AABB& bv) + { + const Matrix3& R = tf.linear(); + const Vector3& T = tf.translation(); + + AABB bv_; + for(int i = 0; i < s.num_points; ++i) + { + Vector3 new_p = R * s.points[i] + T; + bv_ += new_p; + } + + bv = bv_; + } +}; + +//============================================================================== +template +struct BVComputer, Convex> +{ + static void compute(const Convex& s, const Transform3& tf, OBB& bv) + { + fit(s.points, s.num_points, bv); + + bv.axis = tf.linear(); + bv.To = tf * bv.To; + } +}; + +} // namespace detail +} // namespace fcl + +#endif diff --git a/include/fcl/shape/detail/bv_computer_cylinder.h b/include/fcl/shape/detail/bv_computer_cylinder.h new file mode 100644 index 000000000..087933d2e --- /dev/null +++ b/include/fcl/shape/detail/bv_computer_cylinder.h @@ -0,0 +1,96 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_SHAPE_DETAIL_BVCOMPUTERCYLINDER_H +#define FCL_SHAPE_DETAIL_BVCOMPUTERCYLINDER_H + +#include "fcl/BV/AABB.h" +#include "fcl/BV/OBB.h" +#include "fcl/shape/compute_bv.h" + +namespace fcl +{ +namespace detail +{ + +template +struct BVComputer, Cylinder>; + +template +struct BVComputer, Cylinder>; + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +struct BVComputer, Cylinder> +{ + static void compute(const Cylinder& s, const Transform3& tf, AABB& bv) + { + const Matrix3& R = tf.linear(); + const Vector3& T = tf.translation(); + + S x_range = fabs(R(0, 0) * s.radius) + fabs(R(0, 1) * s.radius) + 0.5 * fabs(R(0, 2) * s.lz); + S y_range = fabs(R(1, 0) * s.radius) + fabs(R(1, 1) * s.radius) + 0.5 * fabs(R(1, 2) * s.lz); + S z_range = fabs(R(2, 0) * s.radius) + fabs(R(2, 1) * s.radius) + 0.5 * fabs(R(2, 2) * s.lz); + + Vector3 v_delta(x_range, y_range, z_range); + bv.max_ = T + v_delta; + bv.min_ = T - v_delta; + } +}; + +//============================================================================== +template +struct BVComputer, Cylinder> +{ + static void compute(const Cylinder& s, const Transform3& tf, OBB& bv) + { + bv.axis = tf.linear(); + bv.To = tf.translation(); + bv.extent << s.radius, s.radius, s.lz / 2; + } +}; + +} // namespace detail +} // namespace fcl + +#endif diff --git a/include/fcl/shape/detail/bv_computer_ellipsoid.h b/include/fcl/shape/detail/bv_computer_ellipsoid.h new file mode 100644 index 000000000..b47681eea --- /dev/null +++ b/include/fcl/shape/detail/bv_computer_ellipsoid.h @@ -0,0 +1,95 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_SHAPE_DETAIL_BVCOMPUTERELLIPSOID_H +#define FCL_SHAPE_DETAIL_BVCOMPUTERELLIPSOID_H + +#include "fcl/BV/AABB.h" +#include "fcl/BV/OBB.h" + +namespace fcl +{ +namespace detail +{ + +template +struct BVComputer, Ellipsoid>; + +template +struct BVComputer, Ellipsoid>; + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +struct BVComputer, Ellipsoid> +{ + static void compute(const Ellipsoid& s, const Transform3& tf, AABB& bv) + { + const Matrix3& R = tf.linear(); + const Vector3& T = tf.translation(); + + S x_range = (fabs(R(0, 0) * s.radii[0]) + fabs(R(0, 1) * s.radii[1]) + fabs(R(0, 2) * s.radii[2])); + S y_range = (fabs(R(1, 0) * s.radii[0]) + fabs(R(1, 1) * s.radii[1]) + fabs(R(1, 2) * s.radii[2])); + S z_range = (fabs(R(2, 0) * s.radii[0]) + fabs(R(2, 1) * s.radii[1]) + fabs(R(2, 2) * s.radii[2])); + + Vector3 v_delta(x_range, y_range, z_range); + bv.max_ = T + v_delta; + bv.min_ = T - v_delta; + } +}; + +//============================================================================== +template +struct BVComputer, Ellipsoid> +{ + static void compute(const Ellipsoid& s, const Transform3& tf, OBB& bv) + { + bv.axis = tf.linear(); + bv.To = tf.translation(); + bv.extent = s.radii; + } +}; + +} // namespace detail +} // namespace fcl + +#endif diff --git a/include/fcl/shape/detail/bv_computer_halfspace.h b/include/fcl/shape/detail/bv_computer_halfspace.h new file mode 100644 index 000000000..bb04be5f9 --- /dev/null +++ b/include/fcl/shape/detail/bv_computer_halfspace.h @@ -0,0 +1,376 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_SHAPE_DETAIL_BVCOMPUTERHALFSPACE_H +#define FCL_SHAPE_DETAIL_BVCOMPUTERHALFSPACE_H + +#include "fcl/BV/AABB.h" +#include "fcl/BV/OBB.h" +#include "fcl/BV/RSS.h" +#include "fcl/BV/OBBRSS.h" +#include "fcl/BV/kDOP.h" +#include "fcl/BV/kIOS.h" + +namespace fcl +{ +namespace detail +{ + +template +struct BVComputer, Halfspace>; + +template +struct BVComputer, Halfspace>; + +template +struct BVComputer, Halfspace>; + +template +struct BVComputer, Halfspace>; + +template +struct BVComputer, Halfspace>; + +template +struct BVComputer, Halfspace>; + +template +struct BVComputer, Halfspace>; + +template +struct BVComputer, Halfspace>; + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +struct BVComputer, Halfspace> +{ + static void compute(const Halfspace& s, const Transform3& tf, AABB& bv) + { + Halfspace new_s = transform(s, tf); + const Vector3& n = new_s.n; + const S& d = new_s.d; + + AABB bv_; + bv_.min_ = Vector3::Constant(-std::numeric_limits::max()); + bv_.max_ = Vector3::Constant(std::numeric_limits::max()); + if(n[1] == (S)0.0 && n[2] == (S)0.0) + { + // normal aligned with x axis + if(n[0] < 0) bv_.min_[0] = -d; + else if(n[0] > 0) bv_.max_[0] = d; + } + else if(n[0] == (S)0.0 && n[2] == (S)0.0) + { + // normal aligned with y axis + if(n[1] < 0) bv_.min_[1] = -d; + else if(n[1] > 0) bv_.max_[1] = d; + } + else if(n[0] == (S)0.0 && n[1] == (S)0.0) + { + // normal aligned with z axis + if(n[2] < 0) bv_.min_[2] = -d; + else if(n[2] > 0) bv_.max_[2] = d; + } + + bv = bv_; + } +}; + +//============================================================================== +template +struct BVComputer, Halfspace> +{ + static void compute(const Halfspace& s, const Transform3& tf, OBB& bv) + { + /// Half space can only have very rough OBB + bv.axis.setIdentity(); + bv.To.setZero(); + bv.extent.setConstant(std::numeric_limits::max()); + } +}; + +//============================================================================== +template +struct BVComputer, Halfspace> +{ + static void compute(const Halfspace& s, const Transform3& tf, RSS& bv) + { + /// Half space can only have very rough RSS + bv.axis.setIdentity(); + bv.To.setZero(); + bv.l[0] = bv.l[1] = bv.r = std::numeric_limits::max(); + } +}; + +//============================================================================== +template +struct BVComputer, Halfspace> +{ + static void compute(const Halfspace& s, const Transform3& tf, OBBRSS& bv) + { + computeBV(s, tf, bv.obb); + computeBV(s, tf, bv.rss); + } +}; + +//============================================================================== +template +struct BVComputer, Halfspace> +{ + static void compute(const Halfspace& s, const Transform3& tf, kIOS& bv) + { + bv.num_spheres = 1; + computeBV(s, tf, bv.obb); + bv.spheres[0].o.setZero(); + bv.spheres[0].r = std::numeric_limits::max(); + } +}; + +//============================================================================== +template +struct BVComputer, Halfspace> +{ + static void compute(const Halfspace& s, const Transform3& tf, KDOP& bv) + { + Halfspace new_s = transform(s, tf); + const Vector3& n = new_s.n; + const S& d = new_s.d; + + const std::size_t D = 8; + for(std::size_t i = 0; i < D; ++i) + bv.dist(i) = -std::numeric_limits::max(); + for(std::size_t i = D; i < 2 * D; ++i) + bv.dist(i) = std::numeric_limits::max(); + + if(n[1] == (S)0.0 && n[2] == (S)0.0) + { + if(n[0] > 0) bv.dist(D) = d; + else bv.dist(0) = -d; + } + else if(n[0] == (S)0.0 && n[2] == (S)0.0) + { + if(n[1] > 0) bv.dist(D + 1) = d; + else bv.dist(1) = -d; + } + else if(n[0] == (S)0.0 && n[1] == (S)0.0) + { + if(n[2] > 0) bv.dist(D + 2) = d; + else bv.dist(2) = -d; + } + else if(n[2] == (S)0.0 && n[0] == n[1]) + { + if(n[0] > 0) bv.dist(D + 3) = n[0] * d * 2; + else bv.dist(3) = n[0] * d * 2; + } + else if(n[1] == (S)0.0 && n[0] == n[2]) + { + if(n[1] > 0) bv.dist(D + 4) = n[0] * d * 2; + else bv.dist(4) = n[0] * d * 2; + } + else if(n[0] == (S)0.0 && n[1] == n[2]) + { + if(n[1] > 0) bv.dist(D + 5) = n[1] * d * 2; + else bv.dist(5) = n[1] * d * 2; + } + else if(n[2] == (S)0.0 && n[0] + n[1] == (S)0.0) + { + if(n[0] > 0) bv.dist(D + 6) = n[0] * d * 2; + else bv.dist(6) = n[0] * d * 2; + } + else if(n[1] == (S)0.0 && n[0] + n[2] == (S)0.0) + { + if(n[0] > 0) bv.dist(D + 7) = n[0] * d * 2; + else bv.dist(7) = n[0] * d * 2; + } + } +}; + +//============================================================================== +template +struct BVComputer, Halfspace> +{ + static void compute(const Halfspace& s, const Transform3& tf, KDOP& bv) + { + Halfspace new_s = transform(s, tf); + const Vector3& n = new_s.n; + const S& d = new_s.d; + + const std::size_t D = 9; + + for(std::size_t i = 0; i < D; ++i) + bv.dist(i) = -std::numeric_limits::max(); + for(std::size_t i = D; i < 2 * D; ++i) + bv.dist(i) = std::numeric_limits::max(); + + if(n[1] == (S)0.0 && n[2] == (S)0.0) + { + if(n[0] > 0) bv.dist(D) = d; + else bv.dist(0) = -d; + } + else if(n[0] == (S)0.0 && n[2] == (S)0.0) + { + if(n[1] > 0) bv.dist(D + 1) = d; + else bv.dist(1) = -d; + } + else if(n[0] == (S)0.0 && n[1] == (S)0.0) + { + if(n[2] > 0) bv.dist(D + 2) = d; + else bv.dist(2) = -d; + } + else if(n[2] == (S)0.0 && n[0] == n[1]) + { + if(n[0] > 0) bv.dist(D + 3) = n[0] * d * 2; + else bv.dist(3) = n[0] * d * 2; + } + else if(n[1] == (S)0.0 && n[0] == n[2]) + { + if(n[1] > 0) bv.dist(D + 4) = n[0] * d * 2; + else bv.dist(4) = n[0] * d * 2; + } + else if(n[0] == (S)0.0 && n[1] == n[2]) + { + if(n[1] > 0) bv.dist(D + 5) = n[1] * d * 2; + else bv.dist(5) = n[1] * d * 2; + } + else if(n[2] == (S)0.0 && n[0] + n[1] == (S)0.0) + { + if(n[0] > 0) bv.dist(D + 6) = n[0] * d * 2; + else bv.dist(6) = n[0] * d * 2; + } + else if(n[1] == (S)0.0 && n[0] + n[2] == (S)0.0) + { + if(n[0] > 0) bv.dist(D + 7) = n[0] * d * 2; + else bv.dist(7) = n[0] * d * 2; + } + else if(n[0] == (S)0.0 && n[1] + n[2] == (S)0.0) + { + if(n[1] > 0) bv.dist(D + 8) = n[1] * d * 2; + else bv.dist(8) = n[1] * d * 2; + } + } +}; + +//============================================================================== +template +struct BVComputer, Halfspace> +{ + static void compute(const Halfspace& s, const Transform3& tf, KDOP& bv) + { + Halfspace new_s = transform(s, tf); + const Vector3& n = new_s.n; + const S& d = new_s.d; + + const std::size_t D = 12; + + for(std::size_t i = 0; i < D; ++i) + bv.dist(i) = -std::numeric_limits::max(); + for(std::size_t i = D; i < 2 * D; ++i) + bv.dist(i) = std::numeric_limits::max(); + + if(n[1] == (S)0.0 && n[2] == (S)0.0) + { + if(n[0] > 0) bv.dist(D) = d; + else bv.dist(0) = -d; + } + else if(n[0] == (S)0.0 && n[2] == (S)0.0) + { + if(n[1] > 0) bv.dist(D + 1) = d; + else bv.dist(1) = -d; + } + else if(n[0] == (S)0.0 && n[1] == (S)0.0) + { + if(n[2] > 0) bv.dist(D + 2) = d; + else bv.dist(2) = -d; + } + else if(n[2] == (S)0.0 && n[0] == n[1]) + { + if(n[0] > 0) bv.dist(D + 3) = n[0] * d * 2; + else bv.dist(3) = n[0] * d * 2; + } + else if(n[1] == (S)0.0 && n[0] == n[2]) + { + if(n[1] > 0) bv.dist(D + 4) = n[0] * d * 2; + else bv.dist(4) = n[0] * d * 2; + } + else if(n[0] == (S)0.0 && n[1] == n[2]) + { + if(n[1] > 0) bv.dist(D + 5) = n[1] * d * 2; + else bv.dist(5) = n[1] * d * 2; + } + else if(n[2] == (S)0.0 && n[0] + n[1] == (S)0.0) + { + if(n[0] > 0) bv.dist(D + 6) = n[0] * d * 2; + else bv.dist(6) = n[0] * d * 2; + } + else if(n[1] == (S)0.0 && n[0] + n[2] == (S)0.0) + { + if(n[0] > 0) bv.dist(D + 7) = n[0] * d * 2; + else bv.dist(7) = n[0] * d * 2; + } + else if(n[0] == (S)0.0 && n[1] + n[2] == (S)0.0) + { + if(n[1] > 0) bv.dist(D + 8) = n[1] * d * 2; + else bv.dist(8) = n[1] * d * 2; + } + else if(n[0] + n[2] == (S)0.0 && n[0] + n[1] == (S)0.0) + { + if(n[0] > 0) bv.dist(D + 9) = n[0] * d * 3; + else bv.dist(9) = n[0] * d * 3; + } + else if(n[0] + n[1] == (S)0.0 && n[1] + n[2] == (S)0.0) + { + if(n[0] > 0) bv.dist(D + 10) = n[0] * d * 3; + else bv.dist(10) = n[0] * d * 3; + } + else if(n[0] + n[1] == (S)0.0 && n[0] + n[2] == (S)0.0) + { + if(n[1] > 0) bv.dist(D + 11) = n[1] * d * 3; + else bv.dist(11) = n[1] * d * 3; + } + } +}; + +} // namespace detail +} // namespace fcl + +#endif diff --git a/include/fcl/shape/detail/bv_computer_plane.h b/include/fcl/shape/detail/bv_computer_plane.h new file mode 100644 index 000000000..b87a5fb6f --- /dev/null +++ b/include/fcl/shape/detail/bv_computer_plane.h @@ -0,0 +1,369 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_SHAPE_DETAIL_BVCOMPUTERPLANE_H +#define FCL_SHAPE_DETAIL_BVCOMPUTERPLANE_H + +#include "fcl/BV/AABB.h" +#include "fcl/BV/OBB.h" +#include "fcl/BV/RSS.h" +#include "fcl/BV/OBBRSS.h" +#include "fcl/BV/kDOP.h" +#include "fcl/BV/kIOS.h" + +namespace fcl +{ +namespace detail +{ + +template +struct BVComputer, Plane>; + +template +struct BVComputer, Plane>; + +template +struct BVComputer, Plane>; + +template +struct BVComputer, Plane>; + +template +struct BVComputer, Plane>; + +template +struct BVComputer, Plane>; + +template +struct BVComputer, Plane>; + +template +struct BVComputer, Plane>; + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +struct BVComputer, Plane> +{ + static void compute(const Plane& s, const Transform3& tf, AABB& bv) + { + Plane new_s = transform(s, tf); + const Vector3& n = new_s.n; + const S& d = new_s.d; + + AABB bv_; + bv_.min_ = Vector3::Constant(-std::numeric_limits::max()); + bv_.max_ = Vector3::Constant(std::numeric_limits::max()); + if(n[1] == (S)0.0 && n[2] == (S)0.0) + { + // normal aligned with x axis + if(n[0] < 0) { bv_.min_[0] = bv_.max_[0] = -d; } + else if(n[0] > 0) { bv_.min_[0] = bv_.max_[0] = d; } + } + else if(n[0] == (S)0.0 && n[2] == (S)0.0) + { + // normal aligned with y axis + if(n[1] < 0) { bv_.min_[1] = bv_.max_[1] = -d; } + else if(n[1] > 0) { bv_.min_[1] = bv_.max_[1] = d; } + } + else if(n[0] == (S)0.0 && n[1] == (S)0.0) + { + // normal aligned with z axis + if(n[2] < 0) { bv_.min_[2] = bv_.max_[2] = -d; } + else if(n[2] > 0) { bv_.min_[2] = bv_.max_[2] = d; } + } + + bv = bv_; + } +}; + +//============================================================================== +template +struct BVComputer, Plane> +{ + static void compute(const Plane& s, const Transform3& tf, OBB& bv) + { + Vector3 n = tf.linear() * s.n; + bv.axis.col(0) = n; + generateCoordinateSystem(bv.axis); + + bv.extent << 0, std::numeric_limits::max(), std::numeric_limits::max(); + + Vector3 p = s.n * s.d; + bv.To = tf * p; /// n'd' = R * n * (d + (R * n) * T) = R * (n * d) + T + } +}; + +//============================================================================== +template +struct BVComputer, Plane> +{ + static void compute(const Plane& s, const Transform3& tf, RSS& bv) + { + Vector3 n = tf.linear() * s.n; + + bv.axis.col(0) = n; + generateCoordinateSystem(bv.axis); + + bv.l[0] = std::numeric_limits::max(); + bv.l[1] = std::numeric_limits::max(); + + bv.r = 0; + + Vector3 p = s.n * s.d; + bv.To = tf * p; + } +}; + +//============================================================================== +template +struct BVComputer, Plane> +{ + static void compute(const Plane& s, const Transform3& tf, OBBRSS& bv) + { + computeBV(s, tf, bv.obb); + computeBV(s, tf, bv.rss); + } +}; + +//============================================================================== +template +struct BVComputer, Plane> +{ + static void compute(const Plane& s, const Transform3& tf, kIOS& bv) + { + bv.num_spheres = 1; + computeBV(s, tf, bv.obb); + bv.spheres[0].o.setZero(); + bv.spheres[0].r = std::numeric_limits::max(); + } +}; + +//============================================================================== +template +struct BVComputer, Plane> +{ + static void compute(const Plane& s, const Transform3& tf, KDOP& bv) + { + Plane new_s = transform(s, tf); + const Vector3& n = new_s.n; + const S& d = new_s.d; + + const std::size_t D = 8; + + for(std::size_t i = 0; i < D; ++i) + bv.dist(i) = -std::numeric_limits::max(); + for(std::size_t i = D; i < 2 * D; ++i) + bv.dist(i) = std::numeric_limits::max(); + + if(n[1] == (S)0.0 && n[2] == (S)0.0) + { + if(n[0] > 0) bv.dist(0) = bv.dist(D) = d; + else bv.dist(0) = bv.dist(D) = -d; + } + else if(n[0] == (S)0.0 && n[2] == (S)0.0) + { + if(n[1] > 0) bv.dist(1) = bv.dist(D + 1) = d; + else bv.dist(1) = bv.dist(D + 1) = -d; + } + else if(n[0] == (S)0.0 && n[1] == (S)0.0) + { + if(n[2] > 0) bv.dist(2) = bv.dist(D + 2) = d; + else bv.dist(2) = bv.dist(D + 2) = -d; + } + else if(n[2] == (S)0.0 && n[0] == n[1]) + { + bv.dist(3) = bv.dist(D + 3) = n[0] * d * 2; + } + else if(n[1] == (S)0.0 && n[0] == n[2]) + { + bv.dist(4) = bv.dist(D + 4) = n[0] * d * 2; + } + else if(n[0] == (S)0.0 && n[1] == n[2]) + { + bv.dist(6) = bv.dist(D + 5) = n[1] * d * 2; + } + else if(n[2] == (S)0.0 && n[0] + n[1] == (S)0.0) + { + bv.dist(6) = bv.dist(D + 6) = n[0] * d * 2; + } + else if(n[1] == (S)0.0 && n[0] + n[2] == (S)0.0) + { + bv.dist(7) = bv.dist(D + 7) = n[0] * d * 2; + } + } +}; + +//============================================================================== +template +struct BVComputer, Plane> +{ + static void compute(const Plane& s, const Transform3& tf, KDOP& bv) + { + Plane new_s = transform(s, tf); + const Vector3& n = new_s.n; + const S& d = new_s.d; + + const std::size_t D = 9; + + for(std::size_t i = 0; i < D; ++i) + bv.dist(i) = -std::numeric_limits::max(); + for(std::size_t i = D; i < 2 * D; ++i) + bv.dist(i) = std::numeric_limits::max(); + + if(n[1] == (S)0.0 && n[2] == (S)0.0) + { + if(n[0] > 0) bv.dist(0) = bv.dist(D) = d; + else bv.dist(0) = bv.dist(D) = -d; + } + else if(n[0] == (S)0.0 && n[2] == (S)0.0) + { + if(n[1] > 0) bv.dist(1) = bv.dist(D + 1) = d; + else bv.dist(1) = bv.dist(D + 1) = -d; + } + else if(n[0] == (S)0.0 && n[1] == (S)0.0) + { + if(n[2] > 0) bv.dist(2) = bv.dist(D + 2) = d; + else bv.dist(2) = bv.dist(D + 2) = -d; + } + else if(n[2] == (S)0.0 && n[0] == n[1]) + { + bv.dist(3) = bv.dist(D + 3) = n[0] * d * 2; + } + else if(n[1] == (S)0.0 && n[0] == n[2]) + { + bv.dist(4) = bv.dist(D + 4) = n[0] * d * 2; + } + else if(n[0] == (S)0.0 && n[1] == n[2]) + { + bv.dist(5) = bv.dist(D + 5) = n[1] * d * 2; + } + else if(n[2] == (S)0.0 && n[0] + n[1] == (S)0.0) + { + bv.dist(6) = bv.dist(D + 6) = n[0] * d * 2; + } + else if(n[1] == (S)0.0 && n[0] + n[2] == (S)0.0) + { + bv.dist(7) = bv.dist(D + 7) = n[0] * d * 2; + } + else if(n[0] == (S)0.0 && n[1] + n[2] == (S)0.0) + { + bv.dist(8) = bv.dist(D + 8) = n[1] * d * 2; + } + } +}; + +//============================================================================== +template +struct BVComputer, Plane> +{ + static void compute(const Plane& s, const Transform3& tf, KDOP& bv) + { + Plane new_s = transform(s, tf); + const Vector3& n = new_s.n; + const S& d = new_s.d; + + const std::size_t D = 12; + + for(std::size_t i = 0; i < D; ++i) + bv.dist(i) = -std::numeric_limits::max(); + for(std::size_t i = D; i < 2 * D; ++i) + bv.dist(i) = std::numeric_limits::max(); + + if(n[1] == (S)0.0 && n[2] == (S)0.0) + { + if(n[0] > 0) bv.dist(0) = bv.dist(D) = d; + else bv.dist(0) = bv.dist(D) = -d; + } + else if(n[0] == (S)0.0 && n[2] == (S)0.0) + { + if(n[1] > 0) bv.dist(1) = bv.dist(D + 1) = d; + else bv.dist(1) = bv.dist(D + 1) = -d; + } + else if(n[0] == (S)0.0 && n[1] == (S)0.0) + { + if(n[2] > 0) bv.dist(2) = bv.dist(D + 2) = d; + else bv.dist(2) = bv.dist(D + 2) = -d; + } + else if(n[2] == (S)0.0 && n[0] == n[1]) + { + bv.dist(3) = bv.dist(D + 3) = n[0] * d * 2; + } + else if(n[1] == (S)0.0 && n[0] == n[2]) + { + bv.dist(4) = bv.dist(D + 4) = n[0] * d * 2; + } + else if(n[0] == (S)0.0 && n[1] == n[2]) + { + bv.dist(5) = bv.dist(D + 5) = n[1] * d * 2; + } + else if(n[2] == (S)0.0 && n[0] + n[1] == (S)0.0) + { + bv.dist(6) = bv.dist(D + 6) = n[0] * d * 2; + } + else if(n[1] == (S)0.0 && n[0] + n[2] == (S)0.0) + { + bv.dist(7) = bv.dist(D + 7) = n[0] * d * 2; + } + else if(n[0] == (S)0.0 && n[1] + n[2] == (S)0.0) + { + bv.dist(8) = bv.dist(D + 8) = n[1] * d * 2; + } + else if(n[0] + n[2] == (S)0.0 && n[0] + n[1] == (S)0.0) + { + bv.dist(9) = bv.dist(D + 9) = n[0] * d * 3; + } + else if(n[0] + n[1] == (S)0.0 && n[1] + n[2] == (S)0.0) + { + bv.dist(10) = bv.dist(D + 10) = n[0] * d * 3; + } + else if(n[0] + n[1] == (S)0.0 && n[0] + n[2] == (S)0.0) + { + bv.dist(11) = bv.dist(D + 11) = n[1] * d * 3; + } + } +}; + +} // namespace detail +} // namespace fcl + +#endif diff --git a/src/articulated_model/joint_config.cpp b/include/fcl/shape/detail/bv_computer_sphere.h similarity index 56% rename from src/articulated_model/joint_config.cpp rename to include/fcl/shape/detail/bv_computer_sphere.h index e06a8bfd3..698180c1d 100644 --- a/src/articulated_model/joint_config.cpp +++ b/include/fcl/shape/detail/bv_computer_sphere.h @@ -33,75 +33,56 @@ * POSSIBILITY OF SUCH DAMAGE. */ -/** \author Dalibor Matura, Jia Pan */ +/** \author Jia Pan */ -#include "fcl/articulated_model/joint_config.h" -#include "fcl/articulated_model/joint.h" +#ifndef FCL_SHAPE_DETAIL_BVCOMPUTERSPHERE_H +#define FCL_SHAPE_DETAIL_BVCOMPUTERSPHERE_H -namespace fcl -{ - -JointConfig::JointConfig() {} - -JointConfig::JointConfig(const JointConfig& joint_cfg) : - joint_(joint_cfg.joint_), - values_(joint_cfg.values_), - limits_min_(joint_cfg.limits_min_), - limits_max_(joint_cfg.limits_max_) -{ -} - -JointConfig::JointConfig(const std::shared_ptr& joint, - FCL_REAL default_value, - FCL_REAL default_value_min, - FCL_REAL default_value_max) : - joint_(joint) -{ - values_.resize(joint->getNumDofs(), default_value); - limits_min_.resize(joint->getNumDofs(), default_value_min); - limits_max_.resize(joint->getNumDofs(), default_value_max); -} - -std::size_t JointConfig::getDim() const -{ - return values_.size(); -} +#include "fcl/BV/AABB.h" +#include "fcl/BV/OBB.h" -FCL_REAL JointConfig::getValue(std::size_t i) const +namespace fcl { - return values_[i]; -} - -FCL_REAL& JointConfig::getValue(std::size_t i) +namespace detail { - return values_[i]; -} -FCL_REAL JointConfig::getLimitMin(std::size_t i) const -{ - return limits_min_[i]; -} +template +struct BVComputer, Sphere>; -FCL_REAL& JointConfig::getLimitMin(std::size_t i) -{ - return limits_min_[i]; -} +template +struct BVComputer, Sphere>; -FCL_REAL JointConfig::getLimitMax(std::size_t i) const -{ - return limits_max_[i]; -} +//============================================================================// +// // +// Implementations // +// // +//============================================================================// -FCL_REAL& JointConfig::getLimitMax(std::size_t i) +//============================================================================== +template +struct BVComputer, Sphere> { - return limits_max_[i]; -} - - - -std::shared_ptr JointConfig::getJoint() const + static void compute(const Sphere& s, const Transform3& tf, AABB& bv) + { + const Vector3 v_delta = Vector3::Constant(s.radius); + bv.max_ = tf.translation() + v_delta; + bv.min_ = tf.translation() - v_delta; + } +}; + +//============================================================================== +template +struct BVComputer, Sphere> { - return joint_.lock(); -} - -} + static void compute(const Sphere& s, const Transform3& tf, OBB& bv) + { + bv.To = tf.translation(); + bv.axis.setIdentity(); + bv.extent.setConstant(s.radius); + } +}; + +} // namespace detail +} // namespace fcl + +#endif diff --git a/include/fcl/shape/detail/bv_computer_triangle_p.h b/include/fcl/shape/detail/bv_computer_triangle_p.h new file mode 100644 index 000000000..131c8a859 --- /dev/null +++ b/include/fcl/shape/detail/bv_computer_triangle_p.h @@ -0,0 +1,70 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_SHAPE_DETAIL_BVCOMPUTERTRIANGLEP_H +#define FCL_SHAPE_DETAIL_BVCOMPUTERTRIANGLEP_H + +#include "fcl/BV/AABB.h" + +namespace fcl +{ +namespace detail +{ + +template +struct BVComputer, TrianglePd>; + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +struct BVComputer, TrianglePd> +{ + static void compute(const TrianglePd& s, const Transform3& tf, AABB& bv) + { + bv = AABB(tf * s.a, tf * s.b, tf * s.c); + } +}; + +} // namespace detail +} // namespace fcl + +#endif diff --git a/include/fcl/shape/ellipsoid.h b/include/fcl/shape/ellipsoid.h new file mode 100644 index 000000000..0115db5ae --- /dev/null +++ b/include/fcl/shape/ellipsoid.h @@ -0,0 +1,191 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + + +#ifndef FCL_SHAPE_ELLIPSOID_H +#define FCL_SHAPE_ELLIPSOID_H + +#include "fcl/shape/shape_base.h" +#include "fcl/shape/compute_bv.h" +#include "fcl/BV/OBB.h" + +namespace fcl +{ + +/// @brief Center at zero point ellipsoid +template +class Ellipsoid : public ShapeBase +{ +public: + + using S = S_; + + /// @brief Constructor + Ellipsoid(S a, S b, S c); + + /// @brief Constructor + Ellipsoid(const Vector3& radii); + + /// @brief Radii of the ellipsoid + Vector3 radii; + + /// @brief Compute AABBd + void computeLocalAABB() override; + + /// @brief Get node type: a sphere + NODE_TYPE getNodeType() const override; + + // Documentation inherited + Matrix3 computeMomentofInertia() const override; + + // Documentation inherited + S computeVolume() const override; + + /// @brief get the vertices of some convex shape which can bound this shape in + /// a specific configuration + std::vector> getBoundVertices( + const Transform3& tf) const; +}; + +using Ellipsoidf = Ellipsoid; +using Ellipsoidd = Ellipsoid; + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +Ellipsoid::Ellipsoid(S a, S b, S c) + : ShapeBase(), radii(a, b, c) +{ + // Do nothing +} + +//============================================================================== +template +Ellipsoid::Ellipsoid(const Vector3& radii) + : ShapeBase(), radii(radii) +{ + // Do nothing +} + +//============================================================================== +template +void Ellipsoid::computeLocalAABB() +{ + computeBV(*this, Transform3::Identity(), this->aabb_local); + this->aabb_center = this->aabb_local.center(); + this->aabb_radius = (this->aabb_local.min_ - this->aabb_center).norm(); +} + +//============================================================================== +template +NODE_TYPE Ellipsoid::getNodeType() const +{ + return GEOM_ELLIPSOID; +} + +//============================================================================== +template +Matrix3 Ellipsoid::computeMomentofInertia() const +{ + const S V = computeVolume(); + + const S a2 = radii[0] * radii[0] * V; + const S b2 = radii[1] * radii[1] * V; + const S c2 = radii[2] * radii[2] * V; + + return Vector3(0.2 * (b2 + c2), 0.2 * (a2 + c2), 0.2 * (a2 + b2)).asDiagonal(); +} + +//============================================================================== +template +S Ellipsoid::computeVolume() const +{ + const S pi = constants::pi(); + return 4.0 * pi * radii[0] * radii[1] * radii[2] / 3.0; +} + +//============================================================================== +template +std::vector> Ellipsoid::getBoundVertices( + const Transform3& tf) const +{ + // we use scaled icosahedron to bound the ellipsoid + + std::vector> result(12); + + const auto phi = (1.0 + std::sqrt(5.0)) / 2.0; // golden ratio + + const auto a = std::sqrt(3.0) / (phi * phi); + const auto b = phi * a; + + const auto& A = radii[0]; + const auto& B = radii[1]; + const auto& C = radii[2]; + + const auto Aa = A * a; + const auto Ab = A * b; + const auto Ba = B * a; + const auto Bb = B * b; + const auto Ca = C * a; + const auto Cb = C * b; + + result[0] = tf * Vector3(0, Ba, Cb); + result[1] = tf * Vector3(0, -Ba, Cb); + result[2] = tf * Vector3(0, Ba, -Cb); + result[3] = tf * Vector3(0, -Ba, -Cb); + result[4] = tf * Vector3(Aa, Bb, 0); + result[5] = tf * Vector3(-Aa, Bb, 0); + result[6] = tf * Vector3(Aa, -Bb, 0); + result[7] = tf * Vector3(-Aa, -Bb, 0); + result[8] = tf * Vector3(Ab, 0, Ca); + result[9] = tf * Vector3(Ab, 0, -Ca); + result[10] = tf * Vector3(-Ab, 0, Ca); + result[11] = tf * Vector3(-Ab, 0, -Ca); + + return result; +} + +} // namespace fcl + +#include "fcl/shape/detail/bv_computer_ellipsoid.h" + +#endif diff --git a/include/fcl/shape/geometric_shape_to_BVH_model.h b/include/fcl/shape/geometric_shape_to_BVH_model.h index 8eabec8ee..a534f153c 100644 --- a/include/fcl/shape/geometric_shape_to_BVH_model.h +++ b/include/fcl/shape/geometric_shape_to_BVH_model.h @@ -35,9 +35,8 @@ /** \author Jia Pan */ - -#ifndef GEOMETRIC_SHAPE_TO_BVH_MODEL_H -#define GEOMETRIC_SHAPE_TO_BVH_MODEL_H +#ifndef FCL_SHAPE_GEOMETRICSHAPETOBVHMODEL_H +#define FCL_SHAPE_GEOMETRICSHAPETOBVHMODEL_H #include "fcl/shape/geometric_shapes.h" #include "fcl/BVH/BVH_model.h" @@ -47,12 +46,65 @@ namespace fcl /// @brief Generate BVH model from box template -void generateBVHModel(BVHModel& model, const Box& shape, const Transform3d& pose) +void generateBVHModel(BVHModel& model, const Box& shape, const Transform3& pose); + +/// @brief Generate BVH model from sphere, given the number of segments along longitude and number of rings along latitude. +template +void generateBVHModel(BVHModel& model, const Sphere& shape, const 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 +template +void generateBVHModel(BVHModel& model, const Sphere& shape, const Transform3& pose, unsigned int n_faces_for_unit_sphere); + +/// @brief Generate BVH model from ellipsoid, given the number of segments along longitude and 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 +template +void generateBVHModel(BVHModel& model, const Ellipsoid& shape, const Transform3& pose, unsigned int n_faces_for_unit_ellipsoid); + +/// @brief Generate BVH model from cylinder, given the number of segments along circle and the number of segments along axis. +template +void generateBVHModel(BVHModel& model, const Cylinder& shape, const 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. +template +void generateBVHModel(BVHModel& model, const Cylinder& shape, const Transform3& pose, unsigned int tot_for_unit_cylinder); + +/// @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. +template +void generateBVHModel(BVHModel& model, const Cone& shape, const Transform3& pose, unsigned int tot_for_unit_cone); + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +void generateBVHModel(BVHModel& model, const Box& shape, const Transform3& pose) { - double a = shape.side[0]; - double b = shape.side[1]; - double c = shape.side[2]; - std::vector points(8); + using S = typename BV::S; + + S a = shape.side[0]; + S b = shape.side[1]; + S c = shape.side[2]; + std::vector> points(8); std::vector tri_indices(12); points[0] << 0.5 * a, -0.5 * b, 0.5 * c; points[1] << 0.5 * a, 0.5 * b, 0.5 * c; @@ -87,33 +139,35 @@ void generateBVHModel(BVHModel& model, const Box& shape, const Transform3d& model.computeLocalAABB(); } -/// @brief Generate BVH model from sphere, given the number of segments along longitude and number of rings along latitude. +//============================================================================== template -void generateBVHModel(BVHModel& model, const Sphere& shape, const Transform3d& pose, unsigned int seg, unsigned int ring) +void generateBVHModel(BVHModel& model, const Sphere& shape, const Transform3& pose, unsigned int seg, unsigned int ring) { - std::vector points; + using S = typename BV::S; + + std::vector> points; std::vector tri_indices; - double r = shape.radius; - double phi, phid; - const double pi = constants::pi; + S r = shape.radius; + S phi, phid; + const S pi = constants::pi(); phid = pi * 2 / seg; phi = 0; - double theta, thetad; + S theta, thetad; thetad = pi / (ring + 1); theta = 0; for(unsigned int i = 0; i < ring; ++i) { - double theta_ = theta + thetad * (i + 1); + S theta_ = theta + thetad * (i + 1); for(unsigned int j = 0; j < seg; ++j) { - points.push_back(Vector3d(r * sin(theta_) * cos(phi + j * phid), r * sin(theta_) * sin(phi + j * phid), r * cos(theta_))); + points.emplace_back(r * sin(theta_) * cos(phi + j * phid), r * sin(theta_) * sin(phi + j * phid), r * cos(theta_)); } } - points.push_back(Vector3d(0, 0, r)); - points.push_back(Vector3d(0, 0, -r)); + points.emplace_back(0, 0, r); + points.emplace_back(0, 0, -r); for(unsigned int i = 0; i < ring - 1; ++i) { @@ -124,8 +178,8 @@ void generateBVHModel(BVHModel& model, const Sphere& shape, const Transform3 b = (j == seg - 1) ? (i * seg) : (i * seg + j + 1); c = (i + 1) * seg + j; d = (j == seg - 1) ? ((i + 1) * seg) : ((i + 1) * seg + j + 1); - tri_indices.push_back(Triangle(a, c, b)); - tri_indices.push_back(Triangle(b, c, d)); + tri_indices.emplace_back(a, c, b); + tri_indices.emplace_back(b, c, d); } } @@ -134,11 +188,11 @@ void generateBVHModel(BVHModel& model, const Sphere& shape, const Transform3 unsigned int a, b; a = j; b = (j == seg - 1) ? 0 : (j + 1); - tri_indices.push_back(Triangle(ring * seg, a, b)); + tri_indices.emplace_back(ring * seg, a, b); a = (ring - 1) * seg + j; b = (j == seg - 1) ? (ring - 1) * seg : ((ring - 1) * seg + j + 1); - tri_indices.push_back(Triangle(a, ring * seg + 1, b)); + tri_indices.emplace_back(a, ring * seg + 1, b); } for(unsigned int i = 0; i < points.size(); ++i) @@ -152,50 +206,52 @@ void generateBVHModel(BVHModel& model, const Sphere& shape, const Transform3 model.computeLocalAABB(); } -/// @brief Generate BVH model from sphere -/// The difference between generateBVHModel is that it gives the number of triangles faces N for a sphere with unit radius. For sphere of radius r, -/// then the number of triangles is r * r * N so that the area represented by a single triangle is approximately the same.s +//============================================================================== template -void generateBVHModel(BVHModel& model, const Sphere& shape, const Transform3d& pose, unsigned int n_faces_for_unit_sphere) +void generateBVHModel(BVHModel& model, const Sphere& shape, const Transform3& pose, unsigned int n_faces_for_unit_sphere) { - double r = shape.radius; - double n_low_bound = sqrtf(n_faces_for_unit_sphere / 2.0) * r * r; + using S = typename BV::S; + + S r = shape.radius; + S n_low_bound = sqrtf(n_faces_for_unit_sphere / 2.0) * r * r; unsigned int ring = ceil(n_low_bound); unsigned int seg = ceil(n_low_bound); - generateBVHModel(model, shape, pose, seg, ring); + generateBVHModel(model, shape, pose, seg, ring); } -/// @brief Generate BVH model from ellipsoid, given the number of segments along longitude and number of rings along latitude. +//============================================================================== template -void generateBVHModel(BVHModel& model, const Ellipsoid& shape, const Transform3d& pose, unsigned int seg, unsigned int ring) +void generateBVHModel(BVHModel& model, const Ellipsoid& shape, const Transform3& pose, unsigned int seg, unsigned int ring) { - std::vector points; + using S = typename BV::S; + + std::vector> points; std::vector tri_indices; - const FCL_REAL& a = shape.radii[0]; - const FCL_REAL& b = shape.radii[1]; - const FCL_REAL& c = shape.radii[2]; + const S& a = shape.radii[0]; + const S& b = shape.radii[1]; + const S& c = shape.radii[2]; - FCL_REAL phi, phid; - const FCL_REAL pi = constants::pi; + S phi, phid; + const S pi = constants::pi(); phid = pi * 2 / seg; phi = 0; - FCL_REAL theta, thetad; + S theta, thetad; thetad = pi / (ring + 1); theta = 0; for(unsigned int i = 0; i < ring; ++i) { - double theta_ = theta + thetad * (i + 1); + S theta_ = theta + thetad * (i + 1); for(unsigned int j = 0; j < seg; ++j) { - points.push_back(Vector3d(a * sin(theta_) * cos(phi + j * phid), b * sin(theta_) * sin(phi + j * phid), c * cos(theta_))); + points.emplace_back(a * sin(theta_) * cos(phi + j * phid), b * sin(theta_) * sin(phi + j * phid), c * cos(theta_)); } } - points.push_back(Vector3d(0, 0, c)); - points.push_back(Vector3d(0, 0, -c)); + points.emplace_back(0, 0, c); + points.emplace_back(0, 0, -c); for(unsigned int i = 0; i < ring - 1; ++i) { @@ -206,8 +262,8 @@ void generateBVHModel(BVHModel& model, const Ellipsoid& shape, const Transfo b = (j == seg - 1) ? (i * seg) : (i * seg + j + 1); c = (i + 1) * seg + j; d = (j == seg - 1) ? ((i + 1) * seg) : ((i + 1) * seg + j + 1); - tri_indices.push_back(Triangle(a, c, b)); - tri_indices.push_back(Triangle(b, c, d)); + tri_indices.emplace_back(a, c, b); + tri_indices.emplace_back(b, c, d); } } @@ -216,11 +272,11 @@ void generateBVHModel(BVHModel& model, const Ellipsoid& shape, const Transfo unsigned int a, b; a = j; b = (j == seg - 1) ? 0 : (j + 1); - tri_indices.push_back(Triangle(ring * seg, a, b)); + tri_indices.emplace_back(ring * seg, a, b); a = (ring - 1) * seg + j; b = (j == seg - 1) ? (ring - 1) * seg : ((ring - 1) * seg + j + 1); - tri_indices.push_back(Triangle(a, ring * seg + 1, b)); + tri_indices.emplace_back(a, ring * seg + 1, b); } for(unsigned int i = 0; i < points.size(); ++i) @@ -234,21 +290,20 @@ void generateBVHModel(BVHModel& model, const Ellipsoid& shape, const Transfo model.computeLocalAABB(); } -/// @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 +//============================================================================== template -void generateBVHModel(BVHModel& model, const Ellipsoid& shape, const Transform3d& pose, unsigned int n_faces_for_unit_ellipsoid) +void generateBVHModel(BVHModel& model, const Ellipsoid& shape, const Transform3& pose, unsigned int n_faces_for_unit_ellipsoid) { - const FCL_REAL p = 1.6075; + using S = typename BV::S; + + const S p = 1.6075; - const FCL_REAL& ap = std::pow(shape.radii[0], p); - const FCL_REAL& bp = std::pow(shape.radii[1], p); - const FCL_REAL& cp = std::pow(shape.radii[2], p); + const S& ap = std::pow(shape.radii[0], p); + const S& bp = std::pow(shape.radii[1], p); + const S& cp = std::pow(shape.radii[2], p); - const FCL_REAL ratio = std::pow((ap * bp + bp * cp + cp * ap) / 3.0, 1.0 / p); - const FCL_REAL n_low_bound = std::sqrt(n_faces_for_unit_ellipsoid / 2.0) * ratio; + const S ratio = std::pow((ap * bp + bp * cp + cp * ap) / 3.0, 1.0 / p); + const S n_low_bound = std::sqrt(n_faces_for_unit_ellipsoid / 2.0) * ratio; const unsigned int ring = std::ceil(n_low_bound); const unsigned int seg = std::ceil(n_low_bound); @@ -256,50 +311,46 @@ void generateBVHModel(BVHModel& model, const Ellipsoid& shape, const Transfo generateBVHModel(model, shape, pose, seg, ring); } -/// @brief Generate BVH model from cylinder, given the number of segments along circle and the number of segments along axis. +//============================================================================== template -void generateBVHModel(BVHModel& model, const Cylinder& shape, const Transform3d& pose, unsigned int tot, unsigned int h_num) +void generateBVHModel(BVHModel& model, const Cylinder& shape, const Transform3& pose, unsigned int tot, unsigned int h_num) { - std::vector points; + using S = typename BV::S; + + std::vector> points; std::vector tri_indices; - double r = shape.radius; - double h = shape.lz; - double phi, phid; - const double pi = constants::pi; + S r = shape.radius; + S h = shape.lz; + S phi, phid; + const S pi = constants::pi(); phid = pi * 2 / tot; phi = 0; - double hd = h / h_num; + S hd = h / h_num; for(unsigned int i = 0; i < tot; ++i) - points.push_back(Vector3d(r * cos(phi + phid * i), r * sin(phi + phid * i), h / 2)); + 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) { - points.push_back(Vector3d(r * cos(phi + phid * j), r * sin(phi + phid * j), h / 2 - (i + 1) * hd)); + 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) - points.push_back(Vector3d(r * cos(phi + phid * i), r * sin(phi + phid * i), - h / 2)); + points.emplace_back(r * cos(phi + phid * i), r * sin(phi + phid * i), - h / 2); - points.push_back(Vector3d(0, 0, h / 2)); - points.push_back(Vector3d(0, 0, -h / 2)); + points.emplace_back(0, 0, h / 2); + points.emplace_back(0, 0, -h / 2); for(unsigned int i = 0; i < tot; ++i) - { - Triangle tmp((h_num + 1) * tot, i, ((i == tot - 1) ? 0 : (i + 1))); - tri_indices.push_back(tmp); - } + tri_indices.emplace_back((h_num + 1) * tot, i, ((i == tot - 1) ? 0 : (i + 1))); for(unsigned int i = 0; i < tot; ++i) - { - Triangle tmp((h_num + 1) * tot + 1, h_num * tot + ((i == tot - 1) ? 0 : (i + 1)), h_num * tot + i); - tri_indices.push_back(tmp); - } + 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 < h_num; ++i) { @@ -312,8 +363,8 @@ void generateBVHModel(BVHModel& model, const Cylinder& shape, const Transfor d = (j == tot - 1) ? tot : (j + 1 + tot); int start = i * tot; - tri_indices.push_back(Triangle(start + b, start + a, start + c)); - tri_indices.push_back(Triangle(start + b, start + c, start + d)); + tri_indices.emplace_back(start + b, start + a, start + c); + tri_indices.emplace_back(start + b, start + c, start + d); } } @@ -328,70 +379,66 @@ void generateBVHModel(BVHModel& model, const Cylinder& shape, const Transfor model.computeLocalAABB(); } -/// @brief Generate BVH model from cylinder -/// Difference from generateBVHModel: is that it gives the circle split number tot for a cylinder with unit radius. For cylinder with -/// larger radius, the number of circle split number is r * tot. +//============================================================================== template -void generateBVHModel(BVHModel& model, const Cylinder& shape, const Transform3d& pose, unsigned int tot_for_unit_cylinder) +void generateBVHModel(BVHModel& model, const Cylinder& shape, const Transform3& pose, unsigned int tot_for_unit_cylinder) { - double r = shape.radius; - double h = shape.lz; + using S = typename BV::S; - const double pi = constants::pi; + S r = shape.radius; + S h = shape.lz; + + const S pi = constants::pi(); unsigned int tot = tot_for_unit_cylinder * r; - double phid = pi * 2 / tot; + S phid = pi * 2 / tot; - double circle_edge = phid * r; + S circle_edge = phid * r; unsigned int h_num = ceil(h / circle_edge); - + generateBVHModel(model, shape, pose, tot, h_num); } -/// @brief Generate BVH model from cone, given the number of segments along circle and the number of segments along axis. +//============================================================================== template -void generateBVHModel(BVHModel& model, const Cone& shape, const Transform3d& pose, unsigned int tot, unsigned int h_num) +void generateBVHModel(BVHModel& model, const Cone& shape, const Transform3& pose, unsigned int tot, unsigned int h_num) { - std::vector points; + using S = typename BV::S; + + std::vector> points; std::vector tri_indices; - double r = shape.radius; - double h = shape.lz; + S r = shape.radius; + S h = shape.lz; - double phi, phid; - const double pi = constants::pi; + S phi, phid; + const S pi = constants::pi(); phid = pi * 2 / tot; phi = 0; - double hd = h / h_num; + S hd = h / h_num; for(unsigned int i = 0; i < h_num - 1; ++i) { - double h_i = h / 2 - (i + 1) * hd; - double rh = r * (0.5 - h_i / h); + S h_i = h / 2 - (i + 1) * hd; + S rh = r * (0.5 - h_i / h); for(unsigned int j = 0; j < tot; ++j) { - points.push_back(Vector3d(rh * cos(phi + phid * j), rh * sin(phi + phid * j), h_i)); + points.emplace_back(rh * cos(phi + phid * j), rh * sin(phi + phid * j), h_i); } } for(unsigned int i = 0; i < tot; ++i) - points.push_back(Vector3d(r * cos(phi + phid * i), r * sin(phi + phid * i), - h / 2)); + points.emplace_back(r * cos(phi + phid * i), r * sin(phi + phid * i), - h / 2); - points.push_back(Vector3d(0, 0, h / 2)); - points.push_back(Vector3d(0, 0, -h / 2)); + points.emplace_back(0, 0, h / 2); + points.emplace_back(0, 0, -h / 2); for(unsigned int i = 0; i < tot; ++i) - { - Triangle tmp(h_num * tot, i, (i == tot - 1) ? 0 : (i + 1)); - tri_indices.push_back(tmp); - } + tri_indices.emplace_back(h_num * tot, i, (i == tot - 1) ? 0 : (i + 1)); for(unsigned int i = 0; i < tot; ++i) - { - Triangle tmp(h_num * tot + 1, (h_num - 1) * tot + ((i == tot - 1) ? 0 : (i + 1)), (h_num - 1) * tot + i); - tri_indices.push_back(tmp); - } + 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 < h_num - 1; ++i) { @@ -404,8 +451,8 @@ void generateBVHModel(BVHModel& model, const Cone& shape, const Transform3d& d = (j == tot - 1) ? tot : (j + 1 + tot); int start = i * tot; - tri_indices.push_back(Triangle(start + b, start + a, start + c)); - tri_indices.push_back(Triangle(start + b, start + c, start + d)); + tri_indices.emplace_back(start + b, start + a, start + c); + tri_indices.emplace_back(start + b, start + c, start + d); } } @@ -420,25 +467,25 @@ void generateBVHModel(BVHModel& model, const Cone& shape, const Transform3d& model.computeLocalAABB(); } -/// @brief Generate BVH model from cone -/// Difference from generateBVHModel: is that it gives the circle split number tot for a cylinder with unit radius. For cone with -/// larger radius, the number of circle split number is r * tot. +//============================================================================== template -void generateBVHModel(BVHModel& model, const Cone& shape, const Transform3d& pose, unsigned int tot_for_unit_cone) +void generateBVHModel(BVHModel& model, const Cone& shape, const Transform3& pose, unsigned int tot_for_unit_cone) { - double r = shape.radius; - double h = shape.lz; + using S = typename BV::S; - const double pi = constants::pi; + S r = shape.radius; + S h = shape.lz; + + const S pi = constants::pi(); unsigned int tot = tot_for_unit_cone * r; - double phid = pi * 2 / tot; + S phid = pi * 2 / tot; - double circle_edge = phid * r; + S circle_edge = phid * r; unsigned int h_num = ceil(h / circle_edge); generateBVHModel(model, shape, pose, tot, h_num); } -} +} // namespace fcl #endif diff --git a/include/fcl/shape/geometric_shapes.h b/include/fcl/shape/geometric_shapes.h index 4d28954bb..3f86194bc 100644 --- a/include/fcl/shape/geometric_shapes.h +++ b/include/fcl/shape/geometric_shapes.h @@ -35,570 +35,18 @@ /** \author Jia Pan */ - -#ifndef FCL_GEOMETRIC_SHAPES_H -#define FCL_GEOMETRIC_SHAPES_H - -#include "fcl/collision_object.h" -#include - -namespace fcl -{ - -/// @brief Base class for all basic geometric shapes -class ShapeBase : public CollisionGeometry -{ -public: - ShapeBase() {} - - /// @brief Get object type: a geometric shape - OBJECT_TYPE getObjectType() const { return OT_GEOM; } -}; - -/// @brief Triangle stores the points instead of only indices of points -class TriangleP : public ShapeBase -{ -public: - TriangleP(const Vector3d& a_, const Vector3d& b_, const Vector3d& c_) : ShapeBase(), a(a_), b(b_), c(c_) - { - } - - /// @brief virtual function of compute AABB in local coordinate - void computeLocalAABB(); - - NODE_TYPE getNodeType() const { return GEOM_TRIANGLE; } - - Vector3d a, b, c; -}; - -/// @brief Center at zero point, axis aligned box -class Box : public ShapeBase -{ -public: - Box(FCL_REAL x, FCL_REAL y, FCL_REAL z) : ShapeBase(), side(x, y, z) - { - } - - Box(const Vector3d& side_) : ShapeBase(), side(side_) - { - } - - Box() {} - - /// @brief box side length - Vector3d side; - - /// @brief Compute AABB - void computeLocalAABB(); - - /// @brief Get node type: a box - NODE_TYPE getNodeType() const { return GEOM_BOX; } - - FCL_REAL computeVolume() const - { - return side[0] * side[1] * side[2]; - } - - Matrix3d computeMomentofInertia() const - { - FCL_REAL V = computeVolume(); - - FCL_REAL a2 = side[0] * side[0] * V; - FCL_REAL b2 = side[1] * side[1] * V; - FCL_REAL c2 = side[2] * side[2] * V; - - Vector3d I((b2 + c2) / 12, (a2 + c2) / 12, (a2 + b2) / 12); - - return I.asDiagonal(); - } -}; - -/// @brief Center at zero point sphere -class Sphere : public ShapeBase -{ -public: - Sphere(FCL_REAL radius_) : ShapeBase(), radius(radius_) - { - } - - /// @brief Radius of the sphere - FCL_REAL radius; - - /// @brief Compute AABB - void computeLocalAABB(); - - /// @brief Get node type: a sphere - NODE_TYPE getNodeType() const { return GEOM_SPHERE; } - - Matrix3d computeMomentofInertia() const - { - FCL_REAL I = 0.4 * radius * radius * computeVolume(); - - return Vector3d::Constant(I).asDiagonal(); - } - - FCL_REAL computeVolume() const - { - return 4.0 * constants::pi * radius * radius * radius / 3.0; - } -}; - -/// @brief Center at zero point ellipsoid -class Ellipsoid : public ShapeBase -{ -public: - Ellipsoid(FCL_REAL a, FCL_REAL b, FCL_REAL c) : ShapeBase(), radii(a, b, c) - { - } - - Ellipsoid(const Vector3d& radii_) : ShapeBase(), radii(radii_) - { - } - - /// @brief Radii of the ellipsoid - Vector3d radii; - - /// @brief Compute AABB - void computeLocalAABB(); - - /// @brief Get node type: a sphere - NODE_TYPE getNodeType() const { return GEOM_ELLIPSOID; } - - Matrix3d computeMomentofInertia() const - { - const FCL_REAL V = computeVolume(); - - const FCL_REAL a2 = radii[0] * radii[0] * V; - const FCL_REAL b2 = radii[1] * radii[1] * V; - const FCL_REAL c2 = radii[2] * radii[2] * V; - - return Vector3d(0.2 * (b2 + c2), 0.2 * (a2 + c2), 0.2 * (a2 + b2)).asDiagonal(); - } - - FCL_REAL computeVolume() const - { - const FCL_REAL pi = constants::pi; - return 4.0 * pi * radii[0] * radii[1] * radii[2] / 3.0; - } -}; - -/// @brief Center at zero point capsule -class Capsule : public ShapeBase -{ -public: - Capsule(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_), lz(lz_) - { - } - - /// @brief Radius of capsule - FCL_REAL radius; - - /// @brief Length along z axis - FCL_REAL lz; - - /// @brief Compute AABB - void computeLocalAABB(); - - /// @brief Get node type: a capsule - NODE_TYPE getNodeType() const { return GEOM_CAPSULE; } - - FCL_REAL computeVolume() const - { - return constants::pi * radius * radius *(lz + radius * 4/3.0); - } - - Matrix3d computeMomentofInertia() const - { - FCL_REAL v_cyl = radius * radius * lz * constants::pi; - FCL_REAL v_sph = radius * radius * radius * constants::pi * 4 / 3.0; - - FCL_REAL ix = v_cyl * lz * lz / 12.0 + 0.25 * v_cyl * radius + 0.4 * v_sph * radius * radius + 0.25 * v_sph * lz * lz; - FCL_REAL iz = (0.5 * v_cyl + 0.4 * v_sph) * radius * radius; - - return Vector3d(ix, ix, iz).asDiagonal(); - } - -}; - -/// @brief Center at zero cone -class Cone : public ShapeBase -{ -public: - Cone(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_), lz(lz_) - { - } - - /// @brief Radius of the cone - FCL_REAL radius; - - /// @brief Length along z axis - FCL_REAL lz; - - /// @brief Compute AABB - void computeLocalAABB(); - - /// @brief Get node type: a cone - NODE_TYPE getNodeType() const { return GEOM_CONE; } - - FCL_REAL computeVolume() const - { - return constants::pi * radius * radius * lz / 3; - } - - Matrix3d computeMomentofInertia() const - { - FCL_REAL V = computeVolume(); - FCL_REAL ix = V * (0.1 * lz * lz + 3 * radius * radius / 20); - FCL_REAL iz = 0.3 * V * radius * radius; - - return Vector3d(ix, ix, iz).asDiagonal(); - } - - Vector3d computeCOM() const - { - return Vector3d(0, 0, -0.25 * lz); - } -}; - -/// @brief Center at zero cylinder -class Cylinder : public ShapeBase -{ -public: - Cylinder(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_), lz(lz_) - { - } - - - /// @brief Radius of the cylinder - FCL_REAL radius; - - /// @brief Length along z axis - FCL_REAL lz; - - /// @brief Compute AABB - void computeLocalAABB(); - - /// @brief Get node type: a cylinder - NODE_TYPE getNodeType() const { return GEOM_CYLINDER; } - - FCL_REAL computeVolume() const - { - return constants::pi * radius * radius * lz; - } - - Matrix3d computeMomentofInertia() const - { - FCL_REAL V = computeVolume(); - FCL_REAL ix = V * (3 * radius * radius + lz * lz) / 12; - FCL_REAL iz = V * radius * radius / 2; - - return Vector3d(ix, ix, iz).asDiagonal(); - } -}; - -/// @brief Convex polytope -class Convex : public ShapeBase -{ -public: - /// @brief Constructing a convex, providing normal and offset of each polytype surface, and the points and shape topology information - Convex(Vector3d* plane_normals_, - FCL_REAL* plane_dis_, - int num_planes_, - Vector3d* 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 = NULL; - - Vector3d sum = Vector3d::Zero(); - for(int i = 0; i < num_points; ++i) - { - sum += points[i]; - } - - center = sum * (FCL_REAL)(1.0 / num_points); - - fillEdges(); - } - - /// @brief Copy constructor - 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); - } - - ~Convex() - { - delete [] edges; - } - - /// @brief Compute AABB - void computeLocalAABB(); - - /// @brief Get node type: a conex polytope - NODE_TYPE getNodeType() const { return GEOM_CONVEX; } - - - Vector3d* plane_normals; - FCL_REAL* 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; - - Vector3d* 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) - Vector3d center; - - /// based on http://number-none.com/blow/inertia/bb_inertia.doc - Matrix3d computeMomentofInertia() const - { - - Matrix3d C = Matrix3d::Zero(); - - Matrix3d C_canonical; - C_canonical << 1/ 60.0, 1/120.0, 1/120.0, - 1/120.0, 1/ 60.0, 1/120.0, - 1/120.0, 1/120.0, 1/ 60.0; - - int* points_in_poly = polygons; - int* index = polygons + 1; - for(int i = 0; i < num_planes; ++i) - { - Vector3d plane_center = Vector3d::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 Vector3d& 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 Vector3d& v1 = points[e_first]; - const Vector3d& v2 = points[e_second]; - FCL_REAL d_six_vol = (v1.cross(v2)).dot(v3); - Matrix3d 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 - } - - points_in_poly += (*points_in_poly + 1); - index = points_in_poly + 1; - } - - FCL_REAL trace_C = C(0, 0) + C(1, 1) + C(2, 2); - - Matrix3d m; - m << trace_C - C(0, 0), -C(0, 1), -C(0, 2), - -C(1, 0), trace_C - C(1, 1), -C(1, 2), - -C(2, 0), -C(2, 1), trace_C - C(2, 2); - - return m; - } - - Vector3d computeCOM() const - { - Vector3d com = Vector3d::Zero(); - FCL_REAL vol = 0; - int* points_in_poly = polygons; - int* index = polygons + 1; - for(int i = 0; i < num_planes; ++i) - { - Vector3d plane_center = Vector3d::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 Vector3d& 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 Vector3d& v1 = points[e_first]; - const Vector3d& v2 = points[e_second]; - FCL_REAL d_six_vol = (v1.cross(v2)).dot(v3); - vol += d_six_vol; - com += (points[e_first] + points[e_second] + plane_center) * d_six_vol; - } - - points_in_poly += (*points_in_poly + 1); - index = points_in_poly + 1; - } - - return com / (vol * 4); // here we choose zero as the reference - } - - FCL_REAL computeVolume() const - { - FCL_REAL vol = 0; - int* points_in_poly = polygons; - int* index = polygons + 1; - for(int i = 0; i < num_planes; ++i) - { - Vector3d plane_center = Vector3d::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 Vector3d& 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 Vector3d& v1 = points[e_first]; - const Vector3d& v2 = points[e_second]; - FCL_REAL d_six_vol = (v1.cross(v2)).dot(v3); - vol += d_six_vol; - } - - points_in_poly += (*points_in_poly + 1); - index = points_in_poly + 1; - } - - return vol / 6; - } - - - -protected: - /// @brief Get edge information - void fillEdges(); -}; - - -/// @brief Half Space: this is equivalent to the Plane in ODE. The separation plane is defined as n * x = d; -/// Points in the negative side of the separation plane (i.e. {x | n * x < d}) are inside the half space and points -/// in the positive side of the separation plane (i.e. {x | n * x > d}) are outside the half space -class Halfspace : public ShapeBase -{ -public: - /// @brief Construct a half space with normal direction and offset - Halfspace(const Vector3d& n_, FCL_REAL d_) : ShapeBase(), n(n_), d(d_) - { - unitNormalTest(); - } - - /// @brief Construct a plane with normal direction and offset - Halfspace(FCL_REAL a, FCL_REAL b, FCL_REAL c, FCL_REAL d_) : ShapeBase(), n(a, b, c), d(d_) - { - unitNormalTest(); - } - - Halfspace() : ShapeBase(), n(1, 0, 0), d(0) - { - } - - FCL_REAL signedDistance(const Vector3d& p) const - { - return n.dot(p) - d; - } - - FCL_REAL distance(const Vector3d& p) const - { - return std::abs(n.dot(p) - d); - } - - /// @brief Compute AABB - void computeLocalAABB(); - - /// @brief Get node type: a half space - NODE_TYPE getNodeType() const { return GEOM_HALFSPACE; } - - /// @brief Plane normal - Vector3d n; - - /// @brief Plane offset - FCL_REAL d; - -protected: - - /// @brief Turn non-unit normal into unit - void unitNormalTest(); -}; - -/// @brief Infinite plane -class Plane : public ShapeBase -{ -public: - /// @brief Construct a plane with normal direction and offset - Plane(const Vector3d& n_, FCL_REAL d_) : ShapeBase(), n(n_), d(d_) - { - unitNormalTest(); - } - - /// @brief Construct a plane with normal direction and offset - Plane(FCL_REAL a, FCL_REAL b, FCL_REAL c, FCL_REAL d_) : ShapeBase(), n(a, b, c), d(d_) - { - unitNormalTest(); - } - - Plane() : ShapeBase(), n(1, 0, 0), d(0) - {} - - FCL_REAL signedDistance(const Vector3d& p) const - { - return n.dot(p) - d; - } - - FCL_REAL distance(const Vector3d& p) const - { - return std::abs(n.dot(p) - d); - } - - /// @brief Compute AABB - void computeLocalAABB(); - - /// @brief Get node type: a plane - NODE_TYPE getNodeType() const { return GEOM_PLANE; } - - /// @brief Plane normal - Vector3d n; - - /// @brief Plane offset - FCL_REAL d; - -protected: - - /// @brief Turn non-unit normal into unit - void unitNormalTest(); -}; - - -} +#ifndef FCL_SHAPE_GEOMETRICSHAPES_H +#define FCL_SHAPE_GEOMETRICSHAPES_H + +#include "fcl/shape/box.h" +#include "fcl/shape/capsule.h" +#include "fcl/shape/cone.h" +#include "fcl/shape/convex.h" +#include "fcl/shape/cylinder.h" +#include "fcl/shape/ellipsoid.h" +#include "fcl/shape/halfspace.h" +#include "fcl/shape/plane.h" +#include "fcl/shape/sphere.h" +#include "fcl/shape/triangle_p.h" #endif diff --git a/include/fcl/shape/geometric_shapes_utility.h b/include/fcl/shape/geometric_shapes_utility.h index 62c762ec8..2d92f43ab 100644 --- a/include/fcl/shape/geometric_shapes_utility.h +++ b/include/fcl/shape/geometric_shapes_utility.h @@ -35,174 +35,9 @@ /** \author Jia Pan */ +#ifndef FCL_SHAPE_GEOMETRICSHAPESUTILITY_H +#define FCL_SHAPE_GEOMETRICSHAPESUTILITY_H -#ifndef FCL_GEOMETRIC_SHAPES_UTILITY_H -#define FCL_GEOMETRIC_SHAPES_UTILITY_H - -#include -#include "fcl/shape/geometric_shapes.h" -#include "fcl/BV/BV.h" - -namespace fcl -{ - -/// @cond IGNORE -namespace details -{ -/// @brief get the vertices of some convex shape which can bound the given shape in a specific configuration -std::vector getBoundVertices(const Box& box, const Transform3d& tf); -std::vector getBoundVertices(const Sphere& sphere, const Transform3d& tf); -std::vector getBoundVertices(const Ellipsoid& ellipsoid, const Transform3d& tf); -std::vector getBoundVertices(const Capsule& capsule, const Transform3d& tf); -std::vector getBoundVertices(const Cone& cone, const Transform3d& tf); -std::vector getBoundVertices(const Cylinder& cylinder, const Transform3d& tf); -std::vector getBoundVertices(const Convex& convex, const Transform3d& tf); -std::vector getBoundVertices(const TriangleP& triangle, const Transform3d& tf); -} -/// @endcond - - -/// @brief calculate a bounding volume for a shape in a specific configuration -template -void computeBV(const S& s, const Transform3d& tf, BV& bv) -{ - std::vector convex_bound_vertices = details::getBoundVertices(s, tf); - fit(&convex_bound_vertices[0], (int)convex_bound_vertices.size(), bv); -} - -template<> -void computeBV(const Box& s, const Transform3d& tf, AABB& bv); - -template<> -void computeBV(const Sphere& s, const Transform3d& tf, AABB& bv); - -template<> -void computeBV(const Ellipsoid& s, const Transform3d& tf, AABB& bv); - -template<> -void computeBV(const Capsule& s, const Transform3d& tf, AABB& bv); - -template<> -void computeBV(const Cone& s, const Transform3d& tf, AABB& bv); - -template<> -void computeBV(const Cylinder& s, const Transform3d& tf, AABB& bv); - -template<> -void computeBV(const Convex& s, const Transform3d& tf, AABB& bv); - -template<> -void computeBV(const TriangleP& s, const Transform3d& tf, AABB& bv); - -template<> -void computeBV(const Halfspace& s, const Transform3d& tf, AABB& bv); - -template<> -void computeBV(const Plane& s, const Transform3d& tf, AABB& bv); - - - -template<> -void computeBV(const Box& s, const Transform3d& tf, OBB& bv); - -template<> -void computeBV(const Sphere& s, const Transform3d& tf, OBB& bv); - -template<> -void computeBV(const Ellipsoid& s, const Transform3d& tf, OBB& bv); - -template<> -void computeBV(const Capsule& s, const Transform3d& tf, OBB& bv); - -template<> -void computeBV(const Cone& s, const Transform3d& tf, OBB& bv); - -template<> -void computeBV(const Cylinder& s, const Transform3d& tf, OBB& bv); - -template<> -void computeBV(const Convex& s, const Transform3d& tf, OBB& bv); - -template<> -void computeBV(const Halfspace& s, const Transform3d& tf, OBB& bv); - -template<> -void computeBV(const Halfspace& s, const Transform3d& tf, RSS& bv); - -template<> -void computeBV(const Halfspace& s, const Transform3d& tf, OBBRSS& bv); - -template<> -void computeBV(const Halfspace& s, const Transform3d& tf, kIOS& bv); - -template<> -void computeBV, Halfspace>(const Halfspace& s, const Transform3d& tf, KDOP<16>& bv); - -template<> -void computeBV, Halfspace>(const Halfspace& s, const Transform3d& tf, KDOP<18>& bv); - -template<> -void computeBV, Halfspace>(const Halfspace& s, const Transform3d& tf, KDOP<24>& bv); - -template<> -void computeBV(const Plane& s, const Transform3d& tf, OBB& bv); - -template<> -void computeBV(const Plane& s, const Transform3d& tf, RSS& bv); - -template<> -void computeBV(const Plane& s, const Transform3d& tf, OBBRSS& bv); - -template<> -void computeBV(const Plane& s, const Transform3d& tf, kIOS& bv); - -template<> -void computeBV, Plane>(const Plane& s, const Transform3d& tf, KDOP<16>& bv); - -template<> -void computeBV, Plane>(const Plane& s, const Transform3d& tf, KDOP<18>& bv); - -template<> -void computeBV, Plane>(const Plane& s, const Transform3d& tf, KDOP<24>& bv); - - -/// @brief construct a box shape (with a configuration) from a given bounding volume -void constructBox(const AABB& bv, Box& box, Transform3d& tf); - -void constructBox(const OBB& bv, Box& box, Transform3d& tf); - -void constructBox(const OBBRSS& bv, Box& box, Transform3d& tf); - -void constructBox(const kIOS& bv, Box& box, Transform3d& tf); - -void constructBox(const RSS& bv, Box& box, Transform3d& tf); - -void constructBox(const KDOP<16>& bv, Box& box, Transform3d& tf); - -void constructBox(const KDOP<18>& bv, Box& box, Transform3d& tf); - -void constructBox(const KDOP<24>& bv, Box& box, Transform3d& tf); - -void constructBox(const AABB& bv, const Transform3d& tf_bv, Box& box, Transform3d& tf); - -void constructBox(const OBB& bv, const Transform3d& tf_bv, Box& box, Transform3d& tf); - -void constructBox(const OBBRSS& bv, const Transform3d& tf_bv, Box& box, Transform3d& tf); - -void constructBox(const kIOS& bv, const Transform3d& tf_bv, Box& box, Transform3d& tf); - -void constructBox(const RSS& bv, const Transform3d& tf_bv, Box& box, Transform3d& tf); - -void constructBox(const KDOP<16>& bv, const Transform3d& tf_bv, Box& box, Transform3d& tf); - -void constructBox(const KDOP<18>& bv, const Transform3d& tf_bv, Box& box, Transform3d& tf); - -void constructBox(const KDOP<24>& bv, const Transform3d& tf_bv, Box& box, Transform3d& tf); - -Halfspace transform(const Halfspace& a, const Transform3d& tf); - -Plane transform(const Plane& a, const Transform3d& tf); - -} +#warning "This header has been deprecated in FCL 0.6." #endif diff --git a/include/fcl/shape/halfspace.h b/include/fcl/shape/halfspace.h new file mode 100644 index 000000000..71e5c5591 --- /dev/null +++ b/include/fcl/shape/halfspace.h @@ -0,0 +1,192 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_SHAPE_HALFSPACE_H +#define FCL_SHAPE_HALFSPACE_H + +#include "fcl/shape/shape_base.h" +#include "fcl/shape/compute_bv.h" +#include "fcl/BV/OBB.h" +#include "fcl/BV/RSS.h" +#include "fcl/BV/OBBRSS.h" +#include "fcl/BV/kDOP.h" +#include "fcl/BV/kIOS.h" + +namespace fcl +{ + +/// @brief Half Space: this is equivalent to the Planed in ODE. The separation plane is defined as n * x = d; +/// Points in the negative side of the separation plane (i.e. {x | n * x < d}) are inside the half space and points +/// in the positive side of the separation plane (i.e. {x | n * x > d}) are outside the half space +template +class Halfspace : public ShapeBase +{ +public: + + using S = S_; + + /// @brief Construct a half space with normal direction and offset + Halfspace(const Vector3& n, S d); + + /// @brief Construct a plane with normal direction and offset + Halfspace(S a, S b, S c, S d_); + + Halfspace(); + + S signedDistance(const Vector3& p) const; + + S distance(const Vector3& p) const; + + /// @brief Compute AABB + void computeLocalAABB() override; + + /// @brief Get node type: a half space + NODE_TYPE getNodeType() const override; + + /// @brief Planed normal + Vector3 n; + + /// @brief Planed offset + S d; + +protected: + + /// @brief Turn non-unit normal into unit + void unitNormalTest(); +}; + +using Halfspacef = Halfspace; +using Halfspaced = Halfspace; + +template +Halfspace transform( + const Halfspace& a, const Transform3& tf) +{ + /// suppose the initial halfspace is n * x <= d + /// after transform (R, T), x --> x' = R x + T + /// and the new half space becomes n' * x' <= d' + /// where n' = R * n + /// and d' = d + n' * T + + Vector3 n = tf.linear() * a.n; + S d = a.d + n.dot(tf.translation()); + + return Halfspace(n, d); +} + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +Halfspace::Halfspace(const Vector3& n, S d) + : ShapeBase(), n(n), d(d) +{ + unitNormalTest(); +} + +//============================================================================== +template +Halfspace::Halfspace(S a, S b, S c, S d) + : ShapeBase(), n(a, b, c), d(d) +{ + unitNormalTest(); +} + +//============================================================================== +template +Halfspace::Halfspace() : ShapeBase(), n(1, 0, 0), d(0) +{ + // Do nothing +} + +//============================================================================== +template +S Halfspace::signedDistance(const Vector3& p) const +{ + return n.dot(p) - d; +} + +//============================================================================== +template +S Halfspace::distance(const Vector3& p) const +{ + return std::abs(n.dot(p) - d); +} + +//============================================================================== +template +void Halfspace::computeLocalAABB() +{ + computeBV(*this, Transform3::Identity(), this->aabb_local); + this->aabb_center = this->aabb_local.center(); + this->aabb_radius = (this->aabb_local.min_ - this->aabb_center).norm(); +} + +//============================================================================== +template +NODE_TYPE Halfspace::getNodeType() const +{ + return GEOM_HALFSPACE; +} + +//============================================================================== +template +void Halfspace::unitNormalTest() +{ + S l = n.norm(); + if(l > 0) + { + S inv_l = 1.0 / l; + n *= inv_l; + d *= inv_l; + } + else + { + n << 1, 0, 0; + d = 0; + } +} + +} // namespace fcl + +#include "fcl/shape/detail/bv_computer_halfspace.h" + +#endif diff --git a/include/fcl/shape/plane.h b/include/fcl/shape/plane.h new file mode 100644 index 000000000..0e3acd893 --- /dev/null +++ b/include/fcl/shape/plane.h @@ -0,0 +1,189 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_SHAPE_PLANE_H +#define FCL_SHAPE_PLANE_H + +#include "fcl/shape/shape_base.h" +#include "fcl/shape/compute_bv.h" +#include "fcl/BV/OBB.h" +#include "fcl/BV/RSS.h" +#include "fcl/BV/OBBRSS.h" +#include "fcl/BV/kDOP.h" +#include "fcl/BV/kIOS.h" + +namespace fcl +{ + +/// @brief Infinite plane +template +class Plane : public ShapeBase +{ +public: + + using S = S_; + + /// @brief Construct a plane with normal direction and offset + Plane(const Vector3& n, S d); + + /// @brief Construct a plane with normal direction and offset + Plane(S a, S b, S c, S d); + + Plane(); + + S signedDistance(const Vector3& p) const; + + S distance(const Vector3& p) const; + + /// @brief Compute AABB + void computeLocalAABB() override; + + /// @brief Get node type: a plane + NODE_TYPE getNodeType() const override; + + /// @brief Plane normal + Vector3 n; + + /// @brief Plane offset + S d; + +protected: + + /// @brief Turn non-unit normal into unit + void unitNormalTest(); +}; + +using Planef = Plane; +using Planed = Plane; + +template +Plane transform(const Plane& a, const Transform3& tf) +{ + /// suppose the initial halfspace is n * x <= d + /// after transform (R, T), x --> x' = R x + T + /// and the new half space becomes n' * x' <= d' + /// where n' = R * n + /// and d' = d + n' * T + + Vector3 n = tf.linear() * a.n; + S d = a.d + n.dot(tf.translation()); + + return Plane(n, d); +} + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +Plane::Plane(const Vector3& n, S d) + : ShapeBase(), n(n), d(d) +{ + unitNormalTest(); +} + +//============================================================================== +template +Plane::Plane(S a, S b, S c, S d) + : ShapeBase(), n(a, b, c), d(d) +{ + unitNormalTest(); +} + +//============================================================================== +template +Plane::Plane() : ShapeBase(), n(1, 0, 0), d(0) +{ + // Do nothing +} + +//============================================================================== +template +S Plane::signedDistance(const Vector3& p) const +{ + return n.dot(p) - d; +} + +//============================================================================== +template +S Plane::distance(const Vector3& p) const +{ + return std::abs(n.dot(p) - d); +} + +//============================================================================== +template +void Plane::computeLocalAABB() +{ + computeBV(*this, Transform3::Identity(), this->aabb_local); + this->aabb_center = this->aabb_local.center(); + this->aabb_radius = (this->aabb_local.min_ - this->aabb_center).norm(); +} + +//============================================================================== +template +NODE_TYPE Plane::getNodeType() const +{ + return GEOM_PLANE; +} + +//============================================================================== +template +void Plane::unitNormalTest() +{ + S l = n.norm(); + if(l > 0) + { + S inv_l = 1.0 / l; + n *= inv_l; + d *= inv_l; + } + else + { + n << 1, 0, 0; + d = 0; + } +} + +} // namespace fcl + +#include "fcl/shape/detail/bv_computer_plane.h" + +#endif diff --git a/include/fcl/shape/shape_base.h b/include/fcl/shape/shape_base.h new file mode 100644 index 000000000..4f0e9f7b8 --- /dev/null +++ b/include/fcl/shape/shape_base.h @@ -0,0 +1,87 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + + +#ifndef FCL_SHAPE_SHAPE_BASE_H +#define FCL_SHAPE_SHAPE_BASE_H + +#include "fcl/collision_object.h" + +namespace fcl +{ + +/// @brief Base class for all basic geometric shapes +template +class ShapeBase : public CollisionGeometry +{ +public: + + using S = S_; + + ShapeBase(); + + /// @brief Get object type: a geometric shape + OBJECT_TYPE getObjectType() const; +}; + +using ShapeBasef = ShapeBase; +using ShapeBased = ShapeBase; + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +ShapeBase::ShapeBase() + : CollisionGeometry() +{ + // Do nothing +} + +//============================================================================== +template +OBJECT_TYPE ShapeBase::getObjectType() const +{ + return OT_GEOM; +} + +} + +#endif diff --git a/include/fcl/shape/sphere.h b/include/fcl/shape/sphere.h new file mode 100644 index 000000000..b12c12ef1 --- /dev/null +++ b/include/fcl/shape/sphere.h @@ -0,0 +1,156 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_SHAPE_SPHERE_H +#define FCL_SHAPE_SPHERE_H + +#include "fcl/shape/shape_base.h" +#include "fcl/shape/compute_bv.h" +#include "fcl/BV/OBB.h" + +namespace fcl +{ + +/// @brief Center at zero point sphere +template +class Sphere : public ShapeBase +{ +public: + + using S = S_; + + Sphere(S radius); + + /// @brief Radius of the sphere + S radius; + + /// @brief Compute AABB + void computeLocalAABB() override; + + /// @brief Get node type: a sphere + NODE_TYPE getNodeType() const override; + + Matrix3 computeMomentofInertia() const override; + + S computeVolume() const override; + + /// @brief get the vertices of some convex shape which can bound this shape in + /// a specific configuration + std::vector> getBoundVertices( + const Transform3& tf) const; +}; + +using Spheref = Sphere; +using Sphered = Sphere; + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +Sphere::Sphere(S radius) : ShapeBase(), radius(radius) +{ +} + +//============================================================================== +template +void Sphere::computeLocalAABB() +{ + computeBV(*this, Transform3::Identity(), this->aabb_local); + this->aabb_center = this->aabb_local.center(); + this->aabb_radius = radius; +} + +//============================================================================== +template +NODE_TYPE Sphere::getNodeType() const +{ + return GEOM_SPHERE; } + +//============================================================================== +template +Matrix3 Sphere::computeMomentofInertia() const +{ + S I = (S)0.4 * radius * radius * computeVolume(); + + return Vector3::Constant(I).asDiagonal(); +} + +//============================================================================== +template +S Sphere::computeVolume() const +{ + return (S)4.0 * constants::pi() * radius * radius * radius / (S)3.0; +} + +//============================================================================== +template +std::vector> Sphere::getBoundVertices( + const Transform3& tf) const +{ + // we use icosahedron to bound the sphere + + std::vector> result(12); + const auto m = (1 + std::sqrt(5.0)) / 2.0; + auto edge_size = radius * 6 / (std::sqrt(27.0) + std::sqrt(15.0)); + + auto a = edge_size; + auto b = m * edge_size; + result[0] = tf * Vector3(0, a, b); + result[1] = tf * Vector3(0, -a, b); + result[2] = tf * Vector3(0, a, -b); + result[3] = tf * Vector3(0, -a, -b); + result[4] = tf * Vector3(a, b, 0); + result[5] = tf * Vector3(-a, b, 0); + result[6] = tf * Vector3(a, -b, 0); + result[7] = tf * Vector3(-a, -b, 0); + result[8] = tf * Vector3(b, 0, a); + result[9] = tf * Vector3(b, 0, -a); + result[10] = tf * Vector3(-b, 0, a); + result[11] = tf * Vector3(-b, 0, -a); + + return result; +} + +} // namespace fcl + +#include "fcl/shape/detail/bv_computer_sphere.h" + +#endif diff --git a/include/fcl/shape/triangle_p.h b/include/fcl/shape/triangle_p.h new file mode 100644 index 000000000..5c4ca3b96 --- /dev/null +++ b/include/fcl/shape/triangle_p.h @@ -0,0 +1,128 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_SHAPE_TRIANGLE_P_H +#define FCL_SHAPE_TRIANGLE_P_H + +#include "fcl/shape/shape_base.h" +#include "fcl/shape/compute_bv.h" + +namespace fcl +{ + +/// @brief Triangle stores the points instead of only indices of points +template +class TriangleP : public ShapeBase +{ +public: + + using S = S_; + + TriangleP(const Vector3& a, + const Vector3& b, + const Vector3& c); + + /// @brief virtual function of compute AABB in local coordinate + void computeLocalAABB() override; + + // Documentation inherited + NODE_TYPE getNodeType() const override; + + Vector3 a; + Vector3 b; + Vector3 c; + + /// @brief get the vertices of some convex shape which can bound this shape in + /// a specific configuration + std::vector> getBoundVertices( + const Transform3& tf) const; +}; + +using TrianglePf = TriangleP; +using TrianglePd = TriangleP; + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +TriangleP::TriangleP( + const Vector3& a, + const Vector3& b, + const Vector3& c) + : ShapeBase(), a(a), b(b), c(c) +{ + // Do nothing +} + +//============================================================================== +template +void TriangleP::computeLocalAABB() +{ + computeBV(*this, Transform3::Identity(), this->aabb_local); + this->aabb_center = this->aabb_local.center(); + this->aabb_radius = (this->aabb_local.min_ - this->aabb_center).norm(); +} + +//============================================================================== +template +NODE_TYPE TriangleP::getNodeType() const +{ + return GEOM_TRIANGLE; +} + +//============================================================================== +template +std::vector> TriangleP::getBoundVertices( + const Transform3& tf) const +{ + std::vector> result(3); + result[0] = tf * a; + result[1] = tf * b; + result[2] = tf * c; + + return result; +} + +} // namespace fcl + +#include "fcl/shape/detail/bv_computer_triangle_p.h" + +#endif diff --git a/include/fcl/traversal/collision/bvh_collision_traversal_node.h b/include/fcl/traversal/collision/bvh_collision_traversal_node.h new file mode 100644 index 000000000..62862718f --- /dev/null +++ b/include/fcl/traversal/collision/bvh_collision_traversal_node.h @@ -0,0 +1,180 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_TRAVERSAL_BVHCOLLISIONTRAVERSALNODE_H +#define FCL_TRAVERSAL_BVHCOLLISIONTRAVERSALNODE_H + +#include "fcl/BVH/BVH_model.h" +#include "fcl/traversal/collision/collision_traversal_node_base.h" + +namespace fcl +{ + +/// @brief Traversal node for collision between BVH models +template +class BVHCollisionTraversalNode + : public CollisionTraversalNodeBase +{ +public: + + using S = typename BV::S; + + BVHCollisionTraversalNode(); + + /// @brief Whether the BV node in the first BVH tree is leaf + bool isFirstNodeLeaf(int b) const; + + /// @brief Whether the BV node in the second BVH tree is leaf + bool isSecondNodeLeaf(int b) const; + + /// @brief Determine the traversal order, is the first BVTT subtree better + bool firstOverSecond(int b1, int b2) const; + + /// @brief Obtain the left child of BV node in the first BVH + int getFirstLeftChild(int b) const; + + /// @brief Obtain the right child of BV node in the first BVH + int getFirstRightChild(int b) const; + + /// @brief Obtain the left child of BV node in the second BVH + int getSecondLeftChild(int b) const; + + /// @brief Obtain the right child of BV node in the second BVH + int getSecondRightChild(int b) const; + + /// @brief BV culling test in one BVTT node + bool BVTesting(int b1, int b2) const; + + /// @brief The first BVH model + const BVHModel* model1; + + /// @brief The second BVH model + const BVHModel* model2; + + /// @brief statistical information + mutable int num_bv_tests; + mutable int num_leaf_tests; + mutable S query_time_seconds; +}; + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +BVHCollisionTraversalNode::BVHCollisionTraversalNode() + : CollisionTraversalNodeBase() +{ + model1 = nullptr; + model2 = nullptr; + + num_bv_tests = 0; + num_leaf_tests = 0; + query_time_seconds = 0.0; +} + +//============================================================================== +template +bool BVHCollisionTraversalNode::isFirstNodeLeaf(int b) const +{ + return model1->getBV(b).isLeaf(); +} + +//============================================================================== +template +bool BVHCollisionTraversalNode::isSecondNodeLeaf(int b) const +{ + return model2->getBV(b).isLeaf(); +} + +//============================================================================== +template +bool BVHCollisionTraversalNode::firstOverSecond(int b1, int b2) const +{ + S sz1 = model1->getBV(b1).bv.size(); + S sz2 = model2->getBV(b2).bv.size(); + + bool l1 = model1->getBV(b1).isLeaf(); + bool l2 = model2->getBV(b2).isLeaf(); + + if(l2 || (!l1 && (sz1 > sz2))) + return true; + return false; +} + +//============================================================================== +template +int BVHCollisionTraversalNode::getFirstLeftChild(int b) const +{ + return model1->getBV(b).leftChild(); +} + +//============================================================================== +template +int BVHCollisionTraversalNode::getFirstRightChild(int b) const +{ + return model1->getBV(b).rightChild(); +} + +//============================================================================== +template +int BVHCollisionTraversalNode::getSecondLeftChild(int b) const +{ + return model2->getBV(b).leftChild(); +} + +//============================================================================== +template +int BVHCollisionTraversalNode::getSecondRightChild(int b) const +{ + return model2->getBV(b).rightChild(); +} + +//============================================================================== +template +bool BVHCollisionTraversalNode::BVTesting(int b1, int b2) const +{ + if(this->enable_statistics) num_bv_tests++; + return !model1->getBV(b1).overlap(model2->getBV(b2)); +} + +} // namespace fcl + +#endif diff --git a/include/fcl/traversal/collision/bvh_shape_collision_traversal_node.h b/include/fcl/traversal/collision/bvh_shape_collision_traversal_node.h new file mode 100644 index 000000000..2455524e8 --- /dev/null +++ b/include/fcl/traversal/collision/bvh_shape_collision_traversal_node.h @@ -0,0 +1,130 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + + +#ifndef FCL_TRAVERSAL_BVHSHAPECOLLISIONTRAVERSALNODE_H +#define FCL_TRAVERSAL_BVHSHAPECOLLISIONTRAVERSALNODE_H + +#include "fcl/traversal/collision/collision_traversal_node_base.h" +#include "fcl/BVH/BVH_model.h" + +namespace fcl +{ + +/// @brief Traversal node for collision between BVH and shape +template +class BVHShapeCollisionTraversalNode + : public CollisionTraversalNodeBase +{ +public: + + using S = typename BV::S; + + BVHShapeCollisionTraversalNode(); + + /// @brief Whether the BV node in the first BVH tree is leaf + bool isFirstNodeLeaf(int b) const; + + /// @brief Obtain the left child of BV node in the first BVH + int getFirstLeftChild(int b) const; + + /// @brief Obtain the right child of BV node in the first BVH + int getFirstRightChild(int b) const; + + /// @brief BV culling test in one BVTT node + bool BVTesting(int b1, int b2) const; + + const BVHModel* model1; + const Shape* model2; + BV model2_bv; + + mutable int num_bv_tests; + mutable int num_leaf_tests; + mutable S query_time_seconds; +}; + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +BVHShapeCollisionTraversalNode::BVHShapeCollisionTraversalNode() + : CollisionTraversalNodeBase() +{ + model1 = nullptr; + model2 = nullptr; + + num_bv_tests = 0; + num_leaf_tests = 0; + query_time_seconds = 0.0; +} + +//============================================================================== +template +bool BVHShapeCollisionTraversalNode::isFirstNodeLeaf(int b) const +{ + return model1->getBV(b).isLeaf(); +} + +//============================================================================== +template +int BVHShapeCollisionTraversalNode::getFirstLeftChild(int b) const +{ + return model1->getBV(b).leftChild(); +} + +//============================================================================== +template +int BVHShapeCollisionTraversalNode::getFirstRightChild(int b) const +{ + return model1->getBV(b).rightChild(); +} + +//============================================================================== +template +bool BVHShapeCollisionTraversalNode::BVTesting(int b1, int b2) const +{ + if(this->enable_statistics) num_bv_tests++; + return !model1->getBV(b1).bv.overlap(model2_bv); +} + +} // namespace fcl + +#endif diff --git a/include/fcl/traversal/collision/collision_traversal_node_base.h b/include/fcl/traversal/collision/collision_traversal_node_base.h new file mode 100644 index 000000000..6f6c27497 --- /dev/null +++ b/include/fcl/traversal/collision/collision_traversal_node_base.h @@ -0,0 +1,132 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_TRAVERSAL_COLLISIONTRAVERSALNODEBASE_H +#define FCL_TRAVERSAL_COLLISIONTRAVERSALNODEBASE_H + +#include "fcl/traversal/traversal_node_base.h" +#include "fcl/collision_data.h" + +namespace fcl +{ + +/// @brief Node structure encoding the information required for collision traversal. +template +class CollisionTraversalNodeBase : public TraversalNodeBase +{ +public: + CollisionTraversalNodeBase(); + + virtual ~CollisionTraversalNodeBase(); + + /// @brief BV test between b1 and b2 + virtual bool BVTesting(int b1, int b2) const; + + /// @brief Leaf test between node b1 and b2, if they are both leafs + virtual void leafTesting(int b1, int b2) const; + + /// @brief Check whether the traversal can stop + virtual bool canStop() const; + + /// @brief Whether store some statistics information during traversal + void enableStatistics(bool enable); + + /// @brief request setting for collision + CollisionRequest request; + + /// @brief collision result kept during the traversal iteration + CollisionResult* result; + + /// @brief Whether stores statistics + bool enable_statistics; +}; + +using CollisionTraversalNodeBasef = CollisionTraversalNodeBase; +using CollisionTraversalNodeBased = CollisionTraversalNodeBase; + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +CollisionTraversalNodeBase::CollisionTraversalNodeBase() + : result(nullptr), enable_statistics(false) +{ + // Do nothing +} + +//============================================================================== +template +CollisionTraversalNodeBase::~CollisionTraversalNodeBase() +{ + // Do nothing +} + +//============================================================================== +template +bool CollisionTraversalNodeBase::BVTesting(int b1, int b2) const +{ + return true; +} + +//============================================================================== +template +void CollisionTraversalNodeBase::leafTesting(int b1, int b2) const +{ + // Do nothing +} + +//============================================================================== +template +bool CollisionTraversalNodeBase::canStop() const +{ + return false; +} + +//============================================================================== +template +void CollisionTraversalNodeBase::enableStatistics(bool enable) +{ + enable_statistics = enable; +} + +} // namespace fcl + +#endif diff --git a/include/fcl/traversal/collision/mesh_collision_traversal_node.h b/include/fcl/traversal/collision/mesh_collision_traversal_node.h new file mode 100644 index 000000000..57ec68be0 --- /dev/null +++ b/include/fcl/traversal/collision/mesh_collision_traversal_node.h @@ -0,0 +1,988 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_TRAVERSAL_MESHCOLLISIONTRAVERSALNODE_H +#define FCL_TRAVERSAL_MESHCOLLISIONTRAVERSALNODE_H + +#include "fcl/intersect.h" +#include "fcl/traversal/collision/bvh_collision_traversal_node.h" +#include "fcl/BV/OBB.h" +#include "fcl/BV/RSS.h" +#include "fcl/BV/OBBRSS.h" +#include "fcl/BV/kIOS.h" + +namespace fcl +{ + +/// @brief Traversal node for collision between two meshes +template +class MeshCollisionTraversalNode : public BVHCollisionTraversalNode +{ +public: + + using S = typename BV::S; + + MeshCollisionTraversalNode(); + + /// @brief Intersection testing between leaves (two triangles) + void leafTesting(int b1, int b2) const; + + /// @brief Whether the traversal process can stop early + bool canStop() const; + + Vector3* vertices1; + Vector3* vertices2; + + Triangle* tri_indices1; + Triangle* tri_indices2; + + S cost_density; +}; + +/// @brief Initialize traversal node for collision between two meshes, given the +/// current transforms +template +bool initialize( + MeshCollisionTraversalNode& node, + BVHModel& model1, + Transform3& tf1, + BVHModel& model2, + Transform3& tf2, + const CollisionRequest& request, + CollisionResult& result, + bool use_refit = false, + bool refit_bottomup = false); + +/// @brief Traversal node for collision between two meshes if their underlying +/// BVH node is oriented node (OBB, RSS, OBBRSS, kIOS) +template +class MeshCollisionTraversalNodeOBB : public MeshCollisionTraversalNode> +{ +public: + MeshCollisionTraversalNodeOBB(); + + bool BVTesting(int b1, int b2) const; + + void leafTesting(int b1, int b2) const; + + bool BVTesting(int b1, int b2, const Matrix3& Rc, const Vector3& Tc) const; + + bool BVTesting(int b1, int b2, const Transform3& tf) const; + + void leafTesting(int b1, int b2, const Matrix3& Rc, const Vector3& Tc) const; + + void leafTesting(int b1, int b2, const Transform3& tf) const; + + Matrix3 R; + Vector3 T; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +using MeshCollisionTraversalNodeOBBf = MeshCollisionTraversalNodeOBB; +using MeshCollisionTraversalNodeOBBd = MeshCollisionTraversalNodeOBB; + +/// @brief Initialize traversal node for collision between two meshes, +/// specialized for OBB type +template +bool initialize( + MeshCollisionTraversalNodeOBB& node, + const BVHModel>& model1, + const Transform3& tf1, + const BVHModel>& model2, + const Transform3& tf2, + const CollisionRequest& request, + CollisionResult& result); + +template +class MeshCollisionTraversalNodeRSS : public MeshCollisionTraversalNode> +{ +public: + MeshCollisionTraversalNodeRSS(); + + bool BVTesting(int b1, int b2) const; + + void leafTesting(int b1, int b2) const; + +// FCL_DEPRECATED +// bool BVTesting(int b1, int b2, const Matrix3& Rc, const Vector3& Tc) const; + + bool BVTesting(int b1, int b2, const Transform3& tf) const; + +// FCL_DEPRECATED +// void leafTesting(int b1, int b2, const Matrix3& Rc, const Vector3& Tc) const; + + void leafTesting(int b1, int b2, const Transform3& tf) const; + + Matrix3 R; + Vector3 T; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +using MeshCollisionTraversalNodeRSSf = MeshCollisionTraversalNodeRSS; +using MeshCollisionTraversalNodeRSSd = MeshCollisionTraversalNodeRSS; + +/// @brief Initialize traversal node for collision between two meshes, +/// specialized for RSS type +template +bool initialize( + MeshCollisionTraversalNodeRSS& node, + const BVHModel>& model1, + const Transform3& tf1, + const BVHModel>& model2, + const Transform3& tf2, + const CollisionRequest& request, + CollisionResult& result); + +template +class MeshCollisionTraversalNodekIOS : public MeshCollisionTraversalNode> +{ +public: + MeshCollisionTraversalNodekIOS(); + + bool BVTesting(int b1, int b2) const; + + void leafTesting(int b1, int b2) const; + + Matrix3 R; + Vector3 T; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +using MeshCollisionTraversalNodekIOSf = MeshCollisionTraversalNodekIOS; +using MeshCollisionTraversalNodekIOSd = MeshCollisionTraversalNodekIOS; + +/// @brief Initialize traversal node for collision between two meshes, +/// specialized for kIOS type +template +bool initialize( + MeshCollisionTraversalNodekIOS& node, + const BVHModel>& model1, + const Transform3& tf1, + const BVHModel>& model2, + const Transform3& tf2, + const CollisionRequest& request, + CollisionResult& result); + +template +class MeshCollisionTraversalNodeOBBRSS : public MeshCollisionTraversalNode> +{ +public: + MeshCollisionTraversalNodeOBBRSS(); + + + bool BVTesting(int b1, int b2) const; + + void leafTesting(int b1, int b2) const; + + Matrix3 R; + Vector3 T; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +using MeshCollisionTraversalNodeOBBRSSf = MeshCollisionTraversalNodeOBBRSS; +using MeshCollisionTraversalNodeOBBRSSd = MeshCollisionTraversalNodeOBBRSS; + +/// @brief Initialize traversal node for collision between two meshes, +/// specialized for OBBRSS type +template +bool initialize( + MeshCollisionTraversalNodeOBBRSS& node, + const BVHModel>& model1, + const Transform3& tf1, + const BVHModel>& model2, + const Transform3& tf2, + const CollisionRequest& request, + CollisionResult& result); + +namespace details +{ + +template +void meshCollisionOrientedNodeLeafTesting( + int b1, + int b2, + const BVHModel* model1, + const BVHModel* model2, + Vector3* vertices1, + Vector3* vertices2, + Triangle* tri_indices1, + Triangle* tri_indices2, + const Matrix3& R, + const Vector3& T, + const Transform3& tf1, + const Transform3& tf2, + bool enable_statistics, + typename BV::S cost_density, + int& num_leaf_tests, + const CollisionRequest& request, + CollisionResult& result); + +template +void meshCollisionOrientedNodeLeafTesting( + int b1, + int b2, + const BVHModel* model1, + const BVHModel* model2, + Vector3* vertices1, + Vector3* vertices2, + Triangle* tri_indices1, + Triangle* tri_indices2, + const Transform3& tf, + const Transform3& tf1, + const Transform3& tf2, + bool enable_statistics, + typename BV::S cost_density, + int& num_leaf_tests, + const CollisionRequest& request, + CollisionResult& result); + +} // namespace details + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +MeshCollisionTraversalNode::MeshCollisionTraversalNode() + : BVHCollisionTraversalNode() +{ + vertices1 = nullptr; + vertices2 = nullptr; + tri_indices1 = nullptr; + tri_indices2 = nullptr; +} + +//============================================================================== +template +void MeshCollisionTraversalNode::leafTesting(int b1, int b2) const +{ + if(this->enable_statistics) this->num_leaf_tests++; + + const BVNode& node1 = this->model1->getBV(b1); + const BVNode& node2 = this->model2->getBV(b2); + + int primitive_id1 = node1.primitiveId(); + int primitive_id2 = node2.primitiveId(); + + const Triangle& tri_id1 = tri_indices1[primitive_id1]; + const Triangle& tri_id2 = tri_indices2[primitive_id2]; + + const Vector3& p1 = vertices1[tri_id1[0]]; + const Vector3& p2 = vertices1[tri_id1[1]]; + const Vector3& p3 = vertices1[tri_id1[2]]; + const Vector3& q1 = vertices2[tri_id2[0]]; + const Vector3& q2 = vertices2[tri_id2[1]]; + const Vector3& q3 = vertices2[tri_id2[2]]; + + if(this->model1->isOccupied() && this->model2->isOccupied()) + { + bool is_intersect = false; + + if(!this->request.enable_contact) // only interested in collision or not + { + if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3)) + { + is_intersect = true; + if(this->result->numContacts() < this->request.num_max_contacts) + this->result->addContact(Contact(this->model1, this->model2, primitive_id1, primitive_id2)); + } + } + else // need compute the contact information + { + S penetration; + Vector3 normal; + unsigned int n_contacts; + Vector3 contacts[2]; + + if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3, + contacts, + &n_contacts, + &penetration, + &normal)) + { + is_intersect = true; + + if(this->request.num_max_contacts < n_contacts + this->result->numContacts()) + n_contacts = (this->request.num_max_contacts >= this->result->numContacts()) ? (this->request.num_max_contacts - this->result->numContacts()) : 0; + + for(unsigned int i = 0; i < n_contacts; ++i) + { + this->result->addContact(Contact(this->model1, this->model2, primitive_id1, primitive_id2, contacts[i], normal, penetration)); + } + } + } + + if(is_intersect && this->request.enable_cost) + { + AABB overlap_part; + AABB(p1, p2, p3).overlap(AABB(q1, q2, q3), overlap_part); + this->result->addCostSource(CostSource(overlap_part, cost_density), this->request.num_max_cost_sources); + } + } + else if((!this->model1->isFree() && !this->model2->isFree()) && this->request.enable_cost) + { + if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3)) + { + AABB overlap_part; + AABB(p1, p2, p3).overlap(AABB(q1, q2, q3), overlap_part); + this->result->addCostSource(CostSource(overlap_part, cost_density), this->request.num_max_cost_sources); + } + } +} + +//============================================================================== +template +bool MeshCollisionTraversalNode::canStop() const +{ + return this->request.isSatisfied(*(this->result)); +} + +//============================================================================== +template +bool initialize( + MeshCollisionTraversalNode& node, + BVHModel& model1, + Transform3& tf1, + BVHModel& model2, + Transform3& tf2, + const CollisionRequest& request, + CollisionResult& result, + bool use_refit, + bool refit_bottomup) +{ + using S = typename BV::S; + + if(model1.getModelType() != BVH_MODEL_TRIANGLES + || model2.getModelType() != BVH_MODEL_TRIANGLES) + return false; + + if(!tf1.matrix().isIdentity()) + { + std::vector> vertices_transformed1(model1.num_vertices); + for(int i = 0; i < model1.num_vertices; ++i) + { + Vector3& p = model1.vertices[i]; + Vector3 new_v = tf1 * p; + vertices_transformed1[i] = new_v; + } + + model1.beginReplaceModel(); + model1.replaceSubModel(vertices_transformed1); + model1.endReplaceModel(use_refit, refit_bottomup); + + tf1.setIdentity(); + } + + if(!tf2.matrix().isIdentity()) + { + std::vector> vertices_transformed2(model2.num_vertices); + for(int i = 0; i < model2.num_vertices; ++i) + { + Vector3& p = model2.vertices[i]; + Vector3 new_v = tf2 * p; + vertices_transformed2[i] = new_v; + } + + model2.beginReplaceModel(); + model2.replaceSubModel(vertices_transformed2); + model2.endReplaceModel(use_refit, refit_bottomup); + + tf2.setIdentity(); + } + + node.model1 = &model1; + node.tf1 = tf1; + node.model2 = &model2; + node.tf2 = tf2; + + node.vertices1 = model1.vertices; + node.vertices2 = model2.vertices; + + node.tri_indices1 = model1.tri_indices; + node.tri_indices2 = model2.tri_indices; + + node.request = request; + node.result = &result; + + node.cost_density = model1.cost_density * model2.cost_density; + + return true; +} + +//============================================================================== +template +MeshCollisionTraversalNodeOBB::MeshCollisionTraversalNodeOBB() + : MeshCollisionTraversalNode>(), + R(Matrix3::Identity()) +{ + // Do nothing +} + +//============================================================================== +template +bool MeshCollisionTraversalNodeOBB::BVTesting(int b1, int b2) const +{ + if(this->enable_statistics) this->num_bv_tests++; + + return !overlap(R, T, this->model1->getBV(b1).bv, this->model2->getBV(b2).bv); +} + +//============================================================================== +template +void MeshCollisionTraversalNodeOBB::leafTesting(int b1, int b2) const +{ + details::meshCollisionOrientedNodeLeafTesting( + b1, + b2, + this->model1, + this->model2, + this->vertices1, + this->vertices2, + this->tri_indices1, + this->tri_indices2, + R, + T, + this->tf1, + this->tf2, + this->enable_statistics, + this->cost_density, + this->num_leaf_tests, + this->request, + *this->result); +} + +//============================================================================== +template +bool MeshCollisionTraversalNodeOBB::BVTesting( + int b1, int b2, const Matrix3& Rc, const Vector3& Tc) const +{ + if(this->enable_statistics) this->num_bv_tests++; + + return obbDisjoint( + Rc, Tc, + this->model1->getBV(b1).bv.extent, + this->model2->getBV(b2).bv.extent); +} + +//============================================================================== +template +bool MeshCollisionTraversalNodeOBB::BVTesting( + int b1, int b2, const Transform3& tf) const +{ + if(this->enable_statistics) this->num_bv_tests++; + + return obbDisjoint(tf, + this->model1->getBV(b1).bv.extent, + this->model2->getBV(b2).bv.extent); +} + +//============================================================================== +template +void MeshCollisionTraversalNodeOBB::leafTesting( + int b1, int b2, const Matrix3& Rc, const Vector3& Tc) const +{ + details::meshCollisionOrientedNodeLeafTesting( + b1, + b2, + this->model1, + this->model2, + this->vertices1, + this->vertices2, + this->tri_indices1, + this->tri_indices2, + R, + T, + this->tf1, + this->tf2, + this->enable_statistics, + this->cost_density, + this->num_leaf_tests, + this->request, + *this->result); +} + +//============================================================================== +template +void MeshCollisionTraversalNodeOBB::leafTesting( + int b1, int b2, const Transform3& tf) const +{ + details::meshCollisionOrientedNodeLeafTesting( + b1, + b2, + this->model1, + this->model2, + this->vertices1, + this->vertices2, + this->tri_indices1, + this->tri_indices2, + tf, + this->tf1, + this->tf2, + this->enable_statistics, + this->cost_density, + this->num_leaf_tests, + this->request, + *this->result); +} + +//============================================================================== +template +MeshCollisionTraversalNodeRSS::MeshCollisionTraversalNodeRSS() + : MeshCollisionTraversalNode>(), + R(Matrix3::Identity()) +{ + // Do nothing +} + +//============================================================================== +template +bool MeshCollisionTraversalNodeRSS::BVTesting(int b1, int b2) const +{ + if(this->enable_statistics) this->num_bv_tests++; + + return !overlap(R, T, this->model1->getBV(b1).bv, this->model2->getBV(b2).bv); +} + +//============================================================================== +template +void MeshCollisionTraversalNodeRSS::leafTesting(int b1, int b2) const +{ + details::meshCollisionOrientedNodeLeafTesting( + b1, + b2, + this->model1, + this->model2, + this->vertices1, + this->vertices2, + this->tri_indices1, + this->tri_indices2, + R, + T, + this->tf1, + this->tf2, + this->enable_statistics, + this->cost_density, + this->num_leaf_tests, + this->request, + *this->result); +} + +//============================================================================== +template +MeshCollisionTraversalNodekIOS::MeshCollisionTraversalNodekIOS() + : MeshCollisionTraversalNode>(), + R(Matrix3::Identity()) +{ + // Do nothing +} + +//============================================================================== +template +bool MeshCollisionTraversalNodekIOS::BVTesting(int b1, int b2) const +{ + if(this->enable_statistics) this->num_bv_tests++; + + return !overlap(R, T, this->model1->getBV(b1).bv, this->model2->getBV(b2).bv); +} + +//============================================================================== +template +void MeshCollisionTraversalNodekIOS::leafTesting(int b1, int b2) const +{ + details::meshCollisionOrientedNodeLeafTesting( + b1, + b2, + this->model1, + this->model2, + this->vertices1, + this->vertices2, + this->tri_indices1, + this->tri_indices2, + R, + T, + this->tf1, + this->tf2, + this->enable_statistics, + this->cost_density, + this->num_leaf_tests, + this->request, + *this->result); +} + +//============================================================================== +template +MeshCollisionTraversalNodeOBBRSS::MeshCollisionTraversalNodeOBBRSS() + : MeshCollisionTraversalNode>(), + R(Matrix3::Identity()) +{ + // Do nothing +} + +//============================================================================== +template +bool MeshCollisionTraversalNodeOBBRSS::BVTesting(int b1, int b2) const +{ + if(this->enable_statistics) this->num_bv_tests++; + + return !overlap(R, T, this->model1->getBV(b1).bv, this->model2->getBV(b2).bv); +} + +//============================================================================== +template +void MeshCollisionTraversalNodeOBBRSS::leafTesting(int b1, int b2) const +{ + details::meshCollisionOrientedNodeLeafTesting( + b1, + b2, + this->model1, + this->model2, + this->vertices1, + this->vertices2, + this->tri_indices1, + this->tri_indices2, + R, + T, + this->tf1, + this->tf2, + this->enable_statistics, + this->cost_density, + this->num_leaf_tests, + this->request, + *this->result); +} + +namespace details +{ + +template +void meshCollisionOrientedNodeLeafTesting( + int b1, int b2, + const BVHModel* model1, + const BVHModel* model2, + Vector3* vertices1, + Vector3* vertices2, + Triangle* tri_indices1, + Triangle* tri_indices2, + const Matrix3& R, + const Vector3& T, + const Transform3& tf1, + const Transform3& tf2, + bool enable_statistics, + typename BV::S cost_density, + int& num_leaf_tests, + const CollisionRequest& request, + CollisionResult& result) +{ + using S = typename BV::S; + + if(enable_statistics) num_leaf_tests++; + + const BVNode& node1 = model1->getBV(b1); + const BVNode& node2 = model2->getBV(b2); + + int primitive_id1 = node1.primitiveId(); + int primitive_id2 = node2.primitiveId(); + + const Triangle& tri_id1 = tri_indices1[primitive_id1]; + const Triangle& tri_id2 = tri_indices2[primitive_id2]; + + const Vector3& p1 = vertices1[tri_id1[0]]; + const Vector3& p2 = vertices1[tri_id1[1]]; + const Vector3& p3 = vertices1[tri_id1[2]]; + const Vector3& q1 = vertices2[tri_id2[0]]; + const Vector3& q2 = vertices2[tri_id2[1]]; + const Vector3& q3 = vertices2[tri_id2[2]]; + + if(model1->isOccupied() && model2->isOccupied()) + { + bool is_intersect = false; + + if(!request.enable_contact) // only interested in collision or not + { + if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3, R, T)) + { + is_intersect = true; + if(result.numContacts() < request.num_max_contacts) + result.addContact(Contact(model1, model2, primitive_id1, primitive_id2)); + } + } + else // need compute the contact information + { + S penetration; + Vector3 normal; + unsigned int n_contacts; + Vector3 contacts[2]; + + if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3, + R, T, + contacts, + &n_contacts, + &penetration, + &normal)) + { + is_intersect = true; + + if(request.num_max_contacts < result.numContacts() + n_contacts) + n_contacts = (request.num_max_contacts > result.numContacts()) ? (request.num_max_contacts - result.numContacts()) : 0; + + for(unsigned int i = 0; i < n_contacts; ++i) + { + result.addContact(Contact(model1, model2, primitive_id1, primitive_id2, tf1 * contacts[i], tf1.linear() * normal, penetration)); + } + } + } + + if(is_intersect && request.enable_cost) + { + AABB overlap_part; + AABB(tf1 * p1, tf1 * p2, tf1 * p3).overlap(AABB(tf2 * q1, tf2 * q2, tf2 * q3), overlap_part); + result.addCostSource(CostSource(overlap_part, cost_density), request.num_max_cost_sources); + } + } + else if((!model1->isFree() && !model2->isFree()) && request.enable_cost) + { + if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3, R, T)) + { + AABB overlap_part; + AABB(tf1 * p1, tf1 * p2, tf1 * p3).overlap(AABB(tf2 * q1, tf2 * q2, tf2 * q3), overlap_part); + result.addCostSource(CostSource(overlap_part, cost_density), request.num_max_cost_sources); + } + } +} + +//============================================================================== +template +void meshCollisionOrientedNodeLeafTesting( + int b1, + int b2, + const BVHModel* model1, + const BVHModel* model2, + Vector3* vertices1, + Vector3* vertices2, + Triangle* tri_indices1, + Triangle* tri_indices2, + const Transform3& tf, + const Transform3& tf1, + const Transform3& tf2, + bool enable_statistics, + typename BV::S cost_density, + int& num_leaf_tests, + const CollisionRequest& request, + CollisionResult& result) +{ + using S = typename BV::S; + + if(enable_statistics) num_leaf_tests++; + + const BVNode& node1 = model1->getBV(b1); + const BVNode& node2 = model2->getBV(b2); + + int primitive_id1 = node1.primitiveId(); + int primitive_id2 = node2.primitiveId(); + + const Triangle& tri_id1 = tri_indices1[primitive_id1]; + const Triangle& tri_id2 = tri_indices2[primitive_id2]; + + const Vector3& p1 = vertices1[tri_id1[0]]; + const Vector3& p2 = vertices1[tri_id1[1]]; + const Vector3& p3 = vertices1[tri_id1[2]]; + const Vector3& q1 = vertices2[tri_id2[0]]; + const Vector3& q2 = vertices2[tri_id2[1]]; + const Vector3& q3 = vertices2[tri_id2[2]]; + + if(model1->isOccupied() && model2->isOccupied()) + { + bool is_intersect = false; + + if(!request.enable_contact) // only interested in collision or not + { + if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3, tf)) + { + is_intersect = true; + if(result.numContacts() < request.num_max_contacts) + result.addContact(Contact(model1, model2, primitive_id1, primitive_id2)); + } + } + else // need compute the contact information + { + S penetration; + Vector3 normal; + unsigned int n_contacts; + Vector3 contacts[2]; + + if(Intersect::intersect_Triangle( + p1, p2, p3, q1, q2, q3, tf, contacts, &n_contacts, &penetration, &normal)) + { + is_intersect = true; + + if(request.num_max_contacts < result.numContacts() + n_contacts) + n_contacts = (request.num_max_contacts > result.numContacts()) ? (request.num_max_contacts - result.numContacts()) : 0; + + for(unsigned int i = 0; i < n_contacts; ++i) + { + result.addContact(Contact(model1, model2, primitive_id1, primitive_id2, tf1 * contacts[i], tf1.linear() * normal, penetration)); + } + } + } + + if(is_intersect && request.enable_cost) + { + AABB overlap_part; + AABB(tf1 * p1, tf1 * p2, tf1 * p3).overlap(AABB(tf2 * q1, tf2 * q2, tf2 * q3), overlap_part); + result.addCostSource(CostSource(overlap_part, cost_density), request.num_max_cost_sources); + } + } + else if((!model1->isFree() && !model2->isFree()) && request.enable_cost) + { + if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3, tf)) + { + AABB overlap_part; + AABB(tf1 * p1, tf1 * p2, tf1 * p3).overlap(AABB(tf2 * q1, tf2 * q2, tf2 * q3), overlap_part); + result.addCostSource(CostSource(overlap_part, cost_density), request.num_max_cost_sources); + } + } +} + +} // namespace details + +namespace details +{ + +template +bool setupMeshCollisionOrientedNode( + OrientedNode& node, + const BVHModel& model1, const Transform3& tf1, + const BVHModel& model2, const Transform3& tf2, + const CollisionRequest& request, + CollisionResult& result) +{ + if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES) + return false; + + node.vertices1 = model1.vertices; + node.vertices2 = model2.vertices; + + node.tri_indices1 = model1.tri_indices; + node.tri_indices2 = model2.tri_indices; + + node.model1 = &model1; + node.tf1 = tf1; + node.model2 = &model2; + node.tf2 = tf2; + + node.request = request; + node.result = &result; + + node.cost_density = model1.cost_density * model2.cost_density; + + relativeTransform(tf1, tf2, node.R, node.T); + + return true; +} + +} // namespace details + +//============================================================================== +template +bool initialize( + MeshCollisionTraversalNodeOBB& node, + const BVHModel>& model1, + const Transform3& tf1, + const BVHModel>& model2, + const Transform3& tf2, + const CollisionRequest& request, + CollisionResult& result) +{ + return details::setupMeshCollisionOrientedNode( + node, model1, tf1, model2, tf2, request, result); +} + +//============================================================================== +template +bool initialize( + MeshCollisionTraversalNodeRSS& node, + const BVHModel>& model1, + const Transform3& tf1, + const BVHModel>& model2, + const Transform3& tf2, + const CollisionRequest& request, + CollisionResult& result) +{ + return details::setupMeshCollisionOrientedNode( + node, model1, tf1, model2, tf2, request, result); +} + +//============================================================================== +template +bool initialize( + MeshCollisionTraversalNodekIOS& node, + const BVHModel>& model1, + const Transform3& tf1, + const BVHModel>& model2, + const Transform3& tf2, + const CollisionRequest& request, + CollisionResult& result) +{ + return details::setupMeshCollisionOrientedNode( + node, model1, tf1, model2, tf2, request, result); +} + +//============================================================================== +template +bool initialize( + MeshCollisionTraversalNodeOBBRSS& node, + const BVHModel>& model1, + const Transform3& tf1, + const BVHModel>& model2, + const Transform3& tf2, + const CollisionRequest& request, + CollisionResult& result) +{ + return details::setupMeshCollisionOrientedNode( + node, model1, tf1, model2, tf2, request, result); +} + +} // namespace fcl + +#endif diff --git a/include/fcl/traversal/collision/mesh_continuous_collision_traversal_node.h b/include/fcl/traversal/collision/mesh_continuous_collision_traversal_node.h new file mode 100644 index 000000000..2dd7c9f3c --- /dev/null +++ b/include/fcl/traversal/collision/mesh_continuous_collision_traversal_node.h @@ -0,0 +1,273 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + + +#ifndef FCL_TRAVERSAL_MESHCONTINUOUSCOLLISIONTRAVERSALNODE_H +#define FCL_TRAVERSAL_MESHCONTINUOUSCOLLISIONTRAVERSALNODE_H + +#include "fcl/traversal/collision/bvh_collision_traversal_node.h" + +namespace fcl +{ + +/// @brief Traversal node for continuous collision between BVH models +template +struct BVHContinuousCollisionPair +{ + BVHContinuousCollisionPair(); + + BVHContinuousCollisionPair(int id1_, int id2_, S time); + + /// @brief The index of one in-collision primitive + int id1; + + /// @brief The index of the other in-collision primitive + int id2; + + /// @brief Collision time normalized in [0, 1]. The collision time out of [0, 1] means collision-free + S collision_time; +}; + +/// @brief Traversal node for continuous collision between meshes +template +class MeshContinuousCollisionTraversalNode + : public BVHCollisionTraversalNode +{ +public: + + using S = typename BV::S; + + MeshContinuousCollisionTraversalNode(); + + /// @brief Intersection testing between leaves (two triangles) + void leafTesting(int b1, int b2) const; + + /// @brief Whether the traversal process can stop early + bool canStop() const; + + Vector3* vertices1; + Vector3* vertices2; + + Triangle* tri_indices1; + Triangle* tri_indices2; + + Vector3* prev_vertices1; + Vector3* prev_vertices2; + + mutable int num_vf_tests; + mutable int num_ee_tests; + + mutable std::vector> pairs; + + mutable S time_of_contact; +}; + +/// @brief Initialize traversal node for continuous collision detection between +/// two meshes +template +bool initialize( + MeshContinuousCollisionTraversalNode& node, + const BVHModel& model1, + const Transform3& tf1, + const BVHModel& model2, + const Transform3& tf2, + const CollisionRequest& request); + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +BVHContinuousCollisionPair::BVHContinuousCollisionPair() +{ + // Do nothing +} + +//============================================================================== +template +BVHContinuousCollisionPair::BVHContinuousCollisionPair( + int id1_, int id2_, S time) + : id1(id1_), id2(id2_), collision_time(time) +{ + // Do nothing +} + +//============================================================================== +template +MeshContinuousCollisionTraversalNode::MeshContinuousCollisionTraversalNode() + : BVHCollisionTraversalNode() +{ + vertices1 = nullptr; + vertices2 = nullptr; + tri_indices1 = nullptr; + tri_indices2 = nullptr; + prev_vertices1 = nullptr; + prev_vertices2 = nullptr; + + num_vf_tests = 0; + num_ee_tests = 0; + time_of_contact = 1; +} + +//============================================================================== +template +void MeshContinuousCollisionTraversalNode::leafTesting(int b1, int b2) const +{ + if(this->enable_statistics) this->num_leaf_tests++; + + const BVNode& node1 = this->model1->getBV(b1); + const BVNode& node2 = this->model2->getBV(b2); + + S collision_time = 2; + Vector3 collision_pos; + + int primitive_id1 = node1.primitiveId(); + int primitive_id2 = node2.primitiveId(); + + const Triangle& tri_id1 = tri_indices1[primitive_id1]; + const Triangle& tri_id2 = tri_indices2[primitive_id2]; + + Vector3* S0[3]; + Vector3* S1[3]; + Vector3* T0[3]; + Vector3* T1[3]; + + for(int i = 0; i < 3; ++i) + { + S0[i] = prev_vertices1 + tri_id1[i]; + S1[i] = vertices1 + tri_id1[i]; + T0[i] = prev_vertices2 + tri_id2[i]; + T1[i] = vertices2 + tri_id2[i]; + } + + S tmp; + Vector3 tmpv; + + // 6 VF checks + for(int i = 0; i < 3; ++i) + { + if(this->enable_statistics) num_vf_tests++; + if(Intersect::intersect_VF(*(S0[0]), *(S0[1]), *(S0[2]), *(T0[i]), *(S1[0]), *(S1[1]), *(S1[2]), *(T1[i]), &tmp, &tmpv)) + { + if(collision_time > tmp) + { + collision_time = tmp; collision_pos = tmpv; + } + } + + if(this->enable_statistics) num_vf_tests++; + if(Intersect::intersect_VF(*(T0[0]), *(T0[1]), *(T0[2]), *(S0[i]), *(T1[0]), *(T1[1]), *(T1[2]), *(S1[i]), &tmp, &tmpv)) + { + if(collision_time > tmp) + { + collision_time = tmp; collision_pos = tmpv; + } + } + } + + // 9 EE checks + for(int i = 0; i < 3; ++i) + { + int S_id1 = i; + int S_id2 = i + 1; + if(S_id2 == 3) S_id2 = 0; + for(int j = 0; j < 3; ++j) + { + int T_id1 = j; + int T_id2 = j + 1; + if(T_id2 == 3) T_id2 = 0; + + num_ee_tests++; + if(Intersect::intersect_EE(*(S0[S_id1]), *(S0[S_id2]), *(T0[T_id1]), *(T0[T_id2]), *(S1[S_id1]), *(S1[S_id2]), *(T1[T_id1]), *(T1[T_id2]), &tmp, &tmpv)) + { + if(collision_time > tmp) + { + collision_time = tmp; collision_pos = tmpv; + } + } + } + } + + if(!(collision_time > 1)) // collision happens + { + pairs.emplace_back(primitive_id1, primitive_id2, collision_time); + time_of_contact = std::min(time_of_contact, collision_time); + } +} + +//============================================================================== +template +bool MeshContinuousCollisionTraversalNode::canStop() const +{ + return (pairs.size() > 0) && (this->request.num_max_contacts <= pairs.size()); +} + +//============================================================================== +template +bool initialize( + MeshContinuousCollisionTraversalNode& node, + const BVHModel& model1, + const Transform3& tf1, + const BVHModel& model2, + const Transform3& tf2, + const CollisionRequest& request) +{ + node.model1 = &model1; + node.tf1 = tf1; + node.model2 = &model2; + node.tf2 = tf2; + + node.vertices1 = model1.vertices; + node.vertices2 = model2.vertices; + + node.tri_indices1 = model1.tri_indices; + node.tri_indices2 = model2.tri_indices; + + node.prev_vertices1 = model1.prev_vertices; + node.prev_vertices2 = model2.prev_vertices; + + node.request = request; + + return true; +} + +} // namespace fcl + +#endif diff --git a/include/fcl/traversal/collision/mesh_shape_collision_traversal_node.h b/include/fcl/traversal/collision/mesh_shape_collision_traversal_node.h new file mode 100644 index 000000000..d703a944b --- /dev/null +++ b/include/fcl/traversal/collision/mesh_shape_collision_traversal_node.h @@ -0,0 +1,650 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_TRAVERSAL_MESHSHAPECOLLISIONTRAVERSALNODE_H +#define FCL_TRAVERSAL_MESHSHAPECOLLISIONTRAVERSALNODE_H + +#include "fcl/shape/compute_bv.h" +#include "fcl/traversal/collision/bvh_shape_collision_traversal_node.h" + +namespace fcl +{ + +/// @brief Traversal node for collision between mesh and shape +template +class MeshShapeCollisionTraversalNode + : public BVHShapeCollisionTraversalNode +{ +public: + + using S = typename BV::S; + + MeshShapeCollisionTraversalNode(); + + /// @brief Intersection testing between leaves (one triangle and one shape) + void leafTesting(int b1, int b2) const; + + /// @brief Whether the traversal process can stop early + bool canStop() const; + + Vector3* vertices; + Triangle* tri_indices; + + S cost_density; + + const NarrowPhaseSolver* nsolver; +}; + +/// @brief Initialize traversal node for collision between one mesh and one +/// shape, given current object transform +template +bool initialize( + MeshShapeCollisionTraversalNode& node, + BVHModel& model1, + Transform3& tf1, + const Shape& model2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result, + bool use_refit = false, bool refit_bottomup = false); + +/// @cond IGNORE +namespace details +{ + +template +void meshShapeCollisionOrientedNodeLeafTesting( + int b1, + int b2, + const BVHModel* model1, + const Shape& model2, + Vector3* vertices, + Triangle* tri_indices, + const Transform3& tf1, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + bool enable_statistics, + typename BV::S cost_density, + int& num_leaf_tests, + const CollisionRequest& request, + CollisionResult& result); + +} // namespace detials + +/// @endcond + +/// @brief Traversal node for mesh and shape, when mesh BVH is one of the oriented node (OBB, RSS, OBBRSS, kIOS) +template +class MeshShapeCollisionTraversalNodeOBB + : public MeshShapeCollisionTraversalNode< + OBB, Shape, NarrowPhaseSolver> +{ +public: + MeshShapeCollisionTraversalNodeOBB(); + + bool BVTesting(int b1, int b2) const; + + void leafTesting(int b1, int b2) const; + +}; + +/// @brief Initialize the traversal node for collision between one mesh and one +/// shape, specialized for OBB type +template +bool initialize( + MeshShapeCollisionTraversalNodeOBB& node, + const BVHModel>& model1, + const Transform3& tf1, + const Shape& model2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result); + +template +class MeshShapeCollisionTraversalNodeRSS + : public MeshShapeCollisionTraversalNode< + RSS, Shape, NarrowPhaseSolver> +{ +public: + MeshShapeCollisionTraversalNodeRSS(); + + bool BVTesting(int b1, int b2) const; + + void leafTesting(int b1, int b2) const; + +}; + +/// @brief Initialize the traversal node for collision between one mesh and one +/// shape, specialized for RSS type +template +bool initialize( + MeshShapeCollisionTraversalNodeRSS& node, + const BVHModel>& model1, + const Transform3& tf1, + const Shape& model2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result); + +template +class MeshShapeCollisionTraversalNodekIOS + : public MeshShapeCollisionTraversalNode< + kIOS, Shape, NarrowPhaseSolver> +{ +public: + MeshShapeCollisionTraversalNodekIOS(); + + bool BVTesting(int b1, int b2) const; + + void leafTesting(int b1, int b2) const; + +}; + +/// @brief Initialize the traversal node for collision between one mesh and one +/// shape, specialized for kIOS type +template +bool initialize( + MeshShapeCollisionTraversalNodekIOS& node, + const BVHModel>& model1, + const Transform3& tf1, + const Shape& model2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result); + +template +class MeshShapeCollisionTraversalNodeOBBRSS + : public MeshShapeCollisionTraversalNode< + OBBRSS, Shape, NarrowPhaseSolver> +{ +public: + MeshShapeCollisionTraversalNodeOBBRSS(); + + bool BVTesting(int b1, int b2) const; + + void leafTesting(int b1, int b2) const; + +}; + +/// @brief Initialize the traversal node for collision between one mesh and one +/// shape, specialized for OBBRSS type +template +bool initialize( + MeshShapeCollisionTraversalNodeOBBRSS& node, + const BVHModel>& model1, + const Transform3& tf1, + const Shape& model2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result); + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +MeshShapeCollisionTraversalNode::MeshShapeCollisionTraversalNode() + : BVHShapeCollisionTraversalNode() +{ + vertices = nullptr; + tri_indices = nullptr; + + nsolver = nullptr; +} + +//============================================================================== +template +void MeshShapeCollisionTraversalNode::leafTesting(int b1, int b2) const +{ + if(this->enable_statistics) this->num_leaf_tests++; + const BVNode& node = this->model1->getBV(b1); + + int primitive_id = node.primitiveId(); + + const Triangle& tri_id = tri_indices[primitive_id]; + + const Vector3& p1 = vertices[tri_id[0]]; + const Vector3& p2 = vertices[tri_id[1]]; + const Vector3& p3 = vertices[tri_id[2]]; + + if(this->model1->isOccupied() && this->model2->isOccupied()) + { + bool is_intersect = false; + + if(!this->request.enable_contact) + { + if(nsolver->shapeTriangleIntersect(*(this->model2), this->tf2, p1, p2, p3, nullptr, nullptr, nullptr)) + { + is_intersect = true; + if(this->request.num_max_contacts > this->result->numContacts()) + this->result->addContact(Contact(this->model1, this->model2, primitive_id, Contact::NONE)); + } + } + else + { + S penetration; + Vector3 normal; + Vector3 contactp; + + if(nsolver->shapeTriangleIntersect(*(this->model2), this->tf2, p1, p2, p3, &contactp, &penetration, &normal)) + { + is_intersect = true; + if(this->request.num_max_contacts > this->result->numContacts()) + this->result->addContact(Contact(this->model1, this->model2, primitive_id, Contact::NONE, contactp, -normal, penetration)); + } + } + + if(is_intersect && this->request.enable_cost) + { + AABB overlap_part; + AABB shape_aabb; + computeBV(*(this->model2), this->tf2, shape_aabb); + AABB(p1, p2, p3).overlap(shape_aabb, overlap_part); + this->result->addCostSource(CostSource(overlap_part, cost_density), this->request.num_max_cost_sources); + } + } + if((!this->model1->isFree() && !this->model2->isFree()) && this->request.enable_cost) + { + if(nsolver->shapeTriangleIntersect(*(this->model2), this->tf2, p1, p2, p3, nullptr, nullptr, nullptr)) + { + AABB overlap_part; + AABB shape_aabb; + computeBV(*(this->model2), this->tf2, shape_aabb); + AABB(p1, p2, p3).overlap(shape_aabb, overlap_part); + this->result->addCostSource(CostSource(overlap_part, cost_density), this->request.num_max_cost_sources); + } + } +} + +//============================================================================== +template +bool MeshShapeCollisionTraversalNode::canStop() const +{ + return this->request.isSatisfied(*(this->result)); +} + +//============================================================================== +template +bool initialize( + MeshShapeCollisionTraversalNode& node, + BVHModel& model1, + Transform3& tf1, + const Shape& model2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result, + bool use_refit, + bool refit_bottomup) +{ + using S = typename BV::S; + + if(model1.getModelType() != BVH_MODEL_TRIANGLES) + return false; + + if(!tf1.matrix().isIdentity()) + { + std::vector> vertices_transformed(model1.num_vertices); + for(int i = 0; i < model1.num_vertices; ++i) + { + Vector3& p = model1.vertices[i]; + Vector3 new_v = tf1 * p; + vertices_transformed[i] = new_v; + } + + model1.beginReplaceModel(); + model1.replaceSubModel(vertices_transformed); + model1.endReplaceModel(use_refit, refit_bottomup); + + tf1.setIdentity(); + } + + node.model1 = &model1; + node.tf1 = tf1; + node.model2 = &model2; + node.tf2 = tf2; + node.nsolver = nsolver; + + computeBV(model2, tf2, node.model2_bv); + + node.vertices = model1.vertices; + node.tri_indices = model1.tri_indices; + + node.request = request; + node.result = &result; + + node.cost_density = model1.cost_density * model2.cost_density; + + return true; +} + +/// @cond IGNORE +namespace details +{ + +//============================================================================== +template +void meshShapeCollisionOrientedNodeLeafTesting( + int b1, int b2, + const BVHModel* model1, const Shape& model2, + Vector3* vertices, Triangle* tri_indices, + const Transform3& tf1, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + bool enable_statistics, + typename BV::S cost_density, + int& num_leaf_tests, + const CollisionRequest& request, + CollisionResult& result) +{ + using S = typename BV::S; + + if(enable_statistics) num_leaf_tests++; + const BVNode& node = model1->getBV(b1); + + int primitive_id = node.primitiveId(); + + const Triangle& tri_id = tri_indices[primitive_id]; + + const Vector3& p1 = vertices[tri_id[0]]; + const Vector3& p2 = vertices[tri_id[1]]; + const Vector3& p3 = vertices[tri_id[2]]; + + if(model1->isOccupied() && model2.isOccupied()) + { + bool is_intersect = false; + + if(!request.enable_contact) // only interested in collision or not + { + if(nsolver->shapeTriangleIntersect(model2, tf2, p1, p2, p3, tf1, nullptr, nullptr, nullptr)) + { + is_intersect = true; + if(request.num_max_contacts > result.numContacts()) + result.addContact(Contact(model1, &model2, primitive_id, Contact::NONE)); + } + } + else + { + S penetration; + Vector3 normal; + Vector3 contactp; + + if(nsolver->shapeTriangleIntersect(model2, tf2, p1, p2, p3, tf1, &contactp, &penetration, &normal)) + { + is_intersect = true; + if(request.num_max_contacts > result.numContacts()) + result.addContact(Contact(model1, &model2, primitive_id, Contact::NONE, contactp, -normal, penetration)); + } + } + + if(is_intersect && request.enable_cost) + { + AABB overlap_part; + AABB shape_aabb; + computeBV(model2, tf2, shape_aabb); + /* bool res = */ AABB(tf1 * p1, tf1 * p2, tf1 * p3).overlap(shape_aabb, overlap_part); + result.addCostSource(CostSource(overlap_part, cost_density), request.num_max_cost_sources); + } + } + else if((!model1->isFree() || model2.isFree()) && request.enable_cost) + { + if(nsolver->shapeTriangleIntersect(model2, tf2, p1, p2, p3, tf1, nullptr, nullptr, nullptr)) + { + AABB overlap_part; + AABB shape_aabb; + computeBV(model2, tf2, shape_aabb); + /* bool res = */ AABB(tf1 * p1, tf1 * p2, tf1 * p3).overlap(shape_aabb, overlap_part); + result.addCostSource(CostSource(overlap_part, cost_density), request.num_max_cost_sources); + } + } +} + +} // namespace detials + +//============================================================================== +template +MeshShapeCollisionTraversalNodeOBB:: +MeshShapeCollisionTraversalNodeOBB() + : MeshShapeCollisionTraversalNode, Shape, NarrowPhaseSolver>() +{ +} + +//============================================================================== +template +bool MeshShapeCollisionTraversalNodeOBB::BVTesting(int b1, int b2) const +{ + if(this->enable_statistics) this->num_bv_tests++; + + return !overlap(this->tf1.linear(), this->tf1.translation(), this->model2_bv, this->model1->getBV(b1).bv); +} + +//============================================================================== +template +void MeshShapeCollisionTraversalNodeOBB::leafTesting(int b1, int b2) const +{ + details::meshShapeCollisionOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices, + this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->result)); +} + +//============================================================================== +template +MeshShapeCollisionTraversalNodeRSS::MeshShapeCollisionTraversalNodeRSS() + : MeshShapeCollisionTraversalNode, Shape, NarrowPhaseSolver>() +{ +} + +//============================================================================== +template +bool MeshShapeCollisionTraversalNodeRSS::BVTesting(int b1, int b2) const +{ + if(this->enable_statistics) this->num_bv_tests++; + + return !overlap(this->tf1.linear(), this->tf1.translation(), this->model2_bv, this->model1->getBV(b1).bv); +} + +//============================================================================== +template +void MeshShapeCollisionTraversalNodeRSS::leafTesting(int b1, int b2) const +{ + details::meshShapeCollisionOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices, + this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->result)); +} + +//============================================================================== +template +MeshShapeCollisionTraversalNodekIOS:: +MeshShapeCollisionTraversalNodekIOS() + : MeshShapeCollisionTraversalNode, Shape, NarrowPhaseSolver>() +{ +} + +//============================================================================== +template +bool MeshShapeCollisionTraversalNodekIOS::BVTesting(int b1, int b2) const +{ + if(this->enable_statistics) this->num_bv_tests++; + + return !overlap(this->tf1.linear(), this->tf1.translation(), this->model2_bv, this->model1->getBV(b1).bv); +} + +//============================================================================== +template +void MeshShapeCollisionTraversalNodekIOS::leafTesting(int b1, int b2) const +{ + details::meshShapeCollisionOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices, + this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->result)); +} + +//============================================================================== +template +MeshShapeCollisionTraversalNodeOBBRSS:: +MeshShapeCollisionTraversalNodeOBBRSS() + : MeshShapeCollisionTraversalNode, Shape, NarrowPhaseSolver>() +{ +} + +//============================================================================== +template +bool MeshShapeCollisionTraversalNodeOBBRSS::BVTesting(int b1, int b2) const +{ + if(this->enable_statistics) this->num_bv_tests++; + return !overlap(this->tf1.linear(), this->tf1.translation(), this->model2_bv, this->model1->getBV(b1).bv); +} + +//============================================================================== +template +void MeshShapeCollisionTraversalNodeOBBRSS::leafTesting(int b1, int b2) const +{ + details::meshShapeCollisionOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices, + this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->result)); +} + +/// @cond IGNORE +namespace details +{ + +template class OrientedNode> +bool setupMeshShapeCollisionOrientedNode( + OrientedNode& node, + const BVHModel& model1, + const Transform3& tf1, + const Shape& model2, const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result) +{ + if(model1.getModelType() != BVH_MODEL_TRIANGLES) + return false; + + node.model1 = &model1; + node.tf1 = tf1; + node.model2 = &model2; + node.tf2 = tf2; + node.nsolver = nsolver; + + computeBV(model2, tf2, node.model2_bv); + + node.vertices = model1.vertices; + node.tri_indices = model1.tri_indices; + + node.request = request; + node.result = &result; + + node.cost_density = model1.cost_density * model2.cost_density; + + return true; +} + +} // namespace details +/// @endcond + +//============================================================================== +template +bool initialize( + MeshShapeCollisionTraversalNodeOBB& node, + const BVHModel>& model1, + const Transform3& tf1, + const Shape& model2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result) +{ + return details::setupMeshShapeCollisionOrientedNode( + node, model1, tf1, model2, tf2, nsolver, request, result); +} + +//============================================================================== +template +bool initialize( + MeshShapeCollisionTraversalNodeRSS& node, + const BVHModel>& model1, + const Transform3& tf1, + const Shape& model2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result) +{ + return details::setupMeshShapeCollisionOrientedNode( + node, model1, tf1, model2, tf2, nsolver, request, result); +} + +//============================================================================== +template +bool initialize( + MeshShapeCollisionTraversalNodekIOS& node, + const BVHModel>& model1, + const Transform3& tf1, + const Shape& model2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result) +{ + return details::setupMeshShapeCollisionOrientedNode( + node, model1, tf1, model2, tf2, nsolver, request, result); +} + +//============================================================================== +template +bool initialize( + MeshShapeCollisionTraversalNodeOBBRSS& node, + const BVHModel>& model1, + const Transform3& tf1, + const Shape& model2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result) +{ + return details::setupMeshShapeCollisionOrientedNode( + node, model1, tf1, model2, tf2, nsolver, request, result); +} + +} // namespace fcl + +#endif diff --git a/include/fcl/traversal/collision/shape_bvh_collision_traversal_node.h b/include/fcl/traversal/collision/shape_bvh_collision_traversal_node.h new file mode 100644 index 000000000..572d6ffad --- /dev/null +++ b/include/fcl/traversal/collision/shape_bvh_collision_traversal_node.h @@ -0,0 +1,141 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + + +#ifndef FCL_TRAVERSAL_SHAPEBVHCOLLISIONTRAVERSALNODE_H +#define FCL_TRAVERSAL_SHAPEBVHCOLLISIONTRAVERSALNODE_H + +#include "fcl/traversal/traversal_node_base.h" +#include "fcl/BVH/BVH_model.h" + +namespace fcl +{ + +/// @brief Traversal node for collision between shape and BVH +template +class ShapeBVHCollisionTraversalNode + : public CollisionTraversalNodeBase +{ +public: + + using S = typename BV::S; + + ShapeBVHCollisionTraversalNode(); + + /// @brief Alway extend the second model, which is a BVH model + bool firstOverSecond(int, int) const; + + /// @brief Whether the BV node in the second BVH tree is leaf + bool isSecondNodeLeaf(int b) const; + + /// @brief Obtain the left child of BV node in the second BVH + int getSecondLeftChild(int b) const; + + /// @brief Obtain the right child of BV node in the second BVH + int getSecondRightChild(int b) const; + + /// @brief BV culling test in one BVTT node + bool BVTesting(int b1, int b2) const; + + const Shape* model1; + const BVHModel* model2; + BV model1_bv; + + mutable int num_bv_tests; + mutable int num_leaf_tests; + mutable S query_time_seconds; +}; + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +ShapeBVHCollisionTraversalNode::ShapeBVHCollisionTraversalNode() + : CollisionTraversalNodeBase() +{ + model1 = nullptr; + model2 = nullptr; + + num_bv_tests = 0; + num_leaf_tests = 0; + query_time_seconds = 0.0; +} + +//============================================================================== +template +bool ShapeBVHCollisionTraversalNode::firstOverSecond(int, int) const +{ + return false; +} + +//============================================================================== +template +bool ShapeBVHCollisionTraversalNode::isSecondNodeLeaf(int b) const +{ + return model2->getBV(b).isLeaf(); +} + +//============================================================================== +template +int ShapeBVHCollisionTraversalNode::getSecondLeftChild(int b) const +{ + return model2->getBV(b).leftChild(); +} + +//============================================================================== +template +int ShapeBVHCollisionTraversalNode::getSecondRightChild(int b) const +{ + return model2->getBV(b).rightChild(); +} + +//============================================================================== +template +bool ShapeBVHCollisionTraversalNode::BVTesting(int b1, int b2) const +{ + if(this->enable_statistics) num_bv_tests++; + return !model2->getBV(b2).bv.overlap(model1_bv); +} + + +} // namespace fcl + +#endif diff --git a/include/fcl/traversal/collision/shape_collision_traversal_node.h b/include/fcl/traversal/collision/shape_collision_traversal_node.h new file mode 100644 index 000000000..1db6bf49f --- /dev/null +++ b/include/fcl/traversal/collision/shape_collision_traversal_node.h @@ -0,0 +1,208 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_TRAVERSAL_SHAPECOLLISIONTRAVERSALNODE_H +#define FCL_TRAVERSAL_SHAPECOLLISIONTRAVERSALNODE_H + +#include "fcl/shape/compute_bv.h" +#include "fcl/traversal/collision/collision_traversal_node_base.h" + +namespace fcl +{ + +/// @brief Traversal node for collision between two shapes +template +class ShapeCollisionTraversalNode + : public CollisionTraversalNodeBase +{ +public: + + using S = typename Shape1::S; + + ShapeCollisionTraversalNode(); + + /// @brief BV culling test in one BVTT node + bool BVTesting(int, int) const; + + /// @brief Intersection testing between leaves (two shapes) + void leafTesting(int, int) const; + + const Shape1* model1; + const Shape2* model2; + + S cost_density; + + const NarrowPhaseSolver* nsolver; +}; + +/// @brief Initialize traversal node for collision between two geometric shapes, +/// given current object transform +template +bool initialize( + ShapeCollisionTraversalNode& node, + const Shape1& shape1, + const Transform3& tf1, + const Shape2& shape2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result); + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +ShapeCollisionTraversalNode:: +ShapeCollisionTraversalNode() + : CollisionTraversalNodeBase() +{ + model1 = nullptr; + model2 = nullptr; + + nsolver = nullptr; +} + +//============================================================================== +template +bool ShapeCollisionTraversalNode:: +BVTesting(int, int) const +{ + return false; +} + +//============================================================================== +template +void ShapeCollisionTraversalNode:: +leafTesting(int, int) const +{ + if(model1->isOccupied() && model2->isOccupied()) + { + bool is_collision = false; + if(this->request.enable_contact) + { + std::vector> contacts; + if(nsolver->shapeIntersect(*model1, this->tf1, *model2, this->tf2, &contacts)) + { + is_collision = true; + if(this->request.num_max_contacts > this->result->numContacts()) + { + const size_t free_space = this->request.num_max_contacts - this->result->numContacts(); + size_t num_adding_contacts; + + // If the free space is not enough to add all the new contacts, we add contacts in descent order of penetration depth. + if (free_space < contacts.size()) + { + std::partial_sort(contacts.begin(), contacts.begin() + free_space, contacts.end(), std::bind(comparePenDepth, std::placeholders::_2, std::placeholders::_1)); + num_adding_contacts = free_space; + } + else + { + num_adding_contacts = contacts.size(); + } + + for(size_t i = 0; i < num_adding_contacts; ++i) + this->result->addContact(Contact(model1, model2, Contact::NONE, Contact::NONE, contacts[i].pos, contacts[i].normal, contacts[i].penetration_depth)); + } + } + } + else + { + if(nsolver->shapeIntersect(*model1, this->tf1, *model2, this->tf2, nullptr)) + { + is_collision = true; + if(this->request.num_max_contacts > this->result->numContacts()) + this->result->addContact(Contact(model1, model2, Contact::NONE, Contact::NONE)); + } + } + + if(is_collision && this->request.enable_cost) + { + AABB aabb1, aabb2; + computeBV(*model1, this->tf1, aabb1); + computeBV(*model2, this->tf2, aabb2); + AABB overlap_part; + aabb1.overlap(aabb2, overlap_part); + this->result->addCostSource(CostSource(overlap_part, cost_density), this->request.num_max_cost_sources); + } + } + else if((!model1->isFree() && !model2->isFree()) && this->request.enable_cost) + { + if(nsolver->shapeIntersect(*model1, this->tf1, *model2, this->tf2, nullptr)) + { + AABB aabb1, aabb2; + computeBV(*model1, this->tf1, aabb1); + computeBV(*model2, this->tf2, aabb2); + AABB overlap_part; + aabb1.overlap(aabb2, overlap_part); + this->result->addCostSource(CostSource(overlap_part, cost_density), this->request.num_max_cost_sources); + } + } +} + +//============================================================================== +template +bool initialize( + ShapeCollisionTraversalNode& node, + const Shape1& shape1, + const Transform3& tf1, + const Shape2& shape2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result) +{ + node.model1 = &shape1; + node.tf1 = tf1; + node.model2 = &shape2; + node.tf2 = tf2; + node.nsolver = nsolver; + + node.request = request; + node.result = &result; + + node.cost_density = shape1.cost_density * shape2.cost_density; + + return true; +} + +} // namespace fcl + +#endif diff --git a/include/fcl/traversal/collision/shape_mesh_collision_traversal_node.h b/include/fcl/traversal/collision/shape_mesh_collision_traversal_node.h new file mode 100644 index 000000000..d636246e2 --- /dev/null +++ b/include/fcl/traversal/collision/shape_mesh_collision_traversal_node.h @@ -0,0 +1,533 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_TRAVERSAL_SHAPEMESHCOLLISIONTRAVERSALNODE_H +#define FCL_TRAVERSAL_SHAPEMESHCOLLISIONTRAVERSALNODE_H + +#include "fcl/shape/compute_bv.h" +#include "fcl/traversal/collision/shape_bvh_collision_traversal_node.h" + +namespace fcl +{ + +/// @brief Traversal node for collision between shape and mesh +template +class ShapeMeshCollisionTraversalNode + : public ShapeBVHCollisionTraversalNode +{ +public: + using S = typename BV::S; + + ShapeMeshCollisionTraversalNode(); + + /// @brief Intersection testing between leaves (one shape and one triangle) + void leafTesting(int b1, int b2) const; + + /// @brief Whether the traversal process can stop early + bool canStop() const; + + Vector3* vertices; + Triangle* tri_indices; + + S cost_density; + + const NarrowPhaseSolver* nsolver; +}; + +/// @brief Initialize traversal node for collision between one mesh and one +/// shape, given current object transform +template +bool initialize( + ShapeMeshCollisionTraversalNode& node, + const Shape& model1, + const Transform3& tf1, + BVHModel& model2, + Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result, + bool use_refit = false, bool refit_bottomup = false); + +/// @brief Traversal node for shape and mesh, when mesh BVH is one of the oriented node (OBB, RSS, OBBRSS, kIOS) +template +class ShapeMeshCollisionTraversalNodeOBB + : public ShapeMeshCollisionTraversalNode< + Shape, OBB, NarrowPhaseSolver> +{ +public: + ShapeMeshCollisionTraversalNodeOBB(); + + bool BVTesting(int b1, int b2) const; + + void leafTesting(int b1, int b2) const; +}; + +/// @brief Initialize the traversal node for collision between one mesh and one +/// shape, specialized for OBB type +template +bool initialize( + ShapeMeshCollisionTraversalNodeOBB& node, + const Shape& model1, + const Transform3& tf1, + const BVHModel>& model2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result); + +template +class ShapeMeshCollisionTraversalNodeRSS + : public ShapeMeshCollisionTraversalNode, NarrowPhaseSolver> +{ +public: + ShapeMeshCollisionTraversalNodeRSS(); + + bool BVTesting(int b1, int b2) const; + + void leafTesting(int b1, int b2) const; + +}; + +/// @brief Initialize the traversal node for collision between one mesh and one +/// shape, specialized for RSS type +template +bool initialize( + ShapeMeshCollisionTraversalNodeRSS& node, + const Shape& model1, + const Transform3& tf1, + const BVHModel>& model2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result); + +template +class ShapeMeshCollisionTraversalNodekIOS + : public ShapeMeshCollisionTraversalNode, NarrowPhaseSolver> +{ +public: + ShapeMeshCollisionTraversalNodekIOS(); + + bool BVTesting(int b1, int b2) const; + + void leafTesting(int b1, int b2) const; + +}; + +/// @brief Initialize the traversal node for collision between one mesh and one +/// shape, specialized for kIOS type +template +bool initialize( + ShapeMeshCollisionTraversalNodekIOS& node, + const Shape& model1, + const Transform3& tf1, + const BVHModel>& model2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result); + +template +class ShapeMeshCollisionTraversalNodeOBBRSS + : public ShapeMeshCollisionTraversalNode, NarrowPhaseSolver> +{ +public: + ShapeMeshCollisionTraversalNodeOBBRSS(); + + bool BVTesting(int b1, int b2) const; + + void leafTesting(int b1, int b2) const; + +}; + +/// @brief Initialize the traversal node for collision between one mesh and one +/// shape, specialized for OBBRSS type +template +bool initialize( + ShapeMeshCollisionTraversalNodeOBBRSS& node, + const Shape& model1, + const Transform3& tf1, + const BVHModel>& model2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result); + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +ShapeMeshCollisionTraversalNode::ShapeMeshCollisionTraversalNode() + : ShapeBVHCollisionTraversalNode() +{ + vertices = nullptr; + tri_indices = nullptr; + + nsolver = nullptr; +} + +//============================================================================== +template +void ShapeMeshCollisionTraversalNode::leafTesting(int b1, int b2) const +{ + using S = typename BV::S; + + if(this->enable_statistics) this->num_leaf_tests++; + const BVNode& node = this->model2->getBV(b2); + + int primitive_id = node.primitiveId(); + + const Triangle& tri_id = tri_indices[primitive_id]; + + const Vector3& p1 = vertices[tri_id[0]]; + const Vector3& p2 = vertices[tri_id[1]]; + const Vector3& p3 = vertices[tri_id[2]]; + + if(this->model1->isOccupied() && this->model2->isOccupied()) + { + bool is_intersect = false; + + if(!this->request.enable_contact) + { + if(nsolver->shapeTriangleIntersect(*(this->model1), this->tf1, p1, p2, p3, nullptr, nullptr, nullptr)) + { + is_intersect = true; + if(this->request.num_max_contacts > this->result->numContacts()) + this->result->addContact(Contact(this->model1, this->model2, Contact::NONE, primitive_id)); + } + } + else + { + S penetration; + Vector3 normal; + Vector3 contactp; + + if(nsolver->shapeTriangleIntersect(*(this->model1), this->tf1, p1, p2, p3, &contactp, &penetration, &normal)) + { + is_intersect = true; + if(this->request.num_max_contacts > this->result->numContacts()) + this->result->addContact(Contact(this->model1, this->model2, Contact::NONE, primitive_id, contactp, normal, penetration)); + } + } + + if(is_intersect && this->request.enable_cost) + { + AABB overlap_part; + AABB shape_aabb; + computeBV(*(this->model1), this->tf1, shape_aabb); + AABB(p1, p2, p3).overlap(shape_aabb, overlap_part); + this->result->addCostSource(CostSource(overlap_part, cost_density), this->request.num_max_cost_sources); + } + } + else if((!this->model1->isFree() && !this->model2->isFree()) && this->request.enable_cost) + { + if(nsolver->shapeTriangleIntersect(*(this->model1), this->tf1, p1, p2, p3, nullptr, nullptr, nullptr)) + { + AABB overlap_part; + AABB shape_aabb; + computeBV(*(this->model1), this->tf1, shape_aabb); + AABB(p1, p2, p3).overlap(shape_aabb, overlap_part); + this->result->addCostSource(CostSource(overlap_part, cost_density), this->request.num_max_cost_sources); + } + } +} + +//============================================================================== +template +bool ShapeMeshCollisionTraversalNode::canStop() const +{ + return this->request.isSatisfied(*(this->result)); +} + +//============================================================================== +template +bool initialize( + ShapeMeshCollisionTraversalNode& node, + const Shape& model1, + const Transform3& tf1, + BVHModel& model2, + Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result, + bool use_refit, + bool refit_bottomup) +{ + using S = typename BV::S; + + if(model2.getModelType() != BVH_MODEL_TRIANGLES) + return false; + + if(!tf2.matrix().isIdentity()) + { + std::vector> vertices_transformed(model2.num_vertices); + for(int i = 0; i < model2.num_vertices; ++i) + { + Vector3& p = model2.vertices[i]; + Vector3 new_v = tf2 * p; + vertices_transformed[i] = new_v; + } + + model2.beginReplaceModel(); + model2.replaceSubModel(vertices_transformed); + model2.endReplaceModel(use_refit, refit_bottomup); + + tf2.setIdentity(); + } + + node.model1 = &model1; + node.tf1 = tf1; + node.model2 = &model2; + node.tf2 = tf2; + node.nsolver = nsolver; + + computeBV(model1, tf1, node.model1_bv); + + node.vertices = model2.vertices; + node.tri_indices = model2.tri_indices; + + node.request = request; + node.result = &result; + + node.cost_density = model1.cost_density * model2.cost_density; + + return true; +} + +//============================================================================== +template +ShapeMeshCollisionTraversalNodeOBB::ShapeMeshCollisionTraversalNodeOBB() : ShapeMeshCollisionTraversalNode, NarrowPhaseSolver>() +{ +} + +//============================================================================== +template +bool ShapeMeshCollisionTraversalNodeOBB::BVTesting(int b1, int b2) const +{ + if(this->enable_statistics) this->num_bv_tests++; + return !overlap(this->tf2.linear(), this->tf2.translation(), this->model1_bv, this->model2->getBV(b2).bv); +} + +//============================================================================== +template +void ShapeMeshCollisionTraversalNodeOBB::leafTesting(int b1, int b2) const +{ + details::meshShapeCollisionOrientedNodeLeafTesting(b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices, + this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->request)); + + // may need to change the order in pairs +} + +//============================================================================== +template +ShapeMeshCollisionTraversalNodeRSS::ShapeMeshCollisionTraversalNodeRSS() : ShapeMeshCollisionTraversalNode, NarrowPhaseSolver>() +{ +} + +//============================================================================== +template +bool ShapeMeshCollisionTraversalNodeRSS::BVTesting(int b1, int b2) const +{ + if(this->enable_statistics) this->num_bv_tests++; + return !overlap(this->tf2.linear(), this->tf2.translation(), this->model1_bv, this->model2->getBV(b2).bv); +} + +//============================================================================== +template +void ShapeMeshCollisionTraversalNodeRSS::leafTesting(int b1, int b2) const +{ + details::meshShapeCollisionOrientedNodeLeafTesting(b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices, + this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->request)); + + // may need to change the order in pairs +} + +//============================================================================== +template +ShapeMeshCollisionTraversalNodekIOS::ShapeMeshCollisionTraversalNodekIOS() : ShapeMeshCollisionTraversalNode, NarrowPhaseSolver>() +{ +} + +//============================================================================== +template +bool ShapeMeshCollisionTraversalNodekIOS::BVTesting(int b1, int b2) const +{ + if(this->enable_statistics) this->num_bv_tests++; + return !overlap(this->tf2.linear(), this->tf2.translation(), this->model1_bv, this->model2->getBV(b2).bv); +} + +//============================================================================== +template +void ShapeMeshCollisionTraversalNodekIOS::leafTesting(int b1, int b2) const +{ + details::meshShapeCollisionOrientedNodeLeafTesting(b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices, + this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->request)); + + // may need to change the order in pairs +} + +//============================================================================== +template +ShapeMeshCollisionTraversalNodeOBBRSS::ShapeMeshCollisionTraversalNodeOBBRSS() : ShapeMeshCollisionTraversalNode, NarrowPhaseSolver>() +{ +} + +//============================================================================== +template +bool ShapeMeshCollisionTraversalNodeOBBRSS::BVTesting(int b1, int b2) const +{ + if(this->enable_statistics) this->num_bv_tests++; + return !overlap(this->tf2.linear(), this->tf2.translation(), this->model1_bv, this->model2->getBV(b2).bv); +} + +//============================================================================== +template +void ShapeMeshCollisionTraversalNodeOBBRSS::leafTesting(int b1, int b2) const +{ + details::meshShapeCollisionOrientedNodeLeafTesting(b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices, + this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->request)); + + // may need to change the order in pairs +} + +/// @cond IGNORE +namespace details +{ +template class OrientedNode> +static inline bool setupShapeMeshCollisionOrientedNode(OrientedNode& node, + const Shape& model1, const Transform3& tf1, + const BVHModel& model2, const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result) +{ + if(model2.getModelType() != BVH_MODEL_TRIANGLES) + return false; + + node.model1 = &model1; + node.tf1 = tf1; + node.model2 = &model2; + node.tf2 = tf2; + node.nsolver = nsolver; + + computeBV(model1, tf1, node.model1_bv); + + node.vertices = model2.vertices; + node.tri_indices = model2.tri_indices; + + node.request = request; + node.result = &result; + + node.cost_density = model1.cost_density * model2.cost_density; + + return true; +} + +} // namespace details +/// @endcond + +//============================================================================== +template +bool initialize( + ShapeMeshCollisionTraversalNodeOBB& node, + const Shape& model1, + const Transform3& tf1, + const BVHModel>& model2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result) +{ + return details::setupShapeMeshCollisionOrientedNode( + node, model1, tf1, model2, tf2, nsolver, request, result); +} + +//============================================================================== +template +bool initialize( + ShapeMeshCollisionTraversalNodeRSS& node, + const Shape& model1, + const Transform3& tf1, + const BVHModel>& model2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result) +{ + return details::setupShapeMeshCollisionOrientedNode( + node, model1, tf1, model2, tf2, nsolver, request, result); +} + +//============================================================================== +template +bool initialize( + ShapeMeshCollisionTraversalNodekIOS& node, + const Shape& model1, + const Transform3& tf1, + const BVHModel>& model2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result) +{ + return details::setupShapeMeshCollisionOrientedNode( + node, model1, tf1, model2, tf2, nsolver, request, result); +} + +//============================================================================== +template +bool initialize( + ShapeMeshCollisionTraversalNodeOBBRSS& node, + const Shape& model1, + const Transform3& tf1, + const BVHModel>& model2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result) +{ + return details::setupShapeMeshCollisionOrientedNode( + node, model1, tf1, model2, tf2, nsolver, request, result); +} + +} // namespace fcl + +#endif diff --git a/include/fcl/traversal/distance/bvh_distance_traversal_node.h b/include/fcl/traversal/distance/bvh_distance_traversal_node.h new file mode 100644 index 000000000..154876a01 --- /dev/null +++ b/include/fcl/traversal/distance/bvh_distance_traversal_node.h @@ -0,0 +1,181 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + + +#ifndef FCL_TRAVERSAL_BVHDISTANCETRAVERSALNODE_H +#define FCL_TRAVERSAL_BVHDISTANCETRAVERSALNODE_H + +#include "fcl/traversal/traversal_node_base.h" +#include "fcl/traversal/distance/distance_traversal_node_base.h" +#include "fcl/BVH/BVH_model.h" + +namespace fcl +{ + +/// @brief Traversal node for distance computation between BVH models +template +class BVHDistanceTraversalNode + : public DistanceTraversalNodeBase +{ +public: + + using S = typename BV::S; + + BVHDistanceTraversalNode(); + + /// @brief Whether the BV node in the first BVH tree is leaf + bool isFirstNodeLeaf(int b) const; + + /// @brief Whether the BV node in the second BVH tree is leaf + bool isSecondNodeLeaf(int b) const; + + /// @brief Determine the traversal order, is the first BVTT subtree better + bool firstOverSecond(int b1, int b2) const; + + /// @brief Obtain the left child of BV node in the first BVH + int getFirstLeftChild(int b) const; + + /// @brief Obtain the right child of BV node in the first BVH + int getFirstRightChild(int b) const; + + /// @brief Obtain the left child of BV node in the second BVH + int getSecondLeftChild(int b) const; + + /// @brief Obtain the right child of BV node in the second BVH + int getSecondRightChild(int b) const; + + /// @brief BV culling test in one BVTT node + S BVTesting(int b1, int b2) const; + + /// @brief The first BVH model + const BVHModel* model1; + /// @brief The second BVH model + const BVHModel* model2; + + /// @brief statistical information + mutable int num_bv_tests; + mutable int num_leaf_tests; + mutable S query_time_seconds; +}; + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +BVHDistanceTraversalNode::BVHDistanceTraversalNode() + : DistanceTraversalNodeBase() +{ + model1 = nullptr; + model2 = nullptr; + + num_bv_tests = 0; + num_leaf_tests = 0; + query_time_seconds = 0.0; +} + +//============================================================================== +template +bool BVHDistanceTraversalNode::isFirstNodeLeaf(int b) const +{ + return model1->getBV(b).isLeaf(); +} + +//============================================================================== +template +bool BVHDistanceTraversalNode::isSecondNodeLeaf(int b) const +{ + return model2->getBV(b).isLeaf(); +} + +//============================================================================== +template +bool BVHDistanceTraversalNode::firstOverSecond(int b1, int b2) const +{ + S sz1 = model1->getBV(b1).bv.size(); + S sz2 = model2->getBV(b2).bv.size(); + + bool l1 = model1->getBV(b1).isLeaf(); + bool l2 = model2->getBV(b2).isLeaf(); + + if(l2 || (!l1 && (sz1 > sz2))) + return true; + return false; +} + +//============================================================================== +template +int BVHDistanceTraversalNode::getFirstLeftChild(int b) const +{ + return model1->getBV(b).leftChild(); +} + +//============================================================================== +template +int BVHDistanceTraversalNode::getFirstRightChild(int b) const +{ + return model1->getBV(b).rightChild(); +} + +//============================================================================== +template +int BVHDistanceTraversalNode::getSecondLeftChild(int b) const +{ + return model2->getBV(b).leftChild(); +} + +//============================================================================== +template +int BVHDistanceTraversalNode::getSecondRightChild(int b) const +{ + return model2->getBV(b).rightChild(); +} + +//============================================================================== +template +typename BV::S BVHDistanceTraversalNode::BVTesting(int b1, int b2) const +{ + if(this->enable_statistics) this->num_bv_tests++; + return model1->getBV(b1).distance(model2->getBV(b2)); +} + +} // namespace fcl + +#endif diff --git a/include/fcl/traversal/distance/bvh_shape_distance_traversal_node.h b/include/fcl/traversal/distance/bvh_shape_distance_traversal_node.h new file mode 100644 index 000000000..5c82439c5 --- /dev/null +++ b/include/fcl/traversal/distance/bvh_shape_distance_traversal_node.h @@ -0,0 +1,129 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_TRAVERSAL_BVHSHAPEDISTANCETRAVERSALNODE_H +#define FCL_TRAVERSAL_BVHSHAPEDISTANCETRAVERSALNODE_H + +#include "fcl/traversal/distance/distance_traversal_node_base.h" +#include "fcl/BVH/BVH_model.h" + +namespace fcl +{ + +/// @brief Traversal node for distance computation between BVH and shape +template +class BVHShapeDistanceTraversalNode + : public DistanceTraversalNodeBase +{ +public: + + using S = typename BV::S; + + BVHShapeDistanceTraversalNode(); + + /// @brief Whether the BV node in the first BVH tree is leaf + bool isFirstNodeLeaf(int b) const; + + /// @brief Obtain the left child of BV node in the first BVH + int getFirstLeftChild(int b) const; + + /// @brief Obtain the right child of BV node in the first BVH + int getFirstRightChild(int b) const; + + /// @brief BV culling test in one BVTT node + S BVTesting(int b1, int b2) const; + + const BVHModel* model1; + const Shape* model2; + BV model2_bv; + + mutable int num_bv_tests; + mutable int num_leaf_tests; + mutable S query_time_seconds; +}; + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +BVHShapeDistanceTraversalNode::BVHShapeDistanceTraversalNode() + : DistanceTraversalNodeBase() +{ + model1 = nullptr; + model2 = nullptr; + + num_bv_tests = 0; + num_leaf_tests = 0; + query_time_seconds = 0.0; +} + +//============================================================================== +template +bool BVHShapeDistanceTraversalNode::isFirstNodeLeaf(int b) const +{ + return model1->getBV(b).isLeaf(); +} + +//============================================================================== +template +int BVHShapeDistanceTraversalNode::getFirstLeftChild(int b) const +{ + return model1->getBV(b).leftChild(); +} + +//============================================================================== +template +int BVHShapeDistanceTraversalNode::getFirstRightChild(int b) const +{ + return model1->getBV(b).rightChild(); +} + +//============================================================================== +template +typename BV::S BVHShapeDistanceTraversalNode::BVTesting( + int b1, int b2) const +{ + return model1->getBV(b1).bv.distance(model2_bv); +} + +} // namespace fcl + +#endif diff --git a/include/fcl/traversal/distance/conservative_advancement_stack_data.h b/include/fcl/traversal/distance/conservative_advancement_stack_data.h new file mode 100644 index 000000000..b8d3ec372 --- /dev/null +++ b/include/fcl/traversal/distance/conservative_advancement_stack_data.h @@ -0,0 +1,80 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_TRAVERSAL_CONVERVATIVEADVANCEMENTSTACKDATA_H +#define FCL_TRAVERSAL_CONVERVATIVEADVANCEMENTSTACKDATA_H + +#include "fcl/data_types.h" + +namespace fcl +{ + +template +struct ConservativeAdvancementStackData +{ + ConservativeAdvancementStackData( + const Vector3& P1_, + const Vector3& P2_, + int c1_, int c2_, S d_); + + Vector3 P1; + Vector3 P2; + int c1; + int c2; + S d; +}; + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +ConservativeAdvancementStackData::ConservativeAdvancementStackData( + const Vector3& P1_, + const Vector3& P2_, + int c1_, int c2_, S d_) + : P1(P1_), P2(P2_), c1(c1_), c2(c2_), d(d_) +{ + // Do nothing +} + +} // namespace fcl + +#endif diff --git a/include/fcl/traversal/distance/distance_traversal_node_base.h b/include/fcl/traversal/distance/distance_traversal_node_base.h new file mode 100644 index 000000000..44fbec693 --- /dev/null +++ b/include/fcl/traversal/distance/distance_traversal_node_base.h @@ -0,0 +1,129 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_TRAVERSAL_DISTANCERAVERSALNODEBASE_H +#define FCL_TRAVERSAL_DISTANCERAVERSALNODEBASE_H + +#include "fcl/traversal/traversal_node_base.h" +#include "fcl/collision_data.h" + +namespace fcl +{ + +/// @brief Node structure encoding the information required for distance traversal. +template +class DistanceTraversalNodeBase : public TraversalNodeBase +{ +public: + DistanceTraversalNodeBase(); + + virtual ~DistanceTraversalNodeBase(); + + /// @brief BV test between b1 and b2 + virtual S BVTesting(int b1, int b2) const; + + /// @brief Leaf test between node b1 and b2, if they are both leafs + virtual void leafTesting(int b1, int b2) const; + + /// @brief Check whether the traversal can stop + virtual bool canStop(S c) const; + + /// @brief Whether store some statistics information during traversal + void enableStatistics(bool enable); + + /// @brief request setting for distance + DistanceRequest request; + + /// @brief distance result kept during the traversal iteration + DistanceResult* result; + + /// @brief Whether stores statistics + bool enable_statistics; +}; + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +DistanceTraversalNodeBase::DistanceTraversalNodeBase() + : result(nullptr), enable_statistics(false) +{ + // Do nothing +} + +//============================================================================== +template +DistanceTraversalNodeBase::~DistanceTraversalNodeBase() +{ + // Do nothing +} + +//============================================================================== +template +S DistanceTraversalNodeBase::BVTesting(int b1, int b2) const +{ + return std::numeric_limits::max(); +} + +//============================================================================== +template +void DistanceTraversalNodeBase::leafTesting(int b1, int b2) const +{ + // Do nothing +} + +//============================================================================== +template +bool DistanceTraversalNodeBase::canStop(S c) const +{ + return false; +} + +//============================================================================== +template +void DistanceTraversalNodeBase::enableStatistics(bool enable) +{ + enable_statistics = enable; +} + +} // namespace fcl + +#endif diff --git a/include/fcl/traversal/distance/mesh_conservative_advancement_traversal_node.h b/include/fcl/traversal/distance/mesh_conservative_advancement_traversal_node.h new file mode 100644 index 000000000..81f7921e3 --- /dev/null +++ b/include/fcl/traversal/distance/mesh_conservative_advancement_traversal_node.h @@ -0,0 +1,989 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_TRAVERSAL_MESHCONSERVATIVEADVANCEMENTTRAVERSALNODE_H +#define FCL_TRAVERSAL_MESHCONSERVATIVEADVANCEMENTTRAVERSALNODE_H + +#include "fcl/traversal/distance/mesh_distance_traversal_node.h" +#include "fcl/traversal/distance/conservative_advancement_stack_data.h" +#include "fcl/intersect.h" + +namespace fcl +{ + +/// @brief continuous collision node using conservative advancement. when using this default version, must refit the BVH in current configuration (R_t, T_t) into default configuration +template +class MeshConservativeAdvancementTraversalNode + : public MeshDistanceTraversalNode +{ +public: + + using S = typename BV::S; + + MeshConservativeAdvancementTraversalNode(S w_ = 1); + + /// @brief BV culling test in one BVTT node + S BVTesting(int b1, int b2) const; + + /// @brief Conservative advancement testing between leaves (two triangles) + void leafTesting(int b1, int b2) const; + + /// @brief Whether the traversal process can stop early + bool canStop(S c) const; + + mutable S min_distance; + + mutable Vector3 closest_p1, closest_p2; + + mutable int last_tri_id1, last_tri_id2; + + /// @brief CA controlling variable: early stop for the early iterations of CA + S w; + + /// @brief The time from beginning point + S toc; + S t_err; + + /// @brief The delta_t each step + mutable S delta_t; + + /// @brief Motions for the two objects in query + const MotionBased* motion1; + const MotionBased* motion2; + + mutable std::vector> stack; + + template + friend struct CanStopImpl; +}; + +/// @brief Initialize traversal node for conservative advancement computation +/// between two meshes, given the current transforms +template +bool initialize( + MeshConservativeAdvancementTraversalNode& node, + BVHModel& model1, + const Transform3& tf1, + BVHModel& model2, + const Transform3& tf2, + typename BV::S w = 1, + bool use_refit = false, + bool refit_bottomup = false); + +template +class MeshConservativeAdvancementTraversalNodeRSS + : public MeshConservativeAdvancementTraversalNode> +{ +public: + MeshConservativeAdvancementTraversalNodeRSS(S w_ = 1); + + S BVTesting(int b1, int b2) const + { + if (this->enable_statistics) + this->num_bv_tests++; + + Vector3 P1, P2; + S d = distance( + tf, + this->model1->getBV(b1).bv, + this->model2->getBV(b2).bv, &P1, &P2); + + this->stack.emplace_back(P1, P2, b1, b2, d); + + return d; + } + + void leafTesting(int b1, int b2) const; + + bool canStop(S c) const; + + Transform3 tf; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +using MeshConservativeAdvancementTraversalNodeRSSf = MeshConservativeAdvancementTraversalNodeRSS; +using MeshConservativeAdvancementTraversalNodeRSSd = MeshConservativeAdvancementTraversalNodeRSS; + +/// @brief Initialize traversal node for conservative advancement computation +/// between two meshes, given the current transforms, specialized for RSS +template +bool initialize( + MeshConservativeAdvancementTraversalNodeRSS& node, + const BVHModel>& model1, + const Transform3& tf1, + const BVHModel>& model2, + const Transform3& tf2, + S w = 1); + +template +class MeshConservativeAdvancementTraversalNodeOBBRSS + : public MeshConservativeAdvancementTraversalNode> +{ +public: + MeshConservativeAdvancementTraversalNodeOBBRSS(S w_ = 1); + + S BVTesting(int b1, int b2) const + { + if (this->enable_statistics) + this->num_bv_tests++; + + Vector3 P1, P2; + S d = distance( + tf, + this->model1->getBV(b1).bv, + this->model2->getBV(b2).bv, &P1, &P2); + + this->stack.emplace_back(P1, P2, b1, b2, d); + + return d; + } + + void leafTesting(int b1, int b2) const; + + bool canStop(S c) const; + + Transform3 tf; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +using MeshConservativeAdvancementTraversalNodeOBBRSSf = MeshConservativeAdvancementTraversalNodeOBBRSS; +using MeshConservativeAdvancementTraversalNodeOBBRSSd = MeshConservativeAdvancementTraversalNodeOBBRSS; + +template +bool initialize( + MeshConservativeAdvancementTraversalNodeOBBRSS& node, + const BVHModel>& model1, + const Transform3& tf1, + const BVHModel>& model2, + const Transform3& tf2, + S w = 1); + +namespace details +{ + +template +const Vector3 getBVAxis(const BV& bv, int i); + +template +bool meshConservativeAdvancementTraversalNodeCanStop( + typename BV::S c, + typename BV::S min_distance, + typename BV::S abs_err, + typename BV::S rel_err, + typename BV::S w, + const BVHModel* model1, + const BVHModel* model2, + const MotionBased* motion1, + const MotionBased* motion2, + std::vector>& stack, + typename BV::S& delta_t); + +template +bool meshConservativeAdvancementOrientedNodeCanStop( + typename BV::S c, + typename BV::S min_distance, + typename BV::S abs_err, + typename BV::S rel_err, + typename BV::S w, + const BVHModel* model1, + const BVHModel* model2, + const MotionBased* motion1, + const MotionBased* motion2, + std::vector>& stack, + typename BV::S& delta_t); + +template +void meshConservativeAdvancementOrientedNodeLeafTesting( + int b1, + int b2, + const BVHModel* model1, + const BVHModel* model2, + const Triangle* tri_indices1, + const Triangle* tri_indices2, + const Vector3* vertices1, + const Vector3* vertices2, + const Matrix3& R, + const Vector3& T, + const MotionBased* motion1, + const MotionBased* motion2, + bool enable_statistics, + typename BV::S& min_distance, + Vector3& p1, + Vector3& p2, + int& last_tri_id1, + int& last_tri_id2, + typename BV::S& delta_t, + int& num_leaf_tests); + +} // namespace details + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +MeshConservativeAdvancementTraversalNode:: +MeshConservativeAdvancementTraversalNode(typename BV::S w_) + : MeshDistanceTraversalNode() +{ + delta_t = 1; + toc = 0; + t_err = (S)0.00001; + + w = w_; + + motion1 = nullptr; + motion2 = nullptr; +} + +//============================================================================== +template +typename BV::S +MeshConservativeAdvancementTraversalNode::BVTesting(int b1, int b2) const +{ + if(this->enable_statistics) this->num_bv_tests++; + Vector3 P1, P2; + S d = this->model1->getBV(b1).distance(this->model2->getBV(b2), &P1, &P2); + + stack.emplace_back(P1, P2, b1, b2, d); + + return d; +} + +//============================================================================== +template +void MeshConservativeAdvancementTraversalNode::leafTesting(int b1, int b2) const +{ + if(this->enable_statistics) this->num_leaf_tests++; + + const BVNode& node1 = this->model1->getBV(b1); + const BVNode& node2 = this->model2->getBV(b2); + + int primitive_id1 = node1.primitiveId(); + int primitive_id2 = node2.primitiveId(); + + const Triangle& tri_id1 = this->tri_indices1[primitive_id1]; + const Triangle& tri_id2 = this->tri_indices2[primitive_id2]; + + const Vector3& p1 = this->vertices1[tri_id1[0]]; + const Vector3& p2 = this->vertices1[tri_id1[1]]; + const Vector3& p3 = this->vertices1[tri_id1[2]]; + + const Vector3& q1 = this->vertices2[tri_id2[0]]; + const Vector3& q2 = this->vertices2[tri_id2[1]]; + const Vector3& q3 = this->vertices2[tri_id2[2]]; + + // nearest point pair + Vector3 P1, P2; + + S d = TriangleDistance::triDistance(p1, p2, p3, q1, q2, q3, + P1, P2); + + if(d < this->min_distance) + { + this->min_distance = d; + + closest_p1 = P1; + closest_p2 = P2; + + last_tri_id1 = primitive_id1; + last_tri_id2 = primitive_id2; + } + + Vector3 n = P2 - P1; + n.normalize(); + // here n is already in global frame as we assume the body is in original configuration (I, 0) for general BVH + TriangleMotionBoundVisitor mb_visitor1(p1, p2, p3, n), mb_visitor2(q1, q2, q3, n); + S bound1 = motion1->computeMotionBound(mb_visitor1); + S bound2 = motion2->computeMotionBound(mb_visitor2); + + S bound = bound1 + bound2; + + S cur_delta_t; + if(bound <= d) cur_delta_t = 1; + else cur_delta_t = d / bound; + + if(cur_delta_t < delta_t) + delta_t = cur_delta_t; +} + +//============================================================================== +template +struct CanStopImpl +{ + static bool run( + const MeshConservativeAdvancementTraversalNode& node, S c) + { + if((c >= node.w * (node.min_distance - node.abs_err)) + && (c * (1 + node.rel_err) >= node.w * node.min_distance)) + { + const ConservativeAdvancementStackData& data = node.stack.back(); + S d = data.d; + Vector3 n; + int c1, c2; + + if(d > c) + { + const ConservativeAdvancementStackData& data2 = node.stack[node.stack.size() - 2]; + d = data2.d; + n = data2.P2 - data2.P1; n.normalize(); + c1 = data2.c1; + c2 = data2.c2; + node.stack[node.stack.size() - 2] = node.stack[node.stack.size() - 1]; + } + else + { + n = data.P2 - data.P1; n.normalize(); + c1 = data.c1; + c2 = data.c2; + } + + assert(c == d); + + TBVMotionBoundVisitor mb_visitor1(node.model1->getBV(c1).bv, n), mb_visitor2(node.model2->getBV(c2).bv, n); + S bound1 = node.motion1->computeMotionBound(mb_visitor1); + S bound2 = node.motion2->computeMotionBound(mb_visitor2); + + S bound = bound1 + bound2; + + S cur_delta_t; + if(bound <= c) cur_delta_t = 1; + else cur_delta_t = c / bound; + + if(cur_delta_t < node.delta_t) + node.delta_t = cur_delta_t; + + node.stack.pop_back(); + + return true; + } + else + { + const ConservativeAdvancementStackData& data = node.stack.back(); + S d = data.d; + + if(d > c) + node.stack[node.stack.size() - 2] = node.stack[node.stack.size() - 1]; + + node.stack.pop_back(); + + return false; + } + } +}; + +//============================================================================== +template +bool MeshConservativeAdvancementTraversalNode::canStop( + typename BV::S c) const +{ + return CanStopImpl::run(*this, c); +} + +//============================================================================== +template +struct CanStopImpl> +{ + static bool run( + const MeshConservativeAdvancementTraversalNode>& node, + S c) + { + return details::meshConservativeAdvancementTraversalNodeCanStop( + c, + node.min_distance, + node.abs_err, + node.rel_err, + node.w, + node.model1, + node.model2, + node.motion1, + node.motion2, + node.stack, + node.delta_t); + } +}; + +//============================================================================== +template +struct CanStopImpl> +{ + static bool run( + const MeshConservativeAdvancementTraversalNode>& node, + S c) + { + return details::meshConservativeAdvancementTraversalNodeCanStop( + c, + node.min_distance, + node.abs_err, + node.rel_err, + node.w, + node.model1, + node.model2, + node.motion1, + node.motion2, + node.stack, + node.delta_t); + } +}; + +//============================================================================== +template +struct CanStopImpl> +{ + static bool run( + const MeshConservativeAdvancementTraversalNode>& node, + S c) + { + return details::meshConservativeAdvancementTraversalNodeCanStop( + c, + node.min_distance, + node.abs_err, + node.rel_err, + node.w, + node.model1, + node.model2, + node.motion1, + node.motion2, + node.stack, + node.delta_t); + } +}; + +//============================================================================== +template +bool initialize( + MeshConservativeAdvancementTraversalNode& node, + BVHModel& model1, + const Transform3& tf1, + BVHModel& model2, + const Transform3& tf2, + typename BV::S w, + bool use_refit, + bool refit_bottomup) +{ + using S = typename BV::S; + + std::vector> vertices_transformed1(model1.num_vertices); + for(int i = 0; i < model1.num_vertices; ++i) + { + Vector3& p = model1.vertices[i]; + Vector3 new_v = tf1 * p; + vertices_transformed1[i] = new_v; + } + + std::vector> vertices_transformed2(model2.num_vertices); + for(int i = 0; i < model2.num_vertices; ++i) + { + Vector3& p = model2.vertices[i]; + Vector3 new_v = tf2 * p; + vertices_transformed2[i] = new_v; + } + + model1.beginReplaceModel(); + model1.replaceSubModel(vertices_transformed1); + model1.endReplaceModel(use_refit, refit_bottomup); + + model2.beginReplaceModel(); + model2.replaceSubModel(vertices_transformed2); + model2.endReplaceModel(use_refit, refit_bottomup); + + node.model1 = &model1; + node.model2 = &model2; + + node.vertices1 = model1.vertices; + node.vertices2 = model2.vertices; + + node.tri_indices1 = model1.tri_indices; + node.tri_indices2 = model2.tri_indices; + + node.w = w; + + return true; +} + +//============================================================================== +template +MeshConservativeAdvancementTraversalNodeRSS::MeshConservativeAdvancementTraversalNodeRSS(S w_) + : MeshConservativeAdvancementTraversalNode>(w_) +{ + tf.linear().setIdentity(); +} + +//============================================================================== +template +void MeshConservativeAdvancementTraversalNodeRSS::leafTesting(int b1, int b2) const +{ + details::meshConservativeAdvancementOrientedNodeLeafTesting( + b1, + b2, + this->model1, + this->model2, + this->tri_indices1, + this->tri_indices2, + this->vertices1, + this->vertices2, + tf.linear(), + tf.translation(), + this->motion1, + this->motion2, + this->enable_statistics, + this->min_distance, + this->closest_p1, + this->closest_p2, + this->last_tri_id1, + this->last_tri_id2, + this->delta_t, + this->num_leaf_tests); +} + +//============================================================================== +template +bool MeshConservativeAdvancementTraversalNodeRSS::canStop(S c) const +{ + return details::meshConservativeAdvancementOrientedNodeCanStop( + c, + this->min_distance, + this->abs_err, + this->rel_err, + this->w, + this->model1, + this->model2, + this->motion1, + this->motion2, + this->stack, + this->delta_t); +} + +//============================================================================== +template +MeshConservativeAdvancementTraversalNodeOBBRSS:: +MeshConservativeAdvancementTraversalNodeOBBRSS(S w_) + : MeshConservativeAdvancementTraversalNode>(w_) +{ + tf.linear().setIdentity(); +} + +//============================================================================== +template +void MeshConservativeAdvancementTraversalNodeOBBRSS:: +leafTesting(int b1, int b2) const +{ + details::meshConservativeAdvancementOrientedNodeLeafTesting( + b1, + b2, + this->model1, + this->model2, + this->tri_indices1, + this->tri_indices2, + this->vertices1, + this->vertices2, + this->tf.linear(), + this->tf.translation(), + this->motion1, + this->motion2, + this->enable_statistics, + this->min_distance, + this->closest_p1, + this->closest_p2, + this->last_tri_id1, + this->last_tri_id2, + this->delta_t, + this->num_leaf_tests); +} + +//============================================================================== +template +bool MeshConservativeAdvancementTraversalNodeOBBRSS::canStop(S c) const +{ + return details::meshConservativeAdvancementOrientedNodeCanStop( + c, + this->min_distance, + this->abs_err, + this->rel_err, + this->w, + this->model1, + this->model2, + this->motion1, + this->motion2, + this->stack, + this->delta_t); +} + +/// @brief for OBB and RSS, there is local coordinate of BV, so normal need to be transformed +namespace details +{ + +//============================================================================== +template +struct GetBVAxisImpl +{ + const Vector3 operator()(const BV& bv, int i) + { + return bv.axis.col(i); + } +}; + +//============================================================================== +template +const Vector3 getBVAxis(const BV& bv, int i) +{ + GetBVAxisImpl getBVAxisImpl; + return getBVAxisImpl(bv, i); +} + +//============================================================================== +template +struct GetBVAxisImpl> +{ + const Vector3 operator()(const OBBRSS& bv, int i) + { + return bv.obb.axis.col(i); + } +}; + +//============================================================================== +template +bool meshConservativeAdvancementTraversalNodeCanStop( + typename BV::S c, + typename BV::S min_distance, + typename BV::S abs_err, + typename BV::S rel_err, + typename BV::S w, + const BVHModel* model1, + const BVHModel* model2, + const MotionBased* motion1, + const MotionBased* motion2, + std::vector>& stack, + typename BV::S& delta_t) +{ + using S = typename BV::S; + + if((c >= w * (min_distance - abs_err)) && (c * (1 + rel_err) >= w * min_distance)) + { + const ConservativeAdvancementStackData& data = stack.back(); + S d = data.d; + Vector3 n; + int c1, c2; + + if(d > c) + { + const ConservativeAdvancementStackData& data2 = stack[stack.size() - 2]; + d = data2.d; + n = data2.P2 - data2.P1; n.normalize(); + c1 = data2.c1; + c2 = data2.c2; + stack[stack.size() - 2] = stack[stack.size() - 1]; + } + else + { + n = data.P2 - data.P1; n.normalize(); + c1 = data.c1; + c2 = data.c2; + } + + assert(c == d); + + Vector3 n_transformed = + getBVAxis(model1->getBV(c1).bv, 0) * n[0] + + getBVAxis(model1->getBV(c1).bv, 1) * n[1] + + getBVAxis(model1->getBV(c1).bv, 2) * n[2]; + + TBVMotionBoundVisitor mb_visitor1(model1->getBV(c1).bv, n_transformed), mb_visitor2(model2->getBV(c2).bv, n_transformed); + S bound1 = motion1->computeMotionBound(mb_visitor1); + S bound2 = motion2->computeMotionBound(mb_visitor2); + + S bound = bound1 + bound2; + + S cur_delta_t; + if(bound <= c) cur_delta_t = 1; + else cur_delta_t = c / bound; + + if(cur_delta_t < delta_t) + delta_t = cur_delta_t; + + stack.pop_back(); + + return true; + } + else + { + const ConservativeAdvancementStackData& data = stack.back(); + S d = data.d; + + if(d > c) + stack[stack.size() - 2] = stack[stack.size() - 1]; + + stack.pop_back(); + + return false; + } +} + +//============================================================================== +template +bool meshConservativeAdvancementOrientedNodeCanStop( + typename BV::S c, + typename BV::S min_distance, + typename BV::S abs_err, + typename BV::S rel_err, + typename BV::S w, + const BVHModel* model1, + const BVHModel* model2, + const MotionBased* motion1, + const MotionBased* motion2, + std::vector>& stack, + typename BV::S& delta_t) +{ + using S = typename BV::S; + + if((c >= w * (min_distance - abs_err)) && (c * (1 + rel_err) >= w * min_distance)) + { + const ConservativeAdvancementStackData& data = stack.back(); + S d = data.d; + Vector3 n; + int c1, c2; + + if(d > c) + { + const ConservativeAdvancementStackData& data2 = stack[stack.size() - 2]; + d = data2.d; + n = data2.P2 - data2.P1; n.normalize(); + c1 = data2.c1; + c2 = data2.c2; + stack[stack.size() - 2] = stack[stack.size() - 1]; + } + else + { + n = data.P2 - data.P1; n.normalize(); + c1 = data.c1; + c2 = data.c2; + } + + assert(c == d); + + // n is in local frame of c1, so we need to turn n into the global frame + Vector3 n_transformed = + getBVAxis(model1->getBV(c1).bv, 0) * n[0] + + getBVAxis(model1->getBV(c1).bv, 1) * n[2] + // TODO(JS): not n[1]? + getBVAxis(model1->getBV(c1).bv, 2) * n[2]; + Quaternion R0; + motion1->getCurrentRotation(R0); + n_transformed = R0 * n_transformed; + n_transformed.normalize(); + + TBVMotionBoundVisitor mb_visitor1(model1->getBV(c1).bv, n_transformed), mb_visitor2(model2->getBV(c2).bv, -n_transformed); + S bound1 = motion1->computeMotionBound(mb_visitor1); + S bound2 = motion2->computeMotionBound(mb_visitor2); + + S bound = bound1 + bound2; + + S cur_delta_t; + if(bound <= c) cur_delta_t = 1; + else cur_delta_t = c / bound; + + if(cur_delta_t < delta_t) + delta_t = cur_delta_t; + + stack.pop_back(); + + return true; + } + else + { + const ConservativeAdvancementStackData& data = stack.back(); + S d = data.d; + + if(d > c) + stack[stack.size() - 2] = stack[stack.size() - 1]; + + stack.pop_back(); + + return false; + } +} + +//============================================================================== +template +void meshConservativeAdvancementOrientedNodeLeafTesting( + int b1, + int b2, + const BVHModel* model1, + const BVHModel* model2, + const Triangle* tri_indices1, + const Triangle* tri_indices2, + const Vector3* vertices1, + const Vector3* vertices2, + const Matrix3& R, + const Vector3& T, + const MotionBased* motion1, + const MotionBased* motion2, + bool enable_statistics, + typename BV::S& min_distance, + Vector3& p1, + Vector3& p2, + int& last_tri_id1, + int& last_tri_id2, + typename BV::S& delta_t, + int& num_leaf_tests) +{ + using S = typename BV::S; + + if(enable_statistics) num_leaf_tests++; + + const BVNode& node1 = model1->getBV(b1); + const BVNode& node2 = model2->getBV(b2); + + int primitive_id1 = node1.primitiveId(); + int primitive_id2 = node2.primitiveId(); + + const Triangle& tri_id1 = tri_indices1[primitive_id1]; + const Triangle& tri_id2 = tri_indices2[primitive_id2]; + + const Vector3& t11 = vertices1[tri_id1[0]]; + const Vector3& t12 = vertices1[tri_id1[1]]; + const Vector3& t13 = vertices1[tri_id1[2]]; + + const Vector3& t21 = vertices2[tri_id2[0]]; + const Vector3& t22 = vertices2[tri_id2[1]]; + const Vector3& t23 = vertices2[tri_id2[2]]; + + // nearest point pair + Vector3 P1, P2; + + S d = TriangleDistance::triDistance(t11, t12, t13, t21, t22, t23, + R, T, + P1, P2); + + if(d < min_distance) + { + min_distance = d; + + p1 = P1; + p2 = P2; + + last_tri_id1 = primitive_id1; + last_tri_id2 = primitive_id2; + } + + + /// n is the local frame of object 1, pointing from object 1 to object2 + Vector3 n = P2 - P1; + /// turn n into the global frame, pointing from object 1 to object 2 + Quaternion R0; + motion1->getCurrentRotation(R0); + Vector3 n_transformed = R0 * n; + n_transformed.normalize(); // normalized here + + TriangleMotionBoundVisitor mb_visitor1(t11, t12, t13, n_transformed), mb_visitor2(t21, t22, t23, -n_transformed); + S bound1 = motion1->computeMotionBound(mb_visitor1); + S bound2 = motion2->computeMotionBound(mb_visitor2); + + S bound = bound1 + bound2; + + S cur_delta_t; + if(bound <= d) cur_delta_t = 1; + else cur_delta_t = d / bound; + + if(cur_delta_t < delta_t) + delta_t = cur_delta_t; +} + +//============================================================================== +template +bool setupMeshConservativeAdvancementOrientedDistanceNode( + OrientedDistanceNode& node, + const BVHModel& model1, const Transform3& tf1, + const BVHModel& model2, const Transform3& tf2, + typename BV::S w) +{ + if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES) + return false; + + node.model1 = &model1; + node.model2 = &model2; + + node.vertices1 = model1.vertices; + node.vertices2 = model2.vertices; + + node.tri_indices1 = model1.tri_indices; + node.tri_indices2 = model2.tri_indices; + + node.w = w; + + node.tf = tf1.inverse(Eigen::Isometry) * tf2; + + return true; +} + +} // namespace detials + +//============================================================================== +template +bool initialize( + MeshConservativeAdvancementTraversalNodeRSS& node, + const BVHModel>& model1, + const Transform3& tf1, + const BVHModel>& model2, + const Transform3& tf2, + S w) +{ + return details::setupMeshConservativeAdvancementOrientedDistanceNode( + node, model1, tf1, model2, tf2, w); +} + +//============================================================================== +template +bool initialize( + MeshConservativeAdvancementTraversalNodeOBBRSS& node, + const BVHModel>& model1, + const Transform3& tf1, + const BVHModel>& model2, + const Transform3& tf2, + S w) +{ + return details::setupMeshConservativeAdvancementOrientedDistanceNode( + node, model1, tf1, model2, tf2, w); +} + +} // namespace fcl + +#endif diff --git a/include/fcl/traversal/distance/mesh_distance_traversal_node.h b/include/fcl/traversal/distance/mesh_distance_traversal_node.h new file mode 100644 index 000000000..f587fa951 --- /dev/null +++ b/include/fcl/traversal/distance/mesh_distance_traversal_node.h @@ -0,0 +1,891 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + + +#ifndef FCL_TRAVERSAL_MESHDISTANCETRAVERSALNODE_H +#define FCL_TRAVERSAL_MESHDISTANCETRAVERSALNODE_H + +#include "fcl/intersect.h" +#include "fcl/BV/RSS.h" +#include "fcl/BV/OBBRSS.h" +#include "fcl/BV/kIOS.h" +#include "fcl/traversal/distance/bvh_distance_traversal_node.h" + +namespace fcl +{ + +/// @brief Traversal node for distance computation between two meshes +template +class MeshDistanceTraversalNode : public BVHDistanceTraversalNode +{ +public: + + using S = typename BV::S; + + MeshDistanceTraversalNode(); + + /// @brief Distance testing between leaves (two triangles) + void leafTesting(int b1, int b2) const; + + /// @brief Whether the traversal process can stop early + bool canStop(S c) const; + + Vector3* vertices1; + Vector3* vertices2; + + Triangle* tri_indices1; + Triangle* tri_indices2; + + /// @brief relative and absolute error, default value is 0.01 for both terms + S rel_err; + S abs_err; +}; + +/// @brief Initialize traversal node for distance computation between two +/// meshes, given the current transforms +template +bool initialize( + MeshDistanceTraversalNode& node, + BVHModel& model1, + Transform3& tf1, + BVHModel& model2, + Transform3& tf2, + const DistanceRequest& request, + DistanceResult& result, + bool use_refit = false, bool refit_bottomup = false); + +/// @brief Traversal node for distance computation between two meshes if their underlying BVH node is oriented node (RSS, OBBRSS, kIOS) +template +class MeshDistanceTraversalNodeRSS + : public MeshDistanceTraversalNode> +{ +public: + MeshDistanceTraversalNodeRSS(); + + void preprocess(); + + void postprocess(); + + S BVTesting(int b1, int b2) const + { + if (this->enable_statistics) this->num_bv_tests++; + + return distance(tf.linear(), tf.translation(), this->model1->getBV(b1).bv, this->model2->getBV(b2).bv); + } + + void leafTesting(int b1, int b2) const; + + Transform3 tf; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +using MeshDistanceTraversalNodeRSSf = MeshDistanceTraversalNodeRSS; +using MeshDistanceTraversalNodeRSSd = MeshDistanceTraversalNodeRSS; + +/// @brief Initialize traversal node for distance computation between two +/// meshes, specialized for RSS type +template +bool initialize( + MeshDistanceTraversalNodeRSS& node, + const BVHModel>& model1, + const Transform3& tf1, + const BVHModel>& model2, + const Transform3& tf2, + const DistanceRequest& request, + DistanceResult& result); + +template +class MeshDistanceTraversalNodekIOS + : public MeshDistanceTraversalNode> +{ +public: + MeshDistanceTraversalNodekIOS(); + + void preprocess(); + + void postprocess(); + + S BVTesting(int b1, int b2) const + { + if (this->enable_statistics) this->num_bv_tests++; + + return distance(tf.linear(), tf.translation(), this->model1->getBV(b1).bv, this->model2->getBV(b2).bv); + } + + void leafTesting(int b1, int b2) const; + + Transform3 tf; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +using MeshDistanceTraversalNodekIOSf = MeshDistanceTraversalNodekIOS; +using MeshDistanceTraversalNodekIOSd = MeshDistanceTraversalNodekIOS; + +/// @brief Initialize traversal node for distance computation between two +/// meshes, specialized for kIOS type +template +bool initialize( + MeshDistanceTraversalNodekIOS& node, + const BVHModel>& model1, + const Transform3& tf1, + const BVHModel>& model2, + const Transform3& tf2, + const DistanceRequest& request, + DistanceResult& result); + +template +class MeshDistanceTraversalNodeOBBRSS + : public MeshDistanceTraversalNode> +{ +public: + MeshDistanceTraversalNodeOBBRSS(); + + void preprocess(); + + void postprocess(); + + S BVTesting(int b1, int b2) const + { + if (this->enable_statistics) this->num_bv_tests++; + + return distance(tf.linear(), tf.translation(), this->model1->getBV(b1).bv, this->model2->getBV(b2).bv); + } + + void leafTesting(int b1, int b2) const; + + Transform3 tf; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +using MeshDistanceTraversalNodeOBBRSSf = MeshDistanceTraversalNodeOBBRSS; +using MeshDistanceTraversalNodeOBBRSSd = MeshDistanceTraversalNodeOBBRSS; + +/// @brief Initialize traversal node for distance computation between two +/// meshes, specialized for OBBRSS type +template +bool initialize( + MeshDistanceTraversalNodeOBBRSS& node, + const BVHModel>& model1, + const Transform3& tf1, + const BVHModel>& model2, + const Transform3& tf2, + const DistanceRequest& request, + DistanceResult& result); + +namespace details +{ + +template +FCL_DEPRECATED +void meshDistanceOrientedNodeLeafTesting( + int b1, + int b2, + const BVHModel* model1, + const BVHModel* model2, + Vector3* vertices1, + Vector3* vertices2, + Triangle* tri_indices1, + Triangle* tri_indices2, + const Matrix3& R, + const Vector3& T, + bool enable_statistics, + int& num_leaf_tests, + const DistanceRequest& request, + DistanceResult& result); + +template +void meshDistanceOrientedNodeLeafTesting( + int b1, + int b2, + const BVHModel* model1, + const BVHModel* model2, + Vector3* vertices1, + Vector3* vertices2, + Triangle* tri_indices1, + Triangle* tri_indices2, + const Transform3& tf, + bool enable_statistics, + int& num_leaf_tests, + const DistanceRequest& request, + DistanceResult& result); + +template +void distancePreprocessOrientedNode( + const BVHModel* model1, + const BVHModel* model2, + const Vector3* vertices1, + Vector3* vertices2, + Triangle* tri_indices1, + Triangle* tri_indices2, + int init_tri_id1, + int init_tri_id2, + const Matrix3& R, + const Vector3& T, + const DistanceRequest& request, + DistanceResult& result); + +template +void distancePreprocessOrientedNode( + const BVHModel* model1, + const BVHModel* model2, + const Vector3* vertices1, + Vector3* vertices2, + Triangle* tri_indices1, + Triangle* tri_indices2, + int init_tri_id1, + int init_tri_id2, + const Transform3& tf, + const DistanceRequest& request, + DistanceResult& result); + +template +void distancePostprocessOrientedNode( + const BVHModel* model1, + const BVHModel* model2, + const Transform3& tf1, + const DistanceRequest& request, + DistanceResult& result); + +} // namespace details + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +MeshDistanceTraversalNode::MeshDistanceTraversalNode() : BVHDistanceTraversalNode() +{ + vertices1 = nullptr; + vertices2 = nullptr; + tri_indices1 = nullptr; + tri_indices2 = nullptr; + + rel_err = this->request.rel_err; + abs_err = this->request.abs_err; +} + +//============================================================================== +template +void MeshDistanceTraversalNode::leafTesting(int b1, int b2) const +{ + if(this->enable_statistics) this->num_leaf_tests++; + + const BVNode& node1 = this->model1->getBV(b1); + const BVNode& node2 = this->model2->getBV(b2); + + int primitive_id1 = node1.primitiveId(); + int primitive_id2 = node2.primitiveId(); + + const Triangle& tri_id1 = tri_indices1[primitive_id1]; + const Triangle& tri_id2 = tri_indices2[primitive_id2]; + + const Vector3& t11 = vertices1[tri_id1[0]]; + const Vector3& t12 = vertices1[tri_id1[1]]; + const Vector3& t13 = vertices1[tri_id1[2]]; + + const Vector3& t21 = vertices2[tri_id2[0]]; + const Vector3& t22 = vertices2[tri_id2[1]]; + const Vector3& t23 = vertices2[tri_id2[2]]; + + // nearest point pair + Vector3 P1, P2; + + S d = TriangleDistance::triDistance(t11, t12, t13, t21, t22, t23, + P1, P2); + + if(this->request.enable_nearest_points) + { + this->result->update(d, this->model1, this->model2, primitive_id1, primitive_id2, P1, P2); + } + else + { + this->result->update(d, this->model1, this->model2, primitive_id1, primitive_id2); + } +} + +//============================================================================== +template +bool MeshDistanceTraversalNode::canStop(typename BV::S c) const +{ + if((c >= this->result->min_distance - abs_err) && (c * (1 + rel_err) >= this->result->min_distance)) + return true; + return false; +} + +//============================================================================== +template +bool initialize( + MeshDistanceTraversalNode& node, + BVHModel& model1, + Transform3& tf1, + BVHModel& model2, + Transform3& tf2, + const DistanceRequest& request, + DistanceResult& result, + bool use_refit, + bool refit_bottomup) +{ + using S = typename BV::S; + + if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES) + return false; + + if(!tf1.matrix().isIdentity()) + { + std::vector> vertices_transformed1(model1.num_vertices); + for(int i = 0; i < model1.num_vertices; ++i) + { + Vector3& p = model1.vertices[i]; + Vector3 new_v = tf1 * p; + vertices_transformed1[i] = new_v; + } + + model1.beginReplaceModel(); + model1.replaceSubModel(vertices_transformed1); + model1.endReplaceModel(use_refit, refit_bottomup); + + tf1.setIdentity(); + } + + if(!tf2.matrix().isIdentity()) + { + std::vector> vertices_transformed2(model2.num_vertices); + for(int i = 0; i < model2.num_vertices; ++i) + { + Vector3& p = model2.vertices[i]; + Vector3 new_v = tf2 * p; + vertices_transformed2[i] = new_v; + } + + model2.beginReplaceModel(); + model2.replaceSubModel(vertices_transformed2); + model2.endReplaceModel(use_refit, refit_bottomup); + + tf2.setIdentity(); + } + + node.request = request; + node.result = &result; + + node.model1 = &model1; + node.tf1 = tf1; + node.model2 = &model2; + node.tf2 = tf2; + + node.vertices1 = model1.vertices; + node.vertices2 = model2.vertices; + + node.tri_indices1 = model1.tri_indices; + node.tri_indices2 = model2.tri_indices; + + return true; +} + +//============================================================================== +template +MeshDistanceTraversalNodeRSS::MeshDistanceTraversalNodeRSS() + : MeshDistanceTraversalNode>(), + tf(Transform3::Identity()) +{ +} + +//============================================================================== +template +void MeshDistanceTraversalNodeRSS::preprocess() +{ + details::distancePreprocessOrientedNode( + this->model1, + this->model2, + this->vertices1, + this->vertices2, + this->tri_indices1, + this->tri_indices2, + 0, + 0, + tf, + this->request, + *this->result); +} + +//============================================================================== +template +void MeshDistanceTraversalNodeRSS::postprocess() +{ + details::distancePostprocessOrientedNode( + this->model1, + this->model2, + this->tf1, + this->request, + *this->result); +} + +//============================================================================== +template +void MeshDistanceTraversalNodeRSS::leafTesting(int b1, int b2) const +{ + details::meshDistanceOrientedNodeLeafTesting( + b1, + b2, + this->model1, + this->model2, + this->vertices1, + this->vertices2, + this->tri_indices1, + this->tri_indices2, + tf, + this->enable_statistics, + this->num_leaf_tests, + this->request, + *this->result); +} + +//============================================================================== +template +MeshDistanceTraversalNodekIOS::MeshDistanceTraversalNodekIOS() + : MeshDistanceTraversalNode>(), + tf(Transform3::Identity()) +{ + // Do nothing +} + +//============================================================================== +template +void MeshDistanceTraversalNodekIOS::preprocess() +{ + details::distancePreprocessOrientedNode( + this->model1, + this->model2, + this->vertices1, + this->vertices2, + this->tri_indices1, + this->tri_indices2, + 0, + 0, + tf, + this->request, + *this->result); +} + +//============================================================================== +template +void MeshDistanceTraversalNodekIOS::postprocess() +{ + details::distancePostprocessOrientedNode( + this->model1, + this->model2, + this->tf1, + this->request, + *this->result); +} + +//============================================================================== +template +void MeshDistanceTraversalNodekIOS::leafTesting(int b1, int b2) const +{ + details::meshDistanceOrientedNodeLeafTesting( + b1, + b2, + this->model1, + this->model2, + this->vertices1, + this->vertices2, + this->tri_indices1, + this->tri_indices2, + tf, + this->enable_statistics, + this->num_leaf_tests, + this->request, + *this->result); +} + +//============================================================================== +template +MeshDistanceTraversalNodeOBBRSS::MeshDistanceTraversalNodeOBBRSS() + : MeshDistanceTraversalNode>(), + tf(Transform3::Identity()) +{ + // Do nothing +} + +//============================================================================== +template +void MeshDistanceTraversalNodeOBBRSS::preprocess() +{ + details::distancePreprocessOrientedNode( + this->model1, + this->model2, + this->vertices1, + this->vertices2, + this->tri_indices1, + this->tri_indices2, + 0, + 0, + tf, + this->request, + *this->result); +} + +//============================================================================== +template +void MeshDistanceTraversalNodeOBBRSS::postprocess() +{ + details::distancePostprocessOrientedNode( + this->model1, + this->model2, + this->tf1, + this->request, + *this->result); +} + +//============================================================================== +template +void MeshDistanceTraversalNodeOBBRSS::leafTesting(int b1, int b2) const +{ + details::meshDistanceOrientedNodeLeafTesting( + b1, + b2, + this->model1, + this->model2, + this->vertices1, + this->vertices2, + this->tri_indices1, + this->tri_indices2, + tf, + this->enable_statistics, + this->num_leaf_tests, + this->request, + *this->result); +} + +namespace details +{ + +//============================================================================== +template +void meshDistanceOrientedNodeLeafTesting(int b1, + int b2, + const BVHModel* model1, + const BVHModel* model2, + Vector3* vertices1, + Vector3* vertices2, + Triangle* tri_indices1, + Triangle* tri_indices2, + const Matrix3& R, + const Vector3& T, + bool enable_statistics, + int& num_leaf_tests, + const DistanceRequest& request, + DistanceResult& result) +{ + using S = typename BV::S; + + if(enable_statistics) num_leaf_tests++; + + const BVNode& node1 = model1->getBV(b1); + const BVNode& node2 = model2->getBV(b2); + + int primitive_id1 = node1.primitiveId(); + int primitive_id2 = node2.primitiveId(); + + const Triangle& tri_id1 = tri_indices1[primitive_id1]; + const Triangle& tri_id2 = tri_indices2[primitive_id2]; + + const Vector3& t11 = vertices1[tri_id1[0]]; + const Vector3& t12 = vertices1[tri_id1[1]]; + const Vector3& t13 = vertices1[tri_id1[2]]; + + const Vector3& t21 = vertices2[tri_id2[0]]; + const Vector3& t22 = vertices2[tri_id2[1]]; + const Vector3& t23 = vertices2[tri_id2[2]]; + + // nearest point pair + Vector3 P1, P2; + + S d = TriangleDistance::triDistance(t11, t12, t13, t21, t22, t23, + R, T, + P1, P2); + + if(request.enable_nearest_points) + result.update(d, model1, model2, primitive_id1, primitive_id2, P1, P2); + else + result.update(d, model1, model2, primitive_id1, primitive_id2); +} + +//============================================================================== +template +void meshDistanceOrientedNodeLeafTesting( + int b1, + int b2, + const BVHModel* model1, + const BVHModel* model2, + Vector3* vertices1, + Vector3* vertices2, + Triangle* tri_indices1, + Triangle* tri_indices2, + const Transform3& tf, + bool enable_statistics, + int& num_leaf_tests, + const DistanceRequest& request, + DistanceResult& result) +{ + using S = typename BV::S; + + if(enable_statistics) num_leaf_tests++; + + const BVNode& node1 = model1->getBV(b1); + const BVNode& node2 = model2->getBV(b2); + + int primitive_id1 = node1.primitiveId(); + int primitive_id2 = node2.primitiveId(); + + const Triangle& tri_id1 = tri_indices1[primitive_id1]; + const Triangle& tri_id2 = tri_indices2[primitive_id2]; + + const Vector3& t11 = vertices1[tri_id1[0]]; + const Vector3& t12 = vertices1[tri_id1[1]]; + const Vector3& t13 = vertices1[tri_id1[2]]; + + const Vector3& t21 = vertices2[tri_id2[0]]; + const Vector3& t22 = vertices2[tri_id2[1]]; + const Vector3& t23 = vertices2[tri_id2[2]]; + + // nearest point pair + Vector3 P1, P2; + + S d = TriangleDistance::triDistance( + t11, t12, t13, t21, t22, t23, tf, P1, P2); + + if(request.enable_nearest_points) + result.update(d, model1, model2, primitive_id1, primitive_id2, P1, P2); + else + result.update(d, model1, model2, primitive_id1, primitive_id2); +} + +//============================================================================== +template +void distancePreprocessOrientedNode( + const BVHModel* model1, + const BVHModel* model2, + const Vector3* vertices1, + Vector3* vertices2, + Triangle* tri_indices1, + Triangle* tri_indices2, + int init_tri_id1, + int init_tri_id2, + const Matrix3& R, + const Vector3& T, + const DistanceRequest& request, + DistanceResult& result) +{ + using S = typename BV::S; + + const Triangle& init_tri1 = tri_indices1[init_tri_id1]; + const Triangle& init_tri2 = tri_indices2[init_tri_id2]; + + Vector3 init_tri1_points[3]; + Vector3 init_tri2_points[3]; + + init_tri1_points[0] = vertices1[init_tri1[0]]; + init_tri1_points[1] = vertices1[init_tri1[1]]; + init_tri1_points[2] = vertices1[init_tri1[2]]; + + init_tri2_points[0] = vertices2[init_tri2[0]]; + init_tri2_points[1] = vertices2[init_tri2[1]]; + init_tri2_points[2] = vertices2[init_tri2[2]]; + + Vector3 p1, p2; + S distance = TriangleDistance::triDistance(init_tri1_points[0], init_tri1_points[1], init_tri1_points[2], + init_tri2_points[0], init_tri2_points[1], init_tri2_points[2], + R, T, p1, p2); + + if(request.enable_nearest_points) + result.update(distance, model1, model2, init_tri_id1, init_tri_id2, p1, p2); + else + result.update(distance, model1, model2, init_tri_id1, init_tri_id2); +} + +//============================================================================== +template +void distancePreprocessOrientedNode( + const BVHModel* model1, + const BVHModel* model2, + const Vector3* vertices1, + Vector3* vertices2, + Triangle* tri_indices1, + Triangle* tri_indices2, + int init_tri_id1, + int init_tri_id2, + const Transform3& tf, + const DistanceRequest& request, + DistanceResult& result) +{ + using S = typename BV::S; + + const Triangle& init_tri1 = tri_indices1[init_tri_id1]; + const Triangle& init_tri2 = tri_indices2[init_tri_id2]; + + Vector3 init_tri1_points[3]; + Vector3 init_tri2_points[3]; + + init_tri1_points[0] = vertices1[init_tri1[0]]; + init_tri1_points[1] = vertices1[init_tri1[1]]; + init_tri1_points[2] = vertices1[init_tri1[2]]; + + init_tri2_points[0] = vertices2[init_tri2[0]]; + init_tri2_points[1] = vertices2[init_tri2[1]]; + init_tri2_points[2] = vertices2[init_tri2[2]]; + + Vector3 p1, p2; + S distance + = TriangleDistance::triDistance( + init_tri1_points[0], init_tri1_points[1], init_tri1_points[2], + init_tri2_points[0], init_tri2_points[1], init_tri2_points[2], + tf, p1, p2); + + if(request.enable_nearest_points) + result.update(distance, model1, model2, init_tri_id1, init_tri_id2, p1, p2); + else + result.update(distance, model1, model2, init_tri_id1, init_tri_id2); +} + +//============================================================================== +template +void distancePostprocessOrientedNode( + const BVHModel* model1, + const BVHModel* model2, + const Transform3& tf1, + const DistanceRequest& request, + DistanceResult& result) +{ + /// the points obtained by triDistance are not in world space: both are in object1's local coordinate system, so we need to convert them into the world space. + if(request.enable_nearest_points && (result.o1 == model1) && (result.o2 == model2)) + { + result.nearest_points[0] = tf1 * result.nearest_points[0]; + result.nearest_points[1] = tf1 * result.nearest_points[1]; + } +} + +} // namespace details + +namespace details +{ +template +static inline bool setupMeshDistanceOrientedNode( + OrientedNode& node, + const BVHModel& model1, const Transform3& tf1, + const BVHModel& model2, const Transform3& tf2, + const DistanceRequest& request, + DistanceResult& result) +{ + if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES) + return false; + + node.request = request; + node.result = &result; + + node.model1 = &model1; + node.tf1 = tf1; + node.model2 = &model2; + node.tf2 = tf2; + + node.vertices1 = model1.vertices; + node.vertices2 = model2.vertices; + + node.tri_indices1 = model1.tri_indices; + node.tri_indices2 = model2.tri_indices; + + node.tf = tf1.inverse(Eigen::Isometry) * tf2; + + return true; +} + +} // namespace details + +//============================================================================== +template +bool initialize( + MeshDistanceTraversalNodeRSS& node, + const BVHModel>& model1, + const Transform3& tf1, + const BVHModel>& model2, + const Transform3& tf2, + const DistanceRequest& request, + DistanceResult& result) +{ + return details::setupMeshDistanceOrientedNode( + node, model1, tf1, model2, tf2, request, result); +} + +//============================================================================== +template +bool initialize( + MeshDistanceTraversalNodekIOS& node, + const BVHModel>& model1, + const Transform3& tf1, + const BVHModel>& model2, + const Transform3& tf2, + const DistanceRequest& request, + DistanceResult& result) +{ + return details::setupMeshDistanceOrientedNode( + node, model1, tf1, model2, tf2, request, result); +} + +//============================================================================== +template +bool initialize( + MeshDistanceTraversalNodeOBBRSS& node, + const BVHModel>& model1, + const Transform3& tf1, + const BVHModel>& model2, + const Transform3& tf2, + const DistanceRequest& request, + DistanceResult& result) +{ + return details::setupMeshDistanceOrientedNode( + node, model1, tf1, model2, tf2, request, result); +} + +} // namespace fcl + +#endif diff --git a/include/fcl/traversal/distance/mesh_shape_conservative_advancement_traversal_node.h b/include/fcl/traversal/distance/mesh_shape_conservative_advancement_traversal_node.h new file mode 100644 index 000000000..4b435c19e --- /dev/null +++ b/include/fcl/traversal/distance/mesh_shape_conservative_advancement_traversal_node.h @@ -0,0 +1,593 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_TRAVERSAL_MESHSHAPECONSERVATIVEADVANCEMENTTRAVERSALNODE_H +#define FCL_TRAVERSAL_MESHSHAPECONSERVATIVEADVANCEMENTTRAVERSALNODE_H + +#include "fcl/traversal/distance/conservative_advancement_stack_data.h" +#include "fcl/traversal/distance/mesh_shape_distance_traversal_node.h" + +namespace fcl +{ + +/// @brief Traversal node for conservative advancement computation between BVH and shape +template +class MeshShapeConservativeAdvancementTraversalNode + : public MeshShapeDistanceTraversalNode +{ +public: + + using S = typename BV::S; + + MeshShapeConservativeAdvancementTraversalNode(S w_ = 1); + + /// @brief BV culling test in one BVTT node + S BVTesting(int b1, int b2) const; + + /// @brief Conservative advancement testing between leaves (one triangle and one shape) + void leafTesting(int b1, int b2) const; + + /// @brief Whether the traversal process can stop early + bool canStop(S c) const; + + mutable S min_distance; + + mutable Vector3 closest_p1, closest_p2; + + mutable int last_tri_id; + + /// @brief CA controlling variable: early stop for the early iterations of CA + S w; + + /// @brief The time from beginning point + S toc; + S t_err; + + /// @brief The delta_t each step + mutable S delta_t; + + /// @brief Motions for the two objects in query + const MotionBased* motion1; + const MotionBased* motion2; + + mutable std::vector> stack; +}; + +template +bool initialize( + MeshShapeConservativeAdvancementTraversalNode& node, + BVHModel& model1, + const Transform3& tf1, + const Shape& model2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + typename BV::S w = 1, + bool use_refit = false, + bool refit_bottomup = false); + +namespace details +{ + +template +void meshShapeConservativeAdvancementOrientedNodeLeafTesting( + int b1, + int /* b2 */, + const BVHModel* model1, + const Shape& model2, + const BV& model2_bv, + Vector3* vertices, + Triangle* tri_indices, + const Transform3& tf1, + const Transform3& tf2, + const MotionBased* motion1, + const MotionBased* motion2, + const NarrowPhaseSolver* nsolver, + bool enable_statistics, + typename BV::S& min_distance, + Vector3& p1, + Vector3& p2, + int& last_tri_id, + typename BV::S& delta_t, + int& num_leaf_tests) +{ + using S = typename BV::S; + + if(enable_statistics) num_leaf_tests++; + + const BVNode& node = model1->getBV(b1); + int primitive_id = node.primitiveId(); + + const Triangle& tri_id = tri_indices[primitive_id]; + const Vector3& t1 = vertices[tri_id[0]]; + const Vector3& t2 = vertices[tri_id[1]]; + const Vector3& t3 = vertices[tri_id[2]]; + + S distance; + Vector3 P1 = Vector3::Zero(); + Vector3 P2 = Vector3::Zero(); + nsolver->shapeTriangleDistance(model2, tf2, t1, t2, t3, tf1, &distance, &P2, &P1); + + if(distance < min_distance) + { + min_distance = distance; + + p1 = P1; + p2 = P2; + + last_tri_id = primitive_id; + } + + // n is in global frame + Vector3 n = P2 - P1; n.normalize(); + + TriangleMotionBoundVisitor mb_visitor1(t1, t2, t3, n); + TBVMotionBoundVisitor mb_visitor2(model2_bv, -n); + S bound1 = motion1->computeMotionBound(mb_visitor1); + S bound2 = motion2->computeMotionBound(mb_visitor2); + + S bound = bound1 + bound2; + + S cur_delta_t; + if(bound <= distance) cur_delta_t = 1; + else cur_delta_t = distance / bound; + + if(cur_delta_t < delta_t) + delta_t = cur_delta_t; +} + + +template +bool meshShapeConservativeAdvancementOrientedNodeCanStop( + typename BV::S c, + typename BV::S min_distance, + typename BV::S abs_err, + typename BV::S rel_err, + typename BV::S w, + const BVHModel* model1, + const Shape& model2, + const BV& model2_bv, + const MotionBased* motion1, const MotionBased* motion2, + std::vector>& stack, + typename BV::S& delta_t) +{ + using S = typename BV::S; + + if((c >= w * (min_distance - abs_err)) && (c * (1 + rel_err) >= w * min_distance)) + { + const auto& data = stack.back(); + Vector3 n = data.P2 - data.P1; n.normalize(); + int c1 = data.c1; + + TBVMotionBoundVisitor mb_visitor1(model1->getBV(c1).bv, n); + TBVMotionBoundVisitor mb_visitor2(model2_bv, -n); + + S bound1 = motion1->computeMotionBound(mb_visitor1); + S bound2 = motion2->computeMotionBound(mb_visitor2); + + S bound = bound1 + bound2; + + S cur_delta_t; + if(bound <= c) cur_delta_t = 1; + else cur_delta_t = c / bound; + + if(cur_delta_t < delta_t) + delta_t = cur_delta_t; + + stack.pop_back(); + + return true; + } + else + { + stack.pop_back(); + return false; + } +} + +} // namespace details + +template +class MeshShapeConservativeAdvancementTraversalNodeRSS + : public MeshShapeConservativeAdvancementTraversalNode< + RSS, Shape, NarrowPhaseSolver> +{ +public: + + using S = typename Shape::S; + + MeshShapeConservativeAdvancementTraversalNodeRSS(S w_ = 1) + : MeshShapeConservativeAdvancementTraversalNode< + RSS, Shape, NarrowPhaseSolver>(w_) + { + } + + S BVTesting(int b1, int b2) const + { + if(this->enable_statistics) this->num_bv_tests++; + Vector3 P1, P2; + S d = distance(this->tf1.linear(), this->tf1.translation(), this->model1->getBV(b1).bv, this->model2_bv, &P1, &P2); + + this->stack.emplace_back(P1, P2, b1, b2, d); + + return d; + } + + void leafTesting(int b1, int b2) const + { + details::meshShapeConservativeAdvancementOrientedNodeLeafTesting( + b1, + b2, + this->model1, + *(this->model2), + this->model2_bv, + this->vertices, + this->tri_indices, + this->tf1, + this->tf2, + this->motion1, + this->motion2, + this->nsolver, + this->enable_statistics, + this->min_distance, + this->closest_p1, + this->closest_p2, + this->last_tri_id, + this->delta_t, + this->num_leaf_tests); + } + + bool canStop(S c) const + { + return details::meshShapeConservativeAdvancementOrientedNodeCanStop(c, this->min_distance, + this->abs_err, this->rel_err, this->w, + this->model1, *(this->model2), this->model2_bv, + this->motion1, this->motion2, + this->stack, this->delta_t); + } +}; + +template +bool initialize( + MeshShapeConservativeAdvancementTraversalNodeRSS& node, + const BVHModel>& model1, + const Transform3& tf1, + const Shape& model2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + typename Shape::S w = 1); + +template +class MeshShapeConservativeAdvancementTraversalNodeOBBRSS : + public MeshShapeConservativeAdvancementTraversalNode< + OBBRSS, Shape, NarrowPhaseSolver> +{ +public: + + using S = typename Shape::S; + + MeshShapeConservativeAdvancementTraversalNodeOBBRSS(S w_ = 1) + : MeshShapeConservativeAdvancementTraversalNode< + OBBRSS, Shape, NarrowPhaseSolver>(w_) + { + } + + S BVTesting(int b1, int b2) const + { + if(this->enable_statistics) this->num_bv_tests++; + Vector3 P1, P2; + S d = distance(this->tf1.linear(), this->tf1.translation(), this->model1->getBV(b1).bv, this->model2_bv, &P1, &P2); + + this->stack.emplace_back(P1, P2, b1, b2, d); + + return d; + } + + void leafTesting(int b1, int b2) const + { + details::meshShapeConservativeAdvancementOrientedNodeLeafTesting( + b1, + b2, + this->model1, + *(this->model2), + this->model2_bv, + this->vertices, + this->tri_indices, + this->tf1, + this->tf2, + this->motion1, + this->motion2, + this->nsolver, + this->enable_statistics, + this->min_distance, + this->closest_p1, + this->closest_p2, + this->last_tri_id, + this->delta_t, + this->num_leaf_tests); + } + + bool canStop(S c) const + { + return details::meshShapeConservativeAdvancementOrientedNodeCanStop( + c, + this->min_distance, + this->abs_err, + this->rel_err, + this->w, + this->model1, + *(this->model2), + this->model2_bv, + this->motion1, + this->motion2, + this->stack, + this->delta_t); + } +}; + +template +bool initialize( + MeshShapeConservativeAdvancementTraversalNodeOBBRSS& node, + const BVHModel>& model1, + const Transform3& tf1, + const Shape& model2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + typename Shape::S w = 1); + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +MeshShapeConservativeAdvancementTraversalNode:: +MeshShapeConservativeAdvancementTraversalNode(S w_) : + MeshShapeDistanceTraversalNode() +{ + delta_t = 1; + toc = 0; + t_err = (S)0.0001; + + w = w_; + + motion1 = nullptr; + motion2 = nullptr; +} + +//============================================================================== +template +typename BV::S +MeshShapeConservativeAdvancementTraversalNode:: +BVTesting(int b1, int b2) const +{ + if(this->enable_statistics) this->num_bv_tests++; + Vector3 P1, P2; + S d = this->model2_bv.distance(this->model1->getBV(b1).bv, &P2, &P1); + + stack.emplace_back(P1, P2, b1, b2, d); + + return d; +} + +//============================================================================== +template +void MeshShapeConservativeAdvancementTraversalNode:: +leafTesting(int b1, int b2) const +{ + if(this->enable_statistics) this->num_leaf_tests++; + + const BVNode& node = this->model1->getBV(b1); + + int primitive_id = node.primitiveId(); + + const Triangle& tri_id = this->tri_indices[primitive_id]; + + const Vector3& p1 = this->vertices[tri_id[0]]; + const Vector3& p2 = this->vertices[tri_id[1]]; + const Vector3& p3 = this->vertices[tri_id[2]]; + + S d; + Vector3 P1, P2; + this->nsolver->shapeTriangleDistance(*(this->model2), this->tf2, p1, p2, p3, &d, &P2, &P1); + + if(d < this->min_distance) + { + this->min_distance = d; + + closest_p1 = P1; + closest_p2 = P2; + + last_tri_id = primitive_id; + } + + Vector3 n = this->tf2 * p2 - P1; n.normalize(); + // here n should be in global frame + TriangleMotionBoundVisitor mb_visitor1(p1, p2, p3, n); + TBVMotionBoundVisitor mb_visitor2(this->model2_bv, -n); + S bound1 = motion1->computeMotionBound(mb_visitor1); + S bound2 = motion2->computeMotionBound(mb_visitor2); + + S bound = bound1 + bound2; + + S cur_delta_t; + if(bound <= d) cur_delta_t = 1; + else cur_delta_t = d / bound; + + if(cur_delta_t < delta_t) + delta_t = cur_delta_t; +} + +//============================================================================== +template +bool MeshShapeConservativeAdvancementTraversalNode:: +canStop(S c) const +{ + if((c >= w * (this->min_distance - this->abs_err)) + && (c * (1 + this->rel_err) >= w * this->min_distance)) + { + const auto& data = stack.back(); + + Vector3 n = this->tf2 * data.P2 - data.P1; n.normalize(); + int c1 = data.c1; + + TBVMotionBoundVisitor mb_visitor1(this->model1->getBV(c1).bv, n); + TBVMotionBoundVisitor mb_visitor2(this->model2_bv, -n); + S bound1 = motion1->computeMotionBound(mb_visitor1); + S bound2 = motion2->computeMotionBound(mb_visitor2); + + S bound = bound1 + bound2; + + S cur_delta_t; + if(bound < c) cur_delta_t = 1; + else cur_delta_t = c / bound; + + if(cur_delta_t < delta_t) + delta_t = cur_delta_t; + + stack.pop_back(); + + return true; + } + else + { + stack.pop_back(); + + return false; + } +} + +//============================================================================== +template +bool initialize( + MeshShapeConservativeAdvancementTraversalNode& node, + BVHModel& model1, + const Transform3& tf1, + const Shape& model2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + typename BV::S w, + bool use_refit, + bool refit_bottomup) +{ + using S = typename BV::S; + + std::vector> vertices_transformed(model1.num_vertices); + for(int i = 0; i < model1.num_vertices; ++i) + { + Vector3& p = model1.vertices[i]; + Vector3 new_v = tf1 * p; + vertices_transformed[i] = new_v; + } + + model1.beginReplaceModel(); + model1.replaceSubModel(vertices_transformed); + model1.endReplaceModel(use_refit, refit_bottomup); + + node.model1 = &model1; + node.model2 = &model2; + + node.vertices = model1.vertices; + node.tri_indices = model1.tri_indices; + + node.tf1 = tf1; + node.tf2 = tf2; + + node.nsolver = nsolver; + node.w = w; + + computeBV(model2, Transform3::Identity(), node.model2_bv); + + return true; +} + +//============================================================================== +template +bool initialize( + MeshShapeConservativeAdvancementTraversalNodeRSS& node, + const BVHModel>& model1, + const Transform3& tf1, + const Shape& model2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + typename Shape::S w) +{ + using S = typename Shape::S; + + node.model1 = &model1; + node.tf1 = tf1; + node.model2 = &model2; + node.tf2 = tf2; + node.nsolver = nsolver; + + node.w = w; + + computeBV(model2, Transform3::Identity(), node.model2_bv); + + return true; +} + +//============================================================================== +template +bool initialize( + MeshShapeConservativeAdvancementTraversalNodeOBBRSS& node, + const BVHModel>& model1, + const Transform3& tf1, + const Shape& model2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + typename Shape::S w) +{ + using S = typename Shape::S; + + node.model1 = &model1; + node.tf1 = tf1; + node.model2 = &model2; + node.tf2 = tf2; + node.nsolver = nsolver; + + node.w = w; + + computeBV(model2, Transform3::Identity(), node.model2_bv); + + return true; +} + +} // namespace fcl + +#endif diff --git a/include/fcl/traversal/distance/mesh_shape_distance_traversal_node.h b/include/fcl/traversal/distance/mesh_shape_distance_traversal_node.h new file mode 100644 index 000000000..23bfb1a99 --- /dev/null +++ b/include/fcl/traversal/distance/mesh_shape_distance_traversal_node.h @@ -0,0 +1,629 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_TRAVERSAL_MESHSHAPEDISTANCETRAVERSALNODE_H +#define FCL_TRAVERSAL_MESHSHAPEDISTANCETRAVERSALNODE_H + +#include "fcl/shape/compute_bv.h" +#include "fcl/traversal/distance/bvh_shape_distance_traversal_node.h" + +namespace fcl +{ + +/// @brief Traversal node for distance between mesh and shape +template +class MeshShapeDistanceTraversalNode + : public BVHShapeDistanceTraversalNode +{ +public: + + using S = typename BV::S; + + MeshShapeDistanceTraversalNode(); + + /// @brief Distance testing between leaves (one triangle and one shape) + void leafTesting(int b1, int b2) const; + + /// @brief Whether the traversal process can stop early + bool canStop(S c) const; + + Vector3* vertices; + Triangle* tri_indices; + + S rel_err; + S abs_err; + + const NarrowPhaseSolver* nsolver; +}; + +/// @cond IGNORE +namespace details +{ + +template +void meshShapeDistanceOrientedNodeLeafTesting( + int b1, int /* b2 */, + const BVHModel* model1, + const Shape& model2, + Vector3* vertices, + Triangle* tri_indices, + const Transform3& tf1, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + bool enable_statistics, + int & num_leaf_tests, + const DistanceRequest& /* request */, + DistanceResult& result); + + +template +void distancePreprocessOrientedNode( + const BVHModel* model1, + Vector3* vertices, + Triangle* tri_indices, + int init_tri_id, + const Shape& model2, + const Transform3& tf1, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const DistanceRequest& /* request */, + DistanceResult& result); + +} // namespace details + +/// @endcond + +/// @brief Initialize traversal node for distance computation between one mesh +/// and one shape, given the current transforms +template +bool initialize( + MeshShapeDistanceTraversalNode& node, + BVHModel& model1, + Transform3& tf1, + const Shape& model2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const DistanceRequest& request, + DistanceResult& result, + bool use_refit = false, bool refit_bottomup = false); + +/// @brief Traversal node for distance between mesh and shape, when mesh BVH is one of the oriented node (RSS, OBBRSS, kIOS) +template +class MeshShapeDistanceTraversalNodeRSS + : public MeshShapeDistanceTraversalNode< + RSS, Shape, NarrowPhaseSolver> +{ +public: + using S = typename Shape::S; + + MeshShapeDistanceTraversalNodeRSS(); + + void preprocess(); + + void postprocess(); + + S BVTesting(int b1, int b2) const; + + void leafTesting(int b1, int b2) const; +}; + +template +bool initialize( + MeshShapeDistanceTraversalNodeRSS& node, + const BVHModel>& model1, + const Transform3& tf1, + const Shape& model2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const DistanceRequest& request, + DistanceResult& result); + +template +class MeshShapeDistanceTraversalNodekIOS + : public MeshShapeDistanceTraversalNode, Shape, NarrowPhaseSolver> +{ +public: + using S = typename Shape::S; + + MeshShapeDistanceTraversalNodekIOS(); + + void preprocess(); + + void postprocess(); + + S BVTesting(int b1, int b2) const; + + void leafTesting(int b1, int b2) const; + +}; + +/// @brief Initialize traversal node for distance computation between one mesh and one shape, specialized for kIOS type +template +bool initialize( + MeshShapeDistanceTraversalNodekIOS& node, + const BVHModel>& model1, const Transform3& tf1, + const Shape& model2, const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const DistanceRequest& request, + DistanceResult& result); + +template +class MeshShapeDistanceTraversalNodeOBBRSS + : public MeshShapeDistanceTraversalNode, Shape, NarrowPhaseSolver> +{ +public: + using S = typename Shape::S; + + MeshShapeDistanceTraversalNodeOBBRSS(); + + void preprocess(); + + void postprocess(); + + S BVTesting(int b1, int b2) const; + + void leafTesting(int b1, int b2) const; + +}; + +/// @brief Initialize traversal node for distance computation between one mesh and one shape, specialized for OBBRSS type +template +bool initialize( + MeshShapeDistanceTraversalNodeOBBRSS& node, + const BVHModel>& model1, + const Transform3& tf1, + const Shape& model2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const DistanceRequest& request, + DistanceResult& result); + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +MeshShapeDistanceTraversalNode:: +MeshShapeDistanceTraversalNode() + : BVHShapeDistanceTraversalNode() +{ + vertices = nullptr; + tri_indices = nullptr; + + rel_err = 0; + abs_err = 0; + + nsolver = nullptr; +} + +//============================================================================== +template +void MeshShapeDistanceTraversalNode:: +leafTesting(int b1, int b2) const +{ + if(this->enable_statistics) this->num_leaf_tests++; + + const BVNode& node = this->model1->getBV(b1); + + int primitive_id = node.primitiveId(); + + const Triangle& tri_id = tri_indices[primitive_id]; + + const Vector3& p1 = vertices[tri_id[0]]; + const Vector3& p2 = vertices[tri_id[1]]; + const Vector3& p3 = vertices[tri_id[2]]; + + S d; + Vector3 closest_p1, closest_p2; + nsolver->shapeTriangleDistance(*(this->model2), this->tf2, p1, p2, p3, &d, &closest_p2, &closest_p1); + + this->result->update( + d, + this->model1, + this->model2, + primitive_id, + DistanceResult::NONE, + closest_p1, + closest_p2); +} + +//============================================================================== +template +bool MeshShapeDistanceTraversalNode:: +canStop(S c) const +{ + if((c >= this->result->min_distance - abs_err) && (c * (1 + rel_err) >= this->result->min_distance)) + return true; + return false; +} + +//============================================================================== +template +bool initialize( + MeshShapeDistanceTraversalNode& node, + BVHModel& model1, + Transform3& tf1, + const Shape& model2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const DistanceRequest& request, + DistanceResult& result, + bool use_refit, + bool refit_bottomup) +{ + using S = typename BV::S; + + if(model1.getModelType() != BVH_MODEL_TRIANGLES) + return false; + + if(!tf1.matrix().isIdentity()) + { + std::vector> vertices_transformed1(model1.num_vertices); + for(int i = 0; i < model1.num_vertices; ++i) + { + Vector3& p = model1.vertices[i]; + Vector3 new_v = tf1 * p; + vertices_transformed1[i] = new_v; + } + + model1.beginReplaceModel(); + model1.replaceSubModel(vertices_transformed1); + model1.endReplaceModel(use_refit, refit_bottomup); + + tf1.setIdentity(); + } + + node.request = request; + node.result = &result; + + node.model1 = &model1; + node.tf1 = tf1; + node.model2 = &model2; + node.tf2 = tf2; + node.nsolver = nsolver; + + node.vertices = model1.vertices; + node.tri_indices = model1.tri_indices; + + computeBV(model2, tf2, node.model2_bv); + + return true; +} + +/// @cond IGNORE +namespace details +{ + +//============================================================================== +template +void meshShapeDistanceOrientedNodeLeafTesting( + int b1, int /* b2 */, + const BVHModel* model1, const Shape& model2, + Vector3* vertices, Triangle* tri_indices, + const Transform3& tf1, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + bool enable_statistics, + int & num_leaf_tests, + const DistanceRequest& /* request */, + DistanceResult& result) +{ + using S = typename BV::S; + + if(enable_statistics) num_leaf_tests++; + + const BVNode& node = model1->getBV(b1); + int primitive_id = node.primitiveId(); + + const Triangle& tri_id = tri_indices[primitive_id]; + const Vector3& p1 = vertices[tri_id[0]]; + const Vector3& p2 = vertices[tri_id[1]]; + const Vector3& p3 = vertices[tri_id[2]]; + + S distance; + Vector3 closest_p1, closest_p2; + nsolver->shapeTriangleDistance(model2, tf2, p1, p2, p3, tf1, &distance, &closest_p2, &closest_p1); + + result.update( + distance, + model1, + &model2, + primitive_id, + DistanceResult::NONE, + closest_p1, + closest_p2); +} + +//============================================================================== +template +void distancePreprocessOrientedNode( + const BVHModel* model1, + Vector3* vertices, + Triangle* tri_indices, + int init_tri_id, + const Shape& model2, + const Transform3& tf1, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const DistanceRequest& /* request */, + DistanceResult& result) +{ + using S = typename BV::S; + + const Triangle& init_tri = tri_indices[init_tri_id]; + + const Vector3& p1 = vertices[init_tri[0]]; + const Vector3& p2 = vertices[init_tri[1]]; + const Vector3& p3 = vertices[init_tri[2]]; + + S distance; + Vector3 closest_p1, closest_p2; + nsolver->shapeTriangleDistance(model2, tf2, p1, p2, p3, tf1, &distance, &closest_p2, &closest_p1); + + result.update( + distance, + model1, + &model2, + init_tri_id, + DistanceResult::NONE, + closest_p1, + closest_p2); +} + +} // namespace details + +//============================================================================== +template +MeshShapeDistanceTraversalNodeRSS:: +MeshShapeDistanceTraversalNodeRSS() + : MeshShapeDistanceTraversalNode< + RSS, Shape, NarrowPhaseSolver>() +{ +} + +//============================================================================== +template +void MeshShapeDistanceTraversalNodeRSS::preprocess() +{ + details::distancePreprocessOrientedNode( + this->model1, + this->vertices, + this->tri_indices, + 0, + *(this->model2), + this->tf1, + this->tf2, + this->nsolver, + this->request, + *(this->result)); +} + +//============================================================================== +template +void MeshShapeDistanceTraversalNodeRSS::postprocess() +{ +} + +//============================================================================== +template +typename Shape::S +MeshShapeDistanceTraversalNodeRSS::BVTesting( + int b1, int b2) const +{ + if(this->enable_statistics) this->num_bv_tests++; + + return distance(this->tf1.linear(), this->tf1.translation(), this->model2_bv, this->model1->getBV(b1).bv); +} + +//============================================================================== +template +void MeshShapeDistanceTraversalNodeRSS:: +leafTesting(int b1, int b2) const +{ + details::meshShapeDistanceOrientedNodeLeafTesting( + b1, + b2, + this->model1, + *(this->model2), + this->vertices, + this->tri_indices, + this->tf1, + this->tf2, + this->nsolver, + this->enable_statistics, + this->num_leaf_tests, + this->request, + *(this->result)); +} + +//============================================================================== +template +MeshShapeDistanceTraversalNodekIOS::MeshShapeDistanceTraversalNodekIOS() : MeshShapeDistanceTraversalNode, Shape, NarrowPhaseSolver>() +{ +} + +//============================================================================== +template +void MeshShapeDistanceTraversalNodekIOS::preprocess() +{ + details::distancePreprocessOrientedNode(this->model1, this->vertices, this->tri_indices, 0, + *(this->model2), this->tf1, this->tf2, this->nsolver, this->request, *(this->result)); +} + +//============================================================================== +template +void MeshShapeDistanceTraversalNodekIOS::postprocess() +{ +} + +//============================================================================== +template +typename Shape::S MeshShapeDistanceTraversalNodekIOS::BVTesting(int b1, int b2) const +{ + if(this->enable_statistics) this->num_bv_tests++; + + return distance(this->tf1.linear(), this->tf1.translation(), this->model2_bv, this->model1->getBV(b1).bv); +} + +//============================================================================== +template +void MeshShapeDistanceTraversalNodekIOS::leafTesting(int b1, int b2) const +{ + details::meshShapeDistanceOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices, + this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->request, *(this->result)); +} + +//============================================================================== +template +MeshShapeDistanceTraversalNodeOBBRSS::MeshShapeDistanceTraversalNodeOBBRSS() : MeshShapeDistanceTraversalNode, Shape, NarrowPhaseSolver>() +{ +} + +//============================================================================== +template +void MeshShapeDistanceTraversalNodeOBBRSS::preprocess() +{ + details::distancePreprocessOrientedNode(this->model1, this->vertices, this->tri_indices, 0, + *(this->model2), this->tf1, this->tf2, this->nsolver, this->request, *(this->result)); +} + +//============================================================================== +template +void MeshShapeDistanceTraversalNodeOBBRSS::postprocess() +{ + +} + +//============================================================================== +template +typename Shape::S +MeshShapeDistanceTraversalNodeOBBRSS:: +BVTesting(int b1, int b2) const +{ + if(this->enable_statistics) this->num_bv_tests++; + + return distance(this->tf1.linear(), this->tf1.translation(), this->model2_bv, this->model1->getBV(b1).bv); +} + +//============================================================================== +template +void MeshShapeDistanceTraversalNodeOBBRSS::leafTesting(int b1, int b2) const +{ + details::meshShapeDistanceOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices, + this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->request, *(this->result)); +} + +/// @cond IGNORE +namespace details +{ + +template class OrientedNode> +static inline bool setupMeshShapeDistanceOrientedNode(OrientedNode& node, + const BVHModel& model1, const Transform3& tf1, + const Shape& model2, const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const DistanceRequest& request, + DistanceResult& result) +{ + if(model1.getModelType() != BVH_MODEL_TRIANGLES) + return false; + + node.request = request; + node.result = &result; + + node.model1 = &model1; + node.tf1 = tf1; + node.model2 = &model2; + node.tf2 = tf2; + node.nsolver = nsolver; + + computeBV(model2, tf2, node.model2_bv); + + node.vertices = model1.vertices; + node.tri_indices = model1.tri_indices; + + return true; +} + +} // namespace details +/// @endcond + +//============================================================================== +template +bool initialize( + MeshShapeDistanceTraversalNodeRSS& node, + const BVHModel>& model1, const Transform3& tf1, + const Shape& model2, const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const DistanceRequest& request, + DistanceResult& result) +{ + return details::setupMeshShapeDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result); +} + +//============================================================================== +template +bool initialize( + MeshShapeDistanceTraversalNodekIOS& node, + const BVHModel>& model1, const Transform3& tf1, + const Shape& model2, const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const DistanceRequest& request, + DistanceResult& result) +{ + return details::setupMeshShapeDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result); +} + +//============================================================================== +template +bool initialize( + MeshShapeDistanceTraversalNodeOBBRSS& node, + const BVHModel>& model1, const Transform3& tf1, + const Shape& model2, const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const DistanceRequest& request, + DistanceResult& result) +{ + return details::setupMeshShapeDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result); +} + +} // namespace fcl + +#endif diff --git a/include/fcl/traversal/distance/shape_bvh_distance_traversal_node.h b/include/fcl/traversal/distance/shape_bvh_distance_traversal_node.h new file mode 100644 index 000000000..1997e0e27 --- /dev/null +++ b/include/fcl/traversal/distance/shape_bvh_distance_traversal_node.h @@ -0,0 +1,130 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_TRAVERSAL_SHAPEBVHDISTANCETRAVERSALNODE_H +#define FCL_TRAVERSAL_SHAPEBVHDISTANCETRAVERSALNODE_H + +#include "fcl/traversal/traversal_node_base.h" +#include "fcl/traversal/distance/distance_traversal_node_base.h" +#include "fcl/BVH/BVH_model.h" + +namespace fcl +{ + +/// @brief Traversal node for distance computation between shape and BVH +template +class ShapeBVHDistanceTraversalNode + : public DistanceTraversalNodeBase +{ +public: + + using S = typename BV::S; + + ShapeBVHDistanceTraversalNode(); + + /// @brief Whether the BV node in the second BVH tree is leaf + bool isSecondNodeLeaf(int b) const; + + /// @brief Obtain the left child of BV node in the second BVH + int getSecondLeftChild(int b) const; + + /// @brief Obtain the right child of BV node in the second BVH + int getSecondRightChild(int b) const; + + /// @brief BV culling test in one BVTT node + S BVTesting(int b1, int b2) const; + + const Shape* model1; + const BVHModel* model2; + BV model1_bv; + + mutable int num_bv_tests; + mutable int num_leaf_tests; + mutable S query_time_seconds; +}; + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +ShapeBVHDistanceTraversalNode::ShapeBVHDistanceTraversalNode() + : DistanceTraversalNodeBase() +{ + model1 = nullptr; + model2 = nullptr; + + num_bv_tests = 0; + num_leaf_tests = 0; + query_time_seconds = 0.0; +} + +//============================================================================== +template +bool ShapeBVHDistanceTraversalNode::isSecondNodeLeaf(int b) const +{ + return model2->getBV(b).isLeaf(); +} + +//============================================================================== +template +int ShapeBVHDistanceTraversalNode::getSecondLeftChild(int b) const +{ + return model2->getBV(b).leftChild(); +} + +//============================================================================== +template +int ShapeBVHDistanceTraversalNode::getSecondRightChild(int b) const +{ + return model2->getBV(b).rightChild(); +} + +//============================================================================== +template +typename BV::S +ShapeBVHDistanceTraversalNode::BVTesting(int b1, int b2) const +{ + return model1_bv.distance(model2->getBV(b2).bv); +} + +} // namespace fcl + +#endif diff --git a/include/fcl/traversal/distance/shape_conservative_advancement_traversal_node.h b/include/fcl/traversal/distance/shape_conservative_advancement_traversal_node.h new file mode 100644 index 000000000..071ed771c --- /dev/null +++ b/include/fcl/traversal/distance/shape_conservative_advancement_traversal_node.h @@ -0,0 +1,161 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_TRAVERSAL_SHAPECONSERVATIVEADVANCEMENTTRAVERSALNODE_H +#define FCL_TRAVERSAL_SHAPECONSERVATIVEADVANCEMENTTRAVERSALNODE_H + +#include "fcl/traversal/distance/shape_distance_traversal_node.h" + +namespace fcl +{ + +template +class ShapeConservativeAdvancementTraversalNode + : public ShapeDistanceTraversalNode +{ +public: + using S = typename Shape1::S; + + ShapeConservativeAdvancementTraversalNode(); + + void leafTesting(int, int) const; + + mutable S min_distance; + + /// @brief The time from beginning point + S toc; + S t_err; + + /// @brief The delta_t each step + mutable S delta_t; + + /// @brief Motions for the two objects in query + const MotionBase* motion1; + const MotionBase* motion2; + + RSS model1_bv, model2_bv; // local bv for the two shapes +}; + +template +bool initialize( + ShapeConservativeAdvancementTraversalNode& node, + const Shape1& shape1, + const Transform3& tf1, + const Shape2& shape2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver); + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +ShapeConservativeAdvancementTraversalNode:: +ShapeConservativeAdvancementTraversalNode() + : ShapeDistanceTraversalNode() +{ + delta_t = 1; + toc = 0; + t_err = (S)0.0001; + + motion1 = nullptr; + motion2 = nullptr; +} + +//============================================================================== +template +void ShapeConservativeAdvancementTraversalNode:: +leafTesting(int, int) const +{ + S distance; + // NOTE(JS): The closest points are set to zeros in order to suppress the + // maybe-uninitialized warning. It seems the warnings occur since + // NarrowPhaseSolver::shapeDistance() conditionally set the closest points. + // If this wasn't intentional then please remove the initialization of the + // closest points, and change the function NarrowPhaseSolver::shapeDistance() + // to always set the closest points. + Vector3 closest_p1 = Vector3::Zero(); + Vector3 closest_p2 = Vector3::Zero(); + this->nsolver->shapeDistance(*(this->model1), this->tf1, *(this->model2), this->tf2, &distance, &closest_p1, &closest_p2); + + Vector3 n = this->tf2 * closest_p2 - this->tf1 * closest_p1; + n.normalize(); + TBVMotionBoundVisitor> mb_visitor1(model1_bv, n); + TBVMotionBoundVisitor> mb_visitor2(model2_bv, -n); + S bound1 = motion1->computeMotionBound(mb_visitor1); + S bound2 = motion2->computeMotionBound(mb_visitor2); + + S bound = bound1 + bound2; + + S cur_delta_t; + if(bound <= distance) cur_delta_t = 1; + else cur_delta_t = distance / bound; + + if(cur_delta_t < delta_t) + delta_t = cur_delta_t; +} + +//============================================================================== +template +bool initialize( + ShapeConservativeAdvancementTraversalNode& node, + const Shape1& shape1, + const Transform3& tf1, + const Shape2& shape2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver) +{ + using S = typename Shape1::S; + + node.model1 = &shape1; + node.tf1 = tf1; + node.model2 = &shape2; + node.tf2 = tf2; + node.nsolver = nsolver; + + computeBV(shape1, Transform3::Identity(), node.model1_bv); + computeBV(shape2, Transform3::Identity(), node.model2_bv); + + return true; +} + +} // namespace fcl + +#endif diff --git a/include/fcl/traversal/distance/shape_distance_traversal_node.h b/include/fcl/traversal/distance/shape_distance_traversal_node.h new file mode 100644 index 000000000..2607c2b33 --- /dev/null +++ b/include/fcl/traversal/distance/shape_distance_traversal_node.h @@ -0,0 +1,162 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_TRAVERSAL_SHAPEDISTANCETRAVERSALNODE_H +#define FCL_TRAVERSAL_SHAPEDISTANCETRAVERSALNODE_H + +#include "fcl/traversal/traversal_node_base.h" +#include "fcl/traversal/distance/distance_traversal_node_base.h" + +namespace fcl +{ + +/// @brief Traversal node for distance between two shapes +template +class ShapeDistanceTraversalNode + : public DistanceTraversalNodeBase +{ +public: + using S = typename Shape1::S; + + ShapeDistanceTraversalNode(); + + /// @brief BV culling test in one BVTT node + S BVTesting(int, int) const; + + /// @brief Distance testing between leaves (two shapes) + void leafTesting(int, int) const; + + const Shape1* model1; + const Shape2* model2; + + const NarrowPhaseSolver* nsolver; +}; + +/// @brief Initialize traversal node for distance between two geometric shapes +template +bool initialize( + ShapeDistanceTraversalNode& node, + const Shape1& shape1, + const Transform3& tf1, + const Shape2& shape2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const DistanceRequest& request, + DistanceResult& result); + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +ShapeDistanceTraversalNode:: +ShapeDistanceTraversalNode() : DistanceTraversalNodeBase() +{ + model1 = nullptr; + model2 = nullptr; + + nsolver = nullptr; +} + +//============================================================================== +template +typename Shape1::S +ShapeDistanceTraversalNode::BVTesting(int, int) const +{ + return -1; // should not be used +} + +//============================================================================== +template +void ShapeDistanceTraversalNode::leafTesting( + int, int) const +{ + using S = typename Shape1::S; + + S distance; + // NOTE(JS): The closest points are set to zeros in order to suppress the + // maybe-uninitialized warning. It seems the warnings occur since + // NarrowPhaseSolver::shapeDistance() conditionally set the closest points. + // If this wasn't intentional then please remove the initialization of the + // closest points, and change the function NarrowPhaseSolver::shapeDistance() + // to always set the closest points. + Vector3 closest_p1 = Vector3::Zero(); + Vector3 closest_p2 = Vector3::Zero(); + + nsolver->shapeDistance( + *model1, this->tf1, *model2, this->tf2, &distance, &closest_p1, &closest_p2); + + this->result->update( + distance, + model1, + model2, + DistanceResult::NONE, + DistanceResult::NONE, + closest_p1, + closest_p2); +} + +//============================================================================== +template +bool initialize( + ShapeDistanceTraversalNode& node, + const Shape1& shape1, + const Transform3& tf1, + const Shape2& shape2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const DistanceRequest& request, + DistanceResult& result) +{ + node.request = request; + node.result = &result; + + node.model1 = &shape1; + node.tf1 = tf1; + node.model2 = &shape2; + node.tf2 = tf2; + node.nsolver = nsolver; + + return true; +} + +} // namespace fcl + +#endif diff --git a/include/fcl/traversal/distance/shape_mesh_conservative_advancement_traversal_node.h b/include/fcl/traversal/distance/shape_mesh_conservative_advancement_traversal_node.h new file mode 100644 index 000000000..b52351fd9 --- /dev/null +++ b/include/fcl/traversal/distance/shape_mesh_conservative_advancement_traversal_node.h @@ -0,0 +1,525 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_TRAVERSAL_SHAPEMESHCONSERVATIVEADVANCEMENTTRAVERSALNODE_H +#define FCL_TRAVERSAL_SHAPEMESHCONSERVATIVEADVANCEMENTTRAVERSALNODE_H + +#include "fcl/shape/compute_bv.h" +#include "fcl/traversal/distance/shape_mesh_distance_traversal_node.h" +#include "fcl/traversal/distance/conservative_advancement_stack_data.h" + +namespace fcl +{ + +template +class ShapeMeshConservativeAdvancementTraversalNode + : public ShapeMeshDistanceTraversalNode +{ +public: + + using S = typename BV::S; + + ShapeMeshConservativeAdvancementTraversalNode(S w_ = 1); + + /// @brief BV culling test in one BVTT node + S BVTesting(int b1, int b2) const; + + /// @brief Conservative advancement testing between leaves (one triangle and one shape) + void leafTesting(int b1, int b2) const; + + /// @brief Whether the traversal process can stop early + bool canStop(S c) const; + + mutable S min_distance; + + mutable Vector3 closest_p1, closest_p2; + + mutable int last_tri_id; + + /// @brief CA controlling variable: early stop for the early iterations of CA + S w; + + /// @brief The time from beginning point + S toc; + S t_err; + + /// @brief The delta_t each step + mutable S delta_t; + + /// @brief Motions for the two objects in query + const MotionBased* motion1; + const MotionBased* motion2; + + mutable std::vector> stack; +}; + +template +bool initialize( + ShapeMeshConservativeAdvancementTraversalNode& node, + const Shape& model1, + const Transform3& tf1, + BVHModel& model2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + typename BV::S w = 1, + bool use_refit = false, + bool refit_bottomup = false); + +template +class ShapeMeshConservativeAdvancementTraversalNodeRSS + : public ShapeMeshConservativeAdvancementTraversalNode< + Shape, RSS, NarrowPhaseSolver> +{ +public: + using S = typename Shape::S; + + ShapeMeshConservativeAdvancementTraversalNodeRSS(S w_ = 1); + + S BVTesting(int b1, int b2) const; + + void leafTesting(int b1, int b2) const; + + bool canStop(S c) const; +}; + +template +bool initialize( + ShapeMeshConservativeAdvancementTraversalNodeRSS& node, + const Shape& model1, + const Transform3& tf1, + const BVHModel>& model2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + typename Shape::S w = 1); + +template +class ShapeMeshConservativeAdvancementTraversalNodeOBBRSS + : public ShapeMeshConservativeAdvancementTraversalNode< + Shape, OBBRSS, NarrowPhaseSolver> +{ +public: + using S = typename Shape::S; + + ShapeMeshConservativeAdvancementTraversalNodeOBBRSS(S w_ = 1); + + S BVTesting(int b1, int b2) const; + + void leafTesting(int b1, int b2) const; + + bool canStop(S c) const; +}; + +template +bool initialize( + ShapeMeshConservativeAdvancementTraversalNodeOBBRSS& node, + const Shape& model1, + const Transform3& tf1, + const BVHModel>& model2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + typename Shape::S w = 1); + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +ShapeMeshConservativeAdvancementTraversalNode:: +ShapeMeshConservativeAdvancementTraversalNode(S w_) + : ShapeMeshDistanceTraversalNode() +{ + delta_t = 1; + toc = 0; + t_err = (S)0.0001; + + w = w_; + + motion1 = nullptr; + motion2 = nullptr; +} + +//============================================================================== +template +typename BV::S +ShapeMeshConservativeAdvancementTraversalNode:: +BVTesting(int b1, int b2) const +{ + if(this->enable_statistics) this->num_bv_tests++; + Vector3 P1, P2; + S d = this->model1_bv.distance(this->model2->getBV(b2).bv, &P1, &P2); + + stack.emplace_back(P1, P2, b1, b2, d); + + return d; +} + +//============================================================================== +template +void ShapeMeshConservativeAdvancementTraversalNode::leafTesting(int b1, int b2) const +{ + if(this->enable_statistics) this->num_leaf_tests++; + + const BVNode& node = this->model2->getBV(b2); + + int primitive_id = node.primitiveId(); + + const Triangle& tri_id = this->tri_indices[primitive_id]; + + const Vector3& p1 = this->vertices[tri_id[0]]; + const Vector3& p2 = this->vertices[tri_id[1]]; + const Vector3& p3 = this->vertices[tri_id[2]]; + + S d; + Vector3 P1, P2; + this->nsolver->shapeTriangleDistance(*(this->model1), this->tf1, p1, p2, p3, &d, &P1, &P2); + + if(d < this->min_distance) + { + this->min_distance = d; + + closest_p1 = P1; + closest_p2 = P2; + + last_tri_id = primitive_id; + } + + Vector3 n = P2 - this->tf1 * p1; n.normalize(); + // here n should be in global frame + TBVMotionBoundVisitor mb_visitor1(this->model1_bv, n); + TriangleMotionBoundVisitor mb_visitor2(p1, p2, p3, -n); + S bound1 = motion1->computeMotionBound(mb_visitor1); + S bound2 = motion2->computeMotionBound(mb_visitor2); + + S bound = bound1 + bound2; + + S cur_delta_t; + if(bound <= d) cur_delta_t = 1; + else cur_delta_t = d / bound; + + if(cur_delta_t < delta_t) + delta_t = cur_delta_t; +} + +//============================================================================== +template +bool ShapeMeshConservativeAdvancementTraversalNode:: +canStop(S c) const +{ + if((c >= w * (this->min_distance - this->abs_err)) && (c * (1 + this->rel_err) >= w * this->min_distance)) + { + const auto& data = stack.back(); + + Vector3 n = data.P2 - this->tf1 * data.P1; n.normalize(); + int c2 = data.c2; + + TBVMotionBoundVisitor mb_visitor1(this->model1_bv, n); + TBVMotionBoundVisitor mb_visitor2(this->model2->getBV(c2).bv, -n); + S bound1 = motion1->computeMotionBound(mb_visitor1); + S bound2 = motion2->computeMotionBound(mb_visitor2); + + S bound = bound1 + bound2; + + S cur_delta_t; + if(bound < c) cur_delta_t = 1; + else cur_delta_t = c / bound; + + if(cur_delta_t < delta_t) + delta_t = cur_delta_t; + + stack.pop_back(); + + return true; + } + else + { + stack.pop_back(); + + return false; + } +} + +//============================================================================== +template +bool initialize( + ShapeMeshConservativeAdvancementTraversalNode& node, + const Shape& model1, + const Transform3& tf1, + BVHModel& model2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + typename BV::S w, + bool use_refit, + bool refit_bottomup) +{ + using S = typename BV::S; + + std::vector> vertices_transformed(model2.num_vertices); + for(int i = 0; i < model2.num_vertices; ++i) + { + Vector3& p = model2.vertices[i]; + Vector3 new_v = tf2 * p; + vertices_transformed[i] = new_v; + } + + model2.beginReplaceModel(); + model2.replaceSubModel(vertices_transformed); + model2.endReplaceModel(use_refit, refit_bottomup); + + node.model1 = &model1; + node.model2 = &model2; + + node.vertices = model2.vertices; + node.tri_indices = model2.tri_indices; + + node.tf1 = tf1; + node.tf2 = tf2; + + node.nsolver = nsolver; + node.w = w; + + computeBV(model1, Transform3::Identity(), node.model1_bv); + + return true; +} + +//============================================================================== +template +ShapeMeshConservativeAdvancementTraversalNodeRSS:: +ShapeMeshConservativeAdvancementTraversalNodeRSS( + typename Shape::S w_) + : ShapeMeshConservativeAdvancementTraversalNode< + Shape, RSS, NarrowPhaseSolver>(w_) +{ + // Do nothing +} + +//============================================================================== +template +typename Shape::S +ShapeMeshConservativeAdvancementTraversalNodeRSS:: +BVTesting(int b1, int b2) const +{ + using S = typename Shape::S; + + if(this->enable_statistics) this->num_bv_tests++; + Vector3 P1, P2; + S d = distance(this->tf2.linear(), this->tf2.translation(), this->model2->getBV(b2).bv, this->model1_bv, &P2, &P1); + + this->stack.emplace_back(P1, P2, b1, b2, d); + + return d; +} + +//============================================================================== +template +void ShapeMeshConservativeAdvancementTraversalNodeRSS:: +leafTesting(int b1, int b2) const +{ + details::meshShapeConservativeAdvancementOrientedNodeLeafTesting( + b2, + b1, + this->model2, + *(this->model1), + this->model1_bv, + this->vertices, + this->tri_indices, + this->tf2, + this->tf1, + this->motion2, + this->motion1, + this->nsolver, + this->enable_statistics, + this->min_distance, + this->closest_p2, + this->closest_p1, + this->last_tri_id, + this->delta_t, + this->num_leaf_tests); +} + +//============================================================================== +template +bool ShapeMeshConservativeAdvancementTraversalNodeRSS:: +canStop(typename Shape::S c) const +{ + return details::meshShapeConservativeAdvancementOrientedNodeCanStop( + c, + this->min_distance, + this->abs_err, + this->rel_err, + this->w, + this->model2, + *(this->model1), + this->model1_bv, + this->motion2, + this->motion1, + this->stack, + this->delta_t); +} + +//============================================================================== +template +bool initialize( + ShapeMeshConservativeAdvancementTraversalNodeRSS& node, + const Shape& model1, + const Transform3& tf1, + const BVHModel>& model2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + typename Shape::S w) +{ + using S = typename Shape::S; + + node.model1 = &model1; + node.tf1 = tf1; + node.model2 = &model2; + node.tf2 = tf2; + node.nsolver = nsolver; + + node.w = w; + + computeBV(model1, Transform3::Identity(), node.model1_bv); + + return true; +} + +//============================================================================== +template +ShapeMeshConservativeAdvancementTraversalNodeOBBRSS:: +ShapeMeshConservativeAdvancementTraversalNodeOBBRSS( + typename Shape::S w_) + : ShapeMeshConservativeAdvancementTraversalNode< + Shape, OBBRSS, NarrowPhaseSolver>(w_) +{ +} + +//============================================================================== +template +typename Shape::S +ShapeMeshConservativeAdvancementTraversalNodeOBBRSS:: +BVTesting(int b1, int b2) const +{ + using S = typename Shape::S; + + if(this->enable_statistics) this->num_bv_tests++; + Vector3 P1, P2; + S d = distance(this->tf2.linear(), this->tf2.translation(), this->model2->getBV(b2).bv, this->model1_bv, &P2, &P1); + + this->stack.emplace_back(P1, P2, b1, b2, d); + + return d; +} + +//============================================================================== +template +void ShapeMeshConservativeAdvancementTraversalNodeOBBRSS:: +leafTesting(int b1, int b2) const +{ + details::meshShapeConservativeAdvancementOrientedNodeLeafTesting( + b2, + b1, + this->model2, + *(this->model1), + this->model1_bv, + this->vertices, + this->tri_indices, + this->tf2, + this->tf1, + this->motion2, + this->motion1, + this->nsolver, + this->enable_statistics, + this->min_distance, + this->closest_p2, + this->closest_p1, + this->last_tri_id, + this->delta_t, + this->num_leaf_tests); +} + +//============================================================================== +template +bool ShapeMeshConservativeAdvancementTraversalNodeOBBRSS:: +canStop(typename Shape::S c) const +{ + return details::meshShapeConservativeAdvancementOrientedNodeCanStop( + c, + this->min_distance, + this->abs_err, + this->rel_err, + this->w, + this->model2, + *(this->model1), + this->model1_bv, + this->motion2, + this->motion1, + this->stack, + this->delta_t); +} + +//============================================================================== +template +bool initialize( + ShapeMeshConservativeAdvancementTraversalNodeOBBRSS& node, + const Shape& model1, + const Transform3& tf1, + const BVHModel>& model2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + typename Shape::S w) +{ + using S = typename Shape::S; + + node.model1 = &model1; + node.tf1 = tf1; + node.model2 = &model2; + node.tf2 = tf2; + node.nsolver = nsolver; + + node.w = w; + + computeBV(model1, Transform3::Identity(), node.model1_bv); + + return true; +} + +} // namespace fcl + +#endif diff --git a/include/fcl/traversal/distance/shape_mesh_distance_traversal_node.h b/include/fcl/traversal/distance/shape_mesh_distance_traversal_node.h new file mode 100644 index 000000000..413b84833 --- /dev/null +++ b/include/fcl/traversal/distance/shape_mesh_distance_traversal_node.h @@ -0,0 +1,528 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_TRAVERSAL_SHAPEMESHDISTANCETRAVERSALNODE_H +#define FCL_TRAVERSAL_SHAPEMESHDISTANCETRAVERSALNODE_H + +#include "fcl/traversal/distance/shape_bvh_distance_traversal_node.h" +#include "fcl/BVH/BVH_model.h" + +namespace fcl +{ + +/// @brief Traversal node for distance between shape and mesh +template +class ShapeMeshDistanceTraversalNode + : public ShapeBVHDistanceTraversalNode +{ +public: + + using S = typename BV::S; + + ShapeMeshDistanceTraversalNode(); + + /// @brief Distance testing between leaves (one shape and one triangle) + void leafTesting(int b1, int b2) const; + + /// @brief Whether the traversal process can stop early + bool canStop(S c) const; + + Vector3* vertices; + Triangle* tri_indices; + + S rel_err; + S abs_err; + + const NarrowPhaseSolver* nsolver; +}; + +/// @brief Initialize traversal node for distance computation between one shape +/// and one mesh, given the current transforms +template +bool initialize( + ShapeMeshDistanceTraversalNode& node, + const Shape& model1, + const Transform3& tf1, + BVHModel& model2, + Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const DistanceRequest& request, + DistanceResult& result, + bool use_refit = false, + bool refit_bottomup = false); + +template +class ShapeMeshDistanceTraversalNodeRSS + : public ShapeMeshDistanceTraversalNode< + Shape, RSS, NarrowPhaseSolver> +{ +public: + + using S = typename Shape::S; + + ShapeMeshDistanceTraversalNodeRSS(); + + void preprocess(); + + void postprocess(); + + S BVTesting(int b1, int b2) const; + + void leafTesting(int b1, int b2) const; + +}; + +/// @brief Initialize traversal node for distance computation between one shape +/// and one mesh, specialized for RSS type +template +bool initialize( + ShapeMeshDistanceTraversalNodeRSS& node, + const Shape& model1, + const Transform3& tf1, + const BVHModel>& model2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const DistanceRequest& request, + DistanceResult& result); + +template +class ShapeMeshDistanceTraversalNodekIOS + : public ShapeMeshDistanceTraversalNode< + Shape, kIOS, NarrowPhaseSolver> +{ +public: + + using S = typename Shape::S; + + ShapeMeshDistanceTraversalNodekIOS(); + + void preprocess(); + + void postprocess(); + + S BVTesting(int b1, int b2) const; + + void leafTesting(int b1, int b2) const; + +}; + +/// @brief Initialize traversal node for distance computation between one shape +/// and one mesh, specialized for kIOS type +template +bool initialize( + ShapeMeshDistanceTraversalNodekIOS& node, + const Shape& model1, + const Transform3& tf1, + const BVHModel>& model2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const DistanceRequest& request, + DistanceResult& result); + +template +class ShapeMeshDistanceTraversalNodeOBBRSS + : public ShapeMeshDistanceTraversalNode< + Shape, OBBRSS, NarrowPhaseSolver> +{ +public: + + using S = typename Shape::S; + + ShapeMeshDistanceTraversalNodeOBBRSS(); + + void preprocess(); + + void postprocess(); + + S BVTesting(int b1, int b2) const; + + void leafTesting(int b1, int b2) const; + +}; + +/// @brief Initialize traversal node for distance computation between one shape +/// and one mesh, specialized for OBBRSS type +template +bool initialize( + ShapeMeshDistanceTraversalNodeOBBRSS& node, + const Shape& model1, + const Transform3& tf1, + const BVHModel>& model2, + const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const DistanceRequest& request, + DistanceResult& result); + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +ShapeMeshDistanceTraversalNode:: +ShapeMeshDistanceTraversalNode() + : ShapeBVHDistanceTraversalNode() +{ + vertices = nullptr; + tri_indices = nullptr; + + rel_err = 0; + abs_err = 0; + + nsolver = nullptr; +} + +//============================================================================== +template +void ShapeMeshDistanceTraversalNode::leafTesting(int b1, int b2) const +{ + using S = typename BV::S; + + if(this->enable_statistics) this->num_leaf_tests++; + + const BVNode& node = this->model2->getBV(b2); + + int primitive_id = node.primitiveId(); + + const Triangle& tri_id = tri_indices[primitive_id]; + + const Vector3& p1 = vertices[tri_id[0]]; + const Vector3& p2 = vertices[tri_id[1]]; + const Vector3& p3 = vertices[tri_id[2]]; + + S distance; + Vector3 closest_p1, closest_p2; + nsolver->shapeTriangleDistance(*(this->model1), this->tf1, p1, p2, p3, &distance, &closest_p1, &closest_p2); + + this->result->update( + distance, + this->model1, + this->model2, + DistanceResult::NONE, + primitive_id, + closest_p1, + closest_p2); +} + +//============================================================================== +template +bool ShapeMeshDistanceTraversalNode::canStop(S c) const +{ + if((c >= this->result->min_distance - abs_err) && (c * (1 + rel_err) >= this->result->min_distance)) + return true; + return false; +} + +//============================================================================== +template +bool initialize( + ShapeMeshDistanceTraversalNode& node, + const Shape& model1, + const Transform3& tf1, + BVHModel& model2, + Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const DistanceRequest& request, + DistanceResult& result, + bool use_refit, + bool refit_bottomup) +{ + using S = typename BV::S; + + if(model2.getModelType() != BVH_MODEL_TRIANGLES) + return false; + + if(!tf2.matrix().isIdentity()) + { + std::vector> vertices_transformed(model2.num_vertices); + for(int i = 0; i < model2.num_vertices; ++i) + { + Vector3& p = model2.vertices[i]; + Vector3 new_v = tf2 * p; + vertices_transformed[i] = new_v; + } + + model2.beginReplaceModel(); + model2.replaceSubModel(vertices_transformed); + model2.endReplaceModel(use_refit, refit_bottomup); + + tf2.setIdentity(); + } + + node.request = request; + node.result = &result; + + node.model1 = &model1; + node.tf1 = tf1; + node.model2 = &model2; + node.tf2 = tf2; + node.nsolver = nsolver; + + node.vertices = model2.vertices; + node.tri_indices = model2.tri_indices; + + computeBV(model1, tf1, node.model1_bv); + + return true; +} + +//============================================================================== +template +ShapeMeshDistanceTraversalNodeRSS::ShapeMeshDistanceTraversalNodeRSS() + : ShapeMeshDistanceTraversalNode, NarrowPhaseSolver>() +{ +} + +//============================================================================== +template +void ShapeMeshDistanceTraversalNodeRSS::preprocess() +{ + details::distancePreprocessOrientedNode( + this->model2, this->vertices, this->tri_indices, 0, + *(this->model1), this->tf2, this->tf1, this->nsolver, this->request, *(this->result)); +} + +//============================================================================== +template +void ShapeMeshDistanceTraversalNodeRSS::postprocess() +{ +} + +//============================================================================== +template +typename Shape::S +ShapeMeshDistanceTraversalNodeRSS:: +BVTesting(int b1, int b2) const +{ + if(this->enable_statistics) this->num_bv_tests++; + return distance(this->tf2.linear(), this->tf2.translation(), this->model1_bv, this->model2->getBV(b2).bv); +} + +//============================================================================== +template +void ShapeMeshDistanceTraversalNodeRSS:: +leafTesting(int b1, int b2) const +{ + details::meshShapeDistanceOrientedNodeLeafTesting(b2, b1, this->model2, *(this->model1), this->vertices, this->tri_indices, + this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->request, *(this->result)); +} + +//============================================================================== +template +ShapeMeshDistanceTraversalNodekIOS:: +ShapeMeshDistanceTraversalNodekIOS() + : ShapeMeshDistanceTraversalNode, NarrowPhaseSolver>() +{ +} + +//============================================================================== +template +void ShapeMeshDistanceTraversalNodekIOS::preprocess() +{ + details::distancePreprocessOrientedNode( + this->model2, + this->vertices, + this->tri_indices, + 0, + *(this->model1), + this->tf2, + this->tf1, + this->nsolver, + *(this->result)); +} + +//============================================================================== +template +void ShapeMeshDistanceTraversalNodekIOS::postprocess() +{ +} + +//============================================================================== +template +typename Shape::S +ShapeMeshDistanceTraversalNodekIOS:: +BVTesting(int b1, int b2) const +{ + if(this->enable_statistics) this->num_bv_tests++; + return distance(this->tf2.linear(), this->tf2.translation(), this->model1_bv, this->model2->getBV(b2).bv); +} + +//============================================================================== +template +void ShapeMeshDistanceTraversalNodekIOS:: +leafTesting(int b1, int b2) const +{ + details::meshShapeDistanceOrientedNodeLeafTesting(b2, b1, this->model2, *(this->model1), this->vertices, this->tri_indices, + this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->request, *(this->result)); +} + +//============================================================================== +template +ShapeMeshDistanceTraversalNodeOBBRSS:: +ShapeMeshDistanceTraversalNodeOBBRSS() : ShapeMeshDistanceTraversalNode, NarrowPhaseSolver>() +{ +} + +//============================================================================== +template +void ShapeMeshDistanceTraversalNodeOBBRSS::preprocess() +{ + details::distancePreprocessOrientedNode( + this->model2, + this->vertices, + this->tri_indices, + 0, + *(this->model1), + this->tf2, + this->tf1, + this->nsolver, + *(this->result)); +} + +//============================================================================== +template +void ShapeMeshDistanceTraversalNodeOBBRSS:: +postprocess() +{ +} + +//============================================================================== +template +typename Shape::S +ShapeMeshDistanceTraversalNodeOBBRSS:: +BVTesting(int b1, int b2) const +{ + if(this->enable_statistics) this->num_bv_tests++; + return distance(this->tf2.linear(), this->tf2.translation(), this->model1_bv, this->model2->getBV(b2).bv); +} + +//============================================================================== +template +void ShapeMeshDistanceTraversalNodeOBBRSS:: +leafTesting(int b1, int b2) const +{ + details::meshShapeDistanceOrientedNodeLeafTesting( + b2, + b1, + this->model2, + *(this->model1), + this->vertices, + this->tri_indices, + this->tf2, + this->tf1, + this->nsolver, + this->enable_statistics, + this->num_leaf_tests, + this->request, + *(this->result)); +} + +namespace details +{ +template class OrientedNode> +static inline bool setupShapeMeshDistanceOrientedNode(OrientedNode& node, + const Shape& model1, const Transform3& tf1, + const BVHModel& model2, const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const DistanceRequest& request, + DistanceResult& result) +{ + if(model2.getModelType() != BVH_MODEL_TRIANGLES) + return false; + + node.request = request; + node.result = &result; + + node.model1 = &model1; + node.tf1 = tf1; + node.model2 = &model2; + node.tf2 = tf2; + node.nsolver = nsolver; + + computeBV(model1, tf1, node.model1_bv); + + node.vertices = model2.vertices; + node.tri_indices = model2.tri_indices; + node.R = tf2.linear(); + node.T = tf2.translation(); + + return true; +} +} + + +//============================================================================== +template +bool initialize(ShapeMeshDistanceTraversalNodeRSS& node, + const Shape& model1, const Transform3& tf1, + const BVHModel>& model2, const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const DistanceRequest& request, + DistanceResult& result) +{ + return details::setupShapeMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result); +} + +//============================================================================== +template +bool initialize(ShapeMeshDistanceTraversalNodekIOS& node, + const Shape& model1, const Transform3& tf1, + const BVHModel>& model2, const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const DistanceRequest& request, + DistanceResult& result) +{ + return details::setupShapeMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result); +} + +//============================================================================== +template +bool initialize(ShapeMeshDistanceTraversalNodeOBBRSS& node, + const Shape& model1, const Transform3& tf1, + const BVHModel>& model2, const Transform3& tf2, + const NarrowPhaseSolver* nsolver, + const DistanceRequest& request, + DistanceResult& result) +{ + return details::setupShapeMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result); +} + +} // namespace fcl + +#endif diff --git a/include/fcl/traversal/octree/collision/mesh_octree_collision_traversal_node.h b/include/fcl/traversal/octree/collision/mesh_octree_collision_traversal_node.h new file mode 100644 index 000000000..82874ea6c --- /dev/null +++ b/include/fcl/traversal/octree/collision/mesh_octree_collision_traversal_node.h @@ -0,0 +1,154 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_TRAVERSAL_OCTREE_MESHOCTREECOLLISIONTRAVERSALNODE_H +#define FCL_TRAVERSAL_OCTREE_MESHOCTREECOLLISIONTRAVERSALNODE_H + +#include "fcl/config.h" +#if not(FCL_HAVE_OCTOMAP) +#error "This header requires fcl to be compiled with octomap support" +#endif + +#include "fcl/octree.h" +#include "fcl/BVH/BVH_model.h" +#include "fcl/traversal/collision/collision_traversal_node_base.h" +#include "fcl/traversal/octree/octree_solver.h" + +namespace fcl +{ + +/// @brief Traversal node for mesh-octree collision +template +class MeshOcTreeCollisionTraversalNode + : public CollisionTraversalNodeBase +{ +public: + + using S = typename BV::S; + + MeshOcTreeCollisionTraversalNode(); + + bool BVTesting(int, int) const; + + void leafTesting(int, int) const; + + const BVHModel* model1; + const OcTree* model2; + + Transform3 tf1, tf2; + + const OcTreeSolver* otsolver; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +/// @brief Initialize traversal node for collision between one mesh and one +/// octree, given current object transform +template +bool initialize( + MeshOcTreeCollisionTraversalNode& node, + const BVHModel& model1, + const Transform3& tf1, + const OcTree& model2, + const Transform3& tf2, + const OcTreeSolver* otsolver, + const CollisionRequest& request, + CollisionResult& result); + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +MeshOcTreeCollisionTraversalNode:: +MeshOcTreeCollisionTraversalNode() +{ + model1 = nullptr; + model2 = nullptr; + + otsolver = nullptr; +} + +//============================================================================== +template +bool MeshOcTreeCollisionTraversalNode:: +BVTesting(int, int) const +{ + return false; +} + +//============================================================================== +template +void MeshOcTreeCollisionTraversalNode:: +leafTesting(int, int) const +{ + otsolver->OcTreeMeshIntersect( + model2, model1, tf2, tf1, this->request, *this->result); +} + +//============================================================================== +template +bool initialize( + MeshOcTreeCollisionTraversalNode& node, + const BVHModel& model1, + const Transform3& tf1, + const OcTree& model2, + const Transform3& tf2, + const OcTreeSolver* otsolver, + const CollisionRequest& request, + CollisionResult& result) +{ + node.request = request; + node.result = &result; + + node.model1 = &model1; + node.model2 = &model2; + + node.otsolver = otsolver; + + node.tf1 = tf1; + node.tf2 = tf2; + + return true; +} + +} // namespace fcl + +#endif diff --git a/include/fcl/traversal/octree/collision/octree_collision_traversal_node.h b/include/fcl/traversal/octree/collision/octree_collision_traversal_node.h new file mode 100644 index 000000000..f3c5eeb6a --- /dev/null +++ b/include/fcl/traversal/octree/collision/octree_collision_traversal_node.h @@ -0,0 +1,152 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_TRAVERSAL_OCTREE_OCTREECOLLISIONTRAVERSALNODE_H +#define FCL_TRAVERSAL_OCTREE_OCTREECOLLISIONTRAVERSALNODE_H + +#include "fcl/config.h" +#if not(FCL_HAVE_OCTOMAP) +#error "This header requires fcl to be compiled with octomap support" +#endif + +#include "fcl/octree.h" +#include "fcl/traversal/collision/collision_traversal_node_base.h" +#include "fcl/traversal/octree/octree_solver.h" + +namespace fcl +{ + +/// @brief Traversal node for octree collision +template +class OcTreeCollisionTraversalNode + : public CollisionTraversalNodeBase +{ +public: + + using S = typename NarrowPhaseSolver::S; + + OcTreeCollisionTraversalNode(); + + bool BVTesting(int, int) const; + + void leafTesting(int, int) const; + + const OcTree* model1; + const OcTree* model2; + + Transform3 tf1, tf2; + + const OcTreeSolver* otsolver; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +/// @brief Initialize traversal node for collision between two octrees, given +/// current object transform +template +bool initialize( + OcTreeCollisionTraversalNode& node, + const OcTree& model1, + const Transform3& tf1, + const OcTree& model2, + const Transform3& tf2, + const OcTreeSolver* otsolver, + const CollisionRequest& request, + CollisionResult& result); + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +OcTreeCollisionTraversalNode::OcTreeCollisionTraversalNode() +{ + model1 = nullptr; + model2 = nullptr; + + otsolver = nullptr; +} + +//============================================================================== +template +bool OcTreeCollisionTraversalNode::BVTesting( + int, int) const +{ + return false; +} + +//============================================================================== +template +void OcTreeCollisionTraversalNode::leafTesting( + int, int) const +{ + otsolver->OcTreeIntersect( + model1, model2, tf1, tf2, this->request, *this->result); +} + +//============================================================================== +template +bool initialize( + OcTreeCollisionTraversalNode& node, + const OcTree& model1, + const Transform3& tf1, + const OcTree& model2, + const Transform3& tf2, + const OcTreeSolver* otsolver, + const CollisionRequest& request, + CollisionResult& result) +{ + node.request = request; + node.result = &result; + + node.model1 = &model1; + node.model2 = &model2; + + node.otsolver = otsolver; + + node.tf1 = tf1; + node.tf2 = tf2; + + return true; +} + +} // namespace fcl + +#endif diff --git a/include/fcl/traversal/octree/collision/octree_mesh_collision_traversal_node.h b/include/fcl/traversal/octree/collision/octree_mesh_collision_traversal_node.h new file mode 100644 index 000000000..96052b2d2 --- /dev/null +++ b/include/fcl/traversal/octree/collision/octree_mesh_collision_traversal_node.h @@ -0,0 +1,154 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_TRAVERSAL_OCTREE_OCTREEMESHCOLLISIONTRAVERSALNODE_H +#define FCL_TRAVERSAL_OCTREE_OCTREEMESHCOLLISIONTRAVERSALNODE_H + +#include "fcl/config.h" +#if not(FCL_HAVE_OCTOMAP) +#error "This header requires fcl to be compiled with octomap support" +#endif + +#include "fcl/octree.h" +#include "fcl/BVH/BVH_model.h" +#include "fcl/traversal/collision/collision_traversal_node_base.h" +#include "fcl/traversal/octree/octree_solver.h" + +namespace fcl +{ + +/// @brief Traversal node for octree-mesh collision +template +class OcTreeMeshCollisionTraversalNode + : public CollisionTraversalNodeBase +{ +public: + + using S = typename BV::S; + + OcTreeMeshCollisionTraversalNode(); + + bool BVTesting(int, int) const; + + void leafTesting(int, int) const; + + const OcTree* model1; + const BVHModel* model2; + + Transform3 tf1, tf2; + + const OcTreeSolver* otsolver; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +/// @brief Initialize traversal node for collision between one octree and one +/// mesh, given current object transform +template +bool initialize( + OcTreeMeshCollisionTraversalNode& node, + const OcTree& model1, + const Transform3& tf1, + const BVHModel& model2, + const Transform3& tf2, + const OcTreeSolver* otsolver, + const CollisionRequest& request, + CollisionResult& result); + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +OcTreeMeshCollisionTraversalNode:: +OcTreeMeshCollisionTraversalNode() +{ + model1 = nullptr; + model2 = nullptr; + + otsolver = nullptr; +} + +//============================================================================== +template +bool OcTreeMeshCollisionTraversalNode:: +BVTesting(int, int) const +{ + return false; +} + +//============================================================================== +template +void OcTreeMeshCollisionTraversalNode:: +leafTesting(int, int) const +{ + otsolver->OcTreeMeshIntersect( + model1, model2, tf1, tf2, this->request, *this->result); +} + +//============================================================================== +template +bool initialize( + OcTreeMeshCollisionTraversalNode& node, + const OcTree& model1, + const Transform3& tf1, + const BVHModel& model2, + const Transform3& tf2, + const OcTreeSolver* otsolver, + const CollisionRequest& request, + CollisionResult& result) +{ + node.request = request; + node.result = &result; + + node.model1 = &model1; + node.model2 = &model2; + + node.otsolver = otsolver; + + node.tf1 = tf1; + node.tf2 = tf2; + + return true; +} + +} // namespace fcl + +#endif diff --git a/include/fcl/traversal/octree/collision/octree_shape_collision_traversal_node.h b/include/fcl/traversal/octree/collision/octree_shape_collision_traversal_node.h new file mode 100644 index 000000000..9b9008a39 --- /dev/null +++ b/include/fcl/traversal/octree/collision/octree_shape_collision_traversal_node.h @@ -0,0 +1,154 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_TRAVERSAL_OCTREE_OCTREESHAPECOLLISIONTRAVERSALNODE_H +#define FCL_TRAVERSAL_OCTREE_OCTREESHAPECOLLISIONTRAVERSALNODE_H + +#include "fcl/config.h" +#if not(FCL_HAVE_OCTOMAP) +#error "This header requires fcl to be compiled with octomap support" +#endif + +#include "fcl/octree.h" +#include "fcl/BVH/BVH_model.h" +#include "fcl/traversal/collision/collision_traversal_node_base.h" +#include "fcl/traversal/octree/octree_solver.h" + +namespace fcl +{ + +/// @brief Traversal node for octree-shape collision +template +class OcTreeShapeCollisionTraversalNode + : public CollisionTraversalNodeBase +{ +public: + + using S = typename Shape::S; + + OcTreeShapeCollisionTraversalNode(); + + bool BVTesting(int, int) const; + + void leafTesting(int, int) const; + + const OcTree* model1; + const Shape* model2; + + Transform3 tf1, tf2; + + const OcTreeSolver* otsolver; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +/// @brief Initialize traversal node for collision between one octree and one +/// shape, given current object transform +template +bool initialize( + OcTreeShapeCollisionTraversalNode& node, + const OcTree& model1, + const Transform3& tf1, + const Shape& model2, + const Transform3& tf2, + const OcTreeSolver* otsolver, + const CollisionRequest& request, + CollisionResult& result); + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +OcTreeShapeCollisionTraversalNode:: +OcTreeShapeCollisionTraversalNode() +{ + model1 = nullptr; + model2 = nullptr; + + otsolver = nullptr; +} + +//============================================================================== +template +bool OcTreeShapeCollisionTraversalNode:: +BVTesting(int, int) const +{ + return false; +} + +//============================================================================== +template +void OcTreeShapeCollisionTraversalNode:: +leafTesting(int, int) const +{ + otsolver->OcTreeShapeIntersect( + model1, *model2, tf1, tf2, this->request, *this->result); +} + +//============================================================================== +template +bool initialize( + OcTreeShapeCollisionTraversalNode& node, + const OcTree& model1, + const Transform3& tf1, + const Shape& model2, + const Transform3& tf2, + const OcTreeSolver* otsolver, + const CollisionRequest& request, + CollisionResult& result) +{ + node.request = request; + node.result = &result; + + node.model1 = &model1; + node.model2 = &model2; + + node.otsolver = otsolver; + + node.tf1 = tf1; + node.tf2 = tf2; + + return true; +} + +} // namespace fcl + +#endif diff --git a/include/fcl/traversal/octree/collision/shape_octree_collision_traversal_node.h b/include/fcl/traversal/octree/collision/shape_octree_collision_traversal_node.h new file mode 100644 index 000000000..0a4902e77 --- /dev/null +++ b/include/fcl/traversal/octree/collision/shape_octree_collision_traversal_node.h @@ -0,0 +1,154 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_TRAVERSAL_OCTREE_SHAPEOCTREECOLLISIONTRAVERSALNODE_H +#define FCL_TRAVERSAL_OCTREE_SHAPEOCTREECOLLISIONTRAVERSALNODE_H + +#include "fcl/config.h" +#if not(FCL_HAVE_OCTOMAP) +#error "This header requires fcl to be compiled with octomap support" +#endif + +#include "fcl/octree.h" +#include "fcl/BVH/BVH_model.h" +#include "fcl/traversal/collision/collision_traversal_node_base.h" +#include "fcl/traversal/octree/octree_solver.h" + +namespace fcl +{ + +/// @brief Traversal node for shape-octree collision +template +class ShapeOcTreeCollisionTraversalNode + : public CollisionTraversalNodeBase +{ +public: + + using S = typename Shape::S; + + ShapeOcTreeCollisionTraversalNode(); + + bool BVTesting(int, int) const; + + void leafTesting(int, int) const; + + const Shape* model1; + const OcTree* model2; + + Transform3 tf1, tf2; + + const OcTreeSolver* otsolver; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +/// @brief Initialize traversal node for collision between one shape and one +/// octree, given current object transform +template +bool initialize( + ShapeOcTreeCollisionTraversalNode& node, + const Shape& model1, + const Transform3& tf1, + const OcTree& model2, + const Transform3& tf2, + const OcTreeSolver* otsolver, + const CollisionRequest& request, + CollisionResult& result); + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +ShapeOcTreeCollisionTraversalNode:: +ShapeOcTreeCollisionTraversalNode() +{ + model1 = nullptr; + model2 = nullptr; + + otsolver = nullptr; +} + +//============================================================================== +template +bool ShapeOcTreeCollisionTraversalNode:: +BVTesting(int, int) const +{ + return false; +} + +//============================================================================== +template +void ShapeOcTreeCollisionTraversalNode:: +leafTesting(int, int) const +{ + otsolver->OcTreeShapeIntersect( + model2, *model1, tf2, tf1, this->request, *this->result); +} + +//============================================================================== +template +bool initialize( + ShapeOcTreeCollisionTraversalNode& node, + const Shape& model1, + const Transform3& tf1, + const OcTree& model2, + const Transform3& tf2, + const OcTreeSolver* otsolver, + const CollisionRequest& request, + CollisionResult& result) +{ + node.request = request; + node.result = &result; + + node.model1 = &model1; + node.model2 = &model2; + + node.otsolver = otsolver; + + node.tf1 = tf1; + node.tf2 = tf2; + + return true; +} + +} // namespace fcl + +#endif diff --git a/include/fcl/traversal/octree/distance/mesh_octree_distance_traversal_node.h b/include/fcl/traversal/octree/distance/mesh_octree_distance_traversal_node.h new file mode 100644 index 000000000..3e5b53433 --- /dev/null +++ b/include/fcl/traversal/octree/distance/mesh_octree_distance_traversal_node.h @@ -0,0 +1,151 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_TRAVERSAL_OCTREE_MESHOCTREEDISTANCETRAVERSALNODE_H +#define FCL_TRAVERSAL_OCTREE_MESHOCTREEDISTANCETRAVERSALNODE_H + +#include "fcl/config.h" +#if not(FCL_HAVE_OCTOMAP) +#error "This header requires fcl to be compiled with octomap support" +#endif + +#include "fcl/octree.h" +#include "fcl/BVH/BVH_model.h" +#include "fcl/traversal/distance/distance_traversal_node_base.h" +#include "fcl/traversal/octree/octree_solver.h" + +namespace fcl +{ + +/// @brief Traversal node for mesh-octree distance +template +class MeshOcTreeDistanceTraversalNode + : public DistanceTraversalNodeBase +{ +public: + + using S = typename BV::S; + + MeshOcTreeDistanceTraversalNode(); + + S BVTesting(int, int) const; + + void leafTesting(int, int) const; + + const BVHModel* model1; + const OcTree* model2; + + const OcTreeSolver* otsolver; + +}; + +/// @brief Initialize traversal node for distance between one mesh and one +/// octree, given current object transform +template +bool initialize( + MeshOcTreeDistanceTraversalNode& node, + const BVHModel& model1, + const Transform3& tf1, + const OcTree& model2, + const Transform3& tf2, + const OcTreeSolver* otsolver, + const DistanceRequest& request, + DistanceResult& result); + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +MeshOcTreeDistanceTraversalNode:: +MeshOcTreeDistanceTraversalNode() +{ + model1 = nullptr; + model2 = nullptr; + + otsolver = nullptr; +} + +//============================================================================== +template +typename BV::S MeshOcTreeDistanceTraversalNode:: +BVTesting(int, int) const +{ + return -1; +} + +//============================================================================== +template +void MeshOcTreeDistanceTraversalNode:: +leafTesting(int, int) const +{ + otsolver->OcTreeMeshDistance( + model2, model1, this->tf2, this->tf1, this->request, *this->result); +} + +//============================================================================== +template +bool initialize( + MeshOcTreeDistanceTraversalNode& node, + const BVHModel& model1, + const Transform3& tf1, + const OcTree& model2, + const Transform3& tf2, + const OcTreeSolver* otsolver, + const DistanceRequest& request, + DistanceResult& result) +{ + node.request = request; + node.result = &result; + + node.model1 = &model1; + node.model2 = &model2; + + node.otsolver = otsolver; + + node.tf1 = tf1; + node.tf2 = tf2; + + return true; +} + +} // namespace fcl + +#endif diff --git a/include/fcl/traversal/octree/distance/octree_distance_traversal_node.h b/include/fcl/traversal/octree/distance/octree_distance_traversal_node.h new file mode 100644 index 000000000..e78b4fc1c --- /dev/null +++ b/include/fcl/traversal/octree/distance/octree_distance_traversal_node.h @@ -0,0 +1,150 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_TRAVERSAL_OCTREE_OCTREEDISTANCETRAVERSALNODE_H +#define FCL_TRAVERSAL_OCTREE_OCTREEDISTANCETRAVERSALNODE_H + +#include "fcl/config.h" +#if not(FCL_HAVE_OCTOMAP) +#error "This header requires fcl to be compiled with octomap support" +#endif + +#include "fcl/octree.h" +#include "fcl/traversal/distance/distance_traversal_node_base.h" +#include "fcl/traversal/octree/octree_solver.h" + +namespace fcl +{ + +/// @brief Traversal node for octree distance +template +class OcTreeDistanceTraversalNode + : public DistanceTraversalNodeBase +{ +public: + + using S = typename NarrowPhaseSolver::S; + + OcTreeDistanceTraversalNode(); + + S BVTesting(int, int) const; + + void leafTesting(int, int) const; + + const OcTree* model1; + const OcTree* model2; + + const OcTreeSolver* otsolver; +}; + +/// @brief Initialize traversal node for distance between two octrees, given +/// current object transform +template +bool initialize( + OcTreeDistanceTraversalNode& node, + const OcTree& model1, + const Transform3& tf1, + const OcTree& model2, + const Transform3& tf2, + const OcTreeSolver* otsolver, + const DistanceRequest& request, + DistanceResult& result); + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +OcTreeDistanceTraversalNode:: +OcTreeDistanceTraversalNode() +{ + model1 = nullptr; + model2 = nullptr; + + otsolver = nullptr; +} + +//============================================================================== +template +typename NarrowPhaseSolver::S +OcTreeDistanceTraversalNode:: +BVTesting(int, int) const +{ + return -1; +} + +//============================================================================== +template +void OcTreeDistanceTraversalNode:: +leafTesting(int, int) const +{ + otsolver->OcTreeDistance( + model1, model2, this->tf1, this->tf2, this->request, *this->result); +} + +//============================================================================== +template +bool initialize( + OcTreeDistanceTraversalNode& node, + const OcTree& model1, + const Transform3& tf1, + const OcTree& model2, + const Transform3& tf2, + const OcTreeSolver* otsolver, + const DistanceRequest& request, + DistanceResult& result) +{ + node.request = request; + node.result = &result; + + node.model1 = &model1; + node.model2 = &model2; + + node.otsolver = otsolver; + + node.tf1 = tf1; + node.tf2 = tf2; + + return true; +} + +} // namespace fcl + +#endif diff --git a/include/fcl/traversal/octree/distance/octree_mesh_distance_traversal_node.h b/include/fcl/traversal/octree/distance/octree_mesh_distance_traversal_node.h new file mode 100644 index 000000000..7f5ab768a --- /dev/null +++ b/include/fcl/traversal/octree/distance/octree_mesh_distance_traversal_node.h @@ -0,0 +1,151 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_TRAVERSAL_OCTREE_OCTREEMESHDISTANCETRAVERSALNODE_H +#define FCL_TRAVERSAL_OCTREE_OCTREEMESHDISTANCETRAVERSALNODE_H + +#include "fcl/config.h" +#if not(FCL_HAVE_OCTOMAP) +#error "This header requires fcl to be compiled with octomap support" +#endif + +#include "fcl/octree.h" +#include "fcl/BVH/BVH_model.h" +#include "fcl/traversal/distance/distance_traversal_node_base.h" +#include "fcl/traversal/octree/octree_solver.h" + +namespace fcl +{ + +/// @brief Traversal node for octree-mesh distance +template +class OcTreeMeshDistanceTraversalNode + : public DistanceTraversalNodeBase +{ +public: + + using S = typename BV::S; + + OcTreeMeshDistanceTraversalNode(); + + S BVTesting(int, int) const; + + void leafTesting(int, int) const; + + const OcTree* model1; + const BVHModel* model2; + + const OcTreeSolver* otsolver; + +}; + +/// @brief Initialize traversal node for collision between one octree and one +/// mesh, given current object transform +template +bool initialize( + OcTreeMeshDistanceTraversalNode& node, + const OcTree& model1, + const Transform3& tf1, + const BVHModel& model2, + const Transform3& tf2, + const OcTreeSolver* otsolver, + const DistanceRequest& request, + DistanceResult& result); + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +OcTreeMeshDistanceTraversalNode:: +OcTreeMeshDistanceTraversalNode() +{ + model1 = nullptr; + model2 = nullptr; + + otsolver = nullptr; +} + +//============================================================================== +template +typename BV::S OcTreeMeshDistanceTraversalNode:: +BVTesting(int, int) const +{ + return -1; +} + +//============================================================================== +template +void OcTreeMeshDistanceTraversalNode:: +leafTesting(int, int) const +{ + otsolver->OcTreeMeshDistance( + model1, model2, this->tf1, this->tf2, this->request, *this->result); +} + +//============================================================================== +template +bool initialize( + OcTreeMeshDistanceTraversalNode& node, + const OcTree& model1, + const Transform3& tf1, + const BVHModel& model2, + const Transform3& tf2, + const OcTreeSolver* otsolver, + const DistanceRequest& request, + DistanceResult& result) +{ + node.request = request; + node.result = &result; + + node.model1 = &model1; + node.model2 = &model2; + + node.otsolver = otsolver; + + node.tf1 = tf1; + node.tf2 = tf2; + + return true; +} + +} // namespace fcl + +#endif diff --git a/include/fcl/traversal/octree/distance/octree_shape_distance_traversal_node.h b/include/fcl/traversal/octree/distance/octree_shape_distance_traversal_node.h new file mode 100644 index 000000000..17231ee70 --- /dev/null +++ b/include/fcl/traversal/octree/distance/octree_shape_distance_traversal_node.h @@ -0,0 +1,151 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_TRAVERSAL_OCTREE_OCTREESHAPEDISTANCETRAVERSALNODE_H +#define FCL_TRAVERSAL_OCTREE_OCTREESHAPEDISTANCETRAVERSALNODE_H + +#include "fcl/config.h" +#if not(FCL_HAVE_OCTOMAP) +#error "This header requires fcl to be compiled with octomap support" +#endif + +#include "fcl/octree.h" +#include "fcl/BVH/BVH_model.h" +#include "fcl/traversal/distance/distance_traversal_node_base.h" +#include "fcl/traversal/octree/octree_solver.h" + +namespace fcl +{ + +/// @brief Traversal node for octree-shape distance +template +class OcTreeShapeDistanceTraversalNode + : public DistanceTraversalNodeBase +{ +public: + + using S = typename Shape::S; + + OcTreeShapeDistanceTraversalNode(); + + S BVTesting(int, int) const; + + void leafTesting(int, int) const; + + const OcTree* model1; + const Shape* model2; + + const OcTreeSolver* otsolver; +}; + +/// @brief Initialize traversal node for distance between one octree and one +/// shape, given current object transform +template +bool initialize( + OcTreeShapeDistanceTraversalNode& node, + const OcTree& model1, + const Transform3& tf1, + const Shape& model2, + const Transform3& tf2, + const OcTreeSolver* otsolver, + const DistanceRequest& request, + DistanceResult& result); + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +OcTreeShapeDistanceTraversalNode:: +OcTreeShapeDistanceTraversalNode() +{ + model1 = nullptr; + model2 = nullptr; + + otsolver = nullptr; +} + +//============================================================================== +template +typename Shape::S +OcTreeShapeDistanceTraversalNode:: +BVTesting(int, int) const +{ + return -1; +} + +//============================================================================== +template +void OcTreeShapeDistanceTraversalNode:: +leafTesting(int, int) const +{ + otsolver->OcTreeShapeDistance( + model1, *model2, this->tf1, this->tf2, this->request, *this->result); +} + +//============================================================================== +template +bool initialize( + OcTreeShapeDistanceTraversalNode& node, + const OcTree& model1, + const Transform3& tf1, + const Shape& model2, + const Transform3& tf2, + const OcTreeSolver* otsolver, + const DistanceRequest& request, + DistanceResult& result) +{ + node.request = request; + node.result = &result; + + node.model1 = &model1; + node.model2 = &model2; + + node.otsolver = otsolver; + + node.tf1 = tf1; + node.tf2 = tf2; + + return true; +} + +} // namespace fcl + +#endif diff --git a/include/fcl/traversal/octree/distance/shape_octree_distance_traversal_node.h b/include/fcl/traversal/octree/distance/shape_octree_distance_traversal_node.h new file mode 100644 index 000000000..6a9713090 --- /dev/null +++ b/include/fcl/traversal/octree/distance/shape_octree_distance_traversal_node.h @@ -0,0 +1,151 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_TRAVERSAL_OCTREE_SHAPEOCTREEDISTANCETRAVERSALNODE_H +#define FCL_TRAVERSAL_OCTREE_SHAPEOCTREEDISTANCETRAVERSALNODE_H + +#include "fcl/config.h" +#if not(FCL_HAVE_OCTOMAP) +#error "This header requires fcl to be compiled with octomap support" +#endif + +#include "fcl/octree.h" +#include "fcl/BVH/BVH_model.h" +#include "fcl/traversal/distance/distance_traversal_node_base.h" +#include "fcl/traversal/octree/octree_solver.h" + +namespace fcl +{ + +/// @brief Traversal node for shape-octree distance +template +class ShapeOcTreeDistanceTraversalNode + : public DistanceTraversalNodeBase +{ +public: + + using S = typename Shape::S; + + ShapeOcTreeDistanceTraversalNode(); + + S BVTesting(int, int) const; + + void leafTesting(int, int) const; + + const Shape* model1; + const OcTree* model2; + + const OcTreeSolver* otsolver; +}; + +/// @brief Initialize traversal node for distance between one shape and one +/// octree, given current object transform +template +bool initialize( + ShapeOcTreeDistanceTraversalNode& node, + const Shape& model1, + const Transform3& tf1, + const OcTree& model2, + const Transform3& tf2, + const OcTreeSolver* otsolver, + const DistanceRequest& request, + DistanceResult& result); + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +ShapeOcTreeDistanceTraversalNode:: +ShapeOcTreeDistanceTraversalNode() +{ + model1 = nullptr; + model2 = nullptr; + + otsolver = nullptr; +} + +//============================================================================== +template +typename Shape::S +ShapeOcTreeDistanceTraversalNode:: +BVTesting(int, int) const +{ + return -1; +} + +//============================================================================== +template +void ShapeOcTreeDistanceTraversalNode:: +leafTesting(int, int) const +{ + otsolver->OcTreeShapeDistance( + model2, *model1, this->tf2, this->tf1, this->request, *this->result); +} + +//============================================================================== +template +bool initialize( + ShapeOcTreeDistanceTraversalNode& node, + const Shape& model1, + const Transform3& tf1, + const OcTree& model2, + const Transform3& tf2, + const OcTreeSolver* otsolver, + const DistanceRequest& request, + DistanceResult& result) +{ + node.request = request; + node.result = &result; + + node.model1 = &model1; + node.model2 = &model2; + + node.otsolver = otsolver; + + node.tf1 = tf1; + node.tf2 = tf2; + + return true; +} + +} // namespace fcl + +#endif diff --git a/include/fcl/traversal/octree/octree_solver.h b/include/fcl/traversal/octree/octree_solver.h new file mode 100644 index 000000000..934001c0b --- /dev/null +++ b/include/fcl/traversal/octree/octree_solver.h @@ -0,0 +1,1235 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef FCL_TRAVERSAL_OCTREE_OCTREESOLVER_H +#define FCL_TRAVERSAL_OCTREE_OCTREESOLVER_H + +#include "fcl/config.h" +#if not(FCL_HAVE_OCTOMAP) +#error "This header requires fcl to be compiled with octomap support" +#endif + +#include "fcl/collision_data.h" +#include "fcl/octree.h" +#include "fcl/shape/compute_bv.h" +#include "fcl/shape/box.h" + +namespace fcl +{ + +/// @brief Algorithms for collision related with octree +template +class OcTreeSolver +{ +private: + + using S = typename NarrowPhaseSolver::S; + + const NarrowPhaseSolver* solver; + + mutable const CollisionRequest* crequest; + mutable const DistanceRequest* drequest; + + mutable CollisionResult* cresult; + mutable DistanceResult* dresult; + +public: + OcTreeSolver(const NarrowPhaseSolver* solver_); + + /// @brief collision between two octrees + void OcTreeIntersect(const OcTree* tree1, const OcTree* tree2, + const Transform3& tf1, const Transform3& tf2, + const CollisionRequest& request_, + CollisionResult& result_) const; + + /// @brief distance between two octrees + void OcTreeDistance(const OcTree* tree1, const OcTree* tree2, + const Transform3& tf1, const Transform3& tf2, + const DistanceRequest& request_, + DistanceResult& result_) const; + + /// @brief collision between octree and mesh + template + void OcTreeMeshIntersect(const OcTree* tree1, const BVHModel* tree2, + const Transform3& tf1, const Transform3& tf2, + const CollisionRequest& request_, + CollisionResult& result_) const; + + /// @brief distance between octree and mesh + template + void OcTreeMeshDistance(const OcTree* tree1, const BVHModel* tree2, + const Transform3& tf1, const Transform3& tf2, + const DistanceRequest& request_, + DistanceResult& result_) const; + + /// @brief collision between mesh and octree + template + void MeshOcTreeIntersect(const BVHModel* tree1, const OcTree* tree2, + const Transform3& tf1, const Transform3& tf2, + const CollisionRequest& request_, + CollisionResult& result_) const; + + /// @brief distance between mesh and octree + template + void MeshOcTreeDistance(const BVHModel* tree1, const OcTree* tree2, + const Transform3& tf1, const Transform3& tf2, + const DistanceRequest& request_, + DistanceResult& result_) const; + + /// @brief collision between octree and shape + template + void OcTreeShapeIntersect(const OcTree* tree, const Shape& s, + const Transform3& tf1, const Transform3& tf2, + const CollisionRequest& request_, + CollisionResult& result_) const; + + /// @brief collision between shape and octree + template + void ShapeOcTreeIntersect(const Shape& s, const OcTree* tree, + const Transform3& tf1, const Transform3& tf2, + const CollisionRequest& request_, + CollisionResult& result_) const; + + /// @brief distance between octree and shape + template + void OcTreeShapeDistance(const OcTree* tree, const Shape& s, + const Transform3& tf1, const Transform3& tf2, + const DistanceRequest& request_, + DistanceResult& result_) const; + + /// @brief distance between shape and octree + template + void ShapeOcTreeDistance(const Shape& s, const OcTree* tree, + const Transform3& tf1, const Transform3& tf2, + const DistanceRequest& request_, + DistanceResult& result_) const; + +private: + + template + bool OcTreeShapeDistanceRecurse(const OcTree* tree1, const typename OcTree::OcTreeNode* root1, const AABB& bv1, + const Shape& s, const AABB& aabb2, + const Transform3& tf1, const Transform3& tf2) const; + + template + bool OcTreeShapeIntersectRecurse(const OcTree* tree1, const typename OcTree::OcTreeNode* root1, const AABB& bv1, + const Shape& s, const OBB& obb2, + const Transform3& tf1, const Transform3& tf2) const; + + template + bool OcTreeMeshDistanceRecurse(const OcTree* tree1, const typename OcTree::OcTreeNode* root1, const AABB& bv1, + const BVHModel* tree2, int root2, + const Transform3& tf1, const Transform3& tf2) const; + + + template + bool OcTreeMeshIntersectRecurse(const OcTree* tree1, const typename OcTree::OcTreeNode* root1, const AABB& bv1, + const BVHModel* tree2, int root2, + const Transform3& tf1, const Transform3& tf2) const; + + bool OcTreeDistanceRecurse(const OcTree* tree1, const typename OcTree::OcTreeNode* root1, const AABB& bv1, + const OcTree* tree2, const typename OcTree::OcTreeNode* root2, const AABB& bv2, + const Transform3& tf1, const Transform3& tf2) const; + + + bool OcTreeIntersectRecurse(const OcTree* tree1, const typename OcTree::OcTreeNode* root1, const AABB& bv1, + const OcTree* tree2, const typename OcTree::OcTreeNode* root2, const AABB& bv2, + const Transform3& tf1, const Transform3& tf2) const; +}; + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +OcTreeSolver::OcTreeSolver( + const NarrowPhaseSolver* solver_) + : solver(solver_), + crequest(nullptr), + drequest(nullptr), + cresult(nullptr), + dresult(nullptr) +{ +} + +//============================================================================== +template +void OcTreeSolver::OcTreeIntersect( + const OcTree* tree1, + const OcTree* tree2, + const Transform3& tf1, + const Transform3& tf2, + const CollisionRequest& request_, + CollisionResult& result_) const +{ + crequest = &request_; + cresult = &result_; + + OcTreeIntersectRecurse(tree1, tree1->getRoot(), tree1->getRootBV(), + tree2, tree2->getRoot(), tree2->getRootBV(), + tf1, tf2); +} + +//============================================================================== +template +void OcTreeSolver::OcTreeDistance( + const OcTree* tree1, + const OcTree* tree2, + const Transform3& tf1, + const Transform3& tf2, + const DistanceRequest& request_, + DistanceResult& result_) const +{ + drequest = &request_; + dresult = &result_; + + OcTreeDistanceRecurse(tree1, tree1->getRoot(), tree1->getRootBV(), + tree2, tree2->getRoot(), tree2->getRootBV(), + tf1, tf2); +} + +//============================================================================== +template +template +void OcTreeSolver::OcTreeMeshIntersect( + const OcTree* tree1, + const BVHModel* tree2, + const Transform3& tf1, + const Transform3& tf2, + const CollisionRequest& request_, + CollisionResult& result_) const +{ + crequest = &request_; + cresult = &result_; + + OcTreeMeshIntersectRecurse(tree1, tree1->getRoot(), tree1->getRootBV(), + tree2, 0, + tf1, tf2); +} + +//============================================================================== +template +template +void OcTreeSolver::OcTreeMeshDistance( + const OcTree* tree1, + const BVHModel* tree2, + const Transform3& tf1, + const Transform3& tf2, + const DistanceRequest& request_, + DistanceResult& result_) const +{ + drequest = &request_; + dresult = &result_; + + OcTreeMeshDistanceRecurse(tree1, tree1->getRoot(), tree1->getRootBV(), + tree2, 0, + tf1, tf2); +} + +//============================================================================== +template +template +void OcTreeSolver::MeshOcTreeIntersect( + const BVHModel* tree1, + const OcTree* tree2, + const Transform3& tf1, + const Transform3& tf2, + const CollisionRequest& request_, + CollisionResult& result_) const + +{ + crequest = &request_; + cresult = &result_; + + OcTreeMeshIntersectRecurse(tree2, tree2->getRoot(), tree2->getRootBV(), + tree1, 0, + tf2, tf1); +} + +//============================================================================== +/// @brief distance between mesh and octree +template +template +void OcTreeSolver::MeshOcTreeDistance( + const BVHModel* tree1, + const OcTree* tree2, + const Transform3& tf1, + const Transform3& tf2, + const DistanceRequest& request_, + DistanceResult& result_) const +{ + drequest = &request_; + dresult = &result_; + + OcTreeMeshDistanceRecurse(tree1, 0, + tree2, tree2->getRoot(), tree2->getRootBV(), + tf1, tf2); +} + +//============================================================================== +/// @brief collision between octree and shape +template +template +void OcTreeSolver::OcTreeShapeIntersect( + const OcTree* tree, + const Shape& s, + const Transform3& tf1, + const Transform3& tf2, + const CollisionRequest& request_, + CollisionResult& result_) const +{ + crequest = &request_; + cresult = &result_; + + AABB bv2; + computeBV(s, Transform3::Identity(), bv2); + OBB obb2; + convertBV(bv2, tf2, obb2); + OcTreeShapeIntersectRecurse(tree, tree->getRoot(), tree->getRootBV(), + s, obb2, + tf1, tf2); + +} + +//============================================================================== +/// @brief collision between shape and octree +template +template +void OcTreeSolver::ShapeOcTreeIntersect( + const Shape& s, + const OcTree* tree, + const Transform3& tf1, + const Transform3& tf2, + const CollisionRequest& request_, + CollisionResult& result_) const +{ + crequest = &request_; + cresult = &result_; + + AABB bv1; + computeBV(s, Transform3::Identity(), bv1); + OBB obb1; + convertBV(bv1, tf1, obb1); + OcTreeShapeIntersectRecurse(tree, tree->getRoot(), tree->getRootBV(), + s, obb1, + tf2, tf1); +} + +//============================================================================== +/// @brief distance between octree and shape +template +template +void OcTreeSolver::OcTreeShapeDistance( + const OcTree* tree, + const Shape& s, + const Transform3& tf1, + const Transform3& tf2, + const DistanceRequest& request_, + DistanceResult& result_) const +{ + drequest = &request_; + dresult = &result_; + + AABB aabb2; + computeBV(s, tf2, aabb2); + OcTreeShapeDistanceRecurse(tree, tree->getRoot(), tree->getRootBV(), + s, aabb2, + tf1, tf2); +} + +//============================================================================== +/// @brief distance between shape and octree +template +template +void OcTreeSolver::ShapeOcTreeDistance( + const Shape& s, + const OcTree* tree, + const Transform3& tf1, + const Transform3& tf2, + const DistanceRequest& request_, + DistanceResult& result_) const +{ + drequest = &request_; + dresult = &result_; + + AABB aabb1; + computeBV(s, tf1, aabb1); + OcTreeShapeDistanceRecurse(tree, tree->getRoot(), tree->getRootBV(), + s, aabb1, + tf2, tf1); +} + +//============================================================================== +template +template +bool OcTreeSolver::OcTreeShapeDistanceRecurse(const OcTree* tree1, const typename OcTree::OcTreeNode* root1, const AABB& bv1, + const Shape& s, const AABB& aabb2, + const Transform3& tf1, const Transform3& tf2) const +{ + if(!tree1->nodeHasChildren(root1)) + { + if(tree1->isNodeOccupied(root1)) + { + Box box; + Transform3 box_tf; + constructBox(bv1, tf1, box, box_tf); + + S dist; + // NOTE(JS): The closest points are set to zeros in order to suppress the + // maybe-uninitialized warning. It seems the warnings occur since + // NarrowPhaseSolver::shapeDistance() conditionally set the closest points. + // If this wasn't intentional then please remove the initialization of the + // closest points, and change the function NarrowPhaseSolver::shapeDistance() + // to always set the closest points. + Vector3 closest_p1 = Vector3::Zero(); + Vector3 closest_p2 = Vector3::Zero(); + solver->shapeDistance(box, box_tf, s, tf2, &dist, &closest_p1, &closest_p2); + + dresult->update(dist, tree1, &s, root1 - tree1->getRoot(), DistanceResult::NONE, closest_p1, closest_p2); + + return drequest->isSatisfied(*dresult); + } + else + return false; + } + + if(!tree1->isNodeOccupied(root1)) return false; + + for(unsigned int i = 0; i < 8; ++i) + { + if(tree1->nodeChildExists(root1, i)) + { + const typename OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); + AABB child_bv; + computeChildBV(bv1, i, child_bv); + + AABB aabb1; + convertBV(child_bv, tf1, aabb1); + S d = aabb1.distance(aabb2); + if(d < dresult->min_distance) + { + if(OcTreeShapeDistanceRecurse(tree1, child, child_bv, s, aabb2, tf1, tf2)) + return true; + } + } + } + + return false; +} + +//============================================================================== +template +template +bool OcTreeSolver::OcTreeShapeIntersectRecurse(const OcTree* tree1, const typename OcTree::OcTreeNode* root1, const AABB& bv1, + const Shape& s, const OBB& obb2, + const Transform3& tf1, const Transform3& tf2) const +{ + if(!root1) + { + OBB obb1; + convertBV(bv1, tf1, obb1); + if(obb1.overlap(obb2)) + { + Box box; + Transform3 box_tf; + constructBox(bv1, tf1, box, box_tf); + + if(solver->shapeIntersect(box, box_tf, s, tf2, nullptr)) + { + AABB overlap_part; + AABB aabb1, aabb2; + computeBV(box, box_tf, aabb1); + computeBV(s, tf2, aabb2); + aabb1.overlap(aabb2, overlap_part); + cresult->addCostSource(CostSource(overlap_part, tree1->getOccupancyThres() * s.cost_density), crequest->num_max_cost_sources); + } + } + + return false; + } + else if(!tree1->nodeHasChildren(root1)) + { + if(tree1->isNodeOccupied(root1) && s.isOccupied()) // occupied area + { + OBB obb1; + convertBV(bv1, tf1, obb1); + if(obb1.overlap(obb2)) + { + Box box; + Transform3 box_tf; + constructBox(bv1, tf1, box, box_tf); + + bool is_intersect = false; + if(!crequest->enable_contact) + { + if(solver->shapeIntersect(box, box_tf, s, tf2, nullptr)) + { + is_intersect = true; + if(cresult->numContacts() < crequest->num_max_contacts) + cresult->addContact(Contact(tree1, &s, root1 - tree1->getRoot(), Contact::NONE)); + } + } + else + { + std::vector> contacts; + if(solver->shapeIntersect(box, box_tf, s, tf2, &contacts)) + { + is_intersect = true; + if(crequest->num_max_contacts > cresult->numContacts()) + { + const size_t free_space = crequest->num_max_contacts - cresult->numContacts(); + size_t num_adding_contacts; + + // If the free space is not enough to add all the new contacts, we add contacts in descent order of penetration depth. + if (free_space < contacts.size()) + { + std::partial_sort(contacts.begin(), contacts.begin() + free_space, contacts.end(), std::bind(comparePenDepth, std::placeholders::_2, std::placeholders::_1)); + num_adding_contacts = free_space; + } + else + { + num_adding_contacts = contacts.size(); + } + + for(size_t i = 0; i < num_adding_contacts; ++i) + cresult->addContact(Contact(tree1, &s, root1 - tree1->getRoot(), Contact::NONE, contacts[i].pos, contacts[i].normal, contacts[i].penetration_depth)); + } + } + } + + if(is_intersect && crequest->enable_cost) + { + AABB overlap_part; + AABB aabb1, aabb2; + computeBV(box, box_tf, aabb1); + computeBV(s, tf2, aabb2); + aabb1.overlap(aabb2, overlap_part); + } + + return crequest->isSatisfied(*cresult); + } + else return false; + } + else if(!tree1->isNodeFree(root1) && !s.isFree() && crequest->enable_cost) // uncertain area + { + OBB obb1; + convertBV(bv1, tf1, obb1); + if(obb1.overlap(obb2)) + { + Box box; + Transform3 box_tf; + constructBox(bv1, tf1, box, box_tf); + + if(solver->shapeIntersect(box, box_tf, s, tf2, nullptr)) + { + AABB overlap_part; + AABB aabb1, aabb2; + computeBV(box, box_tf, aabb1); + computeBV(s, tf2, aabb2); + aabb1.overlap(aabb2, overlap_part); + } + } + + return false; + } + else // free area + return false; + } + + /// stop when 1) bounding boxes of two objects not overlap; OR + /// 2) at least of one the nodes is free; OR + /// 2) (two uncertain nodes or one node occupied and one node uncertain) AND cost not required + if(tree1->isNodeFree(root1) || s.isFree()) return false; + else if((tree1->isNodeUncertain(root1) || s.isUncertain()) && !crequest->enable_cost) return false; + else + { + OBB obb1; + convertBV(bv1, tf1, obb1); + if(!obb1.overlap(obb2)) return false; + } + + for(unsigned int i = 0; i < 8; ++i) + { + if(tree1->nodeChildExists(root1, i)) + { + const typename OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); + AABB child_bv; + computeChildBV(bv1, i, child_bv); + + if(OcTreeShapeIntersectRecurse(tree1, child, child_bv, s, obb2, tf1, tf2)) + return true; + } + else if(!s.isFree() && crequest->enable_cost) + { + AABB child_bv; + computeChildBV(bv1, i, child_bv); + + if(OcTreeShapeIntersectRecurse(tree1, nullptr, child_bv, s, obb2, tf1, tf2)) + return true; + } + } + + return false; +} + +//============================================================================== +template +template +bool OcTreeSolver::OcTreeMeshDistanceRecurse(const OcTree* tree1, const typename OcTree::OcTreeNode* root1, const AABB& bv1, + const BVHModel* tree2, int root2, + const Transform3& tf1, const Transform3& tf2) const +{ + if(!tree1->nodeHasChildren(root1) && tree2->getBV(root2).isLeaf()) + { + if(tree1->isNodeOccupied(root1)) + { + Box box; + Transform3 box_tf; + constructBox(bv1, tf1, box, box_tf); + + int primitive_id = tree2->getBV(root2).primitiveId(); + const Triangle& tri_id = tree2->tri_indices[primitive_id]; + const Vector3& p1 = tree2->vertices[tri_id[0]]; + const Vector3& p2 = tree2->vertices[tri_id[1]]; + const Vector3& p3 = tree2->vertices[tri_id[2]]; + + S dist; + Vector3 closest_p1, closest_p2; + solver->shapeTriangleDistance(box, box_tf, p1, p2, p3, tf2, &dist, &closest_p1, &closest_p2); + + dresult->update(dist, tree1, tree2, root1 - tree1->getRoot(), primitive_id); + + return drequest->isSatisfied(*dresult); + } + else + return false; + } + + if(!tree1->isNodeOccupied(root1)) return false; + + if(tree2->getBV(root2).isLeaf() || (tree1->nodeHasChildren(root1) && (bv1.size() > tree2->getBV(root2).bv.size()))) + { + for(unsigned int i = 0; i < 8; ++i) + { + if(tree1->nodeChildExists(root1, i)) + { + const typename OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); + AABB child_bv; + computeChildBV(bv1, i, child_bv); + + S d; + AABB aabb1, aabb2; + convertBV(child_bv, tf1, aabb1); + convertBV(tree2->getBV(root2).bv, tf2, aabb2); + d = aabb1.distance(aabb2); + + if(d < dresult->min_distance) + { + if(OcTreeMeshDistanceRecurse(tree1, child, child_bv, tree2, root2, tf1, tf2)) + return true; + } + } + } + } + else + { + S d; + AABB aabb1, aabb2; + convertBV(bv1, tf1, aabb1); + int child = tree2->getBV(root2).leftChild(); + convertBV(tree2->getBV(child).bv, tf2, aabb2); + d = aabb1.distance(aabb2); + + if(d < dresult->min_distance) + { + if(OcTreeMeshDistanceRecurse(tree1, root1, bv1, tree2, child, tf1, tf2)) + return true; + } + + child = tree2->getBV(root2).rightChild(); + convertBV(tree2->getBV(child).bv, tf2, aabb2); + d = aabb1.distance(aabb2); + + if(d < dresult->min_distance) + { + if(OcTreeMeshDistanceRecurse(tree1, root1, bv1, tree2, child, tf1, tf2)) + return true; + } + } + + return false; +} + +//============================================================================== +template +template +bool OcTreeSolver::OcTreeMeshIntersectRecurse(const OcTree* tree1, const typename OcTree::OcTreeNode* root1, const AABB& bv1, + const BVHModel* tree2, int root2, + const Transform3& tf1, const Transform3& tf2) const +{ + if(!root1) + { + if(tree2->getBV(root2).isLeaf()) + { + OBB obb1, obb2; + convertBV(bv1, tf1, obb1); + convertBV(tree2->getBV(root2).bv, tf2, obb2); + if(obb1.overlap(obb2)) + { + Box box; + Transform3 box_tf; + constructBox(bv1, tf1, box, box_tf); + + int primitive_id = tree2->getBV(root2).primitiveId(); + const Triangle& tri_id = tree2->tri_indices[primitive_id]; + const Vector3& p1 = tree2->vertices[tri_id[0]]; + const Vector3& p2 = tree2->vertices[tri_id[1]]; + const Vector3& p3 = tree2->vertices[tri_id[2]]; + + if(solver->shapeTriangleIntersect(box, box_tf, p1, p2, p3, tf2, nullptr, nullptr, nullptr)) + { + AABB overlap_part; + AABB aabb1; + computeBV(box, box_tf, aabb1); + AABB aabb2(tf2 * p1, tf2 * p2, tf2 * p3); + aabb1.overlap(aabb2, overlap_part); + cresult->addCostSource(CostSource(overlap_part, tree1->getOccupancyThres() * tree2->cost_density), crequest->num_max_cost_sources); + } + } + + return false; + } + else + { + if(OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2, tree2->getBV(root2).leftChild(), tf1, tf2)) + return true; + + if(OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2, tree2->getBV(root2).rightChild(), tf1, tf2)) + return true; + + return false; + } + } + else if(!tree1->nodeHasChildren(root1) && tree2->getBV(root2).isLeaf()) + { + if(tree1->isNodeOccupied(root1) && tree2->isOccupied()) + { + OBB obb1, obb2; + convertBV(bv1, tf1, obb1); + convertBV(tree2->getBV(root2).bv, tf2, obb2); + if(obb1.overlap(obb2)) + { + Box box; + Transform3 box_tf; + constructBox(bv1, tf1, box, box_tf); + + int primitive_id = tree2->getBV(root2).primitiveId(); + const Triangle& tri_id = tree2->tri_indices[primitive_id]; + const Vector3& p1 = tree2->vertices[tri_id[0]]; + const Vector3& p2 = tree2->vertices[tri_id[1]]; + const Vector3& p3 = tree2->vertices[tri_id[2]]; + + bool is_intersect = false; + if(!crequest->enable_contact) + { + if(solver->shapeTriangleIntersect(box, box_tf, p1, p2, p3, tf2, nullptr, nullptr, nullptr)) + { + is_intersect = true; + if(cresult->numContacts() < crequest->num_max_contacts) + cresult->addContact(Contact(tree1, tree2, root1 - tree1->getRoot(), primitive_id)); + } + } + else + { + Vector3 contact; + S depth; + Vector3 normal; + + if(solver->shapeTriangleIntersect(box, box_tf, p1, p2, p3, tf2, &contact, &depth, &normal)) + { + is_intersect = true; + if(cresult->numContacts() < crequest->num_max_contacts) + cresult->addContact(Contact(tree1, tree2, root1 - tree1->getRoot(), primitive_id, contact, normal, depth)); + } + } + + if(is_intersect && crequest->enable_cost) + { + AABB overlap_part; + AABB aabb1; + computeBV(box, box_tf, aabb1); + AABB aabb2(tf2 * p1, tf2 * p2, tf2 * p3); + aabb1.overlap(aabb2, overlap_part); + cresult->addCostSource(CostSource(overlap_part, root1->getOccupancy() * tree2->cost_density), crequest->num_max_cost_sources); + } + + return crequest->isSatisfied(*cresult); + } + else + return false; + } + else if(!tree1->isNodeFree(root1) && !tree2->isFree() && crequest->enable_cost) // uncertain area + { + OBB obb1, obb2; + convertBV(bv1, tf1, obb1); + convertBV(tree2->getBV(root2).bv, tf2, obb2); + if(obb1.overlap(obb2)) + { + Box box; + Transform3 box_tf; + constructBox(bv1, tf1, box, box_tf); + + int primitive_id = tree2->getBV(root2).primitiveId(); + const Triangle& tri_id = tree2->tri_indices[primitive_id]; + const Vector3& p1 = tree2->vertices[tri_id[0]]; + const Vector3& p2 = tree2->vertices[tri_id[1]]; + const Vector3& p3 = tree2->vertices[tri_id[2]]; + + if(solver->shapeTriangleIntersect(box, box_tf, p1, p2, p3, tf2, nullptr, nullptr, nullptr)) + { + AABB overlap_part; + AABB aabb1; + computeBV(box, box_tf, aabb1); + AABB aabb2(tf2 * p1, tf2 * p2, tf2 * p3); + aabb1.overlap(aabb2, overlap_part); + cresult->addCostSource(CostSource(overlap_part, root1->getOccupancy() * tree2->cost_density), crequest->num_max_cost_sources); + } + } + + return false; + } + else // free area + return false; + } + + /// stop when 1) bounding boxes of two objects not overlap; OR + /// 2) at least one of the nodes is free; OR + /// 2) (two uncertain nodes OR one node occupied and one node uncertain) AND cost not required + if(tree1->isNodeFree(root1) || tree2->isFree()) return false; + else if((tree1->isNodeUncertain(root1) || tree2->isUncertain()) && !crequest->enable_cost) return false; + else + { + OBB obb1, obb2; + convertBV(bv1, tf1, obb1); + convertBV(tree2->getBV(root2).bv, tf2, obb2); + if(!obb1.overlap(obb2)) return false; + } + + if(tree2->getBV(root2).isLeaf() || (tree1->nodeHasChildren(root1) && (bv1.size() > tree2->getBV(root2).bv.size()))) + { + for(unsigned int i = 0; i < 8; ++i) + { + if(tree1->nodeChildExists(root1, i)) + { + const typename OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); + AABB child_bv; + computeChildBV(bv1, i, child_bv); + + if(OcTreeMeshIntersectRecurse(tree1, child, child_bv, tree2, root2, tf1, tf2)) + return true; + } +else if(!tree2->isFree() && crequest->enable_cost) + { + AABB child_bv; + computeChildBV(bv1, i, child_bv); + + if(OcTreeMeshIntersectRecurse(tree1, nullptr, child_bv, tree2, root2, tf1, tf2)) + return true; + } + } + } + else + { + if(OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2, tree2->getBV(root2).leftChild(), tf1, tf2)) + return true; + + if(OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2, tree2->getBV(root2).rightChild(), tf1, tf2)) + return true; + + } + + return false; +} + +//============================================================================== +template +bool OcTreeSolver::OcTreeDistanceRecurse(const OcTree* tree1, const typename OcTree::OcTreeNode* root1, const AABB& bv1, const OcTree* tree2, const typename OcTree::OcTreeNode* root2, const AABB& bv2, const Transform3& tf1, const Transform3& tf2) const +{ + if(!tree1->nodeHasChildren(root1) && !tree2->nodeHasChildren(root2)) + { + if(tree1->isNodeOccupied(root1) && tree2->isNodeOccupied(root2)) + { + Box box1, box2; + Transform3 box1_tf, box2_tf; + constructBox(bv1, tf1, box1, box1_tf); + constructBox(bv2, tf2, box2, box2_tf); + + S dist; + // NOTE(JS): The closest points are set to zeros in order to suppress the + // maybe-uninitialized warning. It seems the warnings occur since + // NarrowPhaseSolver::shapeDistance() conditionally set the closest points. + // If this wasn't intentional then please remove the initialization of the + // closest points, and change the function NarrowPhaseSolver::shapeDistance() + // to always set the closest points. + Vector3 closest_p1 = Vector3::Zero(); + Vector3 closest_p2 = Vector3::Zero(); + solver->shapeDistance(box1, box1_tf, box2, box2_tf, &dist, &closest_p1, &closest_p2); + + dresult->update(dist, tree1, tree2, root1 - tree1->getRoot(), root2 - tree2->getRoot(), closest_p1, closest_p2); + + return drequest->isSatisfied(*dresult); + } + else + return false; + } + + if(!tree1->isNodeOccupied(root1) || !tree2->isNodeOccupied(root2)) return false; + + if(!tree2->nodeHasChildren(root2) || (tree1->nodeHasChildren(root1) && (bv1.size() > bv2.size()))) + { + for(unsigned int i = 0; i < 8; ++i) + { + if(tree1->nodeChildExists(root1, i)) + { + const typename OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); + AABB child_bv; + computeChildBV(bv1, i, child_bv); + + S d; + AABB aabb1, aabb2; + convertBV(bv1, tf1, aabb1); + convertBV(bv2, tf2, aabb2); + d = aabb1.distance(aabb2); + + if(d < dresult->min_distance) + { + + if(OcTreeDistanceRecurse(tree1, child, child_bv, tree2, root2, bv2, tf1, tf2)) + return true; + } + } + } + } + else + { + for(unsigned int i = 0; i < 8; ++i) + { + if(tree2->nodeChildExists(root2, i)) + { + const typename OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); + AABB child_bv; + computeChildBV(bv2, i, child_bv); + + S d; + AABB aabb1, aabb2; + convertBV(bv1, tf1, aabb1); + convertBV(bv2, tf2, aabb2); + d = aabb1.distance(aabb2); + + if(d < dresult->min_distance) + { + if(OcTreeDistanceRecurse(tree1, root1, bv1, tree2, child, child_bv, tf1, tf2)) + return true; + } + } + } + } + + return false; +} + +//============================================================================== +template +bool OcTreeSolver::OcTreeIntersectRecurse(const OcTree* tree1, const typename OcTree::OcTreeNode* root1, const AABB& bv1, const OcTree* tree2, const typename OcTree::OcTreeNode* root2, const AABB& bv2, const Transform3& tf1, const Transform3& tf2) const +{ + if(!root1 && !root2) + { + OBB obb1, obb2; + convertBV(bv1, tf1, obb1); + convertBV(bv2, tf2, obb2); + + if(obb1.overlap(obb2)) + { + Box box1, box2; + Transform3 box1_tf, box2_tf; + constructBox(bv1, tf1, box1, box1_tf); + constructBox(bv2, tf2, box2, box2_tf); + + AABB overlap_part; + AABB aabb1, aabb2; + computeBV(box1, box1_tf, aabb1); + computeBV(box2, box2_tf, aabb2); + aabb1.overlap(aabb2, overlap_part); + cresult->addCostSource(CostSource(overlap_part, tree1->getOccupancyThres() * tree2->getOccupancyThres()), crequest->num_max_cost_sources); + } + + return false; + } + else if(!root1 && root2) + { + if(tree2->nodeHasChildren(root2)) + { + for(unsigned int i = 0; i < 8; ++i) + { + if(tree2->nodeChildExists(root2, i)) + { + const typename OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); + AABB child_bv; + computeChildBV(bv2, i, child_bv); + if(OcTreeIntersectRecurse(tree1, nullptr, bv1, tree2, child, child_bv, tf1, tf2)) + return true; + } + else + { + AABB child_bv; + computeChildBV(bv2, i, child_bv); + if(OcTreeIntersectRecurse(tree1, nullptr, bv1, tree2, nullptr, child_bv, tf1, tf2)) + return true; + } + } + } + else + { + if(OcTreeIntersectRecurse(tree1, nullptr, bv1, tree2, nullptr, bv2, tf1, tf2)) + return true; + } + + return false; + } + else if(root1 && !root2) + { + if(tree1->nodeHasChildren(root1)) + { + for(unsigned int i = 0; i < 8; ++i) + { + if(tree1->nodeChildExists(root1, i)) + { + const typename OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); + AABB child_bv; + computeChildBV(bv1, i, child_bv); + if(OcTreeIntersectRecurse(tree1, child, child_bv, tree2, nullptr, bv2, tf1, tf2)) + return true; + } + else + { + AABB child_bv; + computeChildBV(bv1, i, child_bv); + if(OcTreeIntersectRecurse(tree1, nullptr, child_bv, tree2, nullptr, bv2, tf1, tf2)) + return true; + } + } + } + else + { + if(OcTreeIntersectRecurse(tree1, nullptr, bv1, tree2, nullptr, bv2, tf1, tf2)) + return true; + } + + return false; + } + else if(!tree1->nodeHasChildren(root1) && !tree2->nodeHasChildren(root2)) + { + if(tree1->isNodeOccupied(root1) && tree2->isNodeOccupied(root2)) // occupied area + { + bool is_intersect = false; + if(!crequest->enable_contact) + { + OBB obb1, obb2; + convertBV(bv1, tf1, obb1); + convertBV(bv2, tf2, obb2); + + if(obb1.overlap(obb2)) + { + is_intersect = true; + if(cresult->numContacts() < crequest->num_max_contacts) + cresult->addContact(Contact(tree1, tree2, root1 - tree1->getRoot(), root2 - tree2->getRoot())); + } + } + else + { + Box box1, box2; + Transform3 box1_tf, box2_tf; + constructBox(bv1, tf1, box1, box1_tf); + constructBox(bv2, tf2, box2, box2_tf); + + std::vector> contacts; + if(solver->shapeIntersect(box1, box1_tf, box2, box2_tf, &contacts)) + { + is_intersect = true; + if(crequest->num_max_contacts > cresult->numContacts()) + { + const size_t free_space = crequest->num_max_contacts - cresult->numContacts(); + size_t num_adding_contacts; + + // If the free space is not enough to add all the new contacts, we add contacts in descent order of penetration depth. + if (free_space < contacts.size()) + { + std::partial_sort(contacts.begin(), contacts.begin() + free_space, contacts.end(), std::bind(comparePenDepth, std::placeholders::_2, std::placeholders::_1)); + num_adding_contacts = free_space; + } + else + { + num_adding_contacts = contacts.size(); + } + + for(size_t i = 0; i < num_adding_contacts; ++i) + cresult->addContact(Contact(tree1, tree2, root1 - tree1->getRoot(), root2 - tree2->getRoot(), contacts[i].pos, contacts[i].normal, contacts[i].penetration_depth)); + } + } + } + + if(is_intersect && crequest->enable_cost) + { + Box box1, box2; + Transform3 box1_tf, box2_tf; + constructBox(bv1, tf1, box1, box1_tf); + constructBox(bv2, tf2, box2, box2_tf); + + AABB overlap_part; + AABB aabb1, aabb2; + computeBV(box1, box1_tf, aabb1); + computeBV(box2, box2_tf, aabb2); + aabb1.overlap(aabb2, overlap_part); + cresult->addCostSource(CostSource(overlap_part, root1->getOccupancy() * root2->getOccupancy()), crequest->num_max_cost_sources); + } + + return crequest->isSatisfied(*cresult); + } + else if(!tree1->isNodeFree(root1) && !tree2->isNodeFree(root2) && crequest->enable_cost) // uncertain area (here means both are uncertain or one uncertain and one occupied) + { + OBB obb1, obb2; + convertBV(bv1, tf1, obb1); + convertBV(bv2, tf2, obb2); + + if(obb1.overlap(obb2)) + { + Box box1, box2; + Transform3 box1_tf, box2_tf; + constructBox(bv1, tf1, box1, box1_tf); + constructBox(bv2, tf2, box2, box2_tf); + + AABB overlap_part; + AABB aabb1, aabb2; + computeBV(box1, box1_tf, aabb1); + computeBV(box2, box2_tf, aabb2); + aabb1.overlap(aabb2, overlap_part); + cresult->addCostSource(CostSource(overlap_part, root1->getOccupancy() * root2->getOccupancy()), crequest->num_max_cost_sources); + } + + return false; + } + else // free area (at least one node is free) + return false; + } + + /// stop when 1) bounding boxes of two objects not overlap; OR + /// 2) at least one of the nodes is free; OR + /// 2) (two uncertain nodes OR one node occupied and one node uncertain) AND cost not required + if(tree1->isNodeFree(root1) || tree2->isNodeFree(root2)) return false; + else if((tree1->isNodeUncertain(root1) || tree2->isNodeUncertain(root2)) && !crequest->enable_cost) return false; + else + { + OBB obb1, obb2; + convertBV(bv1, tf1, obb1); + convertBV(bv2, tf2, obb2); + if(!obb1.overlap(obb2)) return false; + } + + if(!tree2->nodeHasChildren(root2) || (tree1->nodeHasChildren(root1) && (bv1.size() > bv2.size()))) + { + for(unsigned int i = 0; i < 8; ++i) + { + if(tree1->nodeChildExists(root1, i)) + { + const typename OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); + AABB child_bv; + computeChildBV(bv1, i, child_bv); + + if(OcTreeIntersectRecurse(tree1, child, child_bv, + tree2, root2, bv2, + tf1, tf2)) + return true; + } + else if(!tree2->isNodeFree(root2) && crequest->enable_cost) + { + AABB child_bv; + computeChildBV(bv1, i, child_bv); + + if(OcTreeIntersectRecurse(tree1, nullptr, child_bv, + tree2, root2, bv2, + tf1, tf2)) + return true; + } + } + } + else + { + for(unsigned int i = 0; i < 8; ++i) + { + if(tree2->nodeChildExists(root2, i)) + { + const typename OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); + AABB child_bv; + computeChildBV(bv2, i, child_bv); + + if(OcTreeIntersectRecurse(tree1, root1, bv1, + tree2, child, child_bv, + tf1, tf2)) + return true; + } + else if(!tree1->isNodeFree(root1) && crequest->enable_cost) + { + AABB child_bv; + computeChildBV(bv2, i, child_bv); + + if(OcTreeIntersectRecurse(tree1, root1, bv1, + tree2, nullptr, child_bv, + tf1, tf2)) + return true; + } + } + } + + return false; +} + +} // namespace fcl + +#endif diff --git a/include/fcl/traversal/traversal_node_base.h b/include/fcl/traversal/traversal_node_base.h index 09045dfb0..3358e2eb3 100644 --- a/include/fcl/traversal/traversal_node_base.h +++ b/include/fcl/traversal/traversal_node_base.h @@ -35,24 +35,24 @@ /** \author Jia Pan */ -#ifndef FCL_TRAVERSAL_NODE_BASE_H -#define FCL_TRAVERSAL_NODE_BASE_H +#ifndef FCL_TRAVERSAL_TRAVERSALNODEBASE_H +#define FCL_TRAVERSAL_TRAVERSALNODEBASE_H #include "fcl/data_types.h" -#include "fcl/collision_data.h" namespace fcl { /// @brief Node structure encoding the information required for traversal. +template class TraversalNodeBase { public: virtual ~TraversalNodeBase(); - virtual void preprocess() {} + virtual void preprocess(); - virtual void postprocess() {} + virtual void postprocess(); /// @brief Whether b is a leaf node in the first BVH tree virtual bool isFirstNodeLeaf(int b) const; @@ -79,86 +79,90 @@ class TraversalNodeBase virtual void enableStatistics(bool enable) = 0; /// @brief configuation of first object - Transform3d tf1; + Transform3 tf1; /// @brief configuration of second object - Transform3d tf2; -}; - -/// @brief Node structure encoding the information required for collision traversal. -class CollisionTraversalNodeBase : public TraversalNodeBase -{ -public: - CollisionTraversalNodeBase() : result(NULL), enable_statistics(false) {} - - virtual ~CollisionTraversalNodeBase(); - - /// @brief BV test between b1 and b2 - virtual bool BVTesting(int b1, int b2) const; - - /// @brief Leaf test between node b1 and b2, if they are both leafs - virtual void leafTesting(int b1, int b2) const; - - /// @brief Check whether the traversal can stop - virtual bool canStop() const; - - /// @brief Whether store some statistics information during traversal - void enableStatistics(bool enable) { enable_statistics = enable; } + Transform3 tf2; - /// @brief request setting for collision - CollisionRequest request; - - /// @brief collision result kept during the traversal iteration - CollisionResult* result; - - /// @brief Whether stores statistics - bool enable_statistics; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -/// @brief Node structure encoding the information required for distance traversal. -class DistanceTraversalNodeBase : public TraversalNodeBase -{ -public: - DistanceTraversalNodeBase() : result(NULL), enable_statistics(false) {} - - virtual ~DistanceTraversalNodeBase(); +//============================================================================// +// // +// Implementations // +// // +//============================================================================// - /// @brief BV test between b1 and b2 - virtual FCL_REAL BVTesting(int b1, int b2) const; - - /// @brief Leaf test between node b1 and b2, if they are both leafs - virtual void leafTesting(int b1, int b2) const; +//============================================================================== +template +TraversalNodeBase::~TraversalNodeBase() +{ + // Do nothing +} - /// @brief Check whether the traversal can stop - virtual bool canStop(FCL_REAL c) const; +//============================================================================== +template +void TraversalNodeBase::preprocess() +{ + // Do nothing +} - /// @brief Whether store some statistics information during traversal - void enableStatistics(bool enable) { enable_statistics = enable; } +//============================================================================== +template +void TraversalNodeBase::postprocess() +{ + // Do nothing +} - /// @brief request setting for distance - DistanceRequest request; +//============================================================================== +template +bool TraversalNodeBase::isFirstNodeLeaf(int b) const +{ + return true; +} - /// @brief distance result kept during the traversal iteration - DistanceResult* result; +//============================================================================== +template +bool TraversalNodeBase::isSecondNodeLeaf(int b) const +{ + return true; +} - /// @brief Whether stores statistics - bool enable_statistics; -}; +//============================================================================== +template +bool TraversalNodeBase::firstOverSecond(int b1, int b2) const +{ + return true; +} +//============================================================================== +template +int TraversalNodeBase::getFirstLeftChild(int b) const +{ + return b; +} -struct ConservativeAdvancementStackData +//============================================================================== +template +int TraversalNodeBase::getFirstRightChild(int b) const { - ConservativeAdvancementStackData(const Vector3d& P1_, const Vector3d& P2_, int c1_, int c2_, FCL_REAL d_) - : P1(P1_), P2(P2_), c1(c1_), c2(c2_), d(d_) {} - - Vector3d P1; - Vector3d P2; - int c1; - int c2; - FCL_REAL d; -}; + return b; +} +//============================================================================== +template +int TraversalNodeBase::getSecondLeftChild(int b) const +{ + return b; +} +//============================================================================== +template +int TraversalNodeBase::getSecondRightChild(int b) const +{ + return b; } +} // namespace fcl + #endif diff --git a/include/fcl/traversal/traversal_node_bvh_shape.h b/include/fcl/traversal/traversal_node_bvh_shape.h index bc086e605..e4fd971e3 100644 --- a/include/fcl/traversal/traversal_node_bvh_shape.h +++ b/include/fcl/traversal/traversal_node_bvh_shape.h @@ -35,1599 +35,9 @@ /** \author Jia Pan */ - #ifndef FCL_TRAVERSAL_NODE_MESH_SHAPE_H #define FCL_TRAVERSAL_NODE_MESH_SHAPE_H -#include "fcl/collision_data.h" -#include "fcl/shape/geometric_shapes.h" -#include "fcl/shape/geometric_shapes_utility.h" -#include "fcl/traversal/traversal_node_base.h" -#include "fcl/BVH/BVH_model.h" - - -namespace fcl -{ - -/// @brief Traversal node for collision between BVH and shape -template -class BVHShapeCollisionTraversalNode : public CollisionTraversalNodeBase -{ -public: - BVHShapeCollisionTraversalNode() : CollisionTraversalNodeBase() - { - model1 = NULL; - model2 = NULL; - - num_bv_tests = 0; - num_leaf_tests = 0; - query_time_seconds = 0.0; - } - - /// @brief Whether the BV node in the first BVH tree is leaf - bool isFirstNodeLeaf(int b) const - { - return model1->getBV(b).isLeaf(); - } - - /// @brief Obtain the left child of BV node in the first BVH - int getFirstLeftChild(int b) const - { - return model1->getBV(b).leftChild(); - } - - /// @brief Obtain the right child of BV node in the first BVH - int getFirstRightChild(int b) const - { - return model1->getBV(b).rightChild(); - } - - /// @brief BV culling test in one BVTT node - bool BVTesting(int b1, int b2) const - { - if(this->enable_statistics) num_bv_tests++; - return !model1->getBV(b1).bv.overlap(model2_bv); - } - - const BVHModel* model1; - const S* model2; - BV model2_bv; - - mutable int num_bv_tests; - mutable int num_leaf_tests; - mutable FCL_REAL query_time_seconds; -}; - -/// @brief Traversal node for collision between shape and BVH -template -class ShapeBVHCollisionTraversalNode : public CollisionTraversalNodeBase -{ -public: - ShapeBVHCollisionTraversalNode() : CollisionTraversalNodeBase() - { - model1 = NULL; - model2 = NULL; - - num_bv_tests = 0; - num_leaf_tests = 0; - query_time_seconds = 0.0; - } - - /// @brief Alway extend the second model, which is a BVH model - bool firstOverSecond(int, int) const - { - return false; - } - - /// @brief Whether the BV node in the second BVH tree is leaf - bool isSecondNodeLeaf(int b) const - { - return model2->getBV(b).isLeaf(); - } - - /// @brief Obtain the left child of BV node in the second BVH - int getSecondLeftChild(int b) const - { - return model2->getBV(b).leftChild(); - } - - /// @brief Obtain the right child of BV node in the second BVH - int getSecondRightChild(int b) const - { - return model2->getBV(b).rightChild(); - } - - /// @brief BV culling test in one BVTT node - bool BVTesting(int b1, int b2) const - { - if(this->enable_statistics) num_bv_tests++; - return !model2->getBV(b2).bv.overlap(model1_bv); - } - - const S* model1; - const BVHModel* model2; - BV model1_bv; - - mutable int num_bv_tests; - mutable int num_leaf_tests; - mutable FCL_REAL query_time_seconds; -}; - - -/// @brief Traversal node for collision between mesh and shape -template -class MeshShapeCollisionTraversalNode : public BVHShapeCollisionTraversalNode -{ -public: - MeshShapeCollisionTraversalNode() : BVHShapeCollisionTraversalNode() - { - vertices = NULL; - tri_indices = NULL; - - nsolver = NULL; - } - - /// @brief Intersection testing between leaves (one triangle and one shape) - void leafTesting(int b1, int b2) const - { - if(this->enable_statistics) this->num_leaf_tests++; - const BVNode& node = this->model1->getBV(b1); - - int primitive_id = node.primitiveId(); - - const Triangle& tri_id = tri_indices[primitive_id]; - - const Vector3d& p1 = vertices[tri_id[0]]; - const Vector3d& p2 = vertices[tri_id[1]]; - const Vector3d& p3 = vertices[tri_id[2]]; - - if(this->model1->isOccupied() && this->model2->isOccupied()) - { - bool is_intersect = false; - - if(!this->request.enable_contact) - { - if(nsolver->shapeTriangleIntersect(*(this->model2), this->tf2, p1, p2, p3, NULL, NULL, NULL)) - { - is_intersect = true; - if(this->request.num_max_contacts > this->result->numContacts()) - this->result->addContact(Contact(this->model1, this->model2, primitive_id, Contact::NONE)); - } - } - else - { - FCL_REAL penetration; - Vector3d normal; - Vector3d contactp; - - if(nsolver->shapeTriangleIntersect(*(this->model2), this->tf2, p1, p2, p3, &contactp, &penetration, &normal)) - { - is_intersect = true; - if(this->request.num_max_contacts > this->result->numContacts()) - this->result->addContact(Contact(this->model1, this->model2, primitive_id, Contact::NONE, contactp, -normal, penetration)); - } - } - - if(is_intersect && this->request.enable_cost) - { - AABB overlap_part; - AABB shape_aabb; - computeBV(*(this->model2), this->tf2, shape_aabb); - AABB(p1, p2, p3).overlap(shape_aabb, overlap_part); - this->result->addCostSource(CostSource(overlap_part, cost_density), this->request.num_max_cost_sources); - } - } - if((!this->model1->isFree() && !this->model2->isFree()) && this->request.enable_cost) - { - if(nsolver->shapeTriangleIntersect(*(this->model2), this->tf2, p1, p2, p3, NULL, NULL, NULL)) - { - AABB overlap_part; - AABB shape_aabb; - computeBV(*(this->model2), this->tf2, shape_aabb); - AABB(p1, p2, p3).overlap(shape_aabb, overlap_part); - this->result->addCostSource(CostSource(overlap_part, cost_density), this->request.num_max_cost_sources); - } - } - } - - /// @brief Whether the traversal process can stop early - bool canStop() const - { - return this->request.isSatisfied(*(this->result)); - } - - Vector3d* vertices; - Triangle* tri_indices; - - FCL_REAL cost_density; - - const NarrowPhaseSolver* nsolver; -}; - -/// @cond IGNORE -namespace details -{ -template -static inline void meshShapeCollisionOrientedNodeLeafTesting(int b1, int b2, - const BVHModel* model1, const S& model2, - Vector3d* vertices, Triangle* tri_indices, - const Transform3d& tf1, - const Transform3d& tf2, - const NarrowPhaseSolver* nsolver, - bool enable_statistics, - FCL_REAL cost_density, - int& num_leaf_tests, - const CollisionRequest& request, - CollisionResult& result) -{ - if(enable_statistics) num_leaf_tests++; - const BVNode& node = model1->getBV(b1); - - int primitive_id = node.primitiveId(); - - const Triangle& tri_id = tri_indices[primitive_id]; - - const Vector3d& p1 = vertices[tri_id[0]]; - const Vector3d& p2 = vertices[tri_id[1]]; - const Vector3d& p3 = vertices[tri_id[2]]; - - if(model1->isOccupied() && model2.isOccupied()) - { - bool is_intersect = false; - - if(!request.enable_contact) // only interested in collision or not - { - if(nsolver->shapeTriangleIntersect(model2, tf2, p1, p2, p3, tf1, NULL, NULL, NULL)) - { - is_intersect = true; - if(request.num_max_contacts > result.numContacts()) - result.addContact(Contact(model1, &model2, primitive_id, Contact::NONE)); - } - } - else - { - FCL_REAL penetration; - Vector3d normal; - Vector3d contactp; - - if(nsolver->shapeTriangleIntersect(model2, tf2, p1, p2, p3, tf1, &contactp, &penetration, &normal)) - { - is_intersect = true; - if(request.num_max_contacts > result.numContacts()) - result.addContact(Contact(model1, &model2, primitive_id, Contact::NONE, contactp, -normal, penetration)); - } - } - - if(is_intersect && request.enable_cost) - { - AABB overlap_part; - AABB shape_aabb; - computeBV(model2, tf2, shape_aabb); - /* bool res = */ AABB(tf1 * p1, tf1 * p2, tf1 * p3).overlap(shape_aabb, overlap_part); - result.addCostSource(CostSource(overlap_part, cost_density), request.num_max_cost_sources); - } - } - else if((!model1->isFree() || model2.isFree()) && request.enable_cost) - { - if(nsolver->shapeTriangleIntersect(model2, tf2, p1, p2, p3, tf1, NULL, NULL, NULL)) - { - AABB overlap_part; - AABB shape_aabb; - computeBV(model2, tf2, shape_aabb); - /* bool res = */ AABB(tf1 * p1, tf1 * p2, tf1 * p3).overlap(shape_aabb, overlap_part); - result.addCostSource(CostSource(overlap_part, cost_density), request.num_max_cost_sources); - } - } -} - -} - -/// @endcond - - -/// @brief Traversal node for mesh and shape, when mesh BVH is one of the oriented node (OBB, RSS, OBBRSS, kIOS) -template -class MeshShapeCollisionTraversalNodeOBB : public MeshShapeCollisionTraversalNode -{ -public: - MeshShapeCollisionTraversalNodeOBB() : MeshShapeCollisionTraversalNode() - { - } - - bool BVTesting(int b1, int b2) const - { - if(this->enable_statistics) this->num_bv_tests++; - return !overlap(this->tf1.linear(), this->tf1.translation(), this->model2_bv, this->model1->getBV(b1).bv); - } - - void leafTesting(int b1, int b2) const - { - details::meshShapeCollisionOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices, - this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->result)); - } - -}; - -template -class MeshShapeCollisionTraversalNodeRSS : public MeshShapeCollisionTraversalNode -{ -public: - MeshShapeCollisionTraversalNodeRSS() : MeshShapeCollisionTraversalNode() - { - } - - bool BVTesting(int b1, int b2) const - { - if(this->enable_statistics) this->num_bv_tests++; - return !overlap(this->tf1.linear(), this->tf1.translation(), this->model2_bv, this->model1->getBV(b1).bv); - } - - void leafTesting(int b1, int b2) const - { - details::meshShapeCollisionOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices, - this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->result)); - } - -}; - -template -class MeshShapeCollisionTraversalNodekIOS : public MeshShapeCollisionTraversalNode -{ -public: - MeshShapeCollisionTraversalNodekIOS() : MeshShapeCollisionTraversalNode() - { - } - - bool BVTesting(int b1, int b2) const - { - if(this->enable_statistics) this->num_bv_tests++; - return !overlap(this->tf1.linear(), this->tf1.translation(), this->model2_bv, this->model1->getBV(b1).bv); - } - - void leafTesting(int b1, int b2) const - { - details::meshShapeCollisionOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices, - this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->result)); - } - -}; - -template -class MeshShapeCollisionTraversalNodeOBBRSS : public MeshShapeCollisionTraversalNode -{ -public: - MeshShapeCollisionTraversalNodeOBBRSS() : MeshShapeCollisionTraversalNode() - { - } - - bool BVTesting(int b1, int b2) const - { - if(this->enable_statistics) this->num_bv_tests++; - return !overlap(this->tf1.linear(), this->tf1.translation(), this->model2_bv, this->model1->getBV(b1).bv); - } - - void leafTesting(int b1, int b2) const - { - details::meshShapeCollisionOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices, - this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->result)); - } - -}; - - -/// @brief Traversal node for collision between shape and mesh -template -class ShapeMeshCollisionTraversalNode : public ShapeBVHCollisionTraversalNode -{ -public: - ShapeMeshCollisionTraversalNode() : ShapeBVHCollisionTraversalNode() - { - vertices = NULL; - tri_indices = NULL; - - nsolver = NULL; - } - - /// @brief Intersection testing between leaves (one shape and one triangle) - void leafTesting(int b1, int b2) const - { - if(this->enable_statistics) this->num_leaf_tests++; - const BVNode& node = this->model2->getBV(b2); - - int primitive_id = node.primitiveId(); - - const Triangle& tri_id = tri_indices[primitive_id]; - - const Vector3d& p1 = vertices[tri_id[0]]; - const Vector3d& p2 = vertices[tri_id[1]]; - const Vector3d& p3 = vertices[tri_id[2]]; - - if(this->model1->isOccupied() && this->model2->isOccupied()) - { - bool is_intersect = false; - - if(!this->request.enable_contact) - { - if(nsolver->shapeTriangleIntersect(*(this->model1), this->tf1, p1, p2, p3, NULL, NULL, NULL)) - { - is_intersect = true; - if(this->request.num_max_contacts > this->result->numContacts()) - this->result->addContact(Contact(this->model1, this->model2, Contact::NONE, primitive_id)); - } - } - else - { - FCL_REAL penetration; - Vector3d normal; - Vector3d contactp; - - if(nsolver->shapeTriangleIntersect(*(this->model1), this->tf1, p1, p2, p3, &contactp, &penetration, &normal)) - { - is_intersect = true; - if(this->request.num_max_contacts > this->result->numContacts()) - this->result->addContact(Contact(this->model1, this->model2, Contact::NONE, primitive_id, contactp, normal, penetration)); - } - } - - if(is_intersect && this->request.enable_cost) - { - AABB overlap_part; - AABB shape_aabb; - computeBV(*(this->model1), this->tf1, shape_aabb); - AABB(p1, p2, p3).overlap(shape_aabb, overlap_part); - this->result->addCostSource(CostSource(overlap_part, cost_density), this->request.num_max_cost_sources); - } - } - else if((!this->model1->isFree() && !this->model2->isFree()) && this->request.enable_cost) - { - if(nsolver->shapeTriangleIntersect(*(this->model1), this->tf1, p1, p2, p3, NULL, NULL, NULL)) - { - AABB overlap_part; - AABB shape_aabb; - computeBV(*(this->model1), this->tf1, shape_aabb); - AABB(p1, p2, p3).overlap(shape_aabb, overlap_part); - this->result->addCostSource(CostSource(overlap_part, cost_density), this->request.num_max_cost_sources); - } - } - } - - /// @brief Whether the traversal process can stop early - bool canStop() const - { - return this->request.isSatisfied(*(this->result)); - } - - Vector3d* vertices; - Triangle* tri_indices; - - FCL_REAL cost_density; - - const NarrowPhaseSolver* nsolver; -}; - -/// @brief Traversal node for shape and mesh, when mesh BVH is one of the oriented node (OBB, RSS, OBBRSS, kIOS) -template -class ShapeMeshCollisionTraversalNodeOBB : public ShapeMeshCollisionTraversalNode -{ -public: - ShapeMeshCollisionTraversalNodeOBB() : ShapeMeshCollisionTraversalNode() - { - } - - bool BVTesting(int b1, int b2) const - { - if(this->enable_statistics) this->num_bv_tests++; - return !overlap(this->tf2.linear(), this->tf2.translation(), this->model1_bv, this->model2->getBV(b2).bv); - } - - void leafTesting(int b1, int b2) const - { - details::meshShapeCollisionOrientedNodeLeafTesting(b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices, - this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->request)); - - // may need to change the order in pairs - } -}; - - -template -class ShapeMeshCollisionTraversalNodeRSS : public ShapeMeshCollisionTraversalNode -{ -public: - ShapeMeshCollisionTraversalNodeRSS() : ShapeMeshCollisionTraversalNode() - { - } - - bool BVTesting(int b1, int b2) const - { - if(this->enable_statistics) this->num_bv_tests++; - return !overlap(this->tf2.linear(), this->tf2.translation(), this->model1_bv, this->model2->getBV(b2).bv); - } - - void leafTesting(int b1, int b2) const - { - details::meshShapeCollisionOrientedNodeLeafTesting(b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices, - this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->request)); - - // may need to change the order in pairs - } - -}; - - -template -class ShapeMeshCollisionTraversalNodekIOS : public ShapeMeshCollisionTraversalNode -{ -public: - ShapeMeshCollisionTraversalNodekIOS() : ShapeMeshCollisionTraversalNode() - { - } - - bool BVTesting(int b1, int b2) const - { - if(this->enable_statistics) this->num_bv_tests++; - return !overlap(this->tf2.linear(), this->tf2.translation(), this->model1_bv, this->model2->getBV(b2).bv); - } - - void leafTesting(int b1, int b2) const - { - details::meshShapeCollisionOrientedNodeLeafTesting(b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices, - this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->request)); - - // may need to change the order in pairs - } - -}; - - -template -class ShapeMeshCollisionTraversalNodeOBBRSS : public ShapeMeshCollisionTraversalNode -{ -public: - ShapeMeshCollisionTraversalNodeOBBRSS() : ShapeMeshCollisionTraversalNode() - { - } - - bool BVTesting(int b1, int b2) const - { - if(this->enable_statistics) this->num_bv_tests++; - return !overlap(this->tf2.linear(), this->tf2.translation(), this->model1_bv, this->model2->getBV(b2).bv); - } - - void leafTesting(int b1, int b2) const - { - details::meshShapeCollisionOrientedNodeLeafTesting(b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices, - this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->request)); - - // may need to change the order in pairs - } - -}; - -/// @brief Traversal node for distance computation between BVH and shape -template -class BVHShapeDistanceTraversalNode : public DistanceTraversalNodeBase -{ -public: - BVHShapeDistanceTraversalNode() : DistanceTraversalNodeBase() - { - model1 = NULL; - model2 = NULL; - - num_bv_tests = 0; - num_leaf_tests = 0; - query_time_seconds = 0.0; - } - - /// @brief Whether the BV node in the first BVH tree is leaf - bool isFirstNodeLeaf(int b) const - { - return model1->getBV(b).isLeaf(); - } - - /// @brief Obtain the left child of BV node in the first BVH - int getFirstLeftChild(int b) const - { - return model1->getBV(b).leftChild(); - } - - /// @brief Obtain the right child of BV node in the first BVH - int getFirstRightChild(int b) const - { - return model1->getBV(b).rightChild(); - } - - /// @brief BV culling test in one BVTT node - FCL_REAL BVTesting(int b1, int b2) const - { - return model1->getBV(b1).bv.distance(model2_bv); - } - - const BVHModel* model1; - const S* model2; - BV model2_bv; - - mutable int num_bv_tests; - mutable int num_leaf_tests; - mutable FCL_REAL query_time_seconds; -}; - -/// @brief Traversal node for distance computation between shape and BVH -template -class ShapeBVHDistanceTraversalNode : public DistanceTraversalNodeBase -{ -public: - ShapeBVHDistanceTraversalNode() : DistanceTraversalNodeBase() - { - model1 = NULL; - model2 = NULL; - - num_bv_tests = 0; - num_leaf_tests = 0; - query_time_seconds = 0.0; - } - - /// @brief Whether the BV node in the second BVH tree is leaf - bool isSecondNodeLeaf(int b) const - { - return model2->getBV(b).isLeaf(); - } - - /// @brief Obtain the left child of BV node in the second BVH - int getSecondLeftChild(int b) const - { - return model2->getBV(b).leftChild(); - } - - /// @brief Obtain the right child of BV node in the second BVH - int getSecondRightChild(int b) const - { - return model2->getBV(b).rightChild(); - } - - /// @brief BV culling test in one BVTT node - FCL_REAL BVTesting(int b1, int b2) const - { - return model1_bv.distance(model2->getBV(b2).bv); - } - - const S* model1; - const BVHModel* model2; - BV model1_bv; - - mutable int num_bv_tests; - mutable int num_leaf_tests; - mutable FCL_REAL query_time_seconds; -}; - - -/// @brief Traversal node for distance between mesh and shape -template -class MeshShapeDistanceTraversalNode : public BVHShapeDistanceTraversalNode -{ -public: - MeshShapeDistanceTraversalNode() : BVHShapeDistanceTraversalNode() - { - vertices = NULL; - tri_indices = NULL; - - rel_err = 0; - abs_err = 0; - - nsolver = NULL; - } - - /// @brief Distance testing between leaves (one triangle and one shape) - void leafTesting(int b1, int b2) const - { - if(this->enable_statistics) this->num_leaf_tests++; - - const BVNode& node = this->model1->getBV(b1); - - int primitive_id = node.primitiveId(); - - const Triangle& tri_id = tri_indices[primitive_id]; - - const Vector3d& p1 = vertices[tri_id[0]]; - const Vector3d& p2 = vertices[tri_id[1]]; - const Vector3d& p3 = vertices[tri_id[2]]; - - FCL_REAL d; - Vector3d closest_p1, closest_p2; - nsolver->shapeTriangleDistance(*(this->model2), this->tf2, p1, p2, p3, &d, &closest_p2, &closest_p1); - - this->result->update(d, this->model1, this->model2, primitive_id, DistanceResult::NONE, closest_p1, closest_p2); - } - - /// @brief Whether the traversal process can stop early - bool canStop(FCL_REAL c) const - { - if((c >= this->result->min_distance - abs_err) && (c * (1 + rel_err) >= this->result->min_distance)) - return true; - return false; - } - - Vector3d* vertices; - Triangle* tri_indices; - - FCL_REAL rel_err; - FCL_REAL abs_err; - - const NarrowPhaseSolver* nsolver; -}; - -/// @cond IGNORE -namespace details -{ - -template -void meshShapeDistanceOrientedNodeLeafTesting(int b1, int /* b2 */, - const BVHModel* model1, const S& model2, - Vector3d* vertices, Triangle* tri_indices, - const Transform3d& tf1, - const Transform3d& tf2, - const NarrowPhaseSolver* nsolver, - bool enable_statistics, - int & num_leaf_tests, - const DistanceRequest& /* request */, - DistanceResult& result) -{ - if(enable_statistics) num_leaf_tests++; - - const BVNode& node = model1->getBV(b1); - int primitive_id = node.primitiveId(); - - const Triangle& tri_id = tri_indices[primitive_id]; - const Vector3d& p1 = vertices[tri_id[0]]; - const Vector3d& p2 = vertices[tri_id[1]]; - const Vector3d& p3 = vertices[tri_id[2]]; - - FCL_REAL distance; - Vector3d closest_p1, closest_p2; - nsolver->shapeTriangleDistance(model2, tf2, p1, p2, p3, tf1, &distance, &closest_p2, &closest_p1); - - result.update(distance, model1, &model2, primitive_id, DistanceResult::NONE, closest_p1, closest_p2); -} - - -template -static inline void distancePreprocessOrientedNode(const BVHModel* model1, - Vector3d* vertices, Triangle* tri_indices, int init_tri_id, - const S& model2, const Transform3d& tf1, const Transform3d& tf2, - const NarrowPhaseSolver* nsolver, - const DistanceRequest& /* request */, - DistanceResult& result) -{ - const Triangle& init_tri = tri_indices[init_tri_id]; - - const Vector3d& p1 = vertices[init_tri[0]]; - const Vector3d& p2 = vertices[init_tri[1]]; - const Vector3d& p3 = vertices[init_tri[2]]; - - FCL_REAL distance; - Vector3d closest_p1, closest_p2; - nsolver->shapeTriangleDistance(model2, tf2, p1, p2, p3, tf1, &distance, &closest_p2, &closest_p1); - - result.update(distance, model1, &model2, init_tri_id, DistanceResult::NONE, closest_p1, closest_p2); -} - - -} - -/// @endcond - - - -/// @brief Traversal node for distance between mesh and shape, when mesh BVH is one of the oriented node (RSS, OBBRSS, kIOS) -template -class MeshShapeDistanceTraversalNodeRSS : public MeshShapeDistanceTraversalNode -{ -public: - MeshShapeDistanceTraversalNodeRSS() : MeshShapeDistanceTraversalNode() - { - } - - void preprocess() - { - details::distancePreprocessOrientedNode(this->model1, this->vertices, this->tri_indices, 0, - *(this->model2), this->tf1, this->tf2, this->nsolver, this->request, *(this->result)); - } - - void postprocess() - { - } - - FCL_REAL BVTesting(int b1, int b2) const - { - if(this->enable_statistics) this->num_bv_tests++; - return distance(this->tf1.linear(), this->tf1.translation(), this->model2_bv, this->model1->getBV(b1).bv); - } - - void leafTesting(int b1, int b2) const - { - details::meshShapeDistanceOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices, - this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->request, *(this->result)); - } -}; - - -template -class MeshShapeDistanceTraversalNodekIOS : public MeshShapeDistanceTraversalNode -{ -public: - MeshShapeDistanceTraversalNodekIOS() : MeshShapeDistanceTraversalNode() - { - } - - void preprocess() - { - details::distancePreprocessOrientedNode(this->model1, this->vertices, this->tri_indices, 0, - *(this->model2), this->tf1, this->tf2, this->nsolver, this->request, *(this->result)); - } - - void postprocess() - { - } - - FCL_REAL BVTesting(int b1, int b2) const - { - if(this->enable_statistics) this->num_bv_tests++; - return distance(this->tf1.linear(), this->tf1.translation(), this->model2_bv, this->model1->getBV(b1).bv); - } - - void leafTesting(int b1, int b2) const - { - details::meshShapeDistanceOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices, - this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->request, *(this->result)); - } - -}; - -template -class MeshShapeDistanceTraversalNodeOBBRSS : public MeshShapeDistanceTraversalNode -{ -public: - MeshShapeDistanceTraversalNodeOBBRSS() : MeshShapeDistanceTraversalNode() - { - } - - void preprocess() - { - details::distancePreprocessOrientedNode(this->model1, this->vertices, this->tri_indices, 0, - *(this->model2), this->tf1, this->tf2, this->nsolver, this->request, *(this->result)); - } - - void postprocess() - { - - } - - FCL_REAL BVTesting(int b1, int b2) const - { - if(this->enable_statistics) this->num_bv_tests++; - return distance(this->tf1.linear(), this->tf1.translation(), this->model2_bv, this->model1->getBV(b1).bv); - } - - void leafTesting(int b1, int b2) const - { - details::meshShapeDistanceOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices, - this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->request, *(this->result)); - } - -}; - -/// @brief Traversal node for distance between shape and mesh -template -class ShapeMeshDistanceTraversalNode : public ShapeBVHDistanceTraversalNode -{ -public: - ShapeMeshDistanceTraversalNode() : ShapeBVHDistanceTraversalNode() - { - vertices = NULL; - tri_indices = NULL; - - rel_err = 0; - abs_err = 0; - - nsolver = NULL; - } - - /// @brief Distance testing between leaves (one shape and one triangle) - void leafTesting(int b1, int b2) const - { - if(this->enable_statistics) this->num_leaf_tests++; - - const BVNode& node = this->model2->getBV(b2); - - int primitive_id = node.primitiveId(); - - const Triangle& tri_id = tri_indices[primitive_id]; - - const Vector3d& p1 = vertices[tri_id[0]]; - const Vector3d& p2 = vertices[tri_id[1]]; - const Vector3d& p3 = vertices[tri_id[2]]; - - FCL_REAL distance; - Vector3d closest_p1, closest_p2; - nsolver->shapeTriangleDistance(*(this->model1), this->tf1, p1, p2, p3, &distance, &closest_p1, &closest_p2); - - this->result->update(distance, this->model1, this->model2, DistanceResult::NONE, primitive_id, closest_p1, closest_p2); - } - - /// @brief Whether the traversal process can stop early - bool canStop(FCL_REAL c) const - { - if((c >= this->result->min_distance - abs_err) && (c * (1 + rel_err) >= this->result->min_distance)) - return true; - return false; - } - - Vector3d* vertices; - Triangle* tri_indices; - - FCL_REAL rel_err; - FCL_REAL abs_err; - - const NarrowPhaseSolver* nsolver; -}; - -template -class ShapeMeshDistanceTraversalNodeRSS : public ShapeMeshDistanceTraversalNode -{ -public: - ShapeMeshDistanceTraversalNodeRSS() : ShapeMeshDistanceTraversalNode() - { - } - - void preprocess() - { - details::distancePreprocessOrientedNode(this->model2, this->vertices, this->tri_indices, 0, - *(this->model1), this->tf2, this->tf1, this->nsolver, this->request, *(this->result)); - } - - void postprocess() - { - } - - FCL_REAL BVTesting(int b1, int b2) const - { - if(this->enable_statistics) this->num_bv_tests++; - return distance(this->tf2.linear(), this->tf2.translation(), this->model1_bv, this->model2->getBV(b2).bv); - } - - void leafTesting(int b1, int b2) const - { - details::meshShapeDistanceOrientedNodeLeafTesting(b2, b1, this->model2, *(this->model1), this->vertices, this->tri_indices, - this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->request, *(this->result)); - } - -}; - -template -class ShapeMeshDistanceTraversalNodekIOS : public ShapeMeshDistanceTraversalNode -{ -public: - ShapeMeshDistanceTraversalNodekIOS() : ShapeMeshDistanceTraversalNode() - { - } - - void preprocess() - { - details::distancePreprocessOrientedNode(this->model2, this->vertices, this->tri_indices, 0, - *(this->model1), this->tf2, this->tf1, this->nsolver, *(this->result)); - } - - void postprocess() - { - } - - FCL_REAL BVTesting(int b1, int b2) const - { - if(this->enable_statistics) this->num_bv_tests++; - return distance(this->tf2.linear(), this->tf2.translation(), this->model1_bv, this->model2->getBV(b2).bv); - } - - void leafTesting(int b1, int b2) const - { - details::meshShapeDistanceOrientedNodeLeafTesting(b2, b1, this->model2, *(this->model1), this->vertices, this->tri_indices, - this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->request, *(this->result)); - } - -}; - -template -class ShapeMeshDistanceTraversalNodeOBBRSS : public ShapeMeshDistanceTraversalNode -{ -public: - ShapeMeshDistanceTraversalNodeOBBRSS() : ShapeMeshDistanceTraversalNode() - { - } - - void preprocess() - { - details::distancePreprocessOrientedNode(this->model2, this->vertices, this->tri_indices, 0, - *(this->model1), this->tf2, this->tf1, this->nsolver, *(this->result)); - } - - void postprocess() - { - } - - FCL_REAL BVTesting(int b1, int b2) const - { - if(this->enable_statistics) this->num_bv_tests++; - return distance(this->tf2.linear(), this->tf2.translation(), this->model1_bv, this->model2->getBV(b2).bv); - } - - void leafTesting(int b1, int b2) const - { - details::meshShapeDistanceOrientedNodeLeafTesting(b2, b1, this->model2, *(this->model1), this->vertices, this->tri_indices, - this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->request, *(this->result)); - } - -}; - - -/// @brief Traversal node for conservative advancement computation between BVH and shape -template -class MeshShapeConservativeAdvancementTraversalNode : public MeshShapeDistanceTraversalNode -{ -public: - MeshShapeConservativeAdvancementTraversalNode(FCL_REAL w_ = 1) : MeshShapeDistanceTraversalNode() - { - delta_t = 1; - toc = 0; - t_err = (FCL_REAL)0.0001; - - w = w_; - - motion1 = NULL; - motion2 = NULL; - } - - /// @brief BV culling test in one BVTT node - FCL_REAL BVTesting(int b1, int b2) const - { - if(this->enable_statistics) this->num_bv_tests++; - Vector3d P1, P2; - FCL_REAL d = this->model2_bv.distance(this->model1->getBV(b1).bv, &P2, &P1); - - stack.push_back(ConservativeAdvancementStackData(P1, P2, b1, b2, d)); - - return d; - } - - /// @brief Conservative advancement testing between leaves (one triangle and one shape) - void leafTesting(int b1, int b2) const - { - if(this->enable_statistics) this->num_leaf_tests++; - - const BVNode& node = this->model1->getBV(b1); - - int primitive_id = node.primitiveId(); - - const Triangle& tri_id = this->tri_indices[primitive_id]; - - const Vector3d& p1 = this->vertices[tri_id[0]]; - const Vector3d& p2 = this->vertices[tri_id[1]]; - const Vector3d& p3 = this->vertices[tri_id[2]]; - - FCL_REAL d; - Vector3d P1, P2; - this->nsolver->shapeTriangleDistance(*(this->model2), this->tf2, p1, p2, p3, &d, &P2, &P1); - - if(d < this->min_distance) - { - this->min_distance = d; - - closest_p1 = P1; - closest_p2 = P2; - - last_tri_id = primitive_id; - } - - Vector3d n = this->tf2 * p2 - P1; n.normalize(); - // here n should be in global frame - TriangleMotionBoundVisitor mb_visitor1(p1, p2, p3, n); - TBVMotionBoundVisitor mb_visitor2(this->model2_bv, -n); - FCL_REAL bound1 = motion1->computeMotionBound(mb_visitor1); - FCL_REAL bound2 = motion2->computeMotionBound(mb_visitor2); - - FCL_REAL bound = bound1 + bound2; - - FCL_REAL cur_delta_t; - if(bound <= d) cur_delta_t = 1; - else cur_delta_t = d / bound; - - if(cur_delta_t < delta_t) - delta_t = cur_delta_t; - } - - /// @brief Whether the traversal process can stop early - bool canStop(FCL_REAL c) const - { - if((c >= w * (this->min_distance - this->abs_err)) && (c * (1 + this->rel_err) >= w * this->min_distance)) - { - const ConservativeAdvancementStackData& data = stack.back(); - - Vector3d n = this->tf2 * data.P2 - data.P1; n.normalize(); - int c1 = data.c1; - - TBVMotionBoundVisitor mb_visitor1(this->model1->getBV(c1).bv, n); - TBVMotionBoundVisitor mb_visitor2(this->model2_bv, -n); - FCL_REAL bound1 = motion1->computeMotionBound(mb_visitor1); - FCL_REAL bound2 = motion2->computeMotionBound(mb_visitor2); - - FCL_REAL bound = bound1 + bound2; - - FCL_REAL cur_delta_t; - if(bound < c) cur_delta_t = 1; - else cur_delta_t = c / bound; - - if(cur_delta_t < delta_t) - delta_t = cur_delta_t; - - stack.pop_back(); - - return true; - } - else - { - stack.pop_back(); - - return false; - } - } - - mutable FCL_REAL min_distance; - - mutable Vector3d closest_p1, closest_p2; - - mutable int last_tri_id; - - /// @brief CA controlling variable: early stop for the early iterations of CA - FCL_REAL w; - - /// @brief The time from beginning point - FCL_REAL toc; - FCL_REAL t_err; - - /// @brief The delta_t each step - mutable FCL_REAL delta_t; - - /// @brief Motions for the two objects in query - const MotionBase* motion1; - const MotionBase* motion2; - - mutable std::vector stack; -}; - - -template -class ShapeMeshConservativeAdvancementTraversalNode : public ShapeMeshDistanceTraversalNode -{ -public: - ShapeMeshConservativeAdvancementTraversalNode(FCL_REAL w_ = 1) : ShapeMeshDistanceTraversalNode() - { - delta_t = 1; - toc = 0; - t_err = (FCL_REAL)0.0001; - - w = w_; - - motion1 = NULL; - motion2 = NULL; - } - - /// @brief BV culling test in one BVTT node - FCL_REAL BVTesting(int b1, int b2) const - { - if(this->enable_statistics) this->num_bv_tests++; - Vector3d P1, P2; - FCL_REAL d = this->model1_bv.distance(this->model2->getBV(b2).bv, &P1, &P2); - - stack.push_back(ConservativeAdvancementStackData(P1, P2, b1, b2, d)); - - return d; - } - - /// @brief Conservative advancement testing between leaves (one triangle and one shape) - void leafTesting(int b1, int b2) const - { - if(this->enable_statistics) this->num_leaf_tests++; - - const BVNode& node = this->model2->getBV(b2); - - int primitive_id = node.primitiveId(); - - const Triangle& tri_id = this->tri_indices[primitive_id]; - - const Vector3d& p1 = this->vertices[tri_id[0]]; - const Vector3d& p2 = this->vertices[tri_id[1]]; - const Vector3d& p3 = this->vertices[tri_id[2]]; - - FCL_REAL d; - Vector3d P1, P2; - this->nsolver->shapeTriangleDistance(*(this->model1), this->tf1, p1, p2, p3, &d, &P1, &P2); - - if(d < this->min_distance) - { - this->min_distance = d; - - closest_p1 = P1; - closest_p2 = P2; - - last_tri_id = primitive_id; - } - - Vector3d n = P2 - this->tf1 * p1; n.normalize(); - // here n should be in global frame - TBVMotionBoundVisitor mb_visitor1(this->model1_bv, n); - TriangleMotionBoundVisitor mb_visitor2(p1, p2, p3, -n); - FCL_REAL bound1 = motion1->computeMotionBound(mb_visitor1); - FCL_REAL bound2 = motion2->computeMotionBound(mb_visitor2); - - FCL_REAL bound = bound1 + bound2; - - FCL_REAL cur_delta_t; - if(bound <= d) cur_delta_t = 1; - else cur_delta_t = d / bound; - - if(cur_delta_t < delta_t) - delta_t = cur_delta_t; - } - - /// @brief Whether the traversal process can stop early - bool canStop(FCL_REAL c) const - { - if((c >= w * (this->min_distance - this->abs_err)) && (c * (1 + this->rel_err) >= w * this->min_distance)) - { - const ConservativeAdvancementStackData& data = stack.back(); - - Vector3d n = data.P2 - this->tf1 * data.P1; n.normalize(); - int c2 = data.c2; - - TBVMotionBoundVisitor mb_visitor1(this->model1_bv, n); - TBVMotionBoundVisitor mb_visitor2(this->model2->getBV(c2).bv, -n); - FCL_REAL bound1 = motion1->computeMotionBound(mb_visitor1); - FCL_REAL bound2 = motion2->computeMotionBound(mb_visitor2); - - FCL_REAL bound = bound1 + bound2; - - FCL_REAL cur_delta_t; - if(bound < c) cur_delta_t = 1; - else cur_delta_t = c / bound; - - if(cur_delta_t < delta_t) - delta_t = cur_delta_t; - - stack.pop_back(); - - return true; - } - else - { - stack.pop_back(); - - return false; - } - } - - mutable FCL_REAL min_distance; - - mutable Vector3d closest_p1, closest_p2; - - mutable int last_tri_id; - - /// @brief CA controlling variable: early stop for the early iterations of CA - FCL_REAL w; - - /// @brief The time from beginning point - FCL_REAL toc; - FCL_REAL t_err; - - /// @brief The delta_t each step - mutable FCL_REAL delta_t; - - /// @brief Motions for the two objects in query - const MotionBase* motion1; - const MotionBase* motion2; - - mutable std::vector stack; -}; - -namespace details -{ -template -void meshShapeConservativeAdvancementOrientedNodeLeafTesting(int b1, int /* b2 */, - const BVHModel* model1, const S& model2, - const BV& model2_bv, - Vector3d* vertices, Triangle* tri_indices, - const Transform3d& tf1, - const Transform3d& tf2, - const MotionBase* motion1, const MotionBase* motion2, - const NarrowPhaseSolver* nsolver, - bool enable_statistics, - FCL_REAL& min_distance, - Vector3d& p1, Vector3d& p2, - int& last_tri_id, - FCL_REAL& delta_t, - int& num_leaf_tests) -{ - if(enable_statistics) num_leaf_tests++; - - const BVNode& node = model1->getBV(b1); - int primitive_id = node.primitiveId(); - - const Triangle& tri_id = tri_indices[primitive_id]; - const Vector3d& t1 = vertices[tri_id[0]]; - const Vector3d& t2 = vertices[tri_id[1]]; - const Vector3d& t3 = vertices[tri_id[2]]; - - FCL_REAL distance; - Vector3d P1 = Vector3d::Zero(); - Vector3d P2 = Vector3d::Zero(); - nsolver->shapeTriangleDistance(model2, tf2, t1, t2, t3, tf1, &distance, &P2, &P1); - - if(distance < min_distance) - { - min_distance = distance; - - p1 = P1; - p2 = P2; - - last_tri_id = primitive_id; - } - - // n is in global frame - Vector3d n = P2 - P1; n.normalize(); - - TriangleMotionBoundVisitor mb_visitor1(t1, t2, t3, n); - TBVMotionBoundVisitor mb_visitor2(model2_bv, -n); - FCL_REAL bound1 = motion1->computeMotionBound(mb_visitor1); - FCL_REAL bound2 = motion2->computeMotionBound(mb_visitor2); - - FCL_REAL bound = bound1 + bound2; - - FCL_REAL cur_delta_t; - if(bound <= distance) cur_delta_t = 1; - else cur_delta_t = distance / bound; - - if(cur_delta_t < delta_t) - delta_t = cur_delta_t; -} - - -template -bool meshShapeConservativeAdvancementOrientedNodeCanStop(FCL_REAL c, - FCL_REAL min_distance, - FCL_REAL abs_err, FCL_REAL rel_err, FCL_REAL w, - const BVHModel* model1, const S& model2, - const BV& model2_bv, - const MotionBase* motion1, const MotionBase* motion2, - std::vector& stack, - FCL_REAL& delta_t) -{ - if((c >= w * (min_distance - abs_err)) && (c * (1 + rel_err) >= w * min_distance)) - { - const ConservativeAdvancementStackData& data = stack.back(); - Vector3d n = data.P2 - data.P1; n.normalize(); - int c1 = data.c1; - - TBVMotionBoundVisitor mb_visitor1(model1->getBV(c1).bv, n); - TBVMotionBoundVisitor mb_visitor2(model2_bv, -n); - - FCL_REAL bound1 = motion1->computeMotionBound(mb_visitor1); - FCL_REAL bound2 = motion2->computeMotionBound(mb_visitor2); - - FCL_REAL bound = bound1 + bound2; - - FCL_REAL cur_delta_t; - if(bound <= c) cur_delta_t = 1; - else cur_delta_t = c / bound; - - if(cur_delta_t < delta_t) - delta_t = cur_delta_t; - - stack.pop_back(); - - return true; - } - else - { - stack.pop_back(); - return false; - } -} - - -} - -template -class MeshShapeConservativeAdvancementTraversalNodeRSS : public MeshShapeConservativeAdvancementTraversalNode -{ -public: - MeshShapeConservativeAdvancementTraversalNodeRSS(FCL_REAL w_ = 1) : MeshShapeConservativeAdvancementTraversalNode(w_) - { - } - - FCL_REAL BVTesting(int b1, int b2) const - { - if(this->enable_statistics) this->num_bv_tests++; - Vector3d P1, P2; - FCL_REAL d = distance(this->tf1.linear(), this->tf1.translation(), this->model1->getBV(b1).bv, this->model2_bv, &P1, &P2); - - this->stack.push_back(ConservativeAdvancementStackData(P1, P2, b1, b2, d)); - - return d; - } - - void leafTesting(int b1, int b2) const - { - details::meshShapeConservativeAdvancementOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), - this->model2_bv, - this->vertices, this->tri_indices, - this->tf1, this->tf2, - this->motion1, this->motion2, - this->nsolver, - this->enable_statistics, - this->min_distance, - this->closest_p1, this->closest_p2, - this->last_tri_id, - this->delta_t, - this->num_leaf_tests); - } - - bool canStop(FCL_REAL c) const - { - return details::meshShapeConservativeAdvancementOrientedNodeCanStop(c, this->min_distance, - this->abs_err, this->rel_err, this->w, - this->model1, *(this->model2), this->model2_bv, - this->motion1, this->motion2, - this->stack, this->delta_t); - } -}; - - -template -class MeshShapeConservativeAdvancementTraversalNodeOBBRSS : public MeshShapeConservativeAdvancementTraversalNode -{ -public: - MeshShapeConservativeAdvancementTraversalNodeOBBRSS(FCL_REAL w_ = 1) : MeshShapeConservativeAdvancementTraversalNode(w_) - { - } - - FCL_REAL BVTesting(int b1, int b2) const - { - if(this->enable_statistics) this->num_bv_tests++; - Vector3d P1, P2; - FCL_REAL d = distance(this->tf1.linear(), this->tf1.translation(), this->model1->getBV(b1).bv, this->model2_bv, &P1, &P2); - - this->stack.push_back(ConservativeAdvancementStackData(P1, P2, b1, b2, d)); - - return d; - } - - void leafTesting(int b1, int b2) const - { - details::meshShapeConservativeAdvancementOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), - this->model2_bv, - this->vertices, this->tri_indices, - this->tf1, this->tf2, - this->motion1, this->motion2, - this->nsolver, - this->enable_statistics, - this->min_distance, - this->closest_p1, this->closest_p2, - this->last_tri_id, - this->delta_t, - this->num_leaf_tests); - } - - bool canStop(FCL_REAL c) const - { - return details::meshShapeConservativeAdvancementOrientedNodeCanStop(c, this->min_distance, - this->abs_err, this->rel_err, this->w, - this->model1, *(this->model2), this->model2_bv, - this->motion1, this->motion2, - this->stack, this->delta_t); - } -}; - - - -template -class ShapeMeshConservativeAdvancementTraversalNodeRSS : public ShapeMeshConservativeAdvancementTraversalNode -{ -public: - ShapeMeshConservativeAdvancementTraversalNodeRSS(FCL_REAL w_ = 1) : ShapeMeshConservativeAdvancementTraversalNode(w_) - { - } - - FCL_REAL BVTesting(int b1, int b2) const - { - if(this->enable_statistics) this->num_bv_tests++; - Vector3d P1, P2; - FCL_REAL d = distance(this->tf2.linear(), this->tf2.translation(), this->model2->getBV(b2).bv, this->model1_bv, &P2, &P1); - - this->stack.push_back(ConservativeAdvancementStackData(P1, P2, b1, b2, d)); - - return d; - } - - void leafTesting(int b1, int b2) const - { - details::meshShapeConservativeAdvancementOrientedNodeLeafTesting(b2, b1, this->model2, *(this->model1), - this->model1_bv, - this->vertices, this->tri_indices, - this->tf2, this->tf1, - this->motion2, this->motion1, - this->nsolver, - this->enable_statistics, - this->min_distance, - this->closest_p2, this->closest_p1, - this->last_tri_id, - this->delta_t, - this->num_leaf_tests); - } - - bool canStop(FCL_REAL c) const - { - return details::meshShapeConservativeAdvancementOrientedNodeCanStop(c, this->min_distance, - this->abs_err, this->rel_err, this->w, - this->model2, *(this->model1), this->model1_bv, - this->motion2, this->motion1, - this->stack, this->delta_t); - } -}; - - -template -class ShapeMeshConservativeAdvancementTraversalNodeOBBRSS : public ShapeMeshConservativeAdvancementTraversalNode -{ -public: - ShapeMeshConservativeAdvancementTraversalNodeOBBRSS(FCL_REAL w_ = 1) : ShapeMeshConservativeAdvancementTraversalNode(w_) - { - } - - FCL_REAL BVTesting(int b1, int b2) const - { - if(this->enable_statistics) this->num_bv_tests++; - Vector3d P1, P2; - FCL_REAL d = distance(this->tf2.linear(), this->tf2.translation(), this->model2->getBV(b2).bv, this->model1_bv, &P2, &P1); - - this->stack.push_back(ConservativeAdvancementStackData(P1, P2, b1, b2, d)); - - return d; - } - - void leafTesting(int b1, int b2) const - { - details::meshShapeConservativeAdvancementOrientedNodeLeafTesting(b2, b1, this->model2, *(this->model1), - this->model1_bv, - this->vertices, this->tri_indices, - this->tf2, this->tf1, - this->motion2, this->motion1, - this->nsolver, - this->enable_statistics, - this->min_distance, - this->closest_p2, this->closest_p1, - this->last_tri_id, - this->delta_t, - this->num_leaf_tests); - } - - bool canStop(FCL_REAL c) const - { - return details::meshShapeConservativeAdvancementOrientedNodeCanStop(c, this->min_distance, - this->abs_err, this->rel_err, this->w, - this->model2, *(this->model1), this->model1_bv, - this->motion2, this->motion1, - this->stack, this->delta_t); - } -}; - - -} +#warning "This header has been deprecated in FCL 0.6. " #endif diff --git a/include/fcl/traversal/traversal_node_bvhs.h b/include/fcl/traversal/traversal_node_bvhs.h index acb8ac565..4cc3e33a5 100644 --- a/include/fcl/traversal/traversal_node_bvhs.h +++ b/include/fcl/traversal/traversal_node_bvhs.h @@ -35,958 +35,9 @@ /** \author Jia Pan */ - #ifndef FCL_TRAVERSAL_NODE_MESHES_H #define FCL_TRAVERSAL_NODE_MESHES_H -#include "fcl/collision_data.h" -#include "fcl/traversal/traversal_node_base.h" -#include "fcl/BV/BV_node.h" -#include "fcl/BV/BV.h" -#include "fcl/BVH/BVH_model.h" -#include "fcl/intersect.h" -#include "fcl/ccd/motion.h" - -#include -#include -#include -#include - - -namespace fcl -{ - -/// @brief Traversal node for collision between BVH models -template -class BVHCollisionTraversalNode : public CollisionTraversalNodeBase -{ -public: - BVHCollisionTraversalNode() : CollisionTraversalNodeBase() - { - model1 = NULL; - model2 = NULL; - - num_bv_tests = 0; - num_leaf_tests = 0; - query_time_seconds = 0.0; - } - - /// @brief Whether the BV node in the first BVH tree is leaf - bool isFirstNodeLeaf(int b) const - { - return model1->getBV(b).isLeaf(); - } - - /// @brief Whether the BV node in the second BVH tree is leaf - bool isSecondNodeLeaf(int b) const - { - return model2->getBV(b).isLeaf(); - } - - /// @brief Determine the traversal order, is the first BVTT subtree better - bool firstOverSecond(int b1, int b2) const - { - FCL_REAL sz1 = model1->getBV(b1).bv.size(); - FCL_REAL sz2 = model2->getBV(b2).bv.size(); - - bool l1 = model1->getBV(b1).isLeaf(); - bool l2 = model2->getBV(b2).isLeaf(); - - if(l2 || (!l1 && (sz1 > sz2))) - return true; - return false; - } - - /// @brief Obtain the left child of BV node in the first BVH - int getFirstLeftChild(int b) const - { - return model1->getBV(b).leftChild(); - } - - /// @brief Obtain the right child of BV node in the first BVH - int getFirstRightChild(int b) const - { - return model1->getBV(b).rightChild(); - } - - /// @brief Obtain the left child of BV node in the second BVH - int getSecondLeftChild(int b) const - { - return model2->getBV(b).leftChild(); - } - - /// @brief Obtain the right child of BV node in the second BVH - int getSecondRightChild(int b) const - { - return model2->getBV(b).rightChild(); - } - - /// @brief BV culling test in one BVTT node - bool BVTesting(int b1, int b2) const - { - if(enable_statistics) num_bv_tests++; - return !model1->getBV(b1).overlap(model2->getBV(b2)); - } - - /// @brief The first BVH model - const BVHModel* model1; - /// @brief The second BVH model - const BVHModel* model2; - - /// @brief statistical information - mutable int num_bv_tests; - mutable int num_leaf_tests; - mutable FCL_REAL query_time_seconds; -}; - - -/// @brief Traversal node for collision between two meshes -template -class MeshCollisionTraversalNode : public BVHCollisionTraversalNode -{ -public: - MeshCollisionTraversalNode() : BVHCollisionTraversalNode() - { - vertices1 = NULL; - vertices2 = NULL; - tri_indices1 = NULL; - tri_indices2 = NULL; - } - - /// @brief Intersection testing between leaves (two triangles) - void leafTesting(int b1, int b2) const - { - if(this->enable_statistics) this->num_leaf_tests++; - - const BVNode& node1 = this->model1->getBV(b1); - const BVNode& node2 = this->model2->getBV(b2); - - int primitive_id1 = node1.primitiveId(); - int primitive_id2 = node2.primitiveId(); - - const Triangle& tri_id1 = tri_indices1[primitive_id1]; - const Triangle& tri_id2 = tri_indices2[primitive_id2]; - - const Vector3d& p1 = vertices1[tri_id1[0]]; - const Vector3d& p2 = vertices1[tri_id1[1]]; - const Vector3d& p3 = vertices1[tri_id1[2]]; - const Vector3d& q1 = vertices2[tri_id2[0]]; - const Vector3d& q2 = vertices2[tri_id2[1]]; - const Vector3d& q3 = vertices2[tri_id2[2]]; - - if(this->model1->isOccupied() && this->model2->isOccupied()) - { - bool is_intersect = false; - - if(!this->request.enable_contact) // only interested in collision or not - { - if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3)) - { - is_intersect = true; - if(this->result->numContacts() < this->request.num_max_contacts) - this->result->addContact(Contact(this->model1, this->model2, primitive_id1, primitive_id2)); - } - } - else // need compute the contact information - { - FCL_REAL penetration; - Vector3d normal; - unsigned int n_contacts; - Vector3d contacts[2]; - - if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3, - contacts, - &n_contacts, - &penetration, - &normal)) - { - is_intersect = true; - - if(this->request.num_max_contacts < n_contacts + this->result->numContacts()) - n_contacts = (this->request.num_max_contacts >= this->result->numContacts()) ? (this->request.num_max_contacts - this->result->numContacts()) : 0; - - for(unsigned int i = 0; i < n_contacts; ++i) - { - this->result->addContact(Contact(this->model1, this->model2, primitive_id1, primitive_id2, contacts[i], normal, penetration)); - } - } - } - - if(is_intersect && this->request.enable_cost) - { - AABB overlap_part; - AABB(p1, p2, p3).overlap(AABB(q1, q2, q3), overlap_part); - this->result->addCostSource(CostSource(overlap_part, cost_density), this->request.num_max_cost_sources); - } - } - else if((!this->model1->isFree() && !this->model2->isFree()) && this->request.enable_cost) - { - if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3)) - { - AABB overlap_part; - AABB(p1, p2, p3).overlap(AABB(q1, q2, q3), overlap_part); - this->result->addCostSource(CostSource(overlap_part, cost_density), this->request.num_max_cost_sources); - } - } - } - - /// @brief Whether the traversal process can stop early - bool canStop() const - { - return this->request.isSatisfied(*(this->result)); - } - - Vector3d* vertices1; - Vector3d* vertices2; - - Triangle* tri_indices1; - Triangle* tri_indices2; - - FCL_REAL cost_density; -}; - - -/// @brief Traversal node for collision between two meshes if their underlying BVH node is oriented node (OBB, RSS, OBBRSS, kIOS) -class MeshCollisionTraversalNodeOBB : public MeshCollisionTraversalNode -{ -public: - MeshCollisionTraversalNodeOBB(); - - bool BVTesting(int b1, int b2) const; - - void leafTesting(int b1, int b2) const; - - bool BVTesting(int b1, int b2, const Matrix3d& Rc, const Vector3d& Tc) const; - - void leafTesting(int b1, int b2, const Matrix3d& Rc, const Vector3d& Tc) const; - - Matrix3d R; - Vector3d T; -}; - -class MeshCollisionTraversalNodeRSS : public MeshCollisionTraversalNode -{ -public: - MeshCollisionTraversalNodeRSS(); - - bool BVTesting(int b1, int b2) const; - - void leafTesting(int b1, int b2) const; - - bool BVTesting(int b1, int b2, const Matrix3d& Rc, const Vector3d& Tc) const; - - void leafTesting(int b1, int b2, const Matrix3d& Rc, const Vector3d& Tc) const; - - Matrix3d R; - Vector3d T; -}; - -class MeshCollisionTraversalNodekIOS : public MeshCollisionTraversalNode -{ -public: - MeshCollisionTraversalNodekIOS(); - - bool BVTesting(int b1, int b2) const; - - void leafTesting(int b1, int b2) const; - - Matrix3d R; - Vector3d T; -}; - -class MeshCollisionTraversalNodeOBBRSS : public MeshCollisionTraversalNode -{ -public: - MeshCollisionTraversalNodeOBBRSS(); - - bool BVTesting(int b1, int b2) const; - - void leafTesting(int b1, int b2) const; - - Matrix3d R; - Vector3d T; -}; - -/// @brief Traversal node for continuous collision between BVH models -struct BVHContinuousCollisionPair -{ - BVHContinuousCollisionPair() {} - - BVHContinuousCollisionPair(int id1_, int id2_, FCL_REAL time) : id1(id1_), id2(id2_), collision_time(time) {} - - /// @brief The index of one in-collision primitive - int id1; - - /// @brief The index of the other in-collision primitive - int id2; - - /// @brief Collision time normalized in [0, 1]. The collision time out of [0, 1] means collision-free - FCL_REAL collision_time; -}; - -/// @brief Traversal node for continuous collision between meshes -template -class MeshContinuousCollisionTraversalNode : public BVHCollisionTraversalNode -{ -public: - MeshContinuousCollisionTraversalNode() : BVHCollisionTraversalNode() - { - vertices1 = NULL; - vertices2 = NULL; - tri_indices1 = NULL; - tri_indices2 = NULL; - prev_vertices1 = NULL; - prev_vertices2 = NULL; - - num_vf_tests = 0; - num_ee_tests = 0; - time_of_contact = 1; - } - - /// @brief Intersection testing between leaves (two triangles) - void leafTesting(int b1, int b2) const - { - if(this->enable_statistics) this->num_leaf_tests++; - - const BVNode& node1 = this->model1->getBV(b1); - const BVNode& node2 = this->model2->getBV(b2); - - FCL_REAL collision_time = 2; - Vector3d collision_pos; - - int primitive_id1 = node1.primitiveId(); - int primitive_id2 = node2.primitiveId(); - - const Triangle& tri_id1 = tri_indices1[primitive_id1]; - const Triangle& tri_id2 = tri_indices2[primitive_id2]; - - Vector3d* S0[3]; - Vector3d* S1[3]; - Vector3d* T0[3]; - Vector3d* T1[3]; - - - for(int i = 0; i < 3; ++i) - { - S0[i] = prev_vertices1 + tri_id1[i]; - S1[i] = vertices1 + tri_id1[i]; - T0[i] = prev_vertices2 + tri_id2[i]; - T1[i] = vertices2 + tri_id2[i]; - } - - FCL_REAL tmp; - Vector3d tmpv; - - // 6 VF checks - for(int i = 0; i < 3; ++i) - { - if(this->enable_statistics) num_vf_tests++; - if(Intersect::intersect_VF(*(S0[0]), *(S0[1]), *(S0[2]), *(T0[i]), *(S1[0]), *(S1[1]), *(S1[2]), *(T1[i]), &tmp, &tmpv)) - { - if(collision_time > tmp) - { - collision_time = tmp; collision_pos = tmpv; - } - } - - if(this->enable_statistics) num_vf_tests++; - if(Intersect::intersect_VF(*(T0[0]), *(T0[1]), *(T0[2]), *(S0[i]), *(T1[0]), *(T1[1]), *(T1[2]), *(S1[i]), &tmp, &tmpv)) - { - if(collision_time > tmp) - { - collision_time = tmp; collision_pos = tmpv; - } - } - } - - // 9 EE checks - for(int i = 0; i < 3; ++i) - { - int S_id1 = i; - int S_id2 = i + 1; - if(S_id2 == 3) S_id2 = 0; - for(int j = 0; j < 3; ++j) - { - int T_id1 = j; - int T_id2 = j + 1; - if(T_id2 == 3) T_id2 = 0; - - num_ee_tests++; - if(Intersect::intersect_EE(*(S0[S_id1]), *(S0[S_id2]), *(T0[T_id1]), *(T0[T_id2]), *(S1[S_id1]), *(S1[S_id2]), *(T1[T_id1]), *(T1[T_id2]), &tmp, &tmpv)) - { - if(collision_time > tmp) - { - collision_time = tmp; collision_pos = tmpv; - } - } - } - } - - if(!(collision_time > 1)) // collision happens - { - pairs.push_back(BVHContinuousCollisionPair(primitive_id1, primitive_id2, collision_time)); - time_of_contact = std::min(time_of_contact, collision_time); - } - } - - /// @brief Whether the traversal process can stop early - bool canStop() const - { - return (pairs.size() > 0) && (this->request.num_max_contacts <= pairs.size()); - } - - Vector3d* vertices1; - Vector3d* vertices2; - - Triangle* tri_indices1; - Triangle* tri_indices2; - - Vector3d* prev_vertices1; - Vector3d* prev_vertices2; - - mutable int num_vf_tests; - mutable int num_ee_tests; - - mutable std::vector pairs; - - mutable FCL_REAL time_of_contact; -}; - - - -/// @brief Traversal node for distance computation between BVH models -template -class BVHDistanceTraversalNode : public DistanceTraversalNodeBase -{ -public: - BVHDistanceTraversalNode() : DistanceTraversalNodeBase() - { - model1 = NULL; - model2 = NULL; - - num_bv_tests = 0; - num_leaf_tests = 0; - query_time_seconds = 0.0; - } - - /// @brief Whether the BV node in the first BVH tree is leaf - bool isFirstNodeLeaf(int b) const - { - return model1->getBV(b).isLeaf(); - } - - /// @brief Whether the BV node in the second BVH tree is leaf - bool isSecondNodeLeaf(int b) const - { - return model2->getBV(b).isLeaf(); - } - - /// @brief Determine the traversal order, is the first BVTT subtree better - bool firstOverSecond(int b1, int b2) const - { - FCL_REAL sz1 = model1->getBV(b1).bv.size(); - FCL_REAL sz2 = model2->getBV(b2).bv.size(); - - bool l1 = model1->getBV(b1).isLeaf(); - bool l2 = model2->getBV(b2).isLeaf(); - - if(l2 || (!l1 && (sz1 > sz2))) - return true; - return false; - } - - /// @brief Obtain the left child of BV node in the first BVH - int getFirstLeftChild(int b) const - { - return model1->getBV(b).leftChild(); - } - - /// @brief Obtain the right child of BV node in the first BVH - int getFirstRightChild(int b) const - { - return model1->getBV(b).rightChild(); - } - - /// @brief Obtain the left child of BV node in the second BVH - int getSecondLeftChild(int b) const - { - return model2->getBV(b).leftChild(); - } - - /// @brief Obtain the right child of BV node in the second BVH - int getSecondRightChild(int b) const - { - return model2->getBV(b).rightChild(); - } - - /// @brief BV culling test in one BVTT node - FCL_REAL BVTesting(int b1, int b2) const - { - if(enable_statistics) num_bv_tests++; - return model1->getBV(b1).distance(model2->getBV(b2)); - } - - /// @brief The first BVH model - const BVHModel* model1; - /// @brief The second BVH model - const BVHModel* model2; - - /// @brief statistical information - mutable int num_bv_tests; - mutable int num_leaf_tests; - mutable FCL_REAL query_time_seconds; -}; - - -/// @brief Traversal node for distance computation between two meshes -template -class MeshDistanceTraversalNode : public BVHDistanceTraversalNode -{ -public: - MeshDistanceTraversalNode() : BVHDistanceTraversalNode() - { - vertices1 = NULL; - vertices2 = NULL; - tri_indices1 = NULL; - tri_indices2 = NULL; - - rel_err = this->request.rel_err; - abs_err = this->request.abs_err; - } - - /// @brief Distance testing between leaves (two triangles) - void leafTesting(int b1, int b2) const - { - if(this->enable_statistics) this->num_leaf_tests++; - - const BVNode& node1 = this->model1->getBV(b1); - const BVNode& node2 = this->model2->getBV(b2); - - int primitive_id1 = node1.primitiveId(); - int primitive_id2 = node2.primitiveId(); - - const Triangle& tri_id1 = tri_indices1[primitive_id1]; - const Triangle& tri_id2 = tri_indices2[primitive_id2]; - - const Vector3d& t11 = vertices1[tri_id1[0]]; - const Vector3d& t12 = vertices1[tri_id1[1]]; - const Vector3d& t13 = vertices1[tri_id1[2]]; - - const Vector3d& t21 = vertices2[tri_id2[0]]; - const Vector3d& t22 = vertices2[tri_id2[1]]; - const Vector3d& t23 = vertices2[tri_id2[2]]; - - // nearest point pair - Vector3d P1, P2; - - FCL_REAL d = TriangleDistance::triDistance(t11, t12, t13, t21, t22, t23, - P1, P2); - - if(this->request.enable_nearest_points) - { - this->result->update(d, this->model1, this->model2, primitive_id1, primitive_id2, P1, P2); - } - else - { - this->result->update(d, this->model1, this->model2, primitive_id1, primitive_id2); - } - } - - /// @brief Whether the traversal process can stop early - bool canStop(FCL_REAL c) const - { - if((c >= this->result->min_distance - abs_err) && (c * (1 + rel_err) >= this->result->min_distance)) - return true; - return false; - } - - Vector3d* vertices1; - Vector3d* vertices2; - - Triangle* tri_indices1; - Triangle* tri_indices2; - - /// @brief relative and absolute error, default value is 0.01 for both terms - FCL_REAL rel_err; - FCL_REAL abs_err; -}; - -/// @brief Traversal node for distance computation between two meshes if their underlying BVH node is oriented node (RSS, OBBRSS, kIOS) -class MeshDistanceTraversalNodeRSS : public MeshDistanceTraversalNode -{ -public: - MeshDistanceTraversalNodeRSS(); - - void preprocess(); - - void postprocess(); - - FCL_REAL BVTesting(int b1, int b2) const; - - void leafTesting(int b1, int b2) const; - - Matrix3d R; - Vector3d T; -}; - - -class MeshDistanceTraversalNodekIOS : public MeshDistanceTraversalNode -{ -public: - MeshDistanceTraversalNodekIOS(); - - void preprocess(); - - void postprocess(); - - FCL_REAL BVTesting(int b1, int b2) const; - - void leafTesting(int b1, int b2) const; - - Matrix3d R; - Vector3d T; -}; - -class MeshDistanceTraversalNodeOBBRSS : public MeshDistanceTraversalNode -{ -public: - MeshDistanceTraversalNodeOBBRSS(); - - void preprocess(); - - void postprocess(); - - FCL_REAL BVTesting(int b1, int b2) const; - - void leafTesting(int b1, int b2) const; - - Matrix3d R; - Vector3d T; -}; - - - -/// @brief continuous collision node using conservative advancement. when using this default version, must refit the BVH in current configuration (R_t, T_t) into default configuration -template -class MeshConservativeAdvancementTraversalNode : public MeshDistanceTraversalNode -{ -public: - MeshConservativeAdvancementTraversalNode(FCL_REAL w_ = 1) : MeshDistanceTraversalNode() - { - delta_t = 1; - toc = 0; - t_err = (FCL_REAL)0.00001; - - w = w_; - - motion1 = NULL; - motion2 = NULL; - } - - /// @brief BV culling test in one BVTT node - FCL_REAL BVTesting(int b1, int b2) const - { - if(this->enable_statistics) this->num_bv_tests++; - Vector3d P1, P2; - FCL_REAL d = this->model1->getBV(b1).distance(this->model2->getBV(b2), &P1, &P2); - - stack.push_back(ConservativeAdvancementStackData(P1, P2, b1, b2, d)); - - return d; - } - - /// @brief Conservative advancement testing between leaves (two triangles) - void leafTesting(int b1, int b2) const - { - if(this->enable_statistics) this->num_leaf_tests++; - - const BVNode& node1 = this->model1->getBV(b1); - const BVNode& node2 = this->model2->getBV(b2); - - int primitive_id1 = node1.primitiveId(); - int primitive_id2 = node2.primitiveId(); - - const Triangle& tri_id1 = this->tri_indices1[primitive_id1]; - const Triangle& tri_id2 = this->tri_indices2[primitive_id2]; - - const Vector3d& p1 = this->vertices1[tri_id1[0]]; - const Vector3d& p2 = this->vertices1[tri_id1[1]]; - const Vector3d& p3 = this->vertices1[tri_id1[2]]; - - const Vector3d& q1 = this->vertices2[tri_id2[0]]; - const Vector3d& q2 = this->vertices2[tri_id2[1]]; - const Vector3d& q3 = this->vertices2[tri_id2[2]]; - - // nearest point pair - Vector3d P1, P2; - - FCL_REAL d = TriangleDistance::triDistance(p1, p2, p3, q1, q2, q3, - P1, P2); - - if(d < this->min_distance) - { - this->min_distance = d; - - closest_p1 = P1; - closest_p2 = P2; - - last_tri_id1 = primitive_id1; - last_tri_id2 = primitive_id2; - } - - - Vector3d n = P2 - P1; - n.normalize(); - // here n is already in global frame as we assume the body is in original configuration (I, 0) for general BVH - TriangleMotionBoundVisitor mb_visitor1(p1, p2, p3, n), mb_visitor2(q1, q2, q3, n); - FCL_REAL bound1 = motion1->computeMotionBound(mb_visitor1); - FCL_REAL bound2 = motion2->computeMotionBound(mb_visitor2); - - FCL_REAL bound = bound1 + bound2; - - FCL_REAL cur_delta_t; - if(bound <= d) cur_delta_t = 1; - else cur_delta_t = d / bound; - - if(cur_delta_t < delta_t) - delta_t = cur_delta_t; - } - - /// @brief Whether the traversal process can stop early - bool canStop(FCL_REAL c) const - { - if((c >= w * (this->min_distance - this->abs_err)) && (c * (1 + this->rel_err) >= w * this->min_distance)) - { - const ConservativeAdvancementStackData& data = stack.back(); - FCL_REAL d = data.d; - Vector3d n; - int c1, c2; - - if(d > c) - { - const ConservativeAdvancementStackData& data2 = stack[stack.size() - 2]; - d = data2.d; - n = data2.P2 - data2.P1; n.normalize(); - c1 = data2.c1; - c2 = data2.c2; - stack[stack.size() - 2] = stack[stack.size() - 1]; - } - else - { - n = data.P2 - data.P1; n.normalize(); - c1 = data.c1; - c2 = data.c2; - } - - assert(c == d); - - TBVMotionBoundVisitor mb_visitor1(this->model1->getBV(c1).bv, n), mb_visitor2(this->model2->getBV(c2).bv, n); - FCL_REAL bound1 = motion1->computeMotionBound(mb_visitor1); - FCL_REAL bound2 = motion2->computeMotionBound(mb_visitor2); - - FCL_REAL bound = bound1 + bound2; - - FCL_REAL cur_delta_t; - if(bound <= c) cur_delta_t = 1; - else cur_delta_t = c / bound; - - if(cur_delta_t < delta_t) - delta_t = cur_delta_t; - - stack.pop_back(); - - return true; - } - else - { - const ConservativeAdvancementStackData& data = stack.back(); - FCL_REAL d = data.d; - - if(d > c) - stack[stack.size() - 2] = stack[stack.size() - 1]; - - stack.pop_back(); - - return false; - } - } - - mutable FCL_REAL min_distance; - - mutable Vector3d closest_p1, closest_p2; - - mutable int last_tri_id1, last_tri_id2; - - - /// @brief CA controlling variable: early stop for the early iterations of CA - FCL_REAL w; - - /// @brief The time from beginning point - FCL_REAL toc; - FCL_REAL t_err; - - /// @brief The delta_t each step - mutable FCL_REAL delta_t; - - /// @brief Motions for the two objects in query - const MotionBase* motion1; - const MotionBase* motion2; - - mutable std::vector stack; -}; - - -/// @brief for OBB and RSS, there is local coordinate of BV, so normal need to be transformed -namespace details -{ - -template -const Vector3d getBVAxis(const BV& bv, int i) -{ - return bv.axis.col(i); -} - -template<> -inline const Vector3d getBVAxis(const OBBRSS& bv, int i) -{ - return bv.obb.axis.col(i); -} - - -template -bool meshConservativeAdvancementTraversalNodeCanStop(FCL_REAL c, - FCL_REAL min_distance, - FCL_REAL abs_err, FCL_REAL rel_err, FCL_REAL w, - const BVHModel* model1, const BVHModel* model2, - const MotionBase* motion1, const MotionBase* motion2, - std::vector& stack, - FCL_REAL& delta_t) -{ - if((c >= w * (min_distance - abs_err)) && (c * (1 + rel_err) >= w * min_distance)) - { - const ConservativeAdvancementStackData& data = stack.back(); - FCL_REAL d = data.d; - Vector3d n; - int c1, c2; - - if(d > c) - { - const ConservativeAdvancementStackData& data2 = stack[stack.size() - 2]; - d = data2.d; - n = data2.P2 - data2.P1; n.normalize(); - c1 = data2.c1; - c2 = data2.c2; - stack[stack.size() - 2] = stack[stack.size() - 1]; - } - else - { - n = data.P2 - data.P1; n.normalize(); - c1 = data.c1; - c2 = data.c2; - } - - assert(c == d); - - Vector3d n_transformed = - getBVAxis(model1->getBV(c1).bv, 0) * n[0] + - getBVAxis(model1->getBV(c1).bv, 1) * n[1] + - getBVAxis(model1->getBV(c1).bv, 2) * n[2]; - - TBVMotionBoundVisitor mb_visitor1(model1->getBV(c1).bv, n_transformed), mb_visitor2(model2->getBV(c2).bv, n_transformed); - FCL_REAL bound1 = motion1->computeMotionBound(mb_visitor1); - FCL_REAL bound2 = motion2->computeMotionBound(mb_visitor2); - - FCL_REAL bound = bound1 + bound2; - - FCL_REAL cur_delta_t; - if(bound <= c) cur_delta_t = 1; - else cur_delta_t = c / bound; - - if(cur_delta_t < delta_t) - delta_t = cur_delta_t; - - stack.pop_back(); - - return true; - } - else - { - const ConservativeAdvancementStackData& data = stack.back(); - FCL_REAL d = data.d; - - if(d > c) - stack[stack.size() - 2] = stack[stack.size() - 1]; - - stack.pop_back(); - - return false; - } -} - -} - -/// for OBB, RSS and OBBRSS, there is local coordinate of BV, so normal need to be transformed -template<> -inline bool MeshConservativeAdvancementTraversalNode::canStop(FCL_REAL c) const -{ - return details::meshConservativeAdvancementTraversalNodeCanStop(c, this->min_distance, - this->abs_err, this->rel_err, w, - this->model1, this->model2, - motion1, motion2, - stack, delta_t); -} - -template<> -inline bool MeshConservativeAdvancementTraversalNode::canStop(FCL_REAL c) const -{ - return details::meshConservativeAdvancementTraversalNodeCanStop(c, this->min_distance, - this->abs_err, this->rel_err, w, - this->model1, this->model2, - motion1, motion2, - stack, delta_t); -} - -template<> -inline bool MeshConservativeAdvancementTraversalNode::canStop(FCL_REAL c) const -{ - return details::meshConservativeAdvancementTraversalNodeCanStop(c, this->min_distance, - this->abs_err, this->rel_err, w, - this->model1, this->model2, - motion1, motion2, - stack, delta_t); -} - - -class MeshConservativeAdvancementTraversalNodeRSS : public MeshConservativeAdvancementTraversalNode -{ -public: - MeshConservativeAdvancementTraversalNodeRSS(FCL_REAL w_ = 1); - - FCL_REAL BVTesting(int b1, int b2) const; - - void leafTesting(int b1, int b2) const; - - bool canStop(FCL_REAL c) const; - - Matrix3d R; - Vector3d T; -}; - -class MeshConservativeAdvancementTraversalNodeOBBRSS : public MeshConservativeAdvancementTraversalNode -{ -public: - MeshConservativeAdvancementTraversalNodeOBBRSS(FCL_REAL w_ = 1); - - FCL_REAL BVTesting(int b1, int b2) const; - - void leafTesting(int b1, int b2) const; - - bool canStop(FCL_REAL c) const; - - Matrix3d R; - Vector3d T; -}; -} - +#warning "This header has been deprecated in FCL 0.6. " #endif diff --git a/include/fcl/traversal/traversal_node_octree.h b/include/fcl/traversal/traversal_node_octree.h index 78653ad48..83a7734a6 100644 --- a/include/fcl/traversal/traversal_node_octree.h +++ b/include/fcl/traversal/traversal_node_octree.h @@ -38,1330 +38,6 @@ #ifndef FCL_TRAVERSAL_NODE_OCTREE_H #define FCL_TRAVERSAL_NODE_OCTREE_H -#include "fcl/config.h" -#if not(FCL_HAVE_OCTOMAP) -#error "This header requires fcl to be compiled with octomap support" -#endif - -#include "fcl/collision_data.h" -#include "fcl/traversal/traversal_node_base.h" -#include "fcl/narrowphase/narrowphase.h" -#include "fcl/shape/geometric_shapes_utility.h" -#include "fcl/octree.h" -#include "fcl/BVH/BVH_model.h" - -namespace fcl -{ - -/// @brief Algorithms for collision related with octree -template -class OcTreeSolver -{ -private: - const NarrowPhaseSolver* solver; - - mutable const CollisionRequest* crequest; - mutable const DistanceRequest* drequest; - - mutable CollisionResult* cresult; - mutable DistanceResult* dresult; - -public: - OcTreeSolver(const NarrowPhaseSolver* solver_) : solver(solver_), - crequest(NULL), - drequest(NULL), - cresult(NULL), - dresult(NULL) - { - } - - /// @brief collision between two octrees - void OcTreeIntersect(const OcTree* tree1, const OcTree* tree2, - const Transform3d& tf1, const Transform3d& tf2, - const CollisionRequest& request_, - CollisionResult& result_) const - { - crequest = &request_; - cresult = &result_; - - OcTreeIntersectRecurse(tree1, tree1->getRoot(), tree1->getRootBV(), - tree2, tree2->getRoot(), tree2->getRootBV(), - tf1, tf2); - } - - /// @brief distance between two octrees - void OcTreeDistance(const OcTree* tree1, const OcTree* tree2, - const Transform3d& tf1, const Transform3d& tf2, - const DistanceRequest& request_, - DistanceResult& result_) const - { - drequest = &request_; - dresult = &result_; - - OcTreeDistanceRecurse(tree1, tree1->getRoot(), tree1->getRootBV(), - tree2, tree2->getRoot(), tree2->getRootBV(), - tf1, tf2); - } - - /// @brief collision between octree and mesh - template - void OcTreeMeshIntersect(const OcTree* tree1, const BVHModel* tree2, - const Transform3d& tf1, const Transform3d& tf2, - const CollisionRequest& request_, - CollisionResult& result_) const - { - crequest = &request_; - cresult = &result_; - - OcTreeMeshIntersectRecurse(tree1, tree1->getRoot(), tree1->getRootBV(), - tree2, 0, - tf1, tf2); - } - - /// @brief distance between octree and mesh - template - void OcTreeMeshDistance(const OcTree* tree1, const BVHModel* tree2, - const Transform3d& tf1, const Transform3d& tf2, - const DistanceRequest& request_, - DistanceResult& result_) const - { - drequest = &request_; - dresult = &result_; - - OcTreeMeshDistanceRecurse(tree1, tree1->getRoot(), tree1->getRootBV(), - tree2, 0, - tf1, tf2); - } - - /// @brief collision between mesh and octree - template - void MeshOcTreeIntersect(const BVHModel* tree1, const OcTree* tree2, - const Transform3d& tf1, const Transform3d& tf2, - const CollisionRequest& request_, - CollisionResult& result_) const - - { - crequest = &request_; - cresult = &result_; - - OcTreeMeshIntersectRecurse(tree2, tree2->getRoot(), tree2->getRootBV(), - tree1, 0, - tf2, tf1); - } - - /// @brief distance between mesh and octree - template - void MeshOcTreeDistance(const BVHModel* tree1, const OcTree* tree2, - const Transform3d& tf1, const Transform3d& tf2, - const DistanceRequest& request_, - DistanceResult& result_) const - { - drequest = &request_; - dresult = &result_; - - OcTreeMeshDistanceRecurse(tree1, 0, - tree2, tree2->getRoot(), tree2->getRootBV(), - tf1, tf2); - } - - /// @brief collision between octree and shape - template - void OcTreeShapeIntersect(const OcTree* tree, const S& s, - const Transform3d& tf1, const Transform3d& tf2, - const CollisionRequest& request_, - CollisionResult& result_) const - { - crequest = &request_; - cresult = &result_; - - AABB bv2; - computeBV(s, Transform3d::Identity(), bv2); - OBB obb2; - convertBV(bv2, tf2, obb2); - OcTreeShapeIntersectRecurse(tree, tree->getRoot(), tree->getRootBV(), - s, obb2, - tf1, tf2); - - } - - /// @brief collision between shape and octree - template - void ShapeOcTreeIntersect(const S& s, const OcTree* tree, - const Transform3d& tf1, const Transform3d& tf2, - const CollisionRequest& request_, - CollisionResult& result_) const - { - crequest = &request_; - cresult = &result_; - - AABB bv1; - computeBV(s, Transform3d::Identity(), bv1); - OBB obb1; - convertBV(bv1, tf1, obb1); - OcTreeShapeIntersectRecurse(tree, tree->getRoot(), tree->getRootBV(), - s, obb1, - tf2, tf1); - } - - /// @brief distance between octree and shape - template - void OcTreeShapeDistance(const OcTree* tree, const S& s, - const Transform3d& tf1, const Transform3d& tf2, - const DistanceRequest& request_, - DistanceResult& result_) const - { - drequest = &request_; - dresult = &result_; - - AABB aabb2; - computeBV(s, tf2, aabb2); - OcTreeShapeDistanceRecurse(tree, tree->getRoot(), tree->getRootBV(), - s, aabb2, - tf1, tf2); - } - - /// @brief distance between shape and octree - template - void ShapeOcTreeDistance(const S& s, const OcTree* tree, - const Transform3d& tf1, const Transform3d& tf2, - const DistanceRequest& request_, - DistanceResult& result_) const - { - drequest = &request_; - dresult = &result_; - - AABB aabb1; - computeBV(s, tf1, aabb1); - OcTreeShapeDistanceRecurse(tree, tree->getRoot(), tree->getRootBV(), - s, aabb1, - tf2, tf1); - } - - -private: - template - bool OcTreeShapeDistanceRecurse(const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1, - const S& s, const AABB& aabb2, - const Transform3d& tf1, const Transform3d& tf2) const - { - if(!tree1->nodeHasChildren(root1)) - { - if(tree1->isNodeOccupied(root1)) - { - Box box; - Transform3d box_tf; - constructBox(bv1, tf1, box, box_tf); - - FCL_REAL dist; - Vector3d closest_p1, closest_p2; - solver->shapeDistance(box, box_tf, s, tf2, &dist, &closest_p1, &closest_p2); - - dresult->update(dist, tree1, &s, root1 - tree1->getRoot(), DistanceResult::NONE, closest_p1, closest_p2); - - return drequest->isSatisfied(*dresult); - } - else - return false; - } - - if(!tree1->isNodeOccupied(root1)) return false; - - for(unsigned int i = 0; i < 8; ++i) - { - if(tree1->nodeChildExists(root1, i)) - { - const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); - AABB child_bv; - computeChildBV(bv1, i, child_bv); - - AABB aabb1; - convertBV(child_bv, tf1, aabb1); - FCL_REAL d = aabb1.distance(aabb2); - if(d < dresult->min_distance) - { - if(OcTreeShapeDistanceRecurse(tree1, child, child_bv, s, aabb2, tf1, tf2)) - return true; - } - } - } - - return false; - } - - template - bool OcTreeShapeIntersectRecurse(const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1, - const S& s, const OBB& obb2, - const Transform3d& tf1, const Transform3d& tf2) const - { - if(!root1) - { - OBB obb1; - convertBV(bv1, tf1, obb1); - if(obb1.overlap(obb2)) - { - Box box; - Transform3d box_tf; - constructBox(bv1, tf1, box, box_tf); - - if(solver->shapeIntersect(box, box_tf, s, tf2, NULL)) - { - AABB overlap_part; - AABB aabb1, aabb2; - computeBV(box, box_tf, aabb1); - computeBV(s, tf2, aabb2); - aabb1.overlap(aabb2, overlap_part); - cresult->addCostSource(CostSource(overlap_part, tree1->getOccupancyThres() * s.cost_density), crequest->num_max_cost_sources); - } - } - - return false; - } - else if(!tree1->nodeHasChildren(root1)) - { - if(tree1->isNodeOccupied(root1) && s.isOccupied()) // occupied area - { - OBB obb1; - convertBV(bv1, tf1, obb1); - if(obb1.overlap(obb2)) - { - Box box; - Transform3d box_tf; - constructBox(bv1, tf1, box, box_tf); - - bool is_intersect = false; - if(!crequest->enable_contact) - { - if(solver->shapeIntersect(box, box_tf, s, tf2, NULL)) - { - is_intersect = true; - if(cresult->numContacts() < crequest->num_max_contacts) - cresult->addContact(Contact(tree1, &s, root1 - tree1->getRoot(), Contact::NONE)); - } - } - else - { - std::vector contacts; - if(solver->shapeIntersect(box, box_tf, s, tf2, &contacts)) - { - is_intersect = true; - if(crequest->num_max_contacts > cresult->numContacts()) - { - const size_t free_space = crequest->num_max_contacts - cresult->numContacts(); - size_t num_adding_contacts; - - // If the free space is not enough to add all the new contacts, we add contacts in descent order of penetration depth. - if (free_space < contacts.size()) - { - std::partial_sort(contacts.begin(), contacts.begin() + free_space, contacts.end(), std::bind(comparePenDepth, std::placeholders::_2, std::placeholders::_1)); - num_adding_contacts = free_space; - } - else - { - num_adding_contacts = contacts.size(); - } - - for(size_t i = 0; i < num_adding_contacts; ++i) - cresult->addContact(Contact(tree1, &s, root1 - tree1->getRoot(), Contact::NONE, contacts[i].pos, contacts[i].normal, contacts[i].penetration_depth)); - } - } - } - - if(is_intersect && crequest->enable_cost) - { - AABB overlap_part; - AABB aabb1, aabb2; - computeBV(box, box_tf, aabb1); - computeBV(s, tf2, aabb2); - aabb1.overlap(aabb2, overlap_part); - } - - return crequest->isSatisfied(*cresult); - } - else return false; - } - else if(!tree1->isNodeFree(root1) && !s.isFree() && crequest->enable_cost) // uncertain area - { - OBB obb1; - convertBV(bv1, tf1, obb1); - if(obb1.overlap(obb2)) - { - Box box; - Transform3d box_tf; - constructBox(bv1, tf1, box, box_tf); - - if(solver->shapeIntersect(box, box_tf, s, tf2, NULL)) - { - AABB overlap_part; - AABB aabb1, aabb2; - computeBV(box, box_tf, aabb1); - computeBV(s, tf2, aabb2); - aabb1.overlap(aabb2, overlap_part); - } - } - - return false; - } - else // free area - return false; - } - - /// stop when 1) bounding boxes of two objects not overlap; OR - /// 2) at least of one the nodes is free; OR - /// 2) (two uncertain nodes or one node occupied and one node uncertain) AND cost not required - if(tree1->isNodeFree(root1) || s.isFree()) return false; - else if((tree1->isNodeUncertain(root1) || s.isUncertain()) && !crequest->enable_cost) return false; - else - { - OBB obb1; - convertBV(bv1, tf1, obb1); - if(!obb1.overlap(obb2)) return false; - } - - for(unsigned int i = 0; i < 8; ++i) - { - if(tree1->nodeChildExists(root1, i)) - { - const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); - AABB child_bv; - computeChildBV(bv1, i, child_bv); - - if(OcTreeShapeIntersectRecurse(tree1, child, child_bv, s, obb2, tf1, tf2)) - return true; - } - else if(!s.isFree() && crequest->enable_cost) - { - AABB child_bv; - computeChildBV(bv1, i, child_bv); - - if(OcTreeShapeIntersectRecurse(tree1, NULL, child_bv, s, obb2, tf1, tf2)) - return true; - } - } - - return false; - } - - template - bool OcTreeMeshDistanceRecurse(const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1, - const BVHModel* tree2, int root2, - const Transform3d& tf1, const Transform3d& tf2) const - { - if(!tree1->nodeHasChildren(root1) && tree2->getBV(root2).isLeaf()) - { - if(tree1->isNodeOccupied(root1)) - { - Box box; - Transform3d box_tf; - constructBox(bv1, tf1, box, box_tf); - - int primitive_id = tree2->getBV(root2).primitiveId(); - const Triangle& tri_id = tree2->tri_indices[primitive_id]; - const Vector3d& p1 = tree2->vertices[tri_id[0]]; - const Vector3d& p2 = tree2->vertices[tri_id[1]]; - const Vector3d& p3 = tree2->vertices[tri_id[2]]; - - FCL_REAL dist; - Vector3d closest_p1, closest_p2; - solver->shapeTriangleDistance(box, box_tf, p1, p2, p3, tf2, &dist, &closest_p1, &closest_p2); - - dresult->update(dist, tree1, tree2, root1 - tree1->getRoot(), primitive_id); - - return drequest->isSatisfied(*dresult); - } - else - return false; - } - - if(!tree1->isNodeOccupied(root1)) return false; - - if(tree2->getBV(root2).isLeaf() || (tree1->nodeHasChildren(root1) && (bv1.size() > tree2->getBV(root2).bv.size()))) - { - for(unsigned int i = 0; i < 8; ++i) - { - if(tree1->nodeChildExists(root1, i)) - { - const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); - AABB child_bv; - computeChildBV(bv1, i, child_bv); - - FCL_REAL d; - AABB aabb1, aabb2; - convertBV(child_bv, tf1, aabb1); - convertBV(tree2->getBV(root2).bv, tf2, aabb2); - d = aabb1.distance(aabb2); - - if(d < dresult->min_distance) - { - if(OcTreeMeshDistanceRecurse(tree1, child, child_bv, tree2, root2, tf1, tf2)) - return true; - } - } - } - } - else - { - FCL_REAL d; - AABB aabb1, aabb2; - convertBV(bv1, tf1, aabb1); - int child = tree2->getBV(root2).leftChild(); - convertBV(tree2->getBV(child).bv, tf2, aabb2); - d = aabb1.distance(aabb2); - - if(d < dresult->min_distance) - { - if(OcTreeMeshDistanceRecurse(tree1, root1, bv1, tree2, child, tf1, tf2)) - return true; - } - - child = tree2->getBV(root2).rightChild(); - convertBV(tree2->getBV(child).bv, tf2, aabb2); - d = aabb1.distance(aabb2); - - if(d < dresult->min_distance) - { - if(OcTreeMeshDistanceRecurse(tree1, root1, bv1, tree2, child, tf1, tf2)) - return true; - } - } - - return false; - } - - - template - bool OcTreeMeshIntersectRecurse(const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1, - const BVHModel* tree2, int root2, - const Transform3d& tf1, const Transform3d& tf2) const - { - if(!root1) - { - if(tree2->getBV(root2).isLeaf()) - { - OBB obb1, obb2; - convertBV(bv1, tf1, obb1); - convertBV(tree2->getBV(root2).bv, tf2, obb2); - if(obb1.overlap(obb2)) - { - Box box; - Transform3d box_tf; - constructBox(bv1, tf1, box, box_tf); - - int primitive_id = tree2->getBV(root2).primitiveId(); - const Triangle& tri_id = tree2->tri_indices[primitive_id]; - const Vector3d& p1 = tree2->vertices[tri_id[0]]; - const Vector3d& p2 = tree2->vertices[tri_id[1]]; - const Vector3d& p3 = tree2->vertices[tri_id[2]]; - - if(solver->shapeTriangleIntersect(box, box_tf, p1, p2, p3, tf2, NULL, NULL, NULL)) - { - AABB overlap_part; - AABB aabb1; - computeBV(box, box_tf, aabb1); - AABB aabb2(tf2 * p1, tf2 * p2, tf2 * p3); - aabb1.overlap(aabb2, overlap_part); - cresult->addCostSource(CostSource(overlap_part, tree1->getOccupancyThres() * tree2->cost_density), crequest->num_max_cost_sources); - } - } - - return false; - } - else - { - if(OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2, tree2->getBV(root2).leftChild(), tf1, tf2)) - return true; - - if(OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2, tree2->getBV(root2).rightChild(), tf1, tf2)) - return true; - - return false; - } - } - else if(!tree1->nodeHasChildren(root1) && tree2->getBV(root2).isLeaf()) - { - if(tree1->isNodeOccupied(root1) && tree2->isOccupied()) - { - OBB obb1, obb2; - convertBV(bv1, tf1, obb1); - convertBV(tree2->getBV(root2).bv, tf2, obb2); - if(obb1.overlap(obb2)) - { - Box box; - Transform3d box_tf; - constructBox(bv1, tf1, box, box_tf); - - int primitive_id = tree2->getBV(root2).primitiveId(); - const Triangle& tri_id = tree2->tri_indices[primitive_id]; - const Vector3d& p1 = tree2->vertices[tri_id[0]]; - const Vector3d& p2 = tree2->vertices[tri_id[1]]; - const Vector3d& p3 = tree2->vertices[tri_id[2]]; - - bool is_intersect = false; - if(!crequest->enable_contact) - { - if(solver->shapeTriangleIntersect(box, box_tf, p1, p2, p3, tf2, NULL, NULL, NULL)) - { - is_intersect = true; - if(cresult->numContacts() < crequest->num_max_contacts) - cresult->addContact(Contact(tree1, tree2, root1 - tree1->getRoot(), primitive_id)); - } - } - else - { - Vector3d contact; - FCL_REAL depth; - Vector3d normal; - - if(solver->shapeTriangleIntersect(box, box_tf, p1, p2, p3, tf2, &contact, &depth, &normal)) - { - is_intersect = true; - if(cresult->numContacts() < crequest->num_max_contacts) - cresult->addContact(Contact(tree1, tree2, root1 - tree1->getRoot(), primitive_id, contact, normal, depth)); - } - } - - if(is_intersect && crequest->enable_cost) - { - AABB overlap_part; - AABB aabb1; - computeBV(box, box_tf, aabb1); - AABB aabb2(tf2 * p1, tf2 * p2, tf2 * p3); - aabb1.overlap(aabb2, overlap_part); - cresult->addCostSource(CostSource(overlap_part, root1->getOccupancy() * tree2->cost_density), crequest->num_max_cost_sources); - } - - return crequest->isSatisfied(*cresult); - } - else - return false; - } - else if(!tree1->isNodeFree(root1) && !tree2->isFree() && crequest->enable_cost) // uncertain area - { - OBB obb1, obb2; - convertBV(bv1, tf1, obb1); - convertBV(tree2->getBV(root2).bv, tf2, obb2); - if(obb1.overlap(obb2)) - { - Box box; - Transform3d box_tf; - constructBox(bv1, tf1, box, box_tf); - - int primitive_id = tree2->getBV(root2).primitiveId(); - const Triangle& tri_id = tree2->tri_indices[primitive_id]; - const Vector3d& p1 = tree2->vertices[tri_id[0]]; - const Vector3d& p2 = tree2->vertices[tri_id[1]]; - const Vector3d& p3 = tree2->vertices[tri_id[2]]; - - if(solver->shapeTriangleIntersect(box, box_tf, p1, p2, p3, tf2, NULL, NULL, NULL)) - { - AABB overlap_part; - AABB aabb1; - computeBV(box, box_tf, aabb1); - AABB aabb2(tf2 * p1, tf2 * p2, tf2 * p3); - aabb1.overlap(aabb2, overlap_part); - cresult->addCostSource(CostSource(overlap_part, root1->getOccupancy() * tree2->cost_density), crequest->num_max_cost_sources); - } - } - - return false; - } - else // free area - return false; - } - - /// stop when 1) bounding boxes of two objects not overlap; OR - /// 2) at least one of the nodes is free; OR - /// 2) (two uncertain nodes OR one node occupied and one node uncertain) AND cost not required - if(tree1->isNodeFree(root1) || tree2->isFree()) return false; - else if((tree1->isNodeUncertain(root1) || tree2->isUncertain()) && !crequest->enable_cost) return false; - else - { - OBB obb1, obb2; - convertBV(bv1, tf1, obb1); - convertBV(tree2->getBV(root2).bv, tf2, obb2); - if(!obb1.overlap(obb2)) return false; - } - - if(tree2->getBV(root2).isLeaf() || (tree1->nodeHasChildren(root1) && (bv1.size() > tree2->getBV(root2).bv.size()))) - { - for(unsigned int i = 0; i < 8; ++i) - { - if(tree1->nodeChildExists(root1, i)) - { - const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); - AABB child_bv; - computeChildBV(bv1, i, child_bv); - - if(OcTreeMeshIntersectRecurse(tree1, child, child_bv, tree2, root2, tf1, tf2)) - return true; - } - else if(!tree2->isFree() && crequest->enable_cost) - { - AABB child_bv; - computeChildBV(bv1, i, child_bv); - - if(OcTreeMeshIntersectRecurse(tree1, NULL, child_bv, tree2, root2, tf1, tf2)) - return true; - } - } - } - else - { - if(OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2, tree2->getBV(root2).leftChild(), tf1, tf2)) - return true; - - if(OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2, tree2->getBV(root2).rightChild(), tf1, tf2)) - return true; - - } - - return false; - } - - bool OcTreeDistanceRecurse(const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1, - const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& bv2, - const Transform3d& tf1, const Transform3d& tf2) const - { - if(!tree1->nodeHasChildren(root1) && !tree2->nodeHasChildren(root2)) - { - if(tree1->isNodeOccupied(root1) && tree2->isNodeOccupied(root2)) - { - Box box1, box2; - Transform3d box1_tf, box2_tf; - constructBox(bv1, tf1, box1, box1_tf); - constructBox(bv2, tf2, box2, box2_tf); - - FCL_REAL dist; - Vector3d closest_p1, closest_p2; - solver->shapeDistance(box1, box1_tf, box2, box2_tf, &dist, &closest_p1, &closest_p2); - - dresult->update(dist, tree1, tree2, root1 - tree1->getRoot(), root2 - tree2->getRoot(), closest_p1, closest_p2); - - return drequest->isSatisfied(*dresult); - } - else - return false; - } - - if(!tree1->isNodeOccupied(root1) || !tree2->isNodeOccupied(root2)) return false; - - if(!tree2->nodeHasChildren(root2) || (tree1->nodeHasChildren(root1) && (bv1.size() > bv2.size()))) - { - for(unsigned int i = 0; i < 8; ++i) - { - if(tree1->nodeChildExists(root1, i)) - { - const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); - AABB child_bv; - computeChildBV(bv1, i, child_bv); - - FCL_REAL d; - AABB aabb1, aabb2; - convertBV(bv1, tf1, aabb1); - convertBV(bv2, tf2, aabb2); - d = aabb1.distance(aabb2); - - if(d < dresult->min_distance) - { - - if(OcTreeDistanceRecurse(tree1, child, child_bv, tree2, root2, bv2, tf1, tf2)) - return true; - } - } - } - } - else - { - for(unsigned int i = 0; i < 8; ++i) - { - if(tree2->nodeChildExists(root2, i)) - { - const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); - AABB child_bv; - computeChildBV(bv2, i, child_bv); - - FCL_REAL d; - AABB aabb1, aabb2; - convertBV(bv1, tf1, aabb1); - convertBV(bv2, tf2, aabb2); - d = aabb1.distance(aabb2); - - if(d < dresult->min_distance) - { - if(OcTreeDistanceRecurse(tree1, root1, bv1, tree2, child, child_bv, tf1, tf2)) - return true; - } - } - } - } - - return false; - } - - - bool OcTreeIntersectRecurse(const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1, - const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& bv2, - const Transform3d& tf1, const Transform3d& tf2) const - { - if(!root1 && !root2) - { - OBB obb1, obb2; - convertBV(bv1, tf1, obb1); - convertBV(bv2, tf2, obb2); - - if(obb1.overlap(obb2)) - { - Box box1, box2; - Transform3d box1_tf, box2_tf; - constructBox(bv1, tf1, box1, box1_tf); - constructBox(bv2, tf2, box2, box2_tf); - - AABB overlap_part; - AABB aabb1, aabb2; - computeBV(box1, box1_tf, aabb1); - computeBV(box2, box2_tf, aabb2); - aabb1.overlap(aabb2, overlap_part); - cresult->addCostSource(CostSource(overlap_part, tree1->getOccupancyThres() * tree2->getOccupancyThres()), crequest->num_max_cost_sources); - } - - return false; - } - else if(!root1 && root2) - { - if(tree2->nodeHasChildren(root2)) - { - for(unsigned int i = 0; i < 8; ++i) - { - if(tree2->nodeChildExists(root2, i)) - { - const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); - AABB child_bv; - computeChildBV(bv2, i, child_bv); - if(OcTreeIntersectRecurse(tree1, NULL, bv1, tree2, child, child_bv, tf1, tf2)) - return true; - } - else - { - AABB child_bv; - computeChildBV(bv2, i, child_bv); - if(OcTreeIntersectRecurse(tree1, NULL, bv1, tree2, NULL, child_bv, tf1, tf2)) - return true; - } - } - } - else - { - if(OcTreeIntersectRecurse(tree1, NULL, bv1, tree2, NULL, bv2, tf1, tf2)) - return true; - } - - return false; - } - else if(root1 && !root2) - { - if(tree1->nodeHasChildren(root1)) - { - for(unsigned int i = 0; i < 8; ++i) - { - if(tree1->nodeChildExists(root1, i)) - { - const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); - AABB child_bv; - computeChildBV(bv1, i, child_bv); - if(OcTreeIntersectRecurse(tree1, child, child_bv, tree2, NULL, bv2, tf1, tf2)) - return true; - } - else - { - AABB child_bv; - computeChildBV(bv1, i, child_bv); - if(OcTreeIntersectRecurse(tree1, NULL, child_bv, tree2, NULL, bv2, tf1, tf2)) - return true; - } - } - } - else - { - if(OcTreeIntersectRecurse(tree1, NULL, bv1, tree2, NULL, bv2, tf1, tf2)) - return true; - } - - return false; - } - else if(!tree1->nodeHasChildren(root1) && !tree2->nodeHasChildren(root2)) - { - if(tree1->isNodeOccupied(root1) && tree2->isNodeOccupied(root2)) // occupied area - { - bool is_intersect = false; - if(!crequest->enable_contact) - { - OBB obb1, obb2; - convertBV(bv1, tf1, obb1); - convertBV(bv2, tf2, obb2); - - if(obb1.overlap(obb2)) - { - is_intersect = true; - if(cresult->numContacts() < crequest->num_max_contacts) - cresult->addContact(Contact(tree1, tree2, root1 - tree1->getRoot(), root2 - tree2->getRoot())); - } - } - else - { - Box box1, box2; - Transform3d box1_tf, box2_tf; - constructBox(bv1, tf1, box1, box1_tf); - constructBox(bv2, tf2, box2, box2_tf); - - std::vector contacts; - if(solver->shapeIntersect(box1, box1_tf, box2, box2_tf, &contacts)) - { - is_intersect = true; - if(crequest->num_max_contacts > cresult->numContacts()) - { - const size_t free_space = crequest->num_max_contacts - cresult->numContacts(); - size_t num_adding_contacts; - - // If the free space is not enough to add all the new contacts, we add contacts in descent order of penetration depth. - if (free_space < contacts.size()) - { - std::partial_sort(contacts.begin(), contacts.begin() + free_space, contacts.end(), std::bind(comparePenDepth, std::placeholders::_2, std::placeholders::_1)); - num_adding_contacts = free_space; - } - else - { - num_adding_contacts = contacts.size(); - } - - for(size_t i = 0; i < num_adding_contacts; ++i) - cresult->addContact(Contact(tree1, tree2, root1 - tree1->getRoot(), root2 - tree2->getRoot(), contacts[i].pos, contacts[i].normal, contacts[i].penetration_depth)); - } - } - } - - if(is_intersect && crequest->enable_cost) - { - Box box1, box2; - Transform3d box1_tf, box2_tf; - constructBox(bv1, tf1, box1, box1_tf); - constructBox(bv2, tf2, box2, box2_tf); - - AABB overlap_part; - AABB aabb1, aabb2; - computeBV(box1, box1_tf, aabb1); - computeBV(box2, box2_tf, aabb2); - aabb1.overlap(aabb2, overlap_part); - cresult->addCostSource(CostSource(overlap_part, root1->getOccupancy() * root2->getOccupancy()), crequest->num_max_cost_sources); - } - - return crequest->isSatisfied(*cresult); - } - else if(!tree1->isNodeFree(root1) && !tree2->isNodeFree(root2) && crequest->enable_cost) // uncertain area (here means both are uncertain or one uncertain and one occupied) - { - OBB obb1, obb2; - convertBV(bv1, tf1, obb1); - convertBV(bv2, tf2, obb2); - - if(obb1.overlap(obb2)) - { - Box box1, box2; - Transform3d box1_tf, box2_tf; - constructBox(bv1, tf1, box1, box1_tf); - constructBox(bv2, tf2, box2, box2_tf); - - AABB overlap_part; - AABB aabb1, aabb2; - computeBV(box1, box1_tf, aabb1); - computeBV(box2, box2_tf, aabb2); - aabb1.overlap(aabb2, overlap_part); - cresult->addCostSource(CostSource(overlap_part, root1->getOccupancy() * root2->getOccupancy()), crequest->num_max_cost_sources); - } - - return false; - } - else // free area (at least one node is free) - return false; - } - - /// stop when 1) bounding boxes of two objects not overlap; OR - /// 2) at least one of the nodes is free; OR - /// 2) (two uncertain nodes OR one node occupied and one node uncertain) AND cost not required - if(tree1->isNodeFree(root1) || tree2->isNodeFree(root2)) return false; - else if((tree1->isNodeUncertain(root1) || tree2->isNodeUncertain(root2)) && !crequest->enable_cost) return false; - else - { - OBB obb1, obb2; - convertBV(bv1, tf1, obb1); - convertBV(bv2, tf2, obb2); - if(!obb1.overlap(obb2)) return false; - } - - if(!tree2->nodeHasChildren(root2) || (tree1->nodeHasChildren(root1) && (bv1.size() > bv2.size()))) - { - for(unsigned int i = 0; i < 8; ++i) - { - if(tree1->nodeChildExists(root1, i)) - { - const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); - AABB child_bv; - computeChildBV(bv1, i, child_bv); - - if(OcTreeIntersectRecurse(tree1, child, child_bv, - tree2, root2, bv2, - tf1, tf2)) - return true; - } - else if(!tree2->isNodeFree(root2) && crequest->enable_cost) - { - AABB child_bv; - computeChildBV(bv1, i, child_bv); - - if(OcTreeIntersectRecurse(tree1, NULL, child_bv, - tree2, root2, bv2, - tf1, tf2)) - return true; - } - } - } - else - { - for(unsigned int i = 0; i < 8; ++i) - { - if(tree2->nodeChildExists(root2, i)) - { - const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); - AABB child_bv; - computeChildBV(bv2, i, child_bv); - - if(OcTreeIntersectRecurse(tree1, root1, bv1, - tree2, child, child_bv, - tf1, tf2)) - return true; - } - else if(!tree1->isNodeFree(root1) && crequest->enable_cost) - { - AABB child_bv; - computeChildBV(bv2, i, child_bv); - - if(OcTreeIntersectRecurse(tree1, root1, bv1, - tree2, NULL, child_bv, - tf1, tf2)) - return true; - } - } - } - - return false; - } -}; - - - - -/// @brief Traversal node for octree collision -template -class OcTreeCollisionTraversalNode : public CollisionTraversalNodeBase -{ -public: - OcTreeCollisionTraversalNode() - { - model1 = NULL; - model2 = NULL; - - otsolver = NULL; - } - - bool BVTesting(int, int) const - { - return false; - } - - void leafTesting(int, int) const - { - otsolver->OcTreeIntersect(model1, model2, tf1, tf2, request, *result); - } - - const OcTree* model1; - const OcTree* model2; - - Transform3d tf1, tf2; - - const OcTreeSolver* otsolver; -}; - -/// @brief Traversal node for octree distance -template -class OcTreeDistanceTraversalNode : public DistanceTraversalNodeBase -{ -public: - OcTreeDistanceTraversalNode() - { - model1 = NULL; - model2 = NULL; - - otsolver = NULL; - } - - - FCL_REAL BVTesting(int, int) const - { - return -1; - } - - void leafTesting(int, int) const - { - otsolver->OcTreeDistance(model1, model2, tf1, tf2, request, *result); - } - - const OcTree* model1; - const OcTree* model2; - - const OcTreeSolver* otsolver; -}; - -/// @brief Traversal node for shape-octree collision -template -class ShapeOcTreeCollisionTraversalNode : public CollisionTraversalNodeBase -{ -public: - ShapeOcTreeCollisionTraversalNode() - { - model1 = NULL; - model2 = NULL; - - otsolver = NULL; - } - - bool BVTesting(int, int) const - { - return false; - } - - void leafTesting(int, int) const - { - otsolver->OcTreeShapeIntersect(model2, *model1, tf2, tf1, request, *result); - } - - const S* model1; - const OcTree* model2; - - Transform3d tf1, tf2; - - const OcTreeSolver* otsolver; -}; - -/// @brief Traversal node for octree-shape collision -template -class OcTreeShapeCollisionTraversalNode : public CollisionTraversalNodeBase -{ -public: - OcTreeShapeCollisionTraversalNode() - { - model1 = NULL; - model2 = NULL; - - otsolver = NULL; - } - - bool BVTesting(int, int) const - { - return false; - } - - void leafTesting(int, int) const - { - otsolver->OcTreeShapeIntersect(model1, *model2, tf1, tf2, request, *result); - } - - const OcTree* model1; - const S* model2; - - Transform3d tf1, tf2; - - const OcTreeSolver* otsolver; -}; - -/// @brief Traversal node for shape-octree distance -template -class ShapeOcTreeDistanceTraversalNode : public DistanceTraversalNodeBase -{ -public: - ShapeOcTreeDistanceTraversalNode() - { - model1 = NULL; - model2 = NULL; - - otsolver = NULL; - } - - FCL_REAL BVTesting(int, int) const - { - return -1; - } - - void leafTesting(int, int) const - { - otsolver->OcTreeShapeDistance(model2, *model1, tf2, tf1, request, *result); - } - - const S* model1; - const OcTree* model2; - - const OcTreeSolver* otsolver; -}; - -/// @brief Traversal node for octree-shape distance -template -class OcTreeShapeDistanceTraversalNode : public DistanceTraversalNodeBase -{ -public: - OcTreeShapeDistanceTraversalNode() - { - model1 = NULL; - model2 = NULL; - - otsolver = NULL; - } - - FCL_REAL BVTesting(int, int) const - { - return -1; - } - - void leafTesting(int, int) const - { - otsolver->OcTreeShapeDistance(model1, *model2, tf1, tf2, request, *result); - } - - const OcTree* model1; - const S* model2; - - const OcTreeSolver* otsolver; -}; - -/// @brief Traversal node for mesh-octree collision -template -class MeshOcTreeCollisionTraversalNode : public CollisionTraversalNodeBase -{ -public: - MeshOcTreeCollisionTraversalNode() - { - model1 = NULL; - model2 = NULL; - - otsolver = NULL; - } - - bool BVTesting(int, int) const - { - return false; - } - - void leafTesting(int, int) const - { - otsolver->OcTreeMeshIntersect(model2, model1, tf2, tf1, request, *result); - } - - const BVHModel* model1; - const OcTree* model2; - - Transform3d tf1, tf2; - - const OcTreeSolver* otsolver; -}; - -/// @brief Traversal node for octree-mesh collision -template -class OcTreeMeshCollisionTraversalNode : public CollisionTraversalNodeBase -{ -public: - OcTreeMeshCollisionTraversalNode() - { - model1 = NULL; - model2 = NULL; - - otsolver = NULL; - } - - bool BVTesting(int, int) const - { - return false; - } - - void leafTesting(int, int) const - { - otsolver->OcTreeMeshIntersect(model1, model2, tf1, tf2, request, *result); - } - - const OcTree* model1; - const BVHModel* model2; - - Transform3d tf1, tf2; - - const OcTreeSolver* otsolver; -}; - -/// @brief Traversal node for mesh-octree distance -template -class MeshOcTreeDistanceTraversalNode : public DistanceTraversalNodeBase -{ -public: - MeshOcTreeDistanceTraversalNode() - { - model1 = NULL; - model2 = NULL; - - otsolver = NULL; - } - - FCL_REAL BVTesting(int, int) const - { - return -1; - } - - void leafTesting(int, int) const - { - otsolver->OcTreeMeshDistance(model2, model1, tf2, tf1, request, *result); - } - - const BVHModel* model1; - const OcTree* model2; - - const OcTreeSolver* otsolver; - -}; - -/// @brief Traversal node for octree-mesh distance -template -class OcTreeMeshDistanceTraversalNode : public DistanceTraversalNodeBase -{ -public: - OcTreeMeshDistanceTraversalNode() - { - model1 = NULL; - model2 = NULL; - - otsolver = NULL; - } - - FCL_REAL BVTesting(int, int) const - { - return -1; - } - - void leafTesting(int, int) const - { - otsolver->OcTreeMeshDistance(model1, model2, tf1, tf2, request, *result); - } - - const OcTree* model1; - const BVHModel* model2; - - const OcTreeSolver* otsolver; - -}; - - - -} +#warning "This header has been deprecated in FCL 0.6. " #endif diff --git a/include/fcl/traversal/traversal_node_setup.h b/include/fcl/traversal/traversal_node_setup.h index bac9321b9..0065b7898 100644 --- a/include/fcl/traversal/traversal_node_setup.h +++ b/include/fcl/traversal/traversal_node_setup.h @@ -35,1253 +35,9 @@ /** \author Jia Pan */ - #ifndef FCL_TRAVERSAL_NODE_SETUP_H #define FCL_TRAVERSAL_NODE_SETUP_H -#include "fcl/config.h" -#include "fcl/traversal/traversal_node_bvhs.h" -#include "fcl/traversal/traversal_node_shapes.h" -#include "fcl/traversal/traversal_node_bvh_shape.h" - -#if FCL_HAVE_OCTOMAP -#include "fcl/traversal/traversal_node_octree.h" -#endif - -#include "fcl/BVH/BVH_utility.h" - -namespace fcl -{ - -#if FCL_HAVE_OCTOMAP -/// @brief Initialize traversal node for collision between two octrees, given current object transform -template -bool initialize(OcTreeCollisionTraversalNode& node, - const OcTree& model1, const Transform3d& tf1, - const OcTree& model2, const Transform3d& tf2, - const OcTreeSolver* otsolver, - const CollisionRequest& request, - CollisionResult& result) -{ - node.request = request; - node.result = &result; - - node.model1 = &model1; - node.model2 = &model2; - - node.otsolver = otsolver; - - node.tf1 = tf1; - node.tf2 = tf2; - - return true; -} - -/// @brief Initialize traversal node for distance between two octrees, given current object transform -template -bool initialize(OcTreeDistanceTraversalNode& node, - const OcTree& model1, const Transform3d& tf1, - const OcTree& model2, const Transform3d& tf2, - const OcTreeSolver* otsolver, - const DistanceRequest& request, - DistanceResult& result) -{ - node.request = request; - node.result = &result; - - node.model1 = &model1; - node.model2 = &model2; - - node.otsolver = otsolver; - - node.tf1 = tf1; - node.tf2 = tf2; - - return true; -} - -/// @brief Initialize traversal node for collision between one shape and one octree, given current object transform -template -bool initialize(ShapeOcTreeCollisionTraversalNode& node, - const S& model1, const Transform3d& tf1, - const OcTree& model2, const Transform3d& tf2, - const OcTreeSolver* otsolver, - const CollisionRequest& request, - CollisionResult& result) -{ - node.request = request; - node.result = &result; - - node.model1 = &model1; - node.model2 = &model2; - - node.otsolver = otsolver; - - node.tf1 = tf1; - node.tf2 = tf2; - - return true; -} - -/// @brief Initialize traversal node for collision between one octree and one shape, given current object transform -template -bool initialize(OcTreeShapeCollisionTraversalNode& node, - const OcTree& model1, const Transform3d& tf1, - const S& model2, const Transform3d& tf2, - const OcTreeSolver* otsolver, - const CollisionRequest& request, - CollisionResult& result) -{ - node.request = request; - node.result = &result; - - node.model1 = &model1; - node.model2 = &model2; - - node.otsolver = otsolver; - - node.tf1 = tf1; - node.tf2 = tf2; - - return true; -} - -/// @brief Initialize traversal node for distance between one shape and one octree, given current object transform -template -bool initialize(ShapeOcTreeDistanceTraversalNode& node, - const S& model1, const Transform3d& tf1, - const OcTree& model2, const Transform3d& tf2, - const OcTreeSolver* otsolver, - const DistanceRequest& request, - DistanceResult& result) -{ - node.request = request; - node.result = &result; - - node.model1 = &model1; - node.model2 = &model2; - - node.otsolver = otsolver; - - node.tf1 = tf1; - node.tf2 = tf2; - - return true; -} - -/// @brief Initialize traversal node for distance between one octree and one shape, given current object transform -template -bool initialize(OcTreeShapeDistanceTraversalNode& node, - const OcTree& model1, const Transform3d& tf1, - const S& model2, const Transform3d& tf2, - const OcTreeSolver* otsolver, - const DistanceRequest& request, - DistanceResult& result) -{ - node.request = request; - node.result = &result; - - node.model1 = &model1; - node.model2 = &model2; - - node.otsolver = otsolver; - - node.tf1 = tf1; - node.tf2 = tf2; - - return true; -} - -/// @brief Initialize traversal node for collision between one mesh and one octree, given current object transform -template -bool initialize(MeshOcTreeCollisionTraversalNode& node, - const BVHModel& model1, const Transform3d& tf1, - const OcTree& model2, const Transform3d& tf2, - const OcTreeSolver* otsolver, - const CollisionRequest& request, - CollisionResult& result) -{ - node.request = request; - node.result = &result; - - node.model1 = &model1; - node.model2 = &model2; - - node.otsolver = otsolver; - - node.tf1 = tf1; - node.tf2 = tf2; - - return true; -} - -/// @brief Initialize traversal node for collision between one octree and one mesh, given current object transform -template -bool initialize(OcTreeMeshCollisionTraversalNode& node, - const OcTree& model1, const Transform3d& tf1, - const BVHModel& model2, const Transform3d& tf2, - const OcTreeSolver* otsolver, - const CollisionRequest& request, - CollisionResult& result) -{ - node.request = request; - node.result = &result; - - node.model1 = &model1; - node.model2 = &model2; - - node.otsolver = otsolver; - - node.tf1 = tf1; - node.tf2 = tf2; - - return true; -} - -/// @brief Initialize traversal node for distance between one mesh and one octree, given current object transform -template -bool initialize(MeshOcTreeDistanceTraversalNode& node, - const BVHModel& model1, const Transform3d& tf1, - const OcTree& model2, const Transform3d& tf2, - const OcTreeSolver* otsolver, - const DistanceRequest& request, - DistanceResult& result) -{ - node.request = request; - node.result = &result; - - node.model1 = &model1; - node.model2 = &model2; - - node.otsolver = otsolver; - - node.tf1 = tf1; - node.tf2 = tf2; - - return true; -} - -/// @brief Initialize traversal node for collision between one octree and one mesh, given current object transform -template -bool initialize(OcTreeMeshDistanceTraversalNode& node, - const OcTree& model1, const Transform3d& tf1, - const BVHModel& model2, const Transform3d& tf2, - const OcTreeSolver* otsolver, - const DistanceRequest& request, - DistanceResult& result) -{ - node.request = request; - node.result = &result; - - node.model1 = &model1; - node.model2 = &model2; - - node.otsolver = otsolver; - - node.tf1 = tf1; - node.tf2 = tf2; - - return true; -} - -#endif - - -/// @brief Initialize traversal node for collision between two geometric shapes, given current object transform -template -bool initialize(ShapeCollisionTraversalNode& node, - const S1& shape1, const Transform3d& tf1, - const S2& shape2, const Transform3d& tf2, - const NarrowPhaseSolver* nsolver, - const CollisionRequest& request, - CollisionResult& result) -{ - node.model1 = &shape1; - node.tf1 = tf1; - node.model2 = &shape2; - node.tf2 = tf2; - node.nsolver = nsolver; - - node.request = request; - node.result = &result; - - node.cost_density = shape1.cost_density * shape2.cost_density; - - return true; -} - -/// @brief Initialize traversal node for collision between one mesh and one shape, given current object transform -template -bool initialize(MeshShapeCollisionTraversalNode& node, - BVHModel& model1, Transform3d& tf1, - const S& model2, const Transform3d& tf2, - const NarrowPhaseSolver* nsolver, - const CollisionRequest& request, - CollisionResult& result, - bool use_refit = false, bool refit_bottomup = false) -{ - if(model1.getModelType() != BVH_MODEL_TRIANGLES) - return false; - - if(!tf1.matrix().isIdentity()) - { - std::vector vertices_transformed(model1.num_vertices); - for(int i = 0; i < model1.num_vertices; ++i) - { - Vector3d& p = model1.vertices[i]; - Vector3d new_v = tf1 * p; - vertices_transformed[i] = new_v; - } - - model1.beginReplaceModel(); - model1.replaceSubModel(vertices_transformed); - model1.endReplaceModel(use_refit, refit_bottomup); - - tf1.setIdentity(); - } - - node.model1 = &model1; - node.tf1 = tf1; - node.model2 = &model2; - node.tf2 = tf2; - node.nsolver = nsolver; - - computeBV(model2, tf2, node.model2_bv); - - node.vertices = model1.vertices; - node.tri_indices = model1.tri_indices; - - node.request = request; - node.result = &result; - - node.cost_density = model1.cost_density * model2.cost_density; - - return true; -} - - -/// @brief Initialize traversal node for collision between one mesh and one shape, given current object transform -template -bool initialize(ShapeMeshCollisionTraversalNode& node, - const S& model1, const Transform3d& tf1, - BVHModel& model2, Transform3d& tf2, - const NarrowPhaseSolver* nsolver, - const CollisionRequest& request, - CollisionResult& result, - bool use_refit = false, bool refit_bottomup = false) -{ - if(model2.getModelType() != BVH_MODEL_TRIANGLES) - return false; - - if(!tf2.matrix().isIdentity()) - { - std::vector vertices_transformed(model2.num_vertices); - for(int i = 0; i < model2.num_vertices; ++i) - { - Vector3d& p = model2.vertices[i]; - Vector3d new_v = tf2 * p; - vertices_transformed[i] = new_v; - } - - model2.beginReplaceModel(); - model2.replaceSubModel(vertices_transformed); - model2.endReplaceModel(use_refit, refit_bottomup); - - tf2.setIdentity(); - } - - node.model1 = &model1; - node.tf1 = tf1; - node.model2 = &model2; - node.tf2 = tf2; - node.nsolver = nsolver; - - computeBV(model1, tf1, node.model1_bv); - - node.vertices = model2.vertices; - node.tri_indices = model2.tri_indices; - - node.request = request; - node.result = &result; - - node.cost_density = model1.cost_density * model2.cost_density; - - return true; -} - -/// @cond IGNORE -namespace details -{ - -template class OrientedNode> -static inline bool setupMeshShapeCollisionOrientedNode(OrientedNode& node, - const BVHModel& model1, const Transform3d& tf1, - const S& model2, const Transform3d& tf2, - const NarrowPhaseSolver* nsolver, - const CollisionRequest& request, - CollisionResult& result) -{ - if(model1.getModelType() != BVH_MODEL_TRIANGLES) - return false; - - node.model1 = &model1; - node.tf1 = tf1; - node.model2 = &model2; - node.tf2 = tf2; - node.nsolver = nsolver; - - computeBV(model2, tf2, node.model2_bv); - - node.vertices = model1.vertices; - node.tri_indices = model1.tri_indices; - - node.request = request; - node.result = &result; - - node.cost_density = model1.cost_density * model2.cost_density; - - return true; -} - -} -/// @endcond - - - -/// @brief Initialize the traversal node for collision between one mesh and one shape, specialized for OBB type -template -bool initialize(MeshShapeCollisionTraversalNodeOBB& node, - const BVHModel& model1, const Transform3d& tf1, - const S& model2, const Transform3d& tf2, - const NarrowPhaseSolver* nsolver, - const CollisionRequest& request, - CollisionResult& result) -{ - return details::setupMeshShapeCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result); -} - -/// @brief Initialize the traversal node for collision between one mesh and one shape, specialized for RSS type -template -bool initialize(MeshShapeCollisionTraversalNodeRSS& node, - const BVHModel& model1, const Transform3d& tf1, - const S& model2, const Transform3d& tf2, - const NarrowPhaseSolver* nsolver, - const CollisionRequest& request, - CollisionResult& result) -{ - return details::setupMeshShapeCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result); -} - -/// @brief Initialize the traversal node for collision between one mesh and one shape, specialized for kIOS type -template -bool initialize(MeshShapeCollisionTraversalNodekIOS& node, - const BVHModel& model1, const Transform3d& tf1, - const S& model2, const Transform3d& tf2, - const NarrowPhaseSolver* nsolver, - const CollisionRequest& request, - CollisionResult& result) -{ - return details::setupMeshShapeCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result); -} - -/// @brief Initialize the traversal node for collision between one mesh and one shape, specialized for OBBRSS type -template -bool initialize(MeshShapeCollisionTraversalNodeOBBRSS& node, - const BVHModel& model1, const Transform3d& tf1, - const S& model2, const Transform3d& tf2, - const NarrowPhaseSolver* nsolver, - const CollisionRequest& request, - CollisionResult& result) -{ - return details::setupMeshShapeCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result); -} - - -/// @cond IGNORE -namespace details -{ -template class OrientedNode> -static inline bool setupShapeMeshCollisionOrientedNode(OrientedNode& node, - const S& model1, const Transform3d& tf1, - const BVHModel& model2, const Transform3d& tf2, - const NarrowPhaseSolver* nsolver, - const CollisionRequest& request, - CollisionResult& result) -{ - if(model2.getModelType() != BVH_MODEL_TRIANGLES) - return false; - - node.model1 = &model1; - node.tf1 = tf1; - node.model2 = &model2; - node.tf2 = tf2; - node.nsolver = nsolver; - - computeBV(model1, tf1, node.model1_bv); - - node.vertices = model2.vertices; - node.tri_indices = model2.tri_indices; - - node.request = request; - node.result = &result; - - node.cost_density = model1.cost_density * model2.cost_density; - - return true; -} -} -/// @endcond - -/// @brief Initialize the traversal node for collision between one mesh and one shape, specialized for OBB type -template -bool initialize(ShapeMeshCollisionTraversalNodeOBB& node, - const S& model1, const Transform3d& tf1, - const BVHModel& model2, const Transform3d& tf2, - const NarrowPhaseSolver* nsolver, - const CollisionRequest& request, - CollisionResult& result) -{ - return details::setupShapeMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result); -} - -/// @brief Initialize the traversal node for collision between one mesh and one shape, specialized for RSS type -template -bool initialize(ShapeMeshCollisionTraversalNodeRSS& node, - const S& model1, const Transform3d& tf1, - const BVHModel& model2, const Transform3d& tf2, - const NarrowPhaseSolver* nsolver, - const CollisionRequest& request, - CollisionResult& result) -{ - return details::setupShapeMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result); -} - -/// @brief Initialize the traversal node for collision between one mesh and one shape, specialized for kIOS type -template -bool initialize(ShapeMeshCollisionTraversalNodekIOS& node, - const S& model1, const Transform3d& tf1, - const BVHModel& model2, const Transform3d& tf2, - const NarrowPhaseSolver* nsolver, - const CollisionRequest& request, - CollisionResult& result) -{ - return details::setupShapeMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result); -} - -/// @brief Initialize the traversal node for collision between one mesh and one shape, specialized for OBBRSS type -template -bool initialize(ShapeMeshCollisionTraversalNodeOBBRSS& node, - const S& model1, const Transform3d& tf1, - const BVHModel& model2, const Transform3d& tf2, - const NarrowPhaseSolver* nsolver, - const CollisionRequest& request, - CollisionResult& result) -{ - return details::setupShapeMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result); -} - - - - -/// @brief Initialize traversal node for collision between two meshes, given the current transforms -template -bool initialize(MeshCollisionTraversalNode& node, - BVHModel& model1, Transform3d& tf1, - BVHModel& model2, Transform3d& tf2, - const CollisionRequest& request, - CollisionResult& result, - bool use_refit = false, bool refit_bottomup = false) -{ - if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES) - return false; - - if(!tf1.matrix().isIdentity()) - { - std::vector vertices_transformed1(model1.num_vertices); - for(int i = 0; i < model1.num_vertices; ++i) - { - Vector3d& p = model1.vertices[i]; - Vector3d new_v = tf1 * p; - vertices_transformed1[i] = new_v; - } - - model1.beginReplaceModel(); - model1.replaceSubModel(vertices_transformed1); - model1.endReplaceModel(use_refit, refit_bottomup); - - tf1.setIdentity(); - } - - if(!tf2.matrix().isIdentity()) - { - std::vector vertices_transformed2(model2.num_vertices); - for(int i = 0; i < model2.num_vertices; ++i) - { - Vector3d& p = model2.vertices[i]; - Vector3d new_v = tf2 * p; - vertices_transformed2[i] = new_v; - } - - model2.beginReplaceModel(); - model2.replaceSubModel(vertices_transformed2); - model2.endReplaceModel(use_refit, refit_bottomup); - - tf2.setIdentity(); - } - - node.model1 = &model1; - node.tf1 = tf1; - node.model2 = &model2; - node.tf2 = tf2; - - node.vertices1 = model1.vertices; - node.vertices2 = model2.vertices; - - node.tri_indices1 = model1.tri_indices; - node.tri_indices2 = model2.tri_indices; - - node.request = request; - node.result = &result; - - node.cost_density = model1.cost_density * model2.cost_density; - - return true; -} - - -/// @brief Initialize traversal node for collision between two meshes, specialized for OBB type -bool initialize(MeshCollisionTraversalNodeOBB& node, - const BVHModel& model1, const Transform3d& tf1, - const BVHModel& model2, const Transform3d& tf2, - const CollisionRequest& request, CollisionResult& result); - -/// @brief Initialize traversal node for collision between two meshes, specialized for RSS type -bool initialize(MeshCollisionTraversalNodeRSS& node, - const BVHModel& model1, const Transform3d& tf1, - const BVHModel& model2, const Transform3d& tf2, - const CollisionRequest& request, CollisionResult& result); - -/// @brief Initialize traversal node for collision between two meshes, specialized for OBBRSS type -bool initialize(MeshCollisionTraversalNodeOBBRSS& node, - const BVHModel& model1, const Transform3d& tf1, - const BVHModel& model2, const Transform3d& tf2, - const CollisionRequest& request, CollisionResult& result); - -/// @brief Initialize traversal node for collision between two meshes, specialized for kIOS type -bool initialize(MeshCollisionTraversalNodekIOS& node, - const BVHModel& model1, const Transform3d& tf1, - const BVHModel& model2, const Transform3d& tf2, - const CollisionRequest& request, CollisionResult& result); - - -/// @brief Initialize traversal node for distance between two geometric shapes -template -bool initialize(ShapeDistanceTraversalNode& node, - const S1& shape1, const Transform3d& tf1, - const S2& shape2, const Transform3d& tf2, - const NarrowPhaseSolver* nsolver, - const DistanceRequest& request, - DistanceResult& result) -{ - node.request = request; - node.result = &result; - - node.model1 = &shape1; - node.tf1 = tf1; - node.model2 = &shape2; - node.tf2 = tf2; - node.nsolver = nsolver; - - return true; -} - -/// @brief Initialize traversal node for distance computation between two meshes, given the current transforms -template -bool initialize(MeshDistanceTraversalNode& node, - BVHModel& model1, Transform3d& tf1, - BVHModel& model2, Transform3d& tf2, - const DistanceRequest& request, - DistanceResult& result, - bool use_refit = false, bool refit_bottomup = false) -{ - if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES) - return false; - - if(!tf1.matrix().isIdentity()) - { - std::vector vertices_transformed1(model1.num_vertices); - for(int i = 0; i < model1.num_vertices; ++i) - { - Vector3d& p = model1.vertices[i]; - Vector3d new_v = tf1 * p; - vertices_transformed1[i] = new_v; - } - - model1.beginReplaceModel(); - model1.replaceSubModel(vertices_transformed1); - model1.endReplaceModel(use_refit, refit_bottomup); - - tf1.setIdentity(); - } - - if(!tf2.matrix().isIdentity()) - { - std::vector vertices_transformed2(model2.num_vertices); - for(int i = 0; i < model2.num_vertices; ++i) - { - Vector3d& p = model2.vertices[i]; - Vector3d new_v = tf2 * p; - vertices_transformed2[i] = new_v; - } - - model2.beginReplaceModel(); - model2.replaceSubModel(vertices_transformed2); - model2.endReplaceModel(use_refit, refit_bottomup); - - tf2.setIdentity(); - } - - node.request = request; - node.result = &result; - - node.model1 = &model1; - node.tf1 = tf1; - node.model2 = &model2; - node.tf2 = tf2; - - node.vertices1 = model1.vertices; - node.vertices2 = model2.vertices; - - node.tri_indices1 = model1.tri_indices; - node.tri_indices2 = model2.tri_indices; - - return true; -} - - -/// @brief Initialize traversal node for distance computation between two meshes, specialized for RSS type -bool initialize(MeshDistanceTraversalNodeRSS& node, - const BVHModel& model1, const Transform3d& tf1, - const BVHModel& model2, const Transform3d& tf2, - const DistanceRequest& request, - DistanceResult& result); - -/// @brief Initialize traversal node for distance computation between two meshes, specialized for kIOS type -bool initialize(MeshDistanceTraversalNodekIOS& node, - const BVHModel& model1, const Transform3d& tf1, - const BVHModel& model2, const Transform3d& tf2, - const DistanceRequest& request, - DistanceResult& result); - -/// @brief Initialize traversal node for distance computation between two meshes, specialized for OBBRSS type -bool initialize(MeshDistanceTraversalNodeOBBRSS& node, - const BVHModel& model1, const Transform3d& tf1, - const BVHModel& model2, const Transform3d& tf2, - const DistanceRequest& request, - DistanceResult& result); - -/// @brief Initialize traversal node for distance computation between one mesh and one shape, given the current transforms -template -bool initialize(MeshShapeDistanceTraversalNode& node, - BVHModel& model1, Transform3d& tf1, - const S& model2, const Transform3d& tf2, - const NarrowPhaseSolver* nsolver, - const DistanceRequest& request, - DistanceResult& result, - bool use_refit = false, bool refit_bottomup = false) -{ - if(model1.getModelType() != BVH_MODEL_TRIANGLES) - return false; - - if(!tf1.matrix().isIdentity()) - { - std::vector vertices_transformed1(model1.num_vertices); - for(int i = 0; i < model1.num_vertices; ++i) - { - Vector3d& p = model1.vertices[i]; - Vector3d new_v = tf1 * p; - vertices_transformed1[i] = new_v; - } - - model1.beginReplaceModel(); - model1.replaceSubModel(vertices_transformed1); - model1.endReplaceModel(use_refit, refit_bottomup); - - tf1.setIdentity(); - } - - node.request = request; - node.result = &result; - - node.model1 = &model1; - node.tf1 = tf1; - node.model2 = &model2; - node.tf2 = tf2; - node.nsolver = nsolver; - - node.vertices = model1.vertices; - node.tri_indices = model1.tri_indices; - - computeBV(model2, tf2, node.model2_bv); - - return true; -} - -/// @brief Initialize traversal node for distance computation between one shape and one mesh, given the current transforms -template -bool initialize(ShapeMeshDistanceTraversalNode& node, - const S& model1, const Transform3d& tf1, - BVHModel& model2, Transform3d& tf2, - const NarrowPhaseSolver* nsolver, - const DistanceRequest& request, - DistanceResult& result, - bool use_refit = false, bool refit_bottomup = false) -{ - if(model2.getModelType() != BVH_MODEL_TRIANGLES) - return false; - - if(!tf2.matrix().isIdentity()) - { - std::vector vertices_transformed(model2.num_vertices); - for(int i = 0; i < model2.num_vertices; ++i) - { - Vector3d& p = model2.vertices[i]; - Vector3d new_v = tf2 * p; - vertices_transformed[i] = new_v; - } - - model2.beginReplaceModel(); - model2.replaceSubModel(vertices_transformed); - model2.endReplaceModel(use_refit, refit_bottomup); - - tf2.setIdentity(); - } - - node.request = request; - node.result = &result; - - node.model1 = &model1; - node.tf1 = tf1; - node.model2 = &model2; - node.tf2 = tf2; - node.nsolver = nsolver; - - node.vertices = model2.vertices; - node.tri_indices = model2.tri_indices; - - computeBV(model1, tf1, node.model1_bv); - - return true; -} - -/// @cond IGNORE -namespace details -{ - -template class OrientedNode> -static inline bool setupMeshShapeDistanceOrientedNode(OrientedNode& node, - const BVHModel& model1, const Transform3d& tf1, - const S& model2, const Transform3d& tf2, - const NarrowPhaseSolver* nsolver, - const DistanceRequest& request, - DistanceResult& result) -{ - if(model1.getModelType() != BVH_MODEL_TRIANGLES) - return false; - - node.request = request; - node.result = &result; - - node.model1 = &model1; - node.tf1 = tf1; - node.model2 = &model2; - node.tf2 = tf2; - node.nsolver = nsolver; - - computeBV(model2, tf2, node.model2_bv); - - node.vertices = model1.vertices; - node.tri_indices = model1.tri_indices; - - return true; -} -} -/// @endcond - -/// @brief Initialize traversal node for distance computation between one mesh and one shape, specialized for RSS type -template -bool initialize(MeshShapeDistanceTraversalNodeRSS& node, - const BVHModel& model1, const Transform3d& tf1, - const S& model2, const Transform3d& tf2, - const NarrowPhaseSolver* nsolver, - const DistanceRequest& request, - DistanceResult& result) -{ - return details::setupMeshShapeDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result); -} - -/// @brief Initialize traversal node for distance computation between one mesh and one shape, specialized for kIOS type -template -bool initialize(MeshShapeDistanceTraversalNodekIOS& node, - const BVHModel& model1, const Transform3d& tf1, - const S& model2, const Transform3d& tf2, - const NarrowPhaseSolver* nsolver, - const DistanceRequest& request, - DistanceResult& result) -{ - return details::setupMeshShapeDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result); -} - -/// @brief Initialize traversal node for distance computation between one mesh and one shape, specialized for OBBRSS type -template -bool initialize(MeshShapeDistanceTraversalNodeOBBRSS& node, - const BVHModel& model1, const Transform3d& tf1, - const S& model2, const Transform3d& tf2, - const NarrowPhaseSolver* nsolver, - const DistanceRequest& request, - DistanceResult& result) -{ - return details::setupMeshShapeDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result); -} - - -namespace details -{ -template class OrientedNode> -static inline bool setupShapeMeshDistanceOrientedNode(OrientedNode& node, - const S& model1, const Transform3d& tf1, - const BVHModel& model2, const Transform3d& tf2, - const NarrowPhaseSolver* nsolver, - const DistanceRequest& request, - DistanceResult& result) -{ - if(model2.getModelType() != BVH_MODEL_TRIANGLES) - return false; - - node.request = request; - node.result = &result; - - node.model1 = &model1; - node.tf1 = tf1; - node.model2 = &model2; - node.tf2 = tf2; - node.nsolver = nsolver; - - computeBV(model1, tf1, node.model1_bv); - - node.vertices = model2.vertices; - node.tri_indices = model2.tri_indices; - node.R = tf2.linear(); - node.T = tf2.translation(); - - return true; -} -} - - -/// @brief Initialize traversal node for distance computation between one shape and one mesh, specialized for RSS type -template -bool initialize(ShapeMeshDistanceTraversalNodeRSS& node, - const S& model1, const Transform3d& tf1, - const BVHModel& model2, const Transform3d& tf2, - const NarrowPhaseSolver* nsolver, - const DistanceRequest& request, - DistanceResult& result) -{ - return details::setupShapeMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result); -} - -/// @brief Initialize traversal node for distance computation between one shape and one mesh, specialized for kIOS type -template -bool initialize(ShapeMeshDistanceTraversalNodekIOS& node, - const S& model1, const Transform3d& tf1, - const BVHModel& model2, const Transform3d& tf2, - const NarrowPhaseSolver* nsolver, - const DistanceRequest& request, - DistanceResult& result) -{ - return details::setupShapeMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result); -} - -/// @brief Initialize traversal node for distance computation between one shape and one mesh, specialized for OBBRSS type -template -bool initialize(ShapeMeshDistanceTraversalNodeOBBRSS& node, - const S& model1, const Transform3d& tf1, - const BVHModel& model2, const Transform3d& tf2, - const NarrowPhaseSolver* nsolver, - const DistanceRequest& request, - DistanceResult& result) -{ - return details::setupShapeMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result); -} - - - -/// @brief Initialize traversal node for continuous collision detection between two meshes -template -bool initialize(MeshContinuousCollisionTraversalNode& node, - const BVHModel& model1, const Transform3d& tf1, - const BVHModel& model2, const Transform3d& tf2, - const CollisionRequest& request) -{ - node.model1 = &model1; - node.tf1 = tf1; - node.model2 = &model2; - node.tf2 = tf2; - - node.vertices1 = model1.vertices; - node.vertices2 = model2.vertices; - - node.tri_indices1 = model1.tri_indices; - node.tri_indices2 = model2.tri_indices; - - node.prev_vertices1 = model1.prev_vertices; - node.prev_vertices2 = model2.prev_vertices; - - node.request = request; - - return true; -} - -/// @brief Initialize traversal node for conservative advancement computation between two meshes, given the current transforms -template -bool initialize(MeshConservativeAdvancementTraversalNode& node, - BVHModel& model1, const Transform3d& tf1, - BVHModel& model2, const Transform3d& tf2, - FCL_REAL w = 1, - bool use_refit = false, bool refit_bottomup = false) -{ - std::vector vertices_transformed1(model1.num_vertices); - for(int i = 0; i < model1.num_vertices; ++i) - { - Vector3d& p = model1.vertices[i]; - Vector3d new_v = tf1 * p; - vertices_transformed1[i] = new_v; - } - - - std::vector vertices_transformed2(model2.num_vertices); - for(int i = 0; i < model2.num_vertices; ++i) - { - Vector3d& p = model2.vertices[i]; - Vector3d new_v = tf2 * p; - vertices_transformed2[i] = new_v; - } - - model1.beginReplaceModel(); - model1.replaceSubModel(vertices_transformed1); - model1.endReplaceModel(use_refit, refit_bottomup); - - model2.beginReplaceModel(); - model2.replaceSubModel(vertices_transformed2); - model2.endReplaceModel(use_refit, refit_bottomup); - - node.model1 = &model1; - node.model2 = &model2; - - node.vertices1 = model1.vertices; - node.vertices2 = model2.vertices; - - node.tri_indices1 = model1.tri_indices; - node.tri_indices2 = model2.tri_indices; - - node.w = w; - - return true; -} - - -/// @brief Initialize traversal node for conservative advancement computation between two meshes, given the current transforms, specialized for RSS -bool initialize(MeshConservativeAdvancementTraversalNodeRSS& node, - const BVHModel& model1, const Transform3d& tf1, - const BVHModel& model2, const Transform3d& tf2, - FCL_REAL w = 1); - -bool initialize(MeshConservativeAdvancementTraversalNodeOBBRSS& node, - const BVHModel& model1, const Transform3d& tf1, - const BVHModel& model2, const Transform3d& tf2, - FCL_REAL w = 1); - -template -bool initialize(ShapeConservativeAdvancementTraversalNode& node, - const S1& shape1, const Transform3d& tf1, - const S2& shape2, const Transform3d& tf2, - const NarrowPhaseSolver* nsolver) -{ - node.model1 = &shape1; - node.tf1 = tf1; - node.model2 = &shape2; - node.tf2 = tf2; - node.nsolver = nsolver; - - computeBV(shape1, Transform3d::Identity(), node.model1_bv); - computeBV(shape2, Transform3d::Identity(), node.model2_bv); - - return true; -} - -template -bool initialize(MeshShapeConservativeAdvancementTraversalNode& node, - BVHModel& model1, const Transform3d& tf1, - const S& model2, const Transform3d& tf2, - const NarrowPhaseSolver* nsolver, - FCL_REAL w = 1, - bool use_refit = false, bool refit_bottomup = false) -{ - std::vector vertices_transformed(model1.num_vertices); - for(int i = 0; i < model1.num_vertices; ++i) - { - Vector3d& p = model1.vertices[i]; - Vector3d new_v = tf1 * p; - vertices_transformed[i] = new_v; - } - - model1.beginReplaceModel(); - model1.replaceSubModel(vertices_transformed); - model1.endReplaceModel(use_refit, refit_bottomup); - - node.model1 = &model1; - node.model2 = &model2; - - node.vertices = model1.vertices; - node.tri_indices = model1.tri_indices; - - node.tf1 = tf1; - node.tf2 = tf2; - - node.nsolver = nsolver; - node.w = w; - - computeBV(model2, Transform3d::Identity(), node.model2_bv); - - return true; -} - - -template -bool initialize(MeshShapeConservativeAdvancementTraversalNodeRSS& node, - const BVHModel& model1, const Transform3d& tf1, - const S& model2, const Transform3d& tf2, - const NarrowPhaseSolver* nsolver, - FCL_REAL w = 1) -{ - node.model1 = &model1; - node.tf1 = tf1; - node.model2 = &model2; - node.tf2 = tf2; - node.nsolver = nsolver; - - node.w = w; - - computeBV(model2, Transform3d::Identity(), node.model2_bv); - - return true; -} - - -template -bool initialize(MeshShapeConservativeAdvancementTraversalNodeOBBRSS& node, - const BVHModel& model1, const Transform3d& tf1, - const S& model2, const Transform3d& tf2, - const NarrowPhaseSolver* nsolver, - FCL_REAL w = 1) -{ - node.model1 = &model1; - node.tf1 = tf1; - node.model2 = &model2; - node.tf2 = tf2; - node.nsolver = nsolver; - - node.w = w; - - computeBV(model2, Transform3d::Identity(), node.model2_bv); - - return true; -} - - -template -bool initialize(ShapeMeshConservativeAdvancementTraversalNode& node, - const S& model1, const Transform3d& tf1, - BVHModel& model2, const Transform3d& tf2, - const NarrowPhaseSolver* nsolver, - FCL_REAL w = 1, - bool use_refit = false, bool refit_bottomup = false) -{ - std::vector vertices_transformed(model2.num_vertices); - for(int i = 0; i < model2.num_vertices; ++i) - { - Vector3d& p = model2.vertices[i]; - Vector3d new_v = tf2 * p; - vertices_transformed[i] = new_v; - } - - model2.beginReplaceModel(); - model2.replaceSubModel(vertices_transformed); - model2.endReplaceModel(use_refit, refit_bottomup); - - node.model1 = &model1; - node.model2 = &model2; - - node.vertices = model2.vertices; - node.tri_indices = model2.tri_indices; - - node.tf1 = tf1; - node.tf2 = tf2; - - node.nsolver = nsolver; - node.w = w; - - computeBV(model1, Transform3d::Identity(), node.model1_bv); - - return true; -} - - -template -bool initialize(ShapeMeshConservativeAdvancementTraversalNodeRSS& node, - const S& model1, const Transform3d& tf1, - const BVHModel& model2, const Transform3d& tf2, - const NarrowPhaseSolver* nsolver, - FCL_REAL w = 1) -{ - node.model1 = &model1; - node.tf1 = tf1; - node.model2 = &model2; - node.tf2 = tf2; - node.nsolver = nsolver; - - node.w = w; - - computeBV(model1, Transform3d::Identity(), node.model1_bv); - - return true; -} - - -template -bool initialize(ShapeMeshConservativeAdvancementTraversalNodeOBBRSS& node, - const S& model1, const Transform3d& tf1, - const BVHModel& model2, const Transform3d& tf2, - const NarrowPhaseSolver* nsolver, - FCL_REAL w = 1) -{ - node.model1 = &model1; - node.tf1 = tf1; - node.model2 = &model2; - node.tf2 = tf2; - node.nsolver = nsolver; - - node.w = w; - - computeBV(model1, Transform3d::Identity(), node.model1_bv); - - return true; -} - - - - -} +#warning "This header has been deprecated in FCL 0.6. " #endif diff --git a/include/fcl/traversal/traversal_node_shapes.h b/include/fcl/traversal/traversal_node_shapes.h index 0f442dca3..06cea4f25 100644 --- a/include/fcl/traversal/traversal_node_shapes.h +++ b/include/fcl/traversal/traversal_node_shapes.h @@ -35,205 +35,9 @@ /** \author Jia Pan */ - #ifndef FCL_TRAVERSAL_NODE_SHAPES_H #define FCL_TRAVERSAL_NODE_SHAPES_H -#include - -#include "fcl/collision_data.h" -#include "fcl/traversal/traversal_node_base.h" -#include "fcl/narrowphase/narrowphase.h" -#include "fcl/shape/geometric_shapes_utility.h" -#include "fcl/BV/BV.h" -#include "fcl/shape/geometric_shapes_utility.h" -#include "fcl/ccd/motion.h" - -namespace fcl -{ - -/// @brief Traversal node for collision between two shapes -template -class ShapeCollisionTraversalNode : public CollisionTraversalNodeBase -{ -public: - ShapeCollisionTraversalNode() : CollisionTraversalNodeBase() - { - model1 = NULL; - model2 = NULL; - - nsolver = NULL; - } - - /// @brief BV culling test in one BVTT node - bool BVTesting(int, int) const - { - return false; - } - - /// @brief Intersection testing between leaves (two shapes) - void leafTesting(int, int) const - { - if(model1->isOccupied() && model2->isOccupied()) - { - bool is_collision = false; - if(request.enable_contact) - { - std::vector contacts; - if(nsolver->shapeIntersect(*model1, tf1, *model2, tf2, &contacts)) - { - is_collision = true; - if(request.num_max_contacts > result->numContacts()) - { - const size_t free_space = request.num_max_contacts - result->numContacts(); - size_t num_adding_contacts; - - // If the free space is not enough to add all the new contacts, we add contacts in descent order of penetration depth. - if (free_space < contacts.size()) - { - std::partial_sort(contacts.begin(), contacts.begin() + free_space, contacts.end(), std::bind(comparePenDepth, std::placeholders::_2, std::placeholders::_1)); - num_adding_contacts = free_space; - } - else - { - num_adding_contacts = contacts.size(); - } - - for(size_t i = 0; i < num_adding_contacts; ++i) - result->addContact(Contact(model1, model2, Contact::NONE, Contact::NONE, contacts[i].pos, contacts[i].normal, contacts[i].penetration_depth)); - } - } - } - else - { - if(nsolver->shapeIntersect(*model1, tf1, *model2, tf2, NULL)) - { - is_collision = true; - if(request.num_max_contacts > result->numContacts()) - result->addContact(Contact(model1, model2, Contact::NONE, Contact::NONE)); - } - } - - if(is_collision && request.enable_cost) - { - AABB aabb1, aabb2; - computeBV(*model1, tf1, aabb1); - computeBV(*model2, tf2, aabb2); - AABB overlap_part; - aabb1.overlap(aabb2, overlap_part); - result->addCostSource(CostSource(overlap_part, cost_density), request.num_max_cost_sources); - } - } - else if((!model1->isFree() && !model2->isFree()) && request.enable_cost) - { - if(nsolver->shapeIntersect(*model1, tf1, *model2, tf2, NULL)) - { - AABB aabb1, aabb2; - computeBV(*model1, tf1, aabb1); - computeBV(*model2, tf2, aabb2); - AABB overlap_part; - aabb1.overlap(aabb2, overlap_part); - result->addCostSource(CostSource(overlap_part, cost_density), request.num_max_cost_sources); - } - } - } - - const S1* model1; - const S2* model2; - - FCL_REAL cost_density; - - const NarrowPhaseSolver* nsolver; -}; - -/// @brief Traversal node for distance between two shapes -template -class ShapeDistanceTraversalNode : public DistanceTraversalNodeBase -{ -public: - ShapeDistanceTraversalNode() : DistanceTraversalNodeBase() - { - model1 = NULL; - model2 = NULL; - - nsolver = NULL; - } - - /// @brief BV culling test in one BVTT node - FCL_REAL BVTesting(int, int) const - { - return -1; // should not be used - } - - /// @brief Distance testing between leaves (two shapes) - void leafTesting(int, int) const - { - FCL_REAL distance; - Vector3d closest_p1, closest_p2; - nsolver->shapeDistance(*model1, tf1, *model2, tf2, &distance, &closest_p1, &closest_p2); - result->update(distance, model1, model2, DistanceResult::NONE, DistanceResult::NONE, closest_p1, closest_p2); - } - - const S1* model1; - const S2* model2; - - const NarrowPhaseSolver* nsolver; -}; - -template -class ShapeConservativeAdvancementTraversalNode : public ShapeDistanceTraversalNode -{ -public: - ShapeConservativeAdvancementTraversalNode() : ShapeDistanceTraversalNode() - { - delta_t = 1; - toc = 0; - t_err = (FCL_REAL)0.0001; - - motion1 = NULL; - motion2 = NULL; - } - - void leafTesting(int, int) const - { - FCL_REAL distance; - Vector3d closest_p1, closest_p2; - this->nsolver->shapeDistance(*(this->model1), this->tf1, *(this->model2), this->tf2, &distance, &closest_p1, &closest_p2); - - Vector3d n = this->tf2 * closest_p2 - this->tf1 * closest_p1; - n.normalize(); - TBVMotionBoundVisitor mb_visitor1(model1_bv, n); - TBVMotionBoundVisitor mb_visitor2(model2_bv, -n); - FCL_REAL bound1 = motion1->computeMotionBound(mb_visitor1); - FCL_REAL bound2 = motion2->computeMotionBound(mb_visitor2); - - FCL_REAL bound = bound1 + bound2; - - FCL_REAL cur_delta_t; - if(bound <= distance) cur_delta_t = 1; - else cur_delta_t = distance / bound; - - if(cur_delta_t < delta_t) - delta_t = cur_delta_t; - } - - mutable FCL_REAL min_distance; - - /// @brief The time from beginning point - FCL_REAL toc; - FCL_REAL t_err; - - /// @brief The delta_t each step - mutable FCL_REAL delta_t; - - /// @brief Motions for the two objects in query - const MotionBase* motion1; - const MotionBase* motion2; - - RSS model1_bv, model2_bv; // local bv for the two shapes -}; - - -} +#warning "This header has been deprecated in FCL 0.6. " #endif diff --git a/include/fcl/traversal/traversal_nodes.h b/include/fcl/traversal/traversal_nodes.h new file mode 100644 index 000000000..70aaddf2b --- /dev/null +++ b/include/fcl/traversal/traversal_nodes.h @@ -0,0 +1,84 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jeongseok Lee */ + +#ifndef FCL_TRAVERSAL_TRAVERSALNODES_H +#define FCL_TRAVERSAL_TRAVERSALNODES_H + +#include "fcl/config.h" + +#include "fcl/traversal/collision/bvh_collision_traversal_node.h" +#include "fcl/traversal/collision/bvh_shape_collision_traversal_node.h" +#include "fcl/traversal/collision/collision_traversal_node_base.h" +#include "fcl/traversal/collision/mesh_collision_traversal_node.h" +#include "fcl/traversal/collision/mesh_continuous_collision_traversal_node.h" +#include "fcl/traversal/collision/mesh_shape_collision_traversal_node.h" +#include "fcl/traversal/collision/shape_bvh_collision_traversal_node.h" +#include "fcl/traversal/collision/shape_collision_traversal_node.h" +#include "fcl/traversal/collision/shape_mesh_collision_traversal_node.h" + +#include "fcl/traversal/distance/bvh_distance_traversal_node.h" +#include "fcl/traversal/distance/bvh_shape_distance_traversal_node.h" +#include "fcl/traversal/distance/distance_traversal_node_base.h" +#include "fcl/traversal/distance/mesh_distance_traversal_node.h" +#include "fcl/traversal/distance/mesh_conservative_advancement_traversal_node.h" +#include "fcl/traversal/distance/mesh_shape_distance_traversal_node.h" +#include "fcl/traversal/distance/mesh_shape_conservative_advancement_traversal_node.h" +#include "fcl/traversal/distance/shape_bvh_distance_traversal_node.h" +#include "fcl/traversal/distance/shape_distance_traversal_node.h" +#include "fcl/traversal/distance/shape_conservative_advancement_traversal_node.h" +#include "fcl/traversal/distance/shape_mesh_distance_traversal_node.h" +#include "fcl/traversal/distance/shape_mesh_conservative_advancement_traversal_node.h" + +#if FCL_HAVE_OCTOMAP + +#include "fcl/traversal/octree/octree_solver.h" + +#include "fcl/traversal/octree/collision/mesh_octree_collision_traversal_node.h" +#include "fcl/traversal/octree/collision/octree_collision_traversal_node.h" +#include "fcl/traversal/octree/collision/octree_mesh_collision_traversal_node.h" +#include "fcl/traversal/octree/collision/octree_shape_collision_traversal_node.h" +#include "fcl/traversal/octree/collision/shape_octree_collision_traversal_node.h" + +#include "fcl/traversal/octree/distance/mesh_octree_distance_traversal_node.h" +#include "fcl/traversal/octree/distance/octree_distance_traversal_node.h" +#include "fcl/traversal/octree/distance/octree_mesh_distance_traversal_node.h" +#include "fcl/traversal/octree/distance/octree_shape_distance_traversal_node.h" +#include "fcl/traversal/octree/distance/shape_octree_distance_traversal_node.h" + +#endif // FCL_HAVE_OCTOMAP + +#endif // FCL_TRAVERSAL_TRAVERSALNODES_H diff --git a/include/fcl/traversal/traversal_recurse.h b/include/fcl/traversal/traversal_recurse.h index 2580f5c3b..ff94851bf 100644 --- a/include/fcl/traversal/traversal_recurse.h +++ b/include/fcl/traversal/traversal_recurse.h @@ -39,36 +39,478 @@ #ifndef FCL_TRAVERSAL_RECURSE_H #define FCL_TRAVERSAL_RECURSE_H -#include "fcl/traversal/traversal_node_base.h" -#include "fcl/traversal/traversal_node_bvhs.h" -#include "fcl/BVH/BVH_front.h" #include +#include "fcl/BVH/BVH_front.h" +#include "fcl/traversal/traversal_node_base.h" +#include "fcl/traversal/collision/collision_traversal_node_base.h" +#include "fcl/traversal/collision/mesh_collision_traversal_node.h" +#include "fcl/traversal/distance/distance_traversal_node_base.h" namespace fcl { /// @brief Recurse function for collision -void collisionRecurse(CollisionTraversalNodeBase* node, int b1, int b2, BVHFrontList* front_list); +template +void collisionRecurse(CollisionTraversalNodeBase* node, int b1, int b2, BVHFrontList* front_list); -/// @brief Recurse function for collision, specialized for OBB type -void collisionRecurse(MeshCollisionTraversalNodeOBB* node, int b1, int b2, const Matrix3d& R, const Vector3d& T, BVHFrontList* front_list); +/// @brief Recurse function for collision, specialized for OBBd type +template +void collisionRecurse(MeshCollisionTraversalNodeOBB* node, int b1, int b2, const Matrix3& R, const Vector3& T, BVHFrontList* front_list); -/// @brief Recurse function for collision, specialized for RSS type -void collisionRecurse(MeshCollisionTraversalNodeRSS* node, int b1, int b2, const Matrix3d& R, const Vector3d& T, BVHFrontList* front_list); +/// @brief Recurse function for collision, specialized for RSSd type +template +void collisionRecurse(MeshCollisionTraversalNodeRSS* node, int b1, int b2, const Matrix3& R, const Vector3& T, BVHFrontList* front_list); /// @brief Recurse function for self collision. Make sure node is set correctly so that the first and second tree are the same -void selfCollisionRecurse(CollisionTraversalNodeBase* node, int b, BVHFrontList* front_list); +template +void selfCollisionRecurse(CollisionTraversalNodeBase* node, int b, BVHFrontList* front_list); /// @brief Recurse function for distance -void distanceRecurse(DistanceTraversalNodeBase* node, int b1, int b2, BVHFrontList* front_list); +template +void distanceRecurse(DistanceTraversalNodeBase* node, int b1, int b2, BVHFrontList* front_list); /// @brief Recurse function for distance, using queue acceleration -void distanceQueueRecurse(DistanceTraversalNodeBase* node, int b1, int b2, BVHFrontList* front_list, int qsize); +template +void distanceQueueRecurse(DistanceTraversalNodeBase* node, int b1, int b2, BVHFrontList* front_list, int qsize); /// @brief Recurse function for front list propagation -void propagateBVHFrontListCollisionRecurse(CollisionTraversalNodeBase* node, BVHFrontList* front_list); +template +void propagateBVHFrontListCollisionRecurse(CollisionTraversalNodeBase* node, BVHFrontList* front_list); + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +void collisionRecurse(CollisionTraversalNodeBase* node, int b1, int b2, BVHFrontList* front_list) +{ + bool l1 = node->isFirstNodeLeaf(b1); + bool l2 = node->isSecondNodeLeaf(b2); + + if(l1 && l2) + { + updateFrontList(front_list, b1, b2); + + if(node->BVTesting(b1, b2)) return; + + node->leafTesting(b1, b2); + return; + } + + if(node->BVTesting(b1, b2)) + { + updateFrontList(front_list, b1, b2); + return; + } + + if(node->firstOverSecond(b1, b2)) + { + int c1 = node->getFirstLeftChild(b1); + int c2 = node->getFirstRightChild(b1); + + collisionRecurse(node, c1, b2, front_list); + + // early stop is disabled is front_list is used + if(node->canStop() && !front_list) return; + + collisionRecurse(node, c2, b2, front_list); + } + else + { + int c1 = node->getSecondLeftChild(b2); + int c2 = node->getSecondRightChild(b2); + + collisionRecurse(node, b1, c1, front_list); + + // early stop is disabled is front_list is used + if(node->canStop() && !front_list) return; + + collisionRecurse(node, b1, c2, front_list); + } +} + +//============================================================================== +template +void collisionRecurse(MeshCollisionTraversalNodeOBB* node, int b1, int b2, const Matrix3& R, const Vector3& T, BVHFrontList* front_list) +{ + bool l1 = node->isFirstNodeLeaf(b1); + bool l2 = node->isSecondNodeLeaf(b2); + + if(l1 && l2) + { + updateFrontList(front_list, b1, b2); + + if(node->BVTesting(b1, b2, R, T)) return; + + node->leafTesting(b1, b2, R, T); + return; + } + + if(node->BVTesting(b1, b2, R, T)) + { + updateFrontList(front_list, b1, b2); + return; + } + + Vector3 temp; + + if(node->firstOverSecond(b1, b2)) + { + int c1 = node->getFirstLeftChild(b1); + int c2 = node->getFirstRightChild(b1); + + const OBB& bv1 = node->model1->getBV(c1).bv; + + Matrix3 Rc = R.transpose() * bv1.axis; + temp = T - bv1.To; + Vector3 Tc = temp.transpose() * bv1.axis; + + collisionRecurse(node, c1, b2, Rc, Tc, front_list); + + // early stop is disabled is front_list is used + if(node->canStop() && !front_list) return; + + const OBB& bv2 = node->model1->getBV(c2).bv; + + Rc.noalias() = R.transpose() * bv2.axis; + temp = T - bv2.To; + Tc[0] = bv2.axis.col(0).dot(temp); + Tc[1] = bv2.axis.col(1).dot(temp); + Tc[2] = bv2.axis.col(2).dot(temp); + + collisionRecurse(node, c2, b2, Rc, Tc, front_list); + } + else + { + int c1 = node->getSecondLeftChild(b2); + int c2 = node->getSecondRightChild(b2); + + const OBB& bv1 = node->model2->getBV(c1).bv; + Matrix3 Rc; + temp.noalias() = R * bv1.axis.col(0); + Rc(0, 0) = temp[0]; Rc(1, 0) = temp[1]; Rc(2, 0) = temp[2]; + temp.noalias() = R * bv1.axis.col(1); + Rc(0, 1) = temp[0]; Rc(1, 1) = temp[1]; Rc(2, 1) = temp[2]; + temp.noalias() = R * bv1.axis.col(2); + Rc(0, 2) = temp[0]; Rc(1, 2) = temp[1]; Rc(2, 2) = temp[2]; + Vector3 Tc = R * bv1.To + T; + + collisionRecurse(node, b1, c1, Rc, Tc, front_list); + + // early stop is disabled is front_list is used + if(node->canStop() && !front_list) return; + + const OBB& bv2 = node->model2->getBV(c2).bv; + temp.noalias() = R * bv2.axis.col(0); + Rc(0, 0) = temp[0]; Rc(1, 0) = temp[1]; Rc(2, 0) = temp[2]; + temp.noalias() = R * bv2.axis.col(1); + Rc(0, 1) = temp[0]; Rc(1, 1) = temp[1]; Rc(2, 1) = temp[2]; + temp.noalias() = R * bv2.axis.col(2); + Rc(0, 2) = temp[0]; Rc(1, 2) = temp[1]; Rc(2, 2) = temp[2]; + Tc = T; + Tc.noalias() += R * bv2.To; + + collisionRecurse(node, b1, c2, Rc, Tc, front_list); + } +} + +//============================================================================== +template +void collisionRecurse(MeshCollisionTraversalNodeRSS* node, int b1, int b2, const Matrix3& R, const Vector3& T, BVHFrontList* front_list) +{ + // Do nothing +} + +//============================================================================== +/** Recurse function for self collision + * Make sure node is set correctly so that the first and second tree are the same + */ +template +void selfCollisionRecurse(CollisionTraversalNodeBase* node, int b, BVHFrontList* front_list) +{ + bool l = node->isFirstNodeLeaf(b); + + if(l) return; + + int c1 = node->getFirstLeftChild(b); + int c2 = node->getFirstRightChild(b); + + selfCollisionRecurse(node, c1, front_list); + if(node->canStop() && !front_list) return; + + selfCollisionRecurse(node, c2, front_list); + if(node->canStop() && !front_list) return; + collisionRecurse(node, c1, c2, front_list); +} + +//============================================================================== +template +void distanceRecurse(DistanceTraversalNodeBase* node, int b1, int b2, BVHFrontList* front_list) +{ + bool l1 = node->isFirstNodeLeaf(b1); + bool l2 = node->isSecondNodeLeaf(b2); + + if(l1 && l2) + { + updateFrontList(front_list, b1, b2); + + node->leafTesting(b1, b2); + return; + } + + int a1, a2, c1, c2; + + if(node->firstOverSecond(b1, b2)) + { + a1 = node->getFirstLeftChild(b1); + a2 = b2; + c1 = node->getFirstRightChild(b1); + c2 = b2; + } + else + { + a1 = b1; + a2 = node->getSecondLeftChild(b2); + c1 = b1; + c2 = node->getSecondRightChild(b2); + } + + S d1 = node->BVTesting(a1, a2); + S d2 = node->BVTesting(c1, c2); + + if(d2 < d1) + { + if(!node->canStop(d2)) + distanceRecurse(node, c1, c2, front_list); + else + updateFrontList(front_list, c1, c2); + + if(!node->canStop(d1)) + distanceRecurse(node, a1, a2, front_list); + else + updateFrontList(front_list, a1, a2); + } + else + { + if(!node->canStop(d1)) + distanceRecurse(node, a1, a2, front_list); + else + updateFrontList(front_list, a1, a2); + + if(!node->canStop(d2)) + distanceRecurse(node, c1, c2, front_list); + else + updateFrontList(front_list, c1, c2); + } +} + +//============================================================================== +/** \brief Bounding volume test structure */ +template +struct BVT +{ + /** \brief distance between bvs */ + S d; + + /** \brief bv indices for a pair of bvs in two models */ + int b1, b2; +}; + +//============================================================================== +/** \brief Comparer between two BVT */ +template +struct BVT_Comparer +{ + bool operator() (const BVT& lhs, const BVT& rhs) const + { + return lhs.d > rhs.d; + } +}; + +//============================================================================== +template +struct BVTQ +{ + BVTQ() : qsize(2) {} + + bool empty() const + { + return pq.empty(); + } + + size_t size() const + { + return pq.size(); + } + + const BVT& top() const + { + return pq.top(); + } + + void push(const BVT& x) + { + pq.push(x); + } + + void pop() + { + pq.pop(); + } + + bool full() const + { + return (pq.size() + 1 >= qsize); + } + std::priority_queue, std::vector>, BVT_Comparer> pq; + + /** \brief Queue size */ + unsigned int qsize; +}; + +//============================================================================== +template +void distanceQueueRecurse(DistanceTraversalNodeBase* node, int b1, int b2, BVHFrontList* front_list, int qsize) +{ + BVTQ bvtq; + bvtq.qsize = qsize; + + BVT min_test; + min_test.b1 = b1; + min_test.b2 = b2; + + while(1) + { + bool l1 = node->isFirstNodeLeaf(min_test.b1); + bool l2 = node->isSecondNodeLeaf(min_test.b2); + + if(l1 && l2) + { + updateFrontList(front_list, min_test.b1, min_test.b2); + + node->leafTesting(min_test.b1, min_test.b2); + } + else if(bvtq.full()) + { + // queue should not get two more tests, recur + + distanceQueueRecurse(node, min_test.b1, min_test.b2, front_list, qsize); + } + else + { + // queue capacity is not full yet + BVT bvt1, bvt2; + + if(node->firstOverSecond(min_test.b1, min_test.b2)) + { + int c1 = node->getFirstLeftChild(min_test.b1); + int c2 = node->getFirstRightChild(min_test.b1); + bvt1.b1 = c1; + bvt1.b2 = min_test.b2; + bvt1.d = node->BVTesting(bvt1.b1, bvt1.b2); + + bvt2.b1 = c2; + bvt2.b2 = min_test.b2; + bvt2.d = node->BVTesting(bvt2.b1, bvt2.b2); + } + else + { + int c1 = node->getSecondLeftChild(min_test.b2); + int c2 = node->getSecondRightChild(min_test.b2); + bvt1.b1 = min_test.b1; + bvt1.b2 = c1; + bvt1.d = node->BVTesting(bvt1.b1, bvt1.b2); + + bvt2.b1 = min_test.b1; + bvt2.b2 = c2; + bvt2.d = node->BVTesting(bvt2.b1, bvt2.b2); + } + + bvtq.push(bvt1); + bvtq.push(bvt2); + } + + if(bvtq.empty()) + break; + else + { + min_test = bvtq.top(); + bvtq.pop(); + + if(node->canStop(min_test.d)) + { + updateFrontList(front_list, min_test.b1, min_test.b2); + break; + } + } + } +} + +//============================================================================== +template +void propagateBVHFrontListCollisionRecurse(CollisionTraversalNodeBase* node, BVHFrontList* front_list) +{ + BVHFrontList::iterator front_iter; + BVHFrontList append; + for(front_iter = front_list->begin(); front_iter != front_list->end(); ++front_iter) + { + int b1 = front_iter->left; + int b2 = front_iter->right; + bool l1 = node->isFirstNodeLeaf(b1); + bool l2 = node->isSecondNodeLeaf(b2); + + if(l1 & l2) + { + front_iter->valid = false; // the front node is no longer valid, in collideRecurse will add again. + collisionRecurse(node, b1, b2, &append); + } + else + { + if(!node->BVTesting(b1, b2)) + { + front_iter->valid = false; + + if(node->firstOverSecond(b1, b2)) + { + int c1 = node->getFirstLeftChild(b1); + int c2 = node->getFirstRightChild(b1); + + collisionRecurse(node, c1, b2, front_list); + collisionRecurse(node, c2, b2, front_list); + } + else + { + int c1 = node->getSecondLeftChild(b2); + int c2 = node->getSecondRightChild(b2); + + collisionRecurse(node, b1, c1, front_list); + collisionRecurse(node, b1, c2, front_list); + } + } + } + } + + + // clean the old front list (remove invalid node) + for(front_iter = front_list->begin(); front_iter != front_list->end();) + { + if(!front_iter->valid) + front_iter = front_list->erase(front_iter); + else + ++front_iter; + } + + for(front_iter = append.begin(); front_iter != append.end(); ++front_iter) + { + front_list->push_back(*front_iter); + } } +} // namespace fcl + #endif diff --git a/src/BV/OBB.cpp b/src/BV/OBB.cpp deleted file mode 100644 index 0ada6914f..000000000 --- a/src/BV/OBB.cpp +++ /dev/null @@ -1,396 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#include "fcl/BV/OBB.h" -#include "fcl/BVH/BVH_utility.h" - -#include -#include - -namespace fcl -{ - -/// @brief Compute the 8 vertices of a OBB -inline void computeVertices(const OBB& b, Vector3d vertices[8]) -{ - const Vector3d& extent = b.extent; - const Vector3d& To = b.To; - - Vector3d extAxis0 = b.axis.col(0) * extent[0]; - Vector3d extAxis1 = b.axis.col(1) * extent[1]; - Vector3d extAxis2 = b.axis.col(2) * extent[2]; - - vertices[0] = To - extAxis0 - extAxis1 - extAxis2; - vertices[1] = To + extAxis0 - extAxis1 - extAxis2; - vertices[2] = To + extAxis0 + extAxis1 - extAxis2; - vertices[3] = To - extAxis0 + extAxis1 - extAxis2; - vertices[4] = To - extAxis0 - extAxis1 + extAxis2; - vertices[5] = To + extAxis0 - extAxis1 + extAxis2; - vertices[6] = To + extAxis0 + extAxis1 + extAxis2; - vertices[7] = To - extAxis0 + extAxis1 + extAxis2; -} - -/// @brief OBB merge method when the centers of two smaller OBB are far away -inline OBB merge_largedist(const OBB& b1, const OBB& b2) -{ - OBB b; - Vector3d vertex[16]; - computeVertices(b1, vertex); - computeVertices(b2, vertex + 8); - Matrix3d M; - Matrix3d E; - Vector3d s(0, 0, 0); - - b.axis.col(0) = b1.To - b2.To; - b.axis.col(0).normalize(); - - Vector3d vertex_proj[16]; - for(int i = 0; i < 16; ++i) - vertex_proj[i] = vertex[i] - b.axis.col(0) * vertex[i].dot(b.axis.col(0)); - - getCovariance(vertex_proj, NULL, NULL, NULL, 16, M); - eigen(M, s, E); - - int min, mid, max; - if (s[0] > s[1]) - { - max = 0; - min = 1; - } - else - { - min = 0; - max = 1; - } - - if (s[2] < s[min]) - { - mid = min; - min = 2; - } - else if (s[2] > s[max]) - { - mid = max; - max = 2; - } - else - { - mid = 2; - } - - b.axis.col(1) << E.col(0)[max], E.col(1)[max], E.col(2)[max]; - b.axis.col(2) << E.col(0)[mid], E.col(1)[mid], E.col(2)[mid]; - - // set obb centers and extensions - Vector3d center, extent; - getExtentAndCenter(vertex, NULL, NULL, NULL, 16, b.axis, center, extent); - - b.To = center; - b.extent = extent; - - return b; -} - - -/// @brief OBB merge method when the centers of two smaller OBB are close -inline OBB merge_smalldist(const OBB& b1, const OBB& b2) -{ - OBB b; - b.To = (b1.To + b2.To) * 0.5; - Quaternion3d q0(b1.axis); - Quaternion3d q1(b2.axis); - if(q0.dot(q1) < 0) - q1.coeffs() = -q1.coeffs(); - - Quaternion3d q(q0.coeffs() + q1.coeffs()); - q.normalize(); - b.axis = q.toRotationMatrix(); - - - Vector3d vertex[8], diff; - FCL_REAL real_max = std::numeric_limits::max(); - Vector3d pmin(real_max, real_max, real_max); - Vector3d pmax(-real_max, -real_max, -real_max); - - computeVertices(b1, vertex); - for(int i = 0; i < 8; ++i) - { - diff = vertex[i] - b.To; - for(int j = 0; j < 3; ++j) - { - FCL_REAL dot = diff.dot(b.axis.col(j)); - if(dot > pmax[j]) - pmax[j] = dot; - else if(dot < pmin[j]) - pmin[j] = dot; - } - } - - computeVertices(b2, vertex); - for(int i = 0; i < 8; ++i) - { - diff = vertex[i] - b.To; - for(int j = 0; j < 3; ++j) - { - FCL_REAL dot = diff.dot(b.axis.col(j)); - if(dot > pmax[j]) - pmax[j] = dot; - else if(dot < pmin[j]) - pmin[j] = dot; - } - } - - for(int j = 0; j < 3; ++j) - { - b.To += (b.axis.col(j) * (0.5 * (pmax[j] + pmin[j]))); - b.extent[j] = 0.5 * (pmax[j] - pmin[j]); - } - - return b; -} - -bool obbDisjoint(const Matrix3d& B, const Vector3d& T, const Vector3d& a, const Vector3d& b) -{ - register FCL_REAL t, s; - const FCL_REAL reps = 1e-6; - - Matrix3d Bf = B.cwiseAbs(); - Bf.array() += reps; - - // if any of these tests are one-sided, then the polyhedra are disjoint - - // A1 x A2 = A0 - t = ((T[0] < 0.0) ? -T[0] : T[0]); - - if(t > (a[0] + Bf.row(0).dot(b))) - return true; - - // B1 x B2 = B0 - s = B.col(0).dot(T); - t = ((s < 0.0) ? -s : s); - - if(t > (b[0] + Bf.col(0).dot(a))) - return true; - - // A2 x A0 = A1 - t = ((T[1] < 0.0) ? -T[1] : T[1]); - - if(t > (a[1] + Bf.row(1).dot(b))) - return true; - - // A0 x A1 = A2 - t =((T[2] < 0.0) ? -T[2] : T[2]); - - if(t > (a[2] + Bf.row(2).dot(b))) - return true; - - // B2 x B0 = B1 - s = B.col(1).dot(T); - t = ((s < 0.0) ? -s : s); - - if(t > (b[1] + Bf.col(1).dot(a))) - return true; - - // B0 x B1 = B2 - s = B.col(2).dot(T); - t = ((s < 0.0) ? -s : s); - - if(t > (b[2] + Bf.col(2).dot(a))) - return true; - - // A0 x B0 - s = T[2] * B(1, 0) - T[1] * B(2, 0); - t = ((s < 0.0) ? -s : s); - - if(t > (a[1] * Bf(2, 0) + a[2] * Bf(1, 0) + - b[1] * Bf(0, 2) + b[2] * Bf(0, 1))) - return true; - - // A0 x B1 - s = T[2] * B(1, 1) - T[1] * B(2, 1); - t = ((s < 0.0) ? -s : s); - - if(t > (a[1] * Bf(2, 1) + a[2] * Bf(1, 1) + - b[0] * Bf(0, 2) + b[2] * Bf(0, 0))) - return true; - - // A0 x B2 - s = T[2] * B(1, 2) - T[1] * B(2, 2); - t = ((s < 0.0) ? -s : s); - - if(t > (a[1] * Bf(2, 2) + a[2] * Bf(1, 2) + - b[0] * Bf(0, 1) + b[1] * Bf(0, 0))) - return true; - - // A1 x B0 - s = T[0] * B(2, 0) - T[2] * B(0, 0); - t = ((s < 0.0) ? -s : s); - - if(t > (a[0] * Bf(2, 0) + a[2] * Bf(0, 0) + - b[1] * Bf(1, 2) + b[2] * Bf(1, 1))) - return true; - - // A1 x B1 - s = T[0] * B(2, 1) - T[2] * B(0, 1); - t = ((s < 0.0) ? -s : s); - - if(t > (a[0] * Bf(2, 1) + a[2] * Bf(0, 1) + - b[0] * Bf(1, 2) + b[2] * Bf(1, 0))) - return true; - - // A1 x B2 - s = T[0] * B(2, 2) - T[2] * B(0, 2); - t = ((s < 0.0) ? -s : s); - - if(t > (a[0] * Bf(2, 2) + a[2] * Bf(0, 2) + - b[0] * Bf(1, 1) + b[1] * Bf(1, 0))) - return true; - - // A2 x B0 - s = T[1] * B(0, 0) - T[0] * B(1, 0); - t = ((s < 0.0) ? -s : s); - - if(t > (a[0] * Bf(1, 0) + a[1] * Bf(0, 0) + - b[1] * Bf(2, 2) + b[2] * Bf(2, 1))) - return true; - - // A2 x B1 - s = T[1] * B(0, 1) - T[0] * B(1, 1); - t = ((s < 0.0) ? -s : s); - - if(t > (a[0] * Bf(1, 1) + a[1] * Bf(0, 1) + - b[0] * Bf(2, 2) + b[2] * Bf(2, 0))) - return true; - - // A2 x B2 - s = T[1] * B(0, 2) - T[0] * B(1, 2); - t = ((s < 0.0) ? -s : s); - - if(t > (a[0] * Bf(1, 2) + a[1] * Bf(0, 2) + - b[0] * Bf(2, 1) + b[1] * Bf(2, 0))) - return true; - - return false; - -} - - - -bool OBB::overlap(const OBB& other) const -{ - /// compute what transform [R,T] that takes us from cs1 to cs2. - /// [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)] - /// First compute the rotation part, then translation part - Vector3d t = other.To - To; // T2 - T1 - Vector3d T = t.transpose()*axis; // R1'(T2-T1) - Matrix3d R = axis.transpose() * other.axis; - - return !obbDisjoint(R, T, extent, other.extent); -} - - -bool OBB::contain(const Vector3d& p) const -{ - Vector3d local_p = p - To; - FCL_REAL proj = local_p.dot(axis.col(0)); - if((proj > extent[0]) || (proj < -extent[0])) - return false; - - proj = local_p.dot(axis.col(1)); - if((proj > extent[1]) || (proj < -extent[1])) - return false; - - proj = local_p.dot(axis.col(2)); - if((proj > extent[2]) || (proj < -extent[2])) - return false; - - return true; -} - -OBB& OBB::operator += (const Vector3d& p) -{ - OBB bvp; - bvp.To = p; - bvp.axis = axis; - bvp.extent.setZero(); - - *this += bvp; - return *this; -} - -OBB OBB::operator + (const OBB& other) const -{ - Vector3d center_diff = To - other.To; - FCL_REAL max_extent = std::max(std::max(extent[0], extent[1]), extent[2]); - FCL_REAL max_extent2 = std::max(std::max(other.extent[0], other.extent[1]), other.extent[2]); - if(center_diff.norm() > 2 * (max_extent + max_extent2)) - { - return merge_largedist(*this, other); - } - else - { - return merge_smalldist(*this, other); - } -} - - -FCL_REAL OBB::distance(const OBB& other, Vector3d* P, Vector3d* Q) const -{ - std::cerr << "OBB distance not implemented!" << std::endl; - return 0.0; -} - - -bool overlap(const Matrix3d& R0, const Vector3d& T0, const OBB& b1, const OBB& b2) -{ - Matrix3d R0b2 = R0 * b2.axis; - Matrix3d R = b1.axis.transpose() * R0b2; - - Vector3d Ttemp = R0 * b2.To + T0 - b1.To; - Vector3d T = Ttemp.transpose() * b1.axis; - - return !obbDisjoint(R, T, b1.extent, b2.extent); -} - -OBB translate(const OBB& bv, const Vector3d& t) -{ - OBB res(bv); - res.To += t; - return res; -} - -} diff --git a/src/BV/RSS.cpp b/src/BV/RSS.cpp deleted file mode 100644 index dcc680037..000000000 --- a/src/BV/RSS.cpp +++ /dev/null @@ -1,1137 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#include "fcl/BV/RSS.h" -#include "fcl/BVH/BVH_utility.h" -#include -namespace fcl -{ - -/// @brief Clip value between a and b -void clipToRange(FCL_REAL& val, FCL_REAL a, FCL_REAL b) -{ - if(val < a) val = a; - else if(val > b) val = b; -} - -/// @brief Finds the parameters t & u corresponding to the two closest points on a pair of line segments. -/// The first segment is defined as Pa + A*t, 0 <= t <= a, where "Pa" is one endpoint of the segment, "A" is a unit vector -/// pointing to the other endpoint, and t is a scalar that produces all the points between the two endpoints. Since "A" is a unit -/// vector, "a" is the segment's length. -/// The second segment is defined as Pb + B*u, 0 <= u <= b. -/// Many of the terms needed by the algorithm are already computed for other purposes,so we just pass these terms into the function -/// instead of complete specifications of each segment. "T" in the dot products is the vector betweeen Pa and Pb. -/// Reference: "On fast computation of distance between line segments." Vladimir J. Lumelsky, in Information Processing Letters, no. 21, pages 55-61, 1985. -void segCoords(FCL_REAL& t, FCL_REAL& u, FCL_REAL a, FCL_REAL b, FCL_REAL A_dot_B, FCL_REAL A_dot_T, FCL_REAL B_dot_T) -{ - FCL_REAL denom = 1 - A_dot_B * A_dot_B; - - if(denom == 0) t = 0; - else - { - t = (A_dot_T - B_dot_T * A_dot_B) / denom; - clipToRange(t, 0, a); - } - - u = t * A_dot_B - B_dot_T; - if(u < 0) - { - u = 0; - t = A_dot_T; - clipToRange(t, 0, a); - } - else if(u > b) - { - u = b; - t = u * A_dot_B + A_dot_T; - clipToRange(t, 0, a); - } -} - -/// @brief Returns whether the nearest point on rectangle edge -/// Pb + B*u, 0 <= u <= b, to the rectangle edge, -/// Pa + A*t, 0 <= t <= a, is within the half space -/// determined by the point Pa and the direction Anorm. -/// A,B, and Anorm are unit vectors. T is the vector between Pa and Pb. -bool inVoronoi(FCL_REAL a, FCL_REAL b, FCL_REAL Anorm_dot_B, FCL_REAL Anorm_dot_T, FCL_REAL A_dot_B, FCL_REAL A_dot_T, FCL_REAL B_dot_T) -{ - if(fabs(Anorm_dot_B) < 1e-7) return false; - - FCL_REAL t, u, v; - - u = -Anorm_dot_T / Anorm_dot_B; - clipToRange(u, 0, b); - - t = u * A_dot_B + A_dot_T; - clipToRange(t, 0, a); - - v = t * A_dot_B - B_dot_T; - - if(Anorm_dot_B > 0) - { - if(v > (u + 1e-7)) return true; - } - else - { - if(v < (u - 1e-7)) return true; - } - return false; -} - - -/// @brief Distance between two oriented rectangles; P and Q (optional return values) are the closest points in the rectangles, both are in the local frame of the first rectangle. -FCL_REAL rectDistance(const Matrix3d& Rab, Vector3d const& Tab, const FCL_REAL a[2], const FCL_REAL b[2], Vector3d* P = NULL, Vector3d* Q = NULL) -{ - FCL_REAL A0_dot_B0, A0_dot_B1, A1_dot_B0, A1_dot_B1; - - A0_dot_B0 = Rab(0, 0); - A0_dot_B1 = Rab(0, 1); - A1_dot_B0 = Rab(1, 0); - A1_dot_B1 = Rab(1, 1); - - FCL_REAL aA0_dot_B0, aA0_dot_B1, aA1_dot_B0, aA1_dot_B1; - FCL_REAL bA0_dot_B0, bA0_dot_B1, bA1_dot_B0, bA1_dot_B1; - - aA0_dot_B0 = a[0] * A0_dot_B0; - aA0_dot_B1 = a[0] * A0_dot_B1; - aA1_dot_B0 = a[1] * A1_dot_B0; - aA1_dot_B1 = a[1] * A1_dot_B1; - bA0_dot_B0 = b[0] * A0_dot_B0; - bA1_dot_B0 = b[0] * A1_dot_B0; - bA0_dot_B1 = b[1] * A0_dot_B1; - bA1_dot_B1 = b[1] * A1_dot_B1; - - Vector3d Tba = Rab.transpose() * Tab; - - Vector3d S; - FCL_REAL t, u; - - // determine if any edge pair contains the closest points - - FCL_REAL ALL_x, ALU_x, AUL_x, AUU_x; - FCL_REAL BLL_x, BLU_x, BUL_x, BUU_x; - FCL_REAL LA1_lx, LA1_ux, UA1_lx, UA1_ux, LB1_lx, LB1_ux, UB1_lx, UB1_ux; - - ALL_x = -Tba[0]; - ALU_x = ALL_x + aA1_dot_B0; - AUL_x = ALL_x + aA0_dot_B0; - AUU_x = ALU_x + aA0_dot_B0; - - if(ALL_x < ALU_x) - { - LA1_lx = ALL_x; - LA1_ux = ALU_x; - UA1_lx = AUL_x; - UA1_ux = AUU_x; - } - else - { - LA1_lx = ALU_x; - LA1_ux = ALL_x; - UA1_lx = AUU_x; - UA1_ux = AUL_x; - } - - BLL_x = Tab[0]; - BLU_x = BLL_x + bA0_dot_B1; - BUL_x = BLL_x + bA0_dot_B0; - BUU_x = BLU_x + bA0_dot_B0; - - if(BLL_x < BLU_x) - { - LB1_lx = BLL_x; - LB1_ux = BLU_x; - UB1_lx = BUL_x; - UB1_ux = BUU_x; - } - else - { - LB1_lx = BLU_x; - LB1_ux = BLL_x; - UB1_lx = BUU_x; - UB1_ux = BUL_x; - } - - // UA1, UB1 - - if((UA1_ux > b[0]) && (UB1_ux > a[0])) - { - if(((UA1_lx > b[0]) || - inVoronoi(b[1], a[1], A1_dot_B0, aA0_dot_B0 - b[0] - Tba[0], - A1_dot_B1, aA0_dot_B1 - Tba[1], - -Tab[1] - bA1_dot_B0)) - && - ((UB1_lx > a[0]) || - inVoronoi(a[1], b[1], A0_dot_B1, Tab[0] + bA0_dot_B0 - a[0], - A1_dot_B1, Tab[1] + bA1_dot_B0, Tba[1] - aA0_dot_B1))) - { - segCoords(t, u, a[1], b[1], A1_dot_B1, Tab[1] + bA1_dot_B0, - Tba[1] - aA0_dot_B1); - - S[0] = Tab[0] + Rab(0, 0) * b[0] + Rab(0, 1) * u - a[0] ; - S[1] = Tab[1] + Rab(1, 0) * b[0] + Rab(1, 1) * u - t; - S[2] = Tab[2] + Rab(2, 0) * b[0] + Rab(2, 1) * u; - - if(P && Q) - { - *P << a[0], t, 0; - *Q = S + (*P); - } - - return S.norm(); - } - } - - - // UA1, LB1 - - if((UA1_lx < 0) && (LB1_ux > a[0])) - { - if(((UA1_ux < 0) || - inVoronoi(b[1], a[1], -A1_dot_B0, Tba[0] - aA0_dot_B0, - A1_dot_B1, aA0_dot_B1 - Tba[1], -Tab[1])) - && - ((LB1_lx > a[0]) || - inVoronoi(a[1], b[1], A0_dot_B1, Tab[0] - a[0], - A1_dot_B1, Tab[1], Tba[1] - aA0_dot_B1))) - { - segCoords(t, u, a[1], b[1], A1_dot_B1, Tab[1], Tba[1] - aA0_dot_B1); - - S[0] = Tab[0] + Rab(0, 1) * u - a[0]; - S[1] = Tab[1] + Rab(1, 1) * u - t; - S[2] = Tab[2] + Rab(2, 1) * u; - - if(P && Q) - { - *P << a[0], t, 0; - *Q = S + (*P); - } - - return S.norm(); - } - } - - // LA1, UB1 - - if((LA1_ux > b[0]) && (UB1_lx < 0)) - { - if(((LA1_lx > b[0]) || - inVoronoi(b[1], a[1], A1_dot_B0, -Tba[0] - b[0], - A1_dot_B1, -Tba[1], -Tab[1] - bA1_dot_B0)) - && - ((UB1_ux < 0) || - inVoronoi(a[1], b[1], -A0_dot_B1, -Tab[0] - bA0_dot_B0, - A1_dot_B1, Tab[1] + bA1_dot_B0, Tba[1]))) - { - segCoords(t, u, a[1], b[1], A1_dot_B1, Tab[1] + bA1_dot_B0, Tba[1]); - - S[0] = Tab[0] + Rab(0, 0) * b[0] + Rab(0, 1) * u; - S[1] = Tab[1] + Rab(1, 0) * b[0] + Rab(1, 1) * u - t; - S[2] = Tab[2] + Rab(2, 0) * b[0] + Rab(2, 1) * u; - - if(P && Q) - { - *P << 0, t, 0; - *Q = S + (*P); - } - - return S.norm(); - } - } - - // LA1, LB1 - - if((LA1_lx < 0) && (LB1_lx < 0)) - { - if (((LA1_ux < 0) || - inVoronoi(b[1], a[1], -A1_dot_B0, Tba[0], A1_dot_B1, - -Tba[1], -Tab[1])) - && - ((LB1_ux < 0) || - inVoronoi(a[1], b[1], -A0_dot_B1, -Tab[0], A1_dot_B1, - Tab[1], Tba[1]))) - { - segCoords(t, u, a[1], b[1], A1_dot_B1, Tab[1], Tba[1]); - - S[0] = Tab[0] + Rab(0, 1) * u; - S[1] = Tab[1] + Rab(1, 1) * u - t; - S[2] = Tab[2] + Rab(2, 1) * u; - - if(P && Q) - { - *P << 0, t, 0; - *Q = S + (*P); - } - - return S.norm(); - } - } - - FCL_REAL ALL_y, ALU_y, AUL_y, AUU_y; - - ALL_y = -Tba[1]; - ALU_y = ALL_y + aA1_dot_B1; - AUL_y = ALL_y + aA0_dot_B1; - AUU_y = ALU_y + aA0_dot_B1; - - FCL_REAL LA1_ly, LA1_uy, UA1_ly, UA1_uy, LB0_lx, LB0_ux, UB0_lx, UB0_ux; - - if(ALL_y < ALU_y) - { - LA1_ly = ALL_y; - LA1_uy = ALU_y; - UA1_ly = AUL_y; - UA1_uy = AUU_y; - } - else - { - LA1_ly = ALU_y; - LA1_uy = ALL_y; - UA1_ly = AUU_y; - UA1_uy = AUL_y; - } - - if(BLL_x < BUL_x) - { - LB0_lx = BLL_x; - LB0_ux = BUL_x; - UB0_lx = BLU_x; - UB0_ux = BUU_x; - } - else - { - LB0_lx = BUL_x; - LB0_ux = BLL_x; - UB0_lx = BUU_x; - UB0_ux = BLU_x; - } - - // UA1, UB0 - - if((UA1_uy > b[1]) && (UB0_ux > a[0])) - { - if(((UA1_ly > b[1]) || - inVoronoi(b[0], a[1], A1_dot_B1, aA0_dot_B1 - Tba[1] - b[1], - A1_dot_B0, aA0_dot_B0 - Tba[0], -Tab[1] - bA1_dot_B1)) - && - ((UB0_lx > a[0]) || - inVoronoi(a[1], b[0], A0_dot_B0, Tab[0] - a[0] + bA0_dot_B1, - A1_dot_B0, Tab[1] + bA1_dot_B1, Tba[0] - aA0_dot_B0))) - { - segCoords(t, u, a[1], b[0], A1_dot_B0, Tab[1] + bA1_dot_B1, - Tba[0] - aA0_dot_B0); - - S[0] = Tab[0] + Rab(0, 1) * b[1] + Rab(0, 0) * u - a[0] ; - S[1] = Tab[1] + Rab(1, 1) * b[1] + Rab(1, 0) * u - t; - S[2] = Tab[2] + Rab(2, 1) * b[1] + Rab(2, 0) * u; - - if(P && Q) - { - *P << a[0], t, 0; - *Q = S + (*P); - } - - return S.norm(); - } - } - - // UA1, LB0 - - if((UA1_ly < 0) && (LB0_ux > a[0])) - { - if(((UA1_uy < 0) || - inVoronoi(b[0], a[1], -A1_dot_B1, Tba[1] - aA0_dot_B1, A1_dot_B0, - aA0_dot_B0 - Tba[0], -Tab[1])) - && - ((LB0_lx > a[0]) || - inVoronoi(a[1], b[0], A0_dot_B0, Tab[0] - a[0], - A1_dot_B0, Tab[1], Tba[0] - aA0_dot_B0))) - { - segCoords(t, u, a[1], b[0], A1_dot_B0, Tab[1], Tba[0] - aA0_dot_B0); - - S[0] = Tab[0] + Rab(0, 0) * u - a[0]; - S[1] = Tab[1] + Rab(1, 0) * u - t; - S[2] = Tab[2] + Rab(2, 0) * u; - - if(P && Q) - { - *P << a[0], t, 0; - *Q = S + (*P); - } - - return S.norm(); - } - } - - // LA1, UB0 - - if((LA1_uy > b[1]) && (UB0_lx < 0)) - { - if(((LA1_ly > b[1]) || - inVoronoi(b[0], a[1], A1_dot_B1, -Tba[1] - b[1], - A1_dot_B0, -Tba[0], -Tab[1] - bA1_dot_B1)) - && - - ((UB0_ux < 0) || - inVoronoi(a[1], b[0], -A0_dot_B0, -Tab[0] - bA0_dot_B1, A1_dot_B0, - Tab[1] + bA1_dot_B1, Tba[0]))) - { - segCoords(t, u, a[1], b[0], A1_dot_B0, Tab[1] + bA1_dot_B1, Tba[0]); - - S[0] = Tab[0] + Rab(0, 1) * b[1] + Rab(0, 0) * u; - S[1] = Tab[1] + Rab(1, 1) * b[1] + Rab(1, 0) * u - t; - S[2] = Tab[2] + Rab(2, 1) * b[1] + Rab(2, 0) * u; - - if(P && Q) - { - *P << 0, t, 0; - *Q = S + (*P); - } - - - return S.norm(); - } - } - - // LA1, LB0 - - if((LA1_ly < 0) && (LB0_lx < 0)) - { - if(((LA1_uy < 0) || - inVoronoi(b[0], a[1], -A1_dot_B1, Tba[1], A1_dot_B0, - -Tba[0], -Tab[1])) - && - ((LB0_ux < 0) || - inVoronoi(a[1], b[0], -A0_dot_B0, -Tab[0], A1_dot_B0, - Tab[1], Tba[0]))) - { - segCoords(t, u, a[1], b[0], A1_dot_B0, Tab[1], Tba[0]); - - S[0] = Tab[0] + Rab(0, 0) * u; - S[1] = Tab[1] + Rab(1, 0) * u - t; - S[2] = Tab[2] + Rab(2, 0) * u; - - if(P&& Q) - { - *P << 0, t, 0; - *Q = S + (*P); - } - - return S.norm(); - } - } - - FCL_REAL BLL_y, BLU_y, BUL_y, BUU_y; - - BLL_y = Tab[1]; - BLU_y = BLL_y + bA1_dot_B1; - BUL_y = BLL_y + bA1_dot_B0; - BUU_y = BLU_y + bA1_dot_B0; - - FCL_REAL LA0_lx, LA0_ux, UA0_lx, UA0_ux, LB1_ly, LB1_uy, UB1_ly, UB1_uy; - - if(ALL_x < AUL_x) - { - LA0_lx = ALL_x; - LA0_ux = AUL_x; - UA0_lx = ALU_x; - UA0_ux = AUU_x; - } - else - { - LA0_lx = AUL_x; - LA0_ux = ALL_x; - UA0_lx = AUU_x; - UA0_ux = ALU_x; - } - - if(BLL_y < BLU_y) - { - LB1_ly = BLL_y; - LB1_uy = BLU_y; - UB1_ly = BUL_y; - UB1_uy = BUU_y; - } - else - { - LB1_ly = BLU_y; - LB1_uy = BLL_y; - UB1_ly = BUU_y; - UB1_uy = BUL_y; - } - - // UA0, UB1 - - if((UA0_ux > b[0]) && (UB1_uy > a[1])) - { - if(((UA0_lx > b[0]) || - inVoronoi(b[1], a[0], A0_dot_B0, aA1_dot_B0 - Tba[0] - b[0], - A0_dot_B1, aA1_dot_B1 - Tba[1], -Tab[0] - bA0_dot_B0)) - && - ((UB1_ly > a[1]) || - inVoronoi(a[0], b[1], A1_dot_B1, Tab[1] - a[1] + bA1_dot_B0, - A0_dot_B1, Tab[0] + bA0_dot_B0, Tba[1] - aA1_dot_B1))) - { - segCoords(t, u, a[0], b[1], A0_dot_B1, Tab[0] + bA0_dot_B0, - Tba[1] - aA1_dot_B1); - - S[0] = Tab[0] + Rab(0, 0) * b[0] + Rab(0, 1) * u - t; - S[1] = Tab[1] + Rab(1, 0) * b[0] + Rab(1, 1) * u - a[1]; - S[2] = Tab[2] + Rab(2, 0) * b[0] + Rab(2, 1) * u; - - if(P && Q) - { - *P << t, a[1], 0; - *Q = S + (*P); - } - - return S.norm(); - } - } - - // UA0, LB1 - - if((UA0_lx < 0) && (LB1_uy > a[1])) - { - if(((UA0_ux < 0) || - inVoronoi(b[1], a[0], -A0_dot_B0, Tba[0] - aA1_dot_B0, A0_dot_B1, - aA1_dot_B1 - Tba[1], -Tab[0])) - && - ((LB1_ly > a[1]) || - inVoronoi(a[0], b[1], A1_dot_B1, Tab[1] - a[1], A0_dot_B1, Tab[0], - Tba[1] - aA1_dot_B1))) - { - segCoords(t, u, a[0], b[1], A0_dot_B1, Tab[0], Tba[1] - aA1_dot_B1); - - S[0] = Tab[0] + Rab(0, 1) * u - t; - S[1] = Tab[1] + Rab(1, 1) * u - a[1]; - S[2] = Tab[2] + Rab(2, 1) * u; - - if(P && Q) - { - *P << t, a[1], 0; - *Q = S + (*P); - } - - return S.norm(); - } - } - - // LA0, UB1 - - if((LA0_ux > b[0]) && (UB1_ly < 0)) - { - if(((LA0_lx > b[0]) || - inVoronoi(b[1], a[0], A0_dot_B0, -b[0] - Tba[0], A0_dot_B1, -Tba[1], - -bA0_dot_B0 - Tab[0])) - && - ((UB1_uy < 0) || - inVoronoi(a[0], b[1], -A1_dot_B1, -Tab[1] - bA1_dot_B0, A0_dot_B1, - Tab[0] + bA0_dot_B0, Tba[1]))) - { - segCoords(t, u, a[0], b[1], A0_dot_B1, Tab[0] + bA0_dot_B0, Tba[1]); - - S[0] = Tab[0] + Rab(0, 0) * b[0] + Rab(0, 1) * u - t; - S[1] = Tab[1] + Rab(1, 0) * b[0] + Rab(1, 1) * u; - S[2] = Tab[2] + Rab(2, 0) * b[0] + Rab(2, 1) * u; - - if(P && Q) - { - *P << t, 0, 0; - *Q = S + (*P); - } - - return S.norm(); - } - } - - // LA0, LB1 - - if((LA0_lx < 0) && (LB1_ly < 0)) - { - if(((LA0_ux < 0) || - inVoronoi(b[1], a[0], -A0_dot_B0, Tba[0], A0_dot_B1, -Tba[1], - -Tab[0])) - && - ((LB1_uy < 0) || - inVoronoi(a[0], b[1], -A1_dot_B1, -Tab[1], A0_dot_B1, - Tab[0], Tba[1]))) - { - segCoords(t, u, a[0], b[1], A0_dot_B1, Tab[0], Tba[1]); - - S[0] = Tab[0] + Rab(0, 1) * u - t; - S[1] = Tab[1] + Rab(1, 1) * u; - S[2] = Tab[2] + Rab(2, 1) * u; - - if(P && Q) - { - *P << t, 0, 0; - *Q = S + (*P); - } - - return S.norm(); - } - } - - FCL_REAL LA0_ly, LA0_uy, UA0_ly, UA0_uy, LB0_ly, LB0_uy, UB0_ly, UB0_uy; - - if(ALL_y < AUL_y) - { - LA0_ly = ALL_y; - LA0_uy = AUL_y; - UA0_ly = ALU_y; - UA0_uy = AUU_y; - } - else - { - LA0_ly = AUL_y; - LA0_uy = ALL_y; - UA0_ly = AUU_y; - UA0_uy = ALU_y; - } - - if(BLL_y < BUL_y) - { - LB0_ly = BLL_y; - LB0_uy = BUL_y; - UB0_ly = BLU_y; - UB0_uy = BUU_y; - } - else - { - LB0_ly = BUL_y; - LB0_uy = BLL_y; - UB0_ly = BUU_y; - UB0_uy = BLU_y; - } - - // UA0, UB0 - - if((UA0_uy > b[1]) && (UB0_uy > a[1])) - { - if(((UA0_ly > b[1]) || - inVoronoi(b[0], a[0], A0_dot_B1, aA1_dot_B1 - Tba[1] - b[1], - A0_dot_B0, aA1_dot_B0 - Tba[0], -Tab[0] - bA0_dot_B1)) - && - ((UB0_ly > a[1]) || - inVoronoi(a[0], b[0], A1_dot_B0, Tab[1] - a[1] + bA1_dot_B1, A0_dot_B0, - Tab[0] + bA0_dot_B1, Tba[0] - aA1_dot_B0))) - { - segCoords(t, u, a[0], b[0], A0_dot_B0, Tab[0] + bA0_dot_B1, - Tba[0] - aA1_dot_B0); - - S[0] = Tab[0] + Rab(0, 1) * b[1] + Rab(0, 0) * u - t; - S[1] = Tab[1] + Rab(1, 1) * b[1] + Rab(1, 0) * u - a[1]; - S[2] = Tab[2] + Rab(2, 1) * b[1] + Rab(2, 0) * u; - - if(P && Q) - { - *P << t, a[1], 0; - *Q = S + (*P); - } - - return S.norm(); - } - } - - // UA0, LB0 - - if((UA0_ly < 0) && (LB0_uy > a[1])) - { - if(((UA0_uy < 0) || - inVoronoi(b[0], a[0], -A0_dot_B1, Tba[1] - aA1_dot_B1, A0_dot_B0, - aA1_dot_B0 - Tba[0], -Tab[0])) - && - ((LB0_ly > a[1]) || - inVoronoi(a[0], b[0], A1_dot_B0, Tab[1] - a[1], - A0_dot_B0, Tab[0], Tba[0] - aA1_dot_B0))) - { - segCoords(t, u, a[0], b[0], A0_dot_B0, Tab[0], Tba[0] - aA1_dot_B0); - - S[0] = Tab[0] + Rab(0, 0) * u - t; - S[1] = Tab[1] + Rab(1, 0) * u - a[1]; - S[2] = Tab[2] + Rab(2, 0) * u; - - if(P && Q) - { - *P << t, a[1], 0; - *Q = S + (*P); - } - - return S.norm(); - } - } - - // LA0, UB0 - - if((LA0_uy > b[1]) && (UB0_ly < 0)) - { - if(((LA0_ly > b[1]) || - inVoronoi(b[0], a[0], A0_dot_B1, -Tba[1] - b[1], A0_dot_B0, -Tba[0], - -Tab[0] - bA0_dot_B1)) - && - - ((UB0_uy < 0) || - inVoronoi(a[0], b[0], -A1_dot_B0, -Tab[1] - bA1_dot_B1, A0_dot_B0, - Tab[0] + bA0_dot_B1, Tba[0]))) - { - segCoords(t, u, a[0], b[0], A0_dot_B0, Tab[0] + bA0_dot_B1, Tba[0]); - - S[0] = Tab[0] + Rab(0, 1) * b[1] + Rab(0, 0) * u - t; - S[1] = Tab[1] + Rab(1, 1) * b[1] + Rab(1, 0) * u; - S[2] = Tab[2] + Rab(2, 1) * b[1] + Rab(2, 0) * u; - - if(P && Q) - { - *P << t, 0, 0; - *Q = S + (*P); - } - - return S.norm(); - } - } - - // LA0, LB0 - - if((LA0_ly < 0) && (LB0_ly < 0)) - { - if(((LA0_uy < 0) || - inVoronoi(b[0], a[0], -A0_dot_B1, Tba[1], A0_dot_B0, - -Tba[0], -Tab[0])) - && - ((LB0_uy < 0) || - inVoronoi(a[0], b[0], -A1_dot_B0, -Tab[1], A0_dot_B0, - Tab[0], Tba[0]))) - { - segCoords(t, u, a[0], b[0], A0_dot_B0, Tab[0], Tba[0]); - - S[0] = Tab[0] + Rab(0, 0) * u - t; - S[1] = Tab[1] + Rab(1, 0) * u; - S[2] = Tab[2] + Rab(2, 0) * u; - - if(P && Q) - { - *P << t, 0, 0; - *Q = S + (*P); - } - - return S.norm(); - } - } - - // no edges passed, take max separation along face normals - - FCL_REAL sep1, sep2; - - if(Tab[2] > 0.0) - { - sep1 = Tab[2]; - if (Rab(2, 0) < 0.0) sep1 += b[0] * Rab(2, 0); - if (Rab(2, 1) < 0.0) sep1 += b[1] * Rab(2, 1); - } - else - { - sep1 = -Tab[2]; - if (Rab(2, 0) > 0.0) sep1 -= b[0] * Rab(2, 0); - if (Rab(2, 1) > 0.0) sep1 -= b[1] * Rab(2, 1); - } - - if(Tba[2] < 0) - { - sep2 = -Tba[2]; - if (Rab(0, 2) < 0.0) sep2 += a[0] * Rab(0, 2); - if (Rab(1, 2) < 0.0) sep2 += a[1] * Rab(1, 2); - } - else - { - sep2 = Tba[2]; - if (Rab(0, 2) > 0.0) sep2 -= a[0] * Rab(0, 2); - if (Rab(1, 2) > 0.0) sep2 -= a[1] * Rab(1, 2); - } - - if(sep1 >= sep2 && sep1 >= 0) - { - if(Tab[2] > 0) - S << 0, 0, sep1; - else - S << 0, 0, -sep1; - - if(P && Q) - { - *Q = S; - P->setZero(); - } - } - - if(sep2 >= sep1 && sep2 >= 0) - { - Vector3d Q_(Tab[0], Tab[1], Tab[2]); - Vector3d P_; - if(Tba[2] < 0) - { - P_[0] = Rab(0, 2) * sep2 + Tab[0]; - P_[1] = Rab(1, 2) * sep2 + Tab[1]; - P_[2] = Rab(2, 2) * sep2 + Tab[2]; - } - else - { - P_[0] = -Rab(0, 2) * sep2 + Tab[0]; - P_[1] = -Rab(1, 2) * sep2 + Tab[1]; - P_[2] = -Rab(2, 2) * sep2 + Tab[2]; - } - - S = Q_ - P_; - - if(P && Q) - { - *P = P_; - *Q = Q_; - } - } - - FCL_REAL sep = (sep1 > sep2 ? sep1 : sep2); - return (sep > 0 ? sep : 0); -} - - - -RSS::RSS() - : axis(Matrix3d::Identity()), Tr(Vector3d::Zero()) -{ - // Do nothing -} - -bool RSS::overlap(const RSS& other) const -{ - /// compute what transform [R,T] that takes us from cs1 to cs2. - /// [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)] - /// First compute the rotation part, then translation part - - /// First compute T2 - T1 - Vector3d t = other.Tr - Tr; - - /// Then compute R1'(T2 - T1) - Vector3d T = t.transpose() * axis; - - /// Now compute R1'R2 - Matrix3d R = axis.transpose() * other.axis; - - FCL_REAL dist = rectDistance(R, T, l, other.l); - return (dist <= (r + other.r)); -} - -bool overlap(const Matrix3d& R0, const Vector3d& T0, const RSS& b1, const RSS& b2) -{ - Matrix3d R0b2 = R0 * b2.axis; - Matrix3d R = b1.axis.transpose() * R0b2; - - Vector3d Ttemp = R0 * b2.Tr + T0 - b1.Tr; - Vector3d T = Ttemp.transpose() * b1.axis; - - FCL_REAL dist = rectDistance(R, T, b1.l, b2.l); - return (dist <= (b1.r + b2.r)); -} - -bool RSS::contain(const Vector3d& p) const -{ - Vector3d local_p = p - Tr; - Vector3d proj = local_p.transpose() * axis; - FCL_REAL abs_proj2 = fabs(proj[2]); - - /// projection is within the rectangle - if((proj[0] < l[0]) && (proj[0] > 0) && (proj[1] < l[1]) && (proj[1] > 0)) - { - return (abs_proj2 < r); - } - else if((proj[0] < l[0]) && (proj[0] > 0) && ((proj[1] < 0) || (proj[1] > l[1]))) - { - FCL_REAL y = (proj[1] > 0) ? l[1] : 0; - Vector3d v(proj[0], y, 0); - return ((proj - v).squaredNorm() < r * r); - } - else if((proj[1] < l[1]) && (proj[1] > 0) && ((proj[0] < 0) || (proj[0] > l[0]))) - { - FCL_REAL x = (proj[0] > 0) ? l[0] : 0; - Vector3d v(x, proj[1], 0); - return ((proj - v).squaredNorm() < r * r); - } - else - { - FCL_REAL x = (proj[0] > 0) ? l[0] : 0; - FCL_REAL y = (proj[1] > 0) ? l[1] : 0; - Vector3d v(x, y, 0); - return ((proj - v).squaredNorm() < r * r); - } -} - -RSS& RSS::operator += (const Vector3d& p) -{ - Vector3d local_p = p - Tr; - Vector3d proj = local_p.transpose() * axis; - FCL_REAL abs_proj2 = fabs(proj[2]); - - // projection is within the rectangle - if((proj[0] < l[0]) && (proj[0] > 0) && (proj[1] < l[1]) && (proj[1] > 0)) - { - if(abs_proj2 < r) - ; // do nothing - else - { - r = 0.5 * (r + abs_proj2); // enlarge the r - // change RSS origin position - if(proj[2] > 0) - Tr[2] += 0.5 * (abs_proj2 - r); - else - Tr[2] -= 0.5 * (abs_proj2 - r); - } - } - else if((proj[0] < l[0]) && (proj[0] > 0) && ((proj[1] < 0) || (proj[1] > l[1]))) - { - FCL_REAL y = (proj[1] > 0) ? l[1] : 0; - Vector3d v(proj[0], y, 0); - FCL_REAL new_r_sqr = (proj - v).squaredNorm(); - if(new_r_sqr < r * r) - ; // do nothing - else - { - if(abs_proj2 < r) - { - FCL_REAL delta_y = - std::sqrt(r * r - proj[2] * proj[2]) + fabs(proj[1] - y); - l[1] += delta_y; - if(proj[1] < 0) - Tr[1] -= delta_y; - } - else - { - FCL_REAL delta_y = fabs(proj[1] - y); - l[1] += delta_y; - if(proj[1] < 0) - Tr[1] -= delta_y; - - if(proj[2] > 0) - Tr[2] += 0.5 * (abs_proj2 - r); - else - Tr[2] -= 0.5 * (abs_proj2 - r); - } - } - } - else if((proj[1] < l[1]) && (proj[1] > 0) && ((proj[0] < 0) || (proj[0] > l[0]))) - { - FCL_REAL x = (proj[0] > 0) ? l[0] : 0; - Vector3d v(x, proj[1], 0); - FCL_REAL new_r_sqr = (proj - v).squaredNorm(); - if(new_r_sqr < r * r) - ; // do nothing - else - { - if(abs_proj2 < r) - { - FCL_REAL delta_x = - std::sqrt(r * r - proj[2] * proj[2]) + fabs(proj[0] - x); - l[0] += delta_x; - if(proj[0] < 0) - Tr[0] -= delta_x; - } - else - { - FCL_REAL delta_x = fabs(proj[0] - x); - l[0] += delta_x; - if(proj[0] < 0) - Tr[0] -= delta_x; - - if(proj[2] > 0) - Tr[2] += 0.5 * (abs_proj2 - r); - else - Tr[2] -= 0.5 * (abs_proj2 - r); - } - } - } - else - { - FCL_REAL x = (proj[0] > 0) ? l[0] : 0; - FCL_REAL y = (proj[1] > 0) ? l[1] : 0; - Vector3d v(x, y, 0); - FCL_REAL new_r_sqr = (proj - v).squaredNorm(); - if(new_r_sqr < r * r) - ; // do nothing - else - { - if(abs_proj2 < r) - { - FCL_REAL diag = std::sqrt(new_r_sqr - proj[2] * proj[2]); - FCL_REAL delta_diag = - std::sqrt(r * r - proj[2] * proj[2]) + diag; - - FCL_REAL delta_x = delta_diag / diag * fabs(proj[0] - x); - FCL_REAL delta_y = delta_diag / diag * fabs(proj[1] - y); - l[0] += delta_x; - l[1] += delta_y; - - if(proj[0] < 0 && proj[1] < 0) - { - Tr[0] -= delta_x; - Tr[1] -= delta_y; - } - } - else - { - FCL_REAL delta_x = fabs(proj[0] - x); - FCL_REAL delta_y = fabs(proj[1] - y); - - l[0] += delta_x; - l[1] += delta_y; - - if(proj[0] < 0 && proj[1] < 0) - { - Tr[0] -= delta_x; - Tr[1] -= delta_y; - } - - if(proj[2] > 0) - Tr[2] += 0.5 * (abs_proj2 - r); - else - Tr[2] -= 0.5 * (abs_proj2 - r); - } - } - } - - return *this; -} - -RSS RSS::operator + (const RSS& other) const -{ - RSS bv; - - Vector3d v[16]; - - Vector3d d0_pos = other.axis.col(0) * (other.l[0] + other.r); - Vector3d d1_pos = other.axis.col(1) * (other.l[1] + other.r); - - Vector3d d0_neg = other.axis.col(0) * (-other.r); - Vector3d d1_neg = other.axis.col(1) * (-other.r); - - Vector3d d2_pos = other.axis.col(2) * other.r; - Vector3d d2_neg = other.axis.col(2) * (-other.r); - - v[0] = other.Tr + d0_pos + d1_pos + d2_pos; - v[1] = other.Tr + d0_pos + d1_pos + d2_neg; - v[2] = other.Tr + d0_pos + d1_neg + d2_pos; - v[3] = other.Tr + d0_pos + d1_neg + d2_neg; - v[4] = other.Tr + d0_neg + d1_pos + d2_pos; - v[5] = other.Tr + d0_neg + d1_pos + d2_neg; - v[6] = other.Tr + d0_neg + d1_neg + d2_pos; - v[7] = other.Tr + d0_neg + d1_neg + d2_neg; - - d0_pos = axis.col(0) * (l[0] + r); - d1_pos = axis.col(1) * (l[1] + r); - d0_neg = axis.col(0) * (-r); - d1_neg = axis.col(1) * (-r); - d2_pos = axis.col(2) * r; - d2_neg = axis.col(2) * (-r); - - v[8] = Tr + d0_pos + d1_pos + d2_pos; - v[9] = Tr + d0_pos + d1_pos + d2_neg; - v[10] = Tr + d0_pos + d1_neg + d2_pos; - v[11] = Tr + d0_pos + d1_neg + d2_neg; - v[12] = Tr + d0_neg + d1_pos + d2_pos; - v[13] = Tr + d0_neg + d1_pos + d2_neg; - v[14] = Tr + d0_neg + d1_neg + d2_pos; - v[15] = Tr + d0_neg + d1_neg + d2_neg; - - - Matrix3d M; // row first matrix - Matrix3d E; // row first eigen-vectors - Vector3d s(0, 0, 0); - - getCovariance(v, NULL, NULL, NULL, 16, M); - eigen(M, s, E); - - int min, mid, max; - if(s[0] > s[1]) { max = 0; min = 1; } - else { min = 0; max = 1; } - if(s[2] < s[min]) { mid = min; min = 2; } - else if(s[2] > s[max]) { mid = max; max = 2; } - else { mid = 2; } - - // column first matrix, as the axis in RSS - bv.axis.col(0) = E.col(max); - bv.axis.col(1) = E.col(mid); - bv.axis.col(2) = axis.col(0).cross(axis.col(1)); - - // set rss origin, rectangle size and radius - getRadiusAndOriginAndRectangleSize(v, NULL, NULL, NULL, 16, bv.axis, bv.Tr, bv.l, bv.r); - - return bv; -} - -FCL_REAL RSS::distance(const RSS& other, Vector3d* P, Vector3d* Q) const -{ - // compute what transform [R,T] that takes us from cs1 to cs2. - // [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)] - // First compute the rotation part, then translation part - Vector3d t = other.Tr - Tr; // T2 - T1 - Vector3d T = t.transpose() * axis; // R1'(T2-T1) - Matrix3d R = axis.transpose() * other.axis; - - FCL_REAL dist = rectDistance(R, T, l, other.l, P, Q); - dist -= (r + other.r); - return (dist < (FCL_REAL)0.0) ? (FCL_REAL)0.0 : dist; -} - -FCL_REAL distance(const Matrix3d& R0, const Vector3d& T0, const RSS& b1, const RSS& b2, Vector3d* P, Vector3d* Q) -{ - Matrix3d R0b2 = R0 * b2.axis; - Matrix3d R = b1.axis.transpose() * R0b2; - - Vector3d Ttemp = R0 * b2.Tr + T0 - b1.Tr; - Vector3d T = Ttemp.transpose() * b1.axis; - - FCL_REAL dist = rectDistance(R, T, b1.l, b2.l, P, Q); - dist -= (b1.r + b2.r); - return (dist < (FCL_REAL)0.0) ? (FCL_REAL)0.0 : dist; -} - -RSS translate(const RSS& bv, const Vector3d& t) -{ - RSS res(bv); - res.Tr += t; - return res; -} - - - - - -} diff --git a/src/BV/kDOP.cpp b/src/BV/kDOP.cpp deleted file mode 100644 index b9e634c6a..000000000 --- a/src/BV/kDOP.cpp +++ /dev/null @@ -1,260 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#include "fcl/BV/kDOP.h" -#include -#include - -namespace fcl -{ - -/// @brief Find the smaller and larger one of two values -inline void minmax(FCL_REAL a, FCL_REAL b, FCL_REAL& minv, FCL_REAL& maxv) -{ - if(a > b) - { - minv = b; - maxv = a; - } - else - { - minv = a; - maxv = b; - } -} -/// @brief Merge the interval [minv, maxv] and value p/ -inline void minmax(FCL_REAL p, FCL_REAL& minv, FCL_REAL& maxv) -{ - if(p > maxv) maxv = p; - if(p < minv) minv = p; -} - - -/// @brief Compute the distances to planes with normals from KDOP vectors except those of AABB face planes -template -void getDistances(const Vector3d& p, FCL_REAL* d) {} - -/// @brief Specification of getDistances -template<> -inline void getDistances<5>(const Vector3d& p, FCL_REAL* d) -{ - d[0] = p[0] + p[1]; - d[1] = p[0] + p[2]; - d[2] = p[1] + p[2]; - d[3] = p[0] - p[1]; - d[4] = p[0] - p[2]; -} - -template<> -inline void getDistances<6>(const Vector3d& p, FCL_REAL* d) -{ - d[0] = p[0] + p[1]; - d[1] = p[0] + p[2]; - d[2] = p[1] + p[2]; - d[3] = p[0] - p[1]; - d[4] = p[0] - p[2]; - d[5] = p[1] - p[2]; -} - -template<> -inline void getDistances<9>(const Vector3d& p, FCL_REAL* d) -{ - d[0] = p[0] + p[1]; - d[1] = p[0] + p[2]; - d[2] = p[1] + p[2]; - d[3] = p[0] - p[1]; - d[4] = p[0] - p[2]; - d[5] = p[1] - p[2]; - d[6] = p[0] + p[1] - p[2]; - d[7] = p[0] + p[2] - p[1]; - d[8] = p[1] + p[2] - p[0]; -} - - - -template -KDOP::KDOP() -{ - FCL_REAL real_max = std::numeric_limits::max(); - for(size_t i = 0; i < N / 2; ++i) - { - dist_[i] = real_max; - dist_[i + N / 2] = -real_max; - } -} - -template -KDOP::KDOP(const Vector3d& v) -{ - for(size_t i = 0; i < 3; ++i) - { - dist_[i] = dist_[N / 2 + i] = v[i]; - } - - FCL_REAL d[(N - 6) / 2]; - getDistances<(N - 6) / 2>(v, d); - for(size_t i = 0; i < (N - 6) / 2; ++i) - { - dist_[3 + i] = dist_[3 + i + N / 2] = d[i]; - } -} - -template -KDOP::KDOP(const Vector3d& a, const Vector3d& b) -{ - for(size_t i = 0; i < 3; ++i) - { - minmax(a[i], b[i], dist_[i], dist_[i + N / 2]); - } - - FCL_REAL ad[(N - 6) / 2], bd[(N - 6) / 2]; - getDistances<(N - 6) / 2>(a, ad); - getDistances<(N - 6) / 2>(b, bd); - for(size_t i = 0; i < (N - 6) / 2; ++i) - { - minmax(ad[i], bd[i], dist_[3 + i], dist_[3 + i + N / 2]); - } -} - -template -bool KDOP::overlap(const KDOP& other) const -{ - for(size_t i = 0; i < N / 2; ++i) - { - if(dist_[i] > other.dist_[i + N / 2]) return false; - if(dist_[i + N / 2] < other.dist_[i]) return false; - } - - return true; -} - -template -bool KDOP::inside(const Vector3d& p) const -{ - for(size_t i = 0; i < 3; ++i) - { - if(p[i] < dist_[i] || p[i] > dist_[i + N / 2]) - return false; - } - - FCL_REAL d[(N - 6) / 2]; - getDistances<(N - 6) / 2>(p, d); - for(size_t i = 0; i < (N - 6) / 2; ++i) - { - if(d[i] < dist_[3 + i] || d[i] > dist_[i + 3 + N / 2]) - return false; - } - - return true; -} - -template -KDOP& KDOP::operator += (const Vector3d& p) -{ - for(size_t i = 0; i < 3; ++i) - { - minmax(p[i], dist_[i], dist_[N / 2 + i]); - } - - FCL_REAL pd[(N - 6) / 2]; - getDistances<(N - 6) / 2>(p, pd); - for(size_t i = 0; i < (N - 6) / 2; ++i) - { - minmax(pd[i], dist_[3 + i], dist_[3 + N / 2 + i]); - } - - return *this; -} - -template -KDOP& KDOP::operator += (const KDOP& other) -{ - for(size_t i = 0; i < N / 2; ++i) - { - dist_[i] = std::min(other.dist_[i], dist_[i]); - dist_[i + N / 2] = std::max(other.dist_[i + N / 2], dist_[i + N / 2]); - } - return *this; -} - -template -KDOP KDOP::operator + (const KDOP& other) const -{ - KDOP res(*this); - return res += other; -} - - -template -FCL_REAL KDOP::distance(const KDOP& other, Vector3d* P, Vector3d* Q) const -{ - std::cerr << "KDOP distance not implemented!" << std::endl; - return 0.0; -} - - -template -KDOP translate(const KDOP& bv, const Vector3d& t) -{ - KDOP res(bv); - for(size_t i = 0; i < 3; ++i) - { - res.dist(i) += t[i]; - res.dist(N / 2 + i) += t[i]; - } - - FCL_REAL d[(N - 6) / 2]; - getDistances<(N - 6) / 2>(t, d); - for(size_t i = 0; i < (N - 6) / 2; ++i) - { - res.dist(3 + i) += d[i]; - res.dist(3 + i + N / 2) += d[i]; - } - - return res; -} - - -template class KDOP<16>; -template class KDOP<18>; -template class KDOP<24>; - -template KDOP<16> translate<16>(const KDOP<16>&, const Vector3d&); -template KDOP<18> translate<18>(const KDOP<18>&, const Vector3d&); -template KDOP<24> translate<24>(const KDOP<24>&, const Vector3d&); - -} diff --git a/src/BV/kIOS.cpp b/src/BV/kIOS.cpp deleted file mode 100644 index 3c3cb6294..000000000 --- a/src/BV/kIOS.cpp +++ /dev/null @@ -1,210 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#include "fcl/BV/kIOS.h" -#include "fcl/BVH/BVH_utility.h" - -#include -#include - -namespace fcl -{ - -bool kIOS::overlap(const kIOS& other) const -{ - for(unsigned int i = 0; i < num_spheres; ++i) - { - for(unsigned int j = 0; j < other.num_spheres; ++j) - { - FCL_REAL o_dist = (spheres[i].o - other.spheres[j].o).squaredNorm(); - FCL_REAL sum_r = spheres[i].r + other.spheres[j].r; - if(o_dist > sum_r * sum_r) - return false; - } - } - - return obb.overlap(other.obb); - - return true; -} - - -bool kIOS::contain(const Vector3d& p) const -{ - for(unsigned int i = 0; i < num_spheres; ++i) - { - FCL_REAL r = spheres[i].r; - if((spheres[i].o - p).squaredNorm() > r * r) - return false; - } - - return true; -} - -kIOS& kIOS::operator += (const Vector3d& p) -{ - for(unsigned int i = 0; i < num_spheres; ++i) - { - FCL_REAL r = spheres[i].r; - FCL_REAL new_r_sqr = (p - spheres[i].o).squaredNorm(); - if(new_r_sqr > r * r) - { - spheres[i].r = sqrt(new_r_sqr); - } - } - - obb += p; - return *this; -} - -kIOS kIOS::operator + (const kIOS& other) const -{ - kIOS result; - unsigned int new_num_spheres = std::min(num_spheres, other.num_spheres); - for(unsigned int i = 0; i < new_num_spheres; ++i) - { - result.spheres[i] = encloseSphere(spheres[i], other.spheres[i]); - } - - result.num_spheres = new_num_spheres; - - result.obb = obb + other.obb; - - return result; -} - -FCL_REAL kIOS::width() const -{ - return obb.width(); -} - -FCL_REAL kIOS::height() const -{ - return obb.height(); -} - -FCL_REAL kIOS::depth() const -{ - return obb.depth(); -} - -FCL_REAL kIOS::volume() const -{ - return obb.volume(); -} - -FCL_REAL kIOS::size() const -{ - return volume(); -} - -FCL_REAL kIOS::distance(const kIOS& other, Vector3d* P, Vector3d* Q) const -{ - FCL_REAL d_max = 0; - int id_a = -1, id_b = -1; - for(unsigned int i = 0; i < num_spheres; ++i) - { - for(unsigned int j = 0; j < other.num_spheres; ++j) - { - FCL_REAL d = (spheres[i].o - other.spheres[j].o).norm() - (spheres[i].r + other.spheres[j].r); - if(d_max < d) - { - d_max = d; - if(P && Q) - { - id_a = i; id_b = j; - } - } - } - } - - if(P && Q) - { - if(id_a != -1 && id_b != -1) - { - Vector3d v = spheres[id_a].o - spheres[id_b].o; - FCL_REAL len_v = v.norm(); - *P = spheres[id_a].o - v * (spheres[id_a].r / len_v); - *Q = spheres[id_b].o + v * (spheres[id_b].r / len_v); - } - } - - return d_max; -} - - -bool overlap(const Matrix3d& R0, const Vector3d& T0, const kIOS& b1, const kIOS& b2) -{ - kIOS b2_temp = b2; - for(unsigned int i = 0; i < b2_temp.num_spheres; ++i) - { - b2_temp.spheres[i].o = R0 * b2_temp.spheres[i].o + T0; - } - - - b2_temp.obb.To = R0 * b2_temp.obb.To + T0; - b2_temp.obb.axis = R0 * b2_temp.obb.axis; - - return b1.overlap(b2_temp); -} - -FCL_REAL distance(const Matrix3d& R0, const Vector3d& T0, const kIOS& b1, const kIOS& b2, Vector3d* P, Vector3d* Q) -{ - kIOS b2_temp = b2; - for(unsigned int i = 0; i < b2_temp.num_spheres; ++i) - { - b2_temp.spheres[i].o = R0 * b2_temp.spheres[i].o + T0; - } - - return b1.distance(b2_temp, P, Q); -} - - -kIOS translate(const kIOS& bv, const Vector3d& t) -{ - kIOS res(bv); - for(size_t i = 0; i < res.num_spheres; ++i) - { - res.spheres[i].o += t; - } - - translate(res.obb, t); - return res; -} - - -} diff --git a/src/BVH/BVH_model.cpp b/src/BVH/BVH_model.cpp deleted file mode 100644 index 8acc64691..000000000 --- a/src/BVH/BVH_model.cpp +++ /dev/null @@ -1,991 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#include "fcl/BVH/BVH_model.h" -#include "fcl/BV/BV.h" -#include -#include - -namespace fcl -{ - -template -BVHModel::BVHModel(const BVHModel& other) : CollisionGeometry(other), - num_tris(other.num_tris), - num_vertices(other.num_vertices), - build_state(other.build_state), - bv_splitter(other.bv_splitter), - bv_fitter(other.bv_fitter), - num_tris_allocated(other.num_tris), - num_vertices_allocated(other.num_vertices) -{ - if(other.vertices) - { - vertices = new Vector3d[num_vertices]; - memcpy(vertices, other.vertices, sizeof(Vector3d) * num_vertices); - } - else - vertices = NULL; - - if(other.tri_indices) - { - tri_indices = new Triangle[num_tris]; - memcpy(tri_indices, other.tri_indices, sizeof(Triangle) * num_tris); - } - else - tri_indices = NULL; - - if(other.prev_vertices) - { - prev_vertices = new Vector3d[num_vertices]; - memcpy(prev_vertices, other.prev_vertices, sizeof(Vector3d) * num_vertices); - } - else - prev_vertices = NULL; - - if(other.primitive_indices) - { - int num_primitives = 0; - switch(other.getModelType()) - { - case BVH_MODEL_TRIANGLES: - num_primitives = num_tris; - break; - case BVH_MODEL_POINTCLOUD: - num_primitives = num_vertices; - break; - default: - ; - } - - primitive_indices = new unsigned int[num_primitives]; - memcpy(primitive_indices, other.primitive_indices, sizeof(unsigned int) * num_primitives); - } - else - primitive_indices = NULL; - - num_bvs = num_bvs_allocated = other.num_bvs; - if(other.bvs) - { - bvs = new BVNode[num_bvs]; - memcpy(bvs, other.bvs, sizeof(BVNode) * num_bvs); - } - else - bvs = NULL; -} - - -template -int BVHModel::beginModel(int num_tris_, int num_vertices_) -{ - if(build_state != BVH_BUILD_STATE_EMPTY) - { - delete [] vertices; vertices = NULL; - delete [] tri_indices; tri_indices = NULL; - delete [] bvs; bvs = NULL; - delete [] prev_vertices; prev_vertices = NULL; - delete [] primitive_indices; primitive_indices = NULL; - - num_vertices_allocated = num_vertices = num_tris_allocated = num_tris = num_bvs_allocated = num_bvs = 0; - } - - if(num_tris_ <= 0) num_tris_ = 8; - if(num_vertices_ <= 0) num_vertices_ = 8; - - num_vertices_allocated = num_vertices_; - num_tris_allocated = num_tris_; - - tri_indices = new Triangle[num_tris_allocated]; - vertices = new Vector3d[num_vertices_allocated]; - - if(!tri_indices) - { - std::cerr << "BVH Error! Out of memory for tri_indices array on BeginModel() call!" << std::endl; - return BVH_ERR_MODEL_OUT_OF_MEMORY; - } - if(!vertices) - { - std::cerr << "BVH Error! Out of memory for vertices array on BeginModel() call!" << std::endl; - return BVH_ERR_MODEL_OUT_OF_MEMORY; - } - - if(build_state != BVH_BUILD_STATE_EMPTY) - { - std::cerr << "BVH Warning! Call beginModel() on a BVHModel that is not empty. This model was cleared and previous triangles/vertices were lost." << std::endl; - build_state = BVH_BUILD_STATE_EMPTY; - return BVH_ERR_BUILD_OUT_OF_SEQUENCE; - } - - build_state = BVH_BUILD_STATE_BEGUN; - - return BVH_OK; -} - - -template -int BVHModel::addVertex(const Vector3d& p) -{ - if(build_state != BVH_BUILD_STATE_BEGUN) - { - std::cerr << "BVH Warning! Call addVertex() in a wrong order. addVertex() was ignored. Must do a beginModel() to clear the model for addition of new vertices." << std::endl; - return BVH_ERR_BUILD_OUT_OF_SEQUENCE; - } - - if(num_vertices >= num_vertices_allocated) - { - Vector3d* temp = new Vector3d[num_vertices_allocated * 2]; - if(!temp) - { - std::cerr << "BVH Error! Out of memory for vertices array on addVertex() call!" << std::endl; - return BVH_ERR_MODEL_OUT_OF_MEMORY; - } - - memcpy(temp, vertices, sizeof(Vector3d) * num_vertices); - delete [] vertices; - vertices = temp; - num_vertices_allocated *= 2; - } - - vertices[num_vertices] = p; - num_vertices += 1; - - return BVH_OK; -} - -template -int BVHModel::addTriangle(const Vector3d& p1, const Vector3d& p2, const Vector3d& p3) -{ - if(build_state == BVH_BUILD_STATE_PROCESSED) - { - std::cerr << "BVH Warning! Call addTriangle() in a wrong order. addTriangle() was ignored. Must do a beginModel() to clear the model for addition of new triangles." << std::endl; - return BVH_ERR_BUILD_OUT_OF_SEQUENCE; - } - - if(num_vertices + 2 >= num_vertices_allocated) - { - Vector3d* temp = new Vector3d[num_vertices_allocated * 2 + 2]; - if(!temp) - { - std::cerr << "BVH Error! Out of memory for vertices array on addTriangle() call!" << std::endl; - return BVH_ERR_MODEL_OUT_OF_MEMORY; - } - - memcpy(temp, vertices, sizeof(Vector3d) * num_vertices); - delete [] vertices; - vertices = temp; - num_vertices_allocated = num_vertices_allocated * 2 + 2; - } - - int offset = num_vertices; - - vertices[num_vertices] = p1; - num_vertices++; - vertices[num_vertices] = p2; - num_vertices++; - vertices[num_vertices] = p3; - num_vertices++; - - if(num_tris >= num_tris_allocated) - { - Triangle* temp = new Triangle[num_tris_allocated * 2]; - if(!temp) - { - std::cerr << "BVH Error! Out of memory for tri_indices array on addTriangle() call!" << std::endl; - return BVH_ERR_MODEL_OUT_OF_MEMORY; - } - - memcpy(temp, tri_indices, sizeof(Triangle) * num_tris); - delete [] tri_indices; - tri_indices = temp; - num_tris_allocated *= 2; - } - - tri_indices[num_tris].set(offset, offset + 1, offset + 2); - num_tris++; - - return BVH_OK; -} - -template -int BVHModel::addSubModel(const std::vector& ps) -{ - if(build_state == BVH_BUILD_STATE_PROCESSED) - { - std::cerr << "BVH Warning! Call addSubModel() in a wrong order. addSubModel() was ignored. Must do a beginModel() to clear the model for addition of new vertices." << std::endl; - return BVH_ERR_BUILD_OUT_OF_SEQUENCE; - } - - int num_vertices_to_add = ps.size(); - - if(num_vertices + num_vertices_to_add - 1 >= num_vertices_allocated) - { - Vector3d* temp = new Vector3d[num_vertices_allocated * 2 + num_vertices_to_add - 1]; - if(!temp) - { - std::cerr << "BVH Error! Out of memory for vertices array on addSubModel() call!" << std::endl; - return BVH_ERR_MODEL_OUT_OF_MEMORY; - } - - memcpy(temp, vertices, sizeof(Vector3d) * num_vertices); - delete [] vertices; - vertices = temp; - num_vertices_allocated = num_vertices_allocated * 2 + num_vertices_to_add - 1; - } - - for(int i = 0; i < num_vertices_to_add; ++i) - { - vertices[num_vertices] = ps[i]; - num_vertices++; - } - - return BVH_OK; -} - -template -int BVHModel::addSubModel(const std::vector& ps, const std::vector& ts) -{ - if(build_state == BVH_BUILD_STATE_PROCESSED) - { - std::cerr << "BVH Warning! Call addSubModel() in a wrong order. addSubModel() was ignored. Must do a beginModel() to clear the model for addition of new vertices." << std::endl; - return BVH_ERR_BUILD_OUT_OF_SEQUENCE; - } - - int num_vertices_to_add = ps.size(); - - if(num_vertices + num_vertices_to_add - 1 >= num_vertices_allocated) - { - Vector3d* temp = new Vector3d[num_vertices_allocated * 2 + num_vertices_to_add - 1]; - if(!temp) - { - std::cerr << "BVH Error! Out of memory for vertices array on addSubModel() call!" << std::endl; - return BVH_ERR_MODEL_OUT_OF_MEMORY; - } - - memcpy(temp, vertices, sizeof(Vector3d) * num_vertices); - delete [] vertices; - vertices = temp; - num_vertices_allocated = num_vertices_allocated * 2 + num_vertices_to_add - 1; - } - - int offset = num_vertices; - - for(int i = 0; i < num_vertices_to_add; ++i) - { - vertices[num_vertices] = ps[i]; - num_vertices++; - } - - - int num_tris_to_add = ts.size(); - - if(num_tris + num_tris_to_add - 1 >= num_tris_allocated) - { - Triangle* temp = new Triangle[num_tris_allocated * 2 + num_tris_to_add - 1]; - if(!temp) - { - std::cerr << "BVH Error! Out of memory for tri_indices array on addSubModel() call!" << std::endl; - return BVH_ERR_MODEL_OUT_OF_MEMORY; - } - - memcpy(temp, tri_indices, sizeof(Triangle) * num_tris); - delete [] tri_indices; - tri_indices = temp; - num_tris_allocated = num_tris_allocated * 2 + num_tris_to_add - 1; - } - - for(int i = 0; i < num_tris_to_add; ++i) - { - const Triangle& t = ts[i]; - tri_indices[num_tris].set(t[0] + offset, t[1] + offset, t[2] + offset); - num_tris++; - } - - return BVH_OK; -} - -template -int BVHModel::endModel() -{ - if(build_state != BVH_BUILD_STATE_BEGUN) - { - std::cerr << "BVH Warning! Call endModel() in wrong order. endModel() was ignored." << std::endl; - return BVH_ERR_BUILD_OUT_OF_SEQUENCE; - } - - if(num_tris == 0 && num_vertices == 0) - { - std::cerr << "BVH Error! endModel() called on model with no triangles and vertices." << std::endl; - return BVH_ERR_BUILD_EMPTY_MODEL; - } - - if(num_tris_allocated > num_tris) - { - Triangle* new_tris = new Triangle[num_tris]; - if(!new_tris) - { - std::cerr << "BVH Error! Out of memory for tri_indices array in endModel() call!" << std::endl; - return BVH_ERR_MODEL_OUT_OF_MEMORY; - } - memcpy(new_tris, tri_indices, sizeof(Triangle) * num_tris); - delete [] tri_indices; - tri_indices = new_tris; - num_tris_allocated = num_tris; - } - - if(num_vertices_allocated > num_vertices) - { - Vector3d* new_vertices = new Vector3d[num_vertices]; - if(!new_vertices) - { - std::cerr << "BVH Error! Out of memory for vertices array in endModel() call!" << std::endl; - return BVH_ERR_MODEL_OUT_OF_MEMORY; - } - memcpy(new_vertices, vertices, sizeof(Vector3d) * num_vertices); - delete [] vertices; - vertices = new_vertices; - num_vertices_allocated = num_vertices; - } - - - // construct BVH tree - int num_bvs_to_be_allocated = 0; - if(num_tris == 0) - num_bvs_to_be_allocated = 2 * num_vertices - 1; - else - num_bvs_to_be_allocated = 2 * num_tris - 1; - - - bvs = new BVNode [num_bvs_to_be_allocated]; - primitive_indices = new unsigned int [num_bvs_to_be_allocated]; - if(!bvs || !primitive_indices) - { - std::cerr << "BVH Error! Out of memory for BV array in endModel()!" << std::endl; - return BVH_ERR_MODEL_OUT_OF_MEMORY; - } - num_bvs_allocated = num_bvs_to_be_allocated; - num_bvs = 0; - - buildTree(); - - // finish constructing - build_state = BVH_BUILD_STATE_PROCESSED; - - return BVH_OK; -} - - - -template -int BVHModel::beginReplaceModel() -{ - if(build_state != BVH_BUILD_STATE_PROCESSED) - { - std::cerr << "BVH Error! Call beginReplaceModel() on a BVHModel that has no previous frame." << std::endl; - return BVH_ERR_BUILD_EMPTY_PREVIOUS_FRAME; - } - - if(prev_vertices) delete [] prev_vertices; prev_vertices = NULL; - - num_vertex_updated = 0; - - build_state = BVH_BUILD_STATE_REPLACE_BEGUN; - - return BVH_OK; -} - -template -int BVHModel::replaceVertex(const Vector3d& p) -{ - if(build_state != BVH_BUILD_STATE_REPLACE_BEGUN) - { - std::cerr << "BVH Warning! Call replaceVertex() in a wrong order. replaceVertex() was ignored. Must do a beginReplaceModel() for initialization." << std::endl; - return BVH_ERR_BUILD_OUT_OF_SEQUENCE; - } - - vertices[num_vertex_updated] = p; - num_vertex_updated++; - - return BVH_OK; -} - -template -int BVHModel::replaceTriangle(const Vector3d& p1, const Vector3d& p2, const Vector3d& p3) -{ - if(build_state != BVH_BUILD_STATE_REPLACE_BEGUN) - { - std::cerr << "BVH Warning! Call replaceTriangle() in a wrong order. replaceTriangle() was ignored. Must do a beginReplaceModel() for initialization." << std::endl; - return BVH_ERR_BUILD_OUT_OF_SEQUENCE; - } - - vertices[num_vertex_updated] = p1; num_vertex_updated++; - vertices[num_vertex_updated] = p2; num_vertex_updated++; - vertices[num_vertex_updated] = p3; num_vertex_updated++; - return BVH_OK; -} - -template -int BVHModel::replaceSubModel(const std::vector& ps) -{ - if(build_state != BVH_BUILD_STATE_REPLACE_BEGUN) - { - std::cerr << "BVH Warning! Call replaceSubModel() in a wrong order. replaceSubModel() was ignored. Must do a beginReplaceModel() for initialization." << std::endl; - return BVH_ERR_BUILD_OUT_OF_SEQUENCE; - } - - for(unsigned int i = 0; i < ps.size(); ++i) - { - vertices[num_vertex_updated] = ps[i]; - num_vertex_updated++; - } - return BVH_OK; -} - -template -int BVHModel::endReplaceModel(bool refit, bool bottomup) -{ - if(build_state != BVH_BUILD_STATE_REPLACE_BEGUN) - { - std::cerr << "BVH Warning! Call endReplaceModel() in a wrong order. endReplaceModel() was ignored. " << std::endl; - return BVH_ERR_BUILD_OUT_OF_SEQUENCE; - } - - if(num_vertex_updated != num_vertices) - { - std::cerr << "BVH Error! The replaced model should have the same number of vertices as the old model." << std::endl; - return BVH_ERR_INCORRECT_DATA; - } - - if(refit) // refit, do not change BVH structure - { - refitTree(bottomup); - } - else // reconstruct bvh tree based on current frame data - { - buildTree(); - } - - build_state = BVH_BUILD_STATE_PROCESSED; - - return BVH_OK; -} - - - - - -template -int BVHModel::beginUpdateModel() -{ - if(build_state != BVH_BUILD_STATE_PROCESSED && build_state != BVH_BUILD_STATE_UPDATED) - { - std::cerr << "BVH Error! Call beginUpdatemodel() on a BVHModel that has no previous frame." << std::endl; - return BVH_ERR_BUILD_EMPTY_PREVIOUS_FRAME; - } - - if(prev_vertices) - { - Vector3d* temp = prev_vertices; - prev_vertices = vertices; - vertices = temp; - } - else - { - prev_vertices = vertices; - vertices = new Vector3d[num_vertices]; - } - - num_vertex_updated = 0; - - build_state = BVH_BUILD_STATE_UPDATE_BEGUN; - - return BVH_OK; -} - -template -int BVHModel::updateVertex(const Vector3d& p) -{ - if(build_state != BVH_BUILD_STATE_UPDATE_BEGUN) - { - std::cerr << "BVH Warning! Call updateVertex() in a wrong order. updateVertex() was ignored. Must do a beginUpdateModel() for initialization." << std::endl; - return BVH_ERR_BUILD_OUT_OF_SEQUENCE; - } - - vertices[num_vertex_updated] = p; - num_vertex_updated++; - - return BVH_OK; -} - -template -int BVHModel::updateTriangle(const Vector3d& p1, const Vector3d& p2, const Vector3d& p3) -{ - if(build_state != BVH_BUILD_STATE_UPDATE_BEGUN) - { - std::cerr << "BVH Warning! Call updateTriangle() in a wrong order. updateTriangle() was ignored. Must do a beginUpdateModel() for initialization." << std::endl; - return BVH_ERR_BUILD_OUT_OF_SEQUENCE; - } - - vertices[num_vertex_updated] = p1; num_vertex_updated++; - vertices[num_vertex_updated] = p2; num_vertex_updated++; - vertices[num_vertex_updated] = p3; num_vertex_updated++; - return BVH_OK; -} - -template -int BVHModel::updateSubModel(const std::vector& ps) -{ - if(build_state != BVH_BUILD_STATE_UPDATE_BEGUN) - { - std::cerr << "BVH Warning! Call updateSubModel() in a wrong order. updateSubModel() was ignored. Must do a beginUpdateModel() for initialization." << std::endl; - return BVH_ERR_BUILD_OUT_OF_SEQUENCE; - } - - for(unsigned int i = 0; i < ps.size(); ++i) - { - vertices[num_vertex_updated] = ps[i]; - num_vertex_updated++; - } - return BVH_OK; -} - -template -int BVHModel::endUpdateModel(bool refit, bool bottomup) -{ - if(build_state != BVH_BUILD_STATE_UPDATE_BEGUN) - { - std::cerr << "BVH Warning! Call endUpdateModel() in a wrong order. endUpdateModel() was ignored. " << std::endl; - return BVH_ERR_BUILD_OUT_OF_SEQUENCE; - } - - if(num_vertex_updated != num_vertices) - { - std::cerr << "BVH Error! The updated model should have the same number of vertices as the old model." << std::endl; - return BVH_ERR_INCORRECT_DATA; - } - - if(refit) // refit, do not change BVH structure - { - refitTree(bottomup); - } - else // reconstruct bvh tree based on current frame data - { - buildTree(); - - // then refit - - refitTree(bottomup); - } - - - build_state = BVH_BUILD_STATE_UPDATED; - - return BVH_OK; -} - - - - -template -int BVHModel::memUsage(int msg) const -{ - int mem_bv_list = sizeof(BV) * num_bvs; - int mem_tri_list = sizeof(Triangle) * num_tris; - int mem_vertex_list = sizeof(Vector3d) * num_vertices; - - int total_mem = mem_bv_list + mem_tri_list + mem_vertex_list + sizeof(BVHModel); - if(msg) - { - std::cerr << "Total for model " << total_mem << " bytes." << std::endl; - std::cerr << "BVs: " << num_bvs << " allocated." << std::endl; - std::cerr << "Tris: " << num_tris << " allocated." << std::endl; - std::cerr << "Vertices: " << num_vertices << " allocated." << std::endl; - } - - return BVH_OK; -} - - -template -int BVHModel::buildTree() -{ - // set BVFitter - bv_fitter->set(vertices, tri_indices, getModelType()); - // set SplitRule - bv_splitter->set(vertices, tri_indices, getModelType()); - - num_bvs = 1; - - int num_primitives = 0; - switch(getModelType()) - { - case BVH_MODEL_TRIANGLES: - num_primitives = num_tris; - break; - case BVH_MODEL_POINTCLOUD: - num_primitives = num_vertices; - break; - default: - std::cerr << "BVH Error: Model type not supported!" << std::endl; - return BVH_ERR_UNSUPPORTED_FUNCTION; - } - - for(int i = 0; i < num_primitives; ++i) - primitive_indices[i] = i; - recursiveBuildTree(0, 0, num_primitives); - - bv_fitter->clear(); - bv_splitter->clear(); - - return BVH_OK; -} - -template -int BVHModel::recursiveBuildTree(int bv_id, int first_primitive, int num_primitives) -{ - BVHModelType type = getModelType(); - BVNode* bvnode = bvs + bv_id; - unsigned int* cur_primitive_indices = primitive_indices + first_primitive; - - // constructing BV - BV bv = bv_fitter->fit(cur_primitive_indices, num_primitives); - bv_splitter->computeRule(bv, cur_primitive_indices, num_primitives); - - bvnode->bv = bv; - bvnode->first_primitive = first_primitive; - bvnode->num_primitives = num_primitives; - - if(num_primitives == 1) - { - bvnode->first_child = -((*cur_primitive_indices) + 1); - } - else - { - bvnode->first_child = num_bvs; - num_bvs += 2; - - int c1 = 0; - for(int i = 0; i < num_primitives; ++i) - { - Vector3d p; - if(type == BVH_MODEL_POINTCLOUD) p = vertices[cur_primitive_indices[i]]; - else if(type == BVH_MODEL_TRIANGLES) - { - const Triangle& t = tri_indices[cur_primitive_indices[i]]; - const Vector3d& p1 = vertices[t[0]]; - const Vector3d& p2 = vertices[t[1]]; - const Vector3d& p3 = vertices[t[2]]; - p = (p1 + p2 + p3) / 3.0; - } - else - { - std::cerr << "BVH Error: Model type not supported!" << std::endl; - return BVH_ERR_UNSUPPORTED_FUNCTION; - } - - - // loop invariant: up to (but not including) index c1 in group 1, - // then up to (but not including) index i in group 2 - // - // [1] [1] [1] [1] [2] [2] [2] [x] [x] ... [x] - // c1 i - // - if(bv_splitter->apply(p)) // in the right side - { - // do nothing - } - else - { - std::swap(cur_primitive_indices[i], cur_primitive_indices[c1]); - c1++; - } - } - - - if((c1 == 0) || (c1 == num_primitives)) c1 = num_primitives / 2; - - int num_first_half = c1; - - recursiveBuildTree(bvnode->leftChild(), first_primitive, num_first_half); - recursiveBuildTree(bvnode->rightChild(), first_primitive + num_first_half, num_primitives - num_first_half); - } - - return BVH_OK; -} - -template -int BVHModel::refitTree(bool bottomup) -{ - if(bottomup) - return refitTree_bottomup(); - else - return refitTree_topdown(); -} - -template -int BVHModel::refitTree_bottomup() -{ - int res = recursiveRefitTree_bottomup(0); - - return res; -} - - -template -int BVHModel::recursiveRefitTree_bottomup(int bv_id) -{ - BVNode* bvnode = bvs + bv_id; - if(bvnode->isLeaf()) - { - BVHModelType type = getModelType(); - int primitive_id = -(bvnode->first_child + 1); - if(type == BVH_MODEL_POINTCLOUD) - { - BV bv; - - if(prev_vertices) - { - Vector3d v[2]; - v[0] = prev_vertices[primitive_id]; - v[1] = vertices[primitive_id]; - fit(v, 2, bv); - } - else - fit(vertices + primitive_id, 1, bv); - - bvnode->bv = bv; - } - else if(type == BVH_MODEL_TRIANGLES) - { - BV bv; - const Triangle& triangle = tri_indices[primitive_id]; - - if(prev_vertices) - { - Vector3d v[6]; - for(int i = 0; i < 3; ++i) - { - v[i] = prev_vertices[triangle[i]]; - v[i + 3] = vertices[triangle[i]]; - } - - fit(v, 6, bv); - } - else - { - Vector3d v[3]; - for(int i = 0; i < 3; ++i) - { - v[i] = vertices[triangle[i]]; - } - - fit(v, 3, bv); - } - - bvnode->bv = bv; - } - else - { - std::cerr << "BVH Error: Model type not supported!" << std::endl; - return BVH_ERR_UNSUPPORTED_FUNCTION; - } - } - else - { - recursiveRefitTree_bottomup(bvnode->leftChild()); - recursiveRefitTree_bottomup(bvnode->rightChild()); - bvnode->bv = bvs[bvnode->leftChild()].bv + bvs[bvnode->rightChild()].bv; - } - - return BVH_OK; -} - -template -int BVHModel::refitTree_topdown() -{ - bv_fitter->set(vertices, prev_vertices, tri_indices, getModelType()); - for(int i = 0; i < num_bvs; ++i) - { - BV bv = bv_fitter->fit(primitive_indices + bvs[i].first_primitive, bvs[i].num_primitives); - bvs[i].bv = bv; - } - - bv_fitter->clear(); - - return BVH_OK; -} - -template -void BVHModel::computeLocalAABB() -{ - AABB aabb_; - for(int i = 0; i < num_vertices; ++i) - { - aabb_ += vertices[i]; - } - - aabb_center = aabb_.center(); - - aabb_radius = 0; - for(int i = 0; i < num_vertices; ++i) - { - FCL_REAL r = (aabb_center - vertices[i]).squaredNorm(); - if(r > aabb_radius) aabb_radius = r; - } - - aabb_radius = sqrt(aabb_radius); - - aabb_local = aabb_; -} - - -template<> -void BVHModel::makeParentRelativeRecurse(int bv_id, const Matrix3d& parent_axis, const Vector3d& parent_c) -{ - OBB& obb = bvs[bv_id].bv; - if(!bvs[bv_id].isLeaf()) - { - makeParentRelativeRecurse(bvs[bv_id].first_child, obb.axis, obb.To); - - makeParentRelativeRecurse(bvs[bv_id].first_child + 1, obb.axis, obb.To); - } - - // make self parent relative - obb.axis = parent_axis.transpose() * obb.axis; - obb.To = (obb.To - parent_c).transpose() * parent_axis; -} - -template<> -void BVHModel::makeParentRelativeRecurse(int bv_id, const Matrix3d& parent_axis, const Vector3d& parent_c) -{ - RSS& rss = bvs[bv_id].bv; - if(!bvs[bv_id].isLeaf()) - { - makeParentRelativeRecurse(bvs[bv_id].first_child, rss.axis, rss.Tr); - - makeParentRelativeRecurse(bvs[bv_id].first_child + 1, rss.axis, rss.Tr); - } - - // make self parent relative - rss.axis = parent_axis.transpose() * rss.axis; - rss.Tr = (rss.Tr - parent_c).transpose() * parent_axis; -} - -template<> -void BVHModel::makeParentRelativeRecurse(int bv_id, const Matrix3d& parent_axis, const Vector3d& parent_c) -{ - OBB& obb = bvs[bv_id].bv.obb; - RSS& rss = bvs[bv_id].bv.rss; - if(!bvs[bv_id].isLeaf()) - { - makeParentRelativeRecurse(bvs[bv_id].first_child, obb.axis, obb.To); - - makeParentRelativeRecurse(bvs[bv_id].first_child + 1, obb.axis, obb.To); - } - - // make self parent relative - obb.axis = parent_axis.transpose() * obb.axis; - rss.axis = obb.axis; - - - obb.To = (obb.To - parent_c).transpose() * parent_axis; - rss.Tr = obb.To; -} - - - -template<> -NODE_TYPE BVHModel::getNodeType() const -{ - return BV_AABB; -} - -template<> -NODE_TYPE BVHModel::getNodeType() const -{ - return BV_OBB; -} - -template<> -NODE_TYPE BVHModel::getNodeType() const -{ - return BV_RSS; -} - -template<> -NODE_TYPE BVHModel::getNodeType() const -{ - return BV_kIOS; -} - -template<> -NODE_TYPE BVHModel::getNodeType() const -{ - return BV_OBBRSS; -} - -template<> -NODE_TYPE BVHModel >::getNodeType() const -{ - return BV_KDOP16; -} - -template<> -NODE_TYPE BVHModel >::getNodeType() const -{ - return BV_KDOP18; -} - -template<> -NODE_TYPE BVHModel >::getNodeType() const -{ - return BV_KDOP24; -} - - - - - - -template class BVHModel >; -template class BVHModel >; -template class BVHModel >; -template class BVHModel; -template class BVHModel; -template class BVHModel; -template class BVHModel; -template class BVHModel; -} diff --git a/src/BVH/BVH_utility.cpp b/src/BVH/BVH_utility.cpp deleted file mode 100644 index 5c3d640ee..000000000 --- a/src/BVH/BVH_utility.cpp +++ /dev/null @@ -1,662 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - - -#include "fcl/BVH/BVH_utility.h" - -namespace fcl -{ - -void BVHExpand(BVHModel& model, const Variance3f* ucs, FCL_REAL r = 1.0) -{ - for(int i = 0; i < model.getNumBVs(); ++i) - { - BVNode& bvnode = model.getBV(i); - - Vector3d* vs = new Vector3d[bvnode.num_primitives * 6]; - - for(int j = 0; j < bvnode.num_primitives; ++j) - { - int v_id = bvnode.first_primitive + j; - const Variance3f& uc = ucs[v_id]; - - Vector3d&v = model.vertices[bvnode.first_primitive + j]; - - for(int k = 0; k < 3; ++k) - { - vs[6 * j + 2 * k] = v + uc.axis.col(k) * (r * uc.sigma[k]); - vs[6 * j + 2 * k + 1] = v - uc.axis.col(k) * (r * uc.sigma[k]); - } - } - - OBB bv; - fit(vs, bvnode.num_primitives * 6, bv); - - delete [] vs; - - bvnode.bv = bv; - } -} - -void BVHExpand(BVHModel& model, const Variance3f* ucs, FCL_REAL r = 1.0) -{ - for(int i = 0; i < model.getNumBVs(); ++i) - { - BVNode& bvnode = model.getBV(i); - - Vector3d* vs = new Vector3d[bvnode.num_primitives * 6]; - - for(int j = 0; j < bvnode.num_primitives; ++j) - { - int v_id = bvnode.first_primitive + j; - const Variance3f& uc = ucs[v_id]; - - Vector3d&v = model.vertices[bvnode.first_primitive + j]; - - for(int k = 0; k < 3; ++k) - { - vs[6 * j + 2 * k] = v + uc.axis.col(k) * (r * uc.sigma[k]); - vs[6 * j + 2 * k + 1] = v - uc.axis.col(k) * (r * uc.sigma[k]); - } - } - - RSS bv; - fit(vs, bvnode.num_primitives * 6, bv); - - delete [] vs; - - bvnode.bv = bv; - } -} - - -void getCovariance(Vector3d* ps, Vector3d* ps2, Triangle* ts, unsigned int* indices, int n, Matrix3d& M) -{ - Vector3d S1 = Vector3d::Zero(); - Vector3d S2[3] = {Vector3d::Zero(), Vector3d::Zero(), Vector3d::Zero()}; - - if(ts) - { - for(int i = 0; i < n; ++i) - { - const Triangle& t = (indices) ? ts[indices[i]] : ts[i]; - - const Vector3d& p1 = ps[t[0]]; - const Vector3d& p2 = ps[t[1]]; - const Vector3d& p3 = ps[t[2]]; - - S1[0] += (p1[0] + p2[0] + p3[0]); - S1[1] += (p1[1] + p2[1] + p3[1]); - S1[2] += (p1[2] + p2[2] + p3[2]); - S2[0][0] += (p1[0] * p1[0] + p2[0] * p2[0] + p3[0] * p3[0]); - S2[1][1] += (p1[1] * p1[1] + p2[1] * p2[1] + p3[1] * p3[1]); - S2[2][2] += (p1[2] * p1[2] + p2[2] * p2[2] + p3[2] * p3[2]); - S2[0][1] += (p1[0] * p1[1] + p2[0] * p2[1] + p3[0] * p3[1]); - S2[0][2] += (p1[0] * p1[2] + p2[0] * p2[2] + p3[0] * p3[2]); - S2[1][2] += (p1[1] * p1[2] + p2[1] * p2[2] + p3[1] * p3[2]); - - if(ps2) - { - const Vector3d& p1 = ps2[t[0]]; - const Vector3d& p2 = ps2[t[1]]; - const Vector3d& p3 = ps2[t[2]]; - - S1[0] += (p1[0] + p2[0] + p3[0]); - S1[1] += (p1[1] + p2[1] + p3[1]); - S1[2] += (p1[2] + p2[2] + p3[2]); - - S2[0][0] += (p1[0] * p1[0] + p2[0] * p2[0] + p3[0] * p3[0]); - S2[1][1] += (p1[1] * p1[1] + p2[1] * p2[1] + p3[1] * p3[1]); - S2[2][2] += (p1[2] * p1[2] + p2[2] * p2[2] + p3[2] * p3[2]); - S2[0][1] += (p1[0] * p1[1] + p2[0] * p2[1] + p3[0] * p3[1]); - S2[0][2] += (p1[0] * p1[2] + p2[0] * p2[2] + p3[0] * p3[2]); - S2[1][2] += (p1[1] * p1[2] + p2[1] * p2[2] + p3[1] * p3[2]); - } - } - } - else - { - for(int i = 0; i < n; ++i) - { - const Vector3d& p = (indices) ? ps[indices[i]] : ps[i]; - S1 += p; - S2[0][0] += (p[0] * p[0]); - S2[1][1] += (p[1] * p[1]); - S2[2][2] += (p[2] * p[2]); - S2[0][1] += (p[0] * p[1]); - S2[0][2] += (p[0] * p[2]); - S2[1][2] += (p[1] * p[2]); - - if(ps2) // another frame - { - const Vector3d& p = (indices) ? ps2[indices[i]] : ps2[i]; - S1 += p; - S2[0][0] += (p[0] * p[0]); - S2[1][1] += (p[1] * p[1]); - S2[2][2] += (p[2] * p[2]); - S2[0][1] += (p[0] * p[1]); - S2[0][2] += (p[0] * p[2]); - S2[1][2] += (p[1] * p[2]); - } - } - } - - int n_points = ((ps2) ? 2 : 1) * ((ts) ? 3 : 1) * n; - - M(0, 0) = S2[0][0] - S1[0]*S1[0] / n_points; - M(1, 1) = S2[1][1] - S1[1]*S1[1] / n_points; - M(2, 2) = S2[2][2] - S1[2]*S1[2] / n_points; - M(0, 1) = S2[0][1] - S1[0]*S1[1] / n_points; - M(1, 2) = S2[1][2] - S1[1]*S1[2] / n_points; - M(0, 2) = S2[0][2] - S1[0]*S1[2] / n_points; - M(1, 0) = M(0, 1); - M(2, 0) = M(0, 2); - M(2, 1) = M(1, 2); -} - - -/** \brief Compute the RSS bounding volume parameters: radius, rectangle size and the origin. - * The bounding volume axes are known. - */ -void getRadiusAndOriginAndRectangleSize(Vector3d* ps, Vector3d* ps2, Triangle* ts, unsigned int* indices, int n, const Matrix3d& axis, Vector3d& origin, FCL_REAL l[2], FCL_REAL& r) -{ - bool indirect_index = true; - if(!indices) indirect_index = false; - - int size_P = ((ps2) ? 2 : 1) * ((ts) ? 3 : 1) * n; - - std::vector P(size_P); - - int P_id = 0; - - if(ts) - { - for(int i = 0; i < n; ++i) - { - int index = indirect_index ? indices[i] : i; - const Triangle& t = ts[index]; - - for(int j = 0; j < 3; ++j) - { - int point_id = t[j]; - const Vector3d& p = ps[point_id]; - Vector3d v(p[0], p[1], p[2]); - P[P_id] = v.transpose() * axis; - P_id++; - } - - if(ps2) - { - for(int j = 0; j < 3; ++j) - { - int point_id = t[j]; - const Vector3d& p = ps2[point_id]; - Vector3d v(p[0], p[1], p[2]); - P[P_id] = v.transpose() * axis; - P_id++; - } - } - } - } - else - { - for(int i = 0; i < n; ++i) - { - int index = indirect_index ? indices[i] : i; - - const Vector3d& p = ps[index]; - Vector3d v(p[0], p[1], p[2]); - P[P_id] = v.transpose() * axis; - P_id++; - - if(ps2) - { - const Vector3d& v = ps2[index]; - P[P_id] = v.transpose() * axis; - P_id++; - } - } - } - - FCL_REAL minx, maxx, miny, maxy, minz, maxz; - - FCL_REAL cz, radsqr; - - minz = maxz = P[0][2]; - - for(int i = 1; i < size_P; ++i) - { - FCL_REAL z_value = P[i][2]; - if(z_value < minz) minz = z_value; - else if(z_value > maxz) maxz = z_value; - } - - r = (FCL_REAL)0.5 * (maxz - minz); - radsqr = r * r; - cz = (FCL_REAL)0.5 * (maxz + minz); - - // compute an initial length of rectangle along x direction - - // find minx and maxx as starting points - - int minindex, maxindex; - minindex = maxindex = 0; - FCL_REAL mintmp, maxtmp; - mintmp = maxtmp = P[0][0]; - - for(int i = 1; i < size_P; ++i) - { - FCL_REAL x_value = P[i][0]; - if(x_value < mintmp) - { - minindex = i; - mintmp = x_value; - } - else if(x_value > maxtmp) - { - maxindex = i; - maxtmp = x_value; - } - } - - FCL_REAL x, dz; - dz = P[minindex][2] - cz; - minx = P[minindex][0] + std::sqrt(std::max(radsqr - dz * dz, 0)); - dz = P[maxindex][2] - cz; - maxx = P[maxindex][0] - std::sqrt(std::max(radsqr - dz * dz, 0)); - - - // grow minx - - for(int i = 0; i < size_P; ++i) - { - if(P[i][0] < minx) - { - dz = P[i][2] - cz; - x = P[i][0] + std::sqrt(std::max(radsqr - dz * dz, 0)); - if(x < minx) minx = x; - } - } - - // grow maxx - - for(int i = 0; i < size_P; ++i) - { - if(P[i][0] > maxx) - { - dz = P[i][2] - cz; - x = P[i][0] - std::sqrt(std::max(radsqr - dz * dz, 0)); - if(x > maxx) maxx = x; - } - } - - // compute an initial length of rectangle along y direction - - // find miny and maxy as starting points - - minindex = maxindex = 0; - mintmp = maxtmp = P[0][1]; - for(int i = 1; i < size_P; ++i) - { - FCL_REAL y_value = P[i][1]; - if(y_value < mintmp) - { - minindex = i; - mintmp = y_value; - } - else if(y_value > maxtmp) - { - maxindex = i; - maxtmp = y_value; - } - } - - FCL_REAL y; - dz = P[minindex][2] - cz; - miny = P[minindex][1] + std::sqrt(std::max(radsqr - dz * dz, 0)); - dz = P[maxindex][2] - cz; - maxy = P[maxindex][1] - std::sqrt(std::max(radsqr - dz * dz, 0)); - - // grow miny - - for(int i = 0; i < size_P; ++i) - { - if(P[i][1] < miny) - { - dz = P[i][2] - cz; - y = P[i][1] + std::sqrt(std::max(radsqr - dz * dz, 0)); - if(y < miny) miny = y; - } - } - - // grow maxy - - for(int i = 0; i < size_P; ++i) - { - if(P[i][1] > maxy) - { - dz = P[i][2] - cz; - y = P[i][1] - std::sqrt(std::max(radsqr - dz * dz, 0)); - if(y > maxy) maxy = y; - } - } - - // corners may have some points which are not covered - grow lengths if necessary - // quite conservative (can be improved) - FCL_REAL dx, dy, u, t; - FCL_REAL a = std::sqrt((FCL_REAL)0.5); - for(int i = 0; i < size_P; ++i) - { - if(P[i][0] > maxx) - { - if(P[i][1] > maxy) - { - dx = P[i][0] - maxx; - dy = P[i][1] - maxy; - u = dx * a + dy * a; - t = (a*u - dx)*(a*u - dx) + - (a*u - dy)*(a*u - dy) + - (cz - P[i][2])*(cz - P[i][2]); - u = u - std::sqrt(std::max(radsqr - t, 0)); - if(u > 0) - { - maxx += u*a; - maxy += u*a; - } - } - else if(P[i][1] < miny) - { - dx = P[i][0] - maxx; - dy = P[i][1] - miny; - u = dx * a - dy * a; - t = (a*u - dx)*(a*u - dx) + - (-a*u - dy)*(-a*u - dy) + - (cz - P[i][2])*(cz - P[i][2]); - u = u - std::sqrt(std::max(radsqr - t, 0)); - if(u > 0) - { - maxx += u*a; - miny -= u*a; - } - } - } - else if(P[i][0] < minx) - { - if(P[i][1] > maxy) - { - dx = P[i][0] - minx; - dy = P[i][1] - maxy; - u = dy * a - dx * a; - t = (-a*u - dx)*(-a*u - dx) + - (a*u - dy)*(a*u - dy) + - (cz - P[i][2])*(cz - P[i][2]); - u = u - std::sqrt(std::max(radsqr - t, 0)); - if(u > 0) - { - minx -= u*a; - maxy += u*a; - } - } - else if(P[i][1] < miny) - { - dx = P[i][0] - minx; - dy = P[i][1] - miny; - u = -dx * a - dy * a; - t = (-a*u - dx)*(-a*u - dx) + - (-a*u - dy)*(-a*u - dy) + - (cz - P[i][2])*(cz - P[i][2]); - u = u - std::sqrt(std::max(radsqr - t, 0)); - if (u > 0) - { - minx -= u*a; - miny -= u*a; - } - } - } - } - - origin = axis.col(0) * minx + axis.col(1) * miny + axis.col(2) * cz; - - l[0] = maxx - minx; - if(l[0] < 0) l[0] = 0; - l[1] = maxy - miny; - if(l[1] < 0) l[1] = 0; - -} - - -/** \brief Compute the bounding volume extent and center for a set or subset of points. - * The bounding volume axes are known. - */ -static inline void getExtentAndCenter_pointcloud(Vector3d* ps, Vector3d* ps2, unsigned int* indices, int n, const Matrix3d& axis, Vector3d& center, Vector3d& extent) -{ - bool indirect_index = true; - if(!indices) indirect_index = false; - - FCL_REAL real_max = std::numeric_limits::max(); - - Vector3d min_coord = Vector3d::Constant(real_max); - Vector3d max_coord = Vector3d::Constant(-real_max); - - for(int i = 0; i < n; ++i) - { - int index = indirect_index ? indices[i] : i; - - const Vector3d& p = ps[index]; - Vector3d v(p[0], p[1], p[2]); - Vector3d proj = v.transpose() * axis; - - for(int j = 0; j < 3; ++j) - { - if(proj[j] > max_coord[j]) max_coord[j] = proj[j]; - if(proj[j] < min_coord[j]) min_coord[j] = proj[j]; - } - - if(ps2) - { - const Vector3d& v = ps2[index]; - proj = v.transpose() * axis; - - for(int j = 0; j < 3; ++j) - { - if(proj[j] > max_coord[j]) max_coord[j] = proj[j]; - if(proj[j] < min_coord[j]) min_coord[j] = proj[j]; - } - } - } - - Vector3d o((max_coord[0] + min_coord[0]) / 2, - (max_coord[1] + min_coord[1]) / 2, - (max_coord[2] + min_coord[2]) / 2); - - center = axis * o; - extent = (max_coord - min_coord) / 2; -} - - -/** \brief Compute the bounding volume extent and center for a set or subset of points. - * The bounding volume axes are known. - */ -static inline void getExtentAndCenter_mesh(Vector3d* ps, Vector3d* ps2, Triangle* ts, unsigned int* indices, int n, const Matrix3d& axis, Vector3d& center, Vector3d& extent) -{ - bool indirect_index = true; - if(!indices) indirect_index = false; - - FCL_REAL real_max = std::numeric_limits::max(); - - Vector3d min_coord = Vector3d::Constant(real_max); - Vector3d max_coord = Vector3d::Constant(-real_max); - - for(int i = 0; i < n; ++i) - { - unsigned int index = indirect_index? indices[i] : i; - const Triangle& t = ts[index]; - - for(int j = 0; j < 3; ++j) - { - int point_id = t[j]; - const Vector3d& p = ps[point_id]; - Vector3d v(p[0], p[1], p[2]); - Vector3d proj = v.transpose() * axis; - - for(int k = 0; k < 3; ++k) - { - if(proj[k] > max_coord[k]) max_coord[k] = proj[k]; - if(proj[k] < min_coord[k]) min_coord[k] = proj[k]; - } - } - - if(ps2) - { - for(int j = 0; j < 3; ++j) - { - int point_id = t[j]; - const Vector3d& p = ps2[point_id]; - Vector3d v(p[0], p[1], p[2]); - Vector3d proj = v.transpose() * axis; - - for(int k = 0; k < 3; ++k) - { - if(proj[k] > max_coord[k]) max_coord[k] = proj[k]; - if(proj[k] < min_coord[k]) min_coord[k] = proj[k]; - } - } - } - } - - Vector3d o((max_coord[0] + min_coord[0]) / 2, - (max_coord[1] + min_coord[1]) / 2, - (max_coord[2] + min_coord[2]) / 2); - - center = axis * o; - extent = (max_coord - min_coord) / 2; -} - -void getExtentAndCenter(Vector3d* ps, Vector3d* ps2, Triangle* ts, unsigned int* indices, int n, const Matrix3d& axis, Vector3d& center, Vector3d& extent) -{ - if(ts) - getExtentAndCenter_mesh(ps, ps2, ts, indices, n, axis, center, extent); - else - getExtentAndCenter_pointcloud(ps, ps2, indices, n, axis, center, extent); -} - -void circumCircleComputation(const Vector3d& a, const Vector3d& b, const Vector3d& c, Vector3d& center, FCL_REAL& radius) -{ - Vector3d e1 = a - c; - Vector3d e2 = b - c; - FCL_REAL e1_len2 = e1.squaredNorm(); - FCL_REAL e2_len2 = e2.squaredNorm(); - Vector3d e3 = e1.cross(e2); - FCL_REAL e3_len2 = e3.squaredNorm(); - radius = e1_len2 * e2_len2 * (e1 - e2).squaredNorm() / e3_len2; - radius = std::sqrt(radius) * 0.5; - - center = (e2 * e1_len2 - e1 * e2_len2).cross(e3) * (0.5 * 1 / e3_len2) + c; -} - - -static inline FCL_REAL maximumDistance_mesh(Vector3d* ps, Vector3d* ps2, Triangle* ts, unsigned int* indices, int n, const Vector3d& query) -{ - bool indirect_index = true; - if(!indices) indirect_index = false; - - FCL_REAL maxD = 0; - for(int i = 0; i < n; ++i) - { - unsigned int index = indirect_index ? indices[i] : i; - const Triangle& t = ts[index]; - - for(int j = 0; j < 3; ++j) - { - int point_id = t[j]; - const Vector3d& p = ps[point_id]; - - FCL_REAL d = (p - query).squaredNorm(); - if(d > maxD) maxD = d; - } - - if(ps2) - { - for(int j = 0; j < 3; ++j) - { - int point_id = t[j]; - const Vector3d& p = ps2[point_id]; - - FCL_REAL d = (p - query).squaredNorm(); - if(d > maxD) maxD = d; - } - } - } - - return std::sqrt(maxD); -} - -static inline FCL_REAL maximumDistance_pointcloud(Vector3d* ps, Vector3d* ps2, unsigned int* indices, int n, const Vector3d& query) -{ - bool indirect_index = true; - if(!indices) indirect_index = false; - - FCL_REAL maxD = 0; - for(int i = 0; i < n; ++i) - { - int index = indirect_index ? indices[i] : i; - - const Vector3d& p = ps[index]; - FCL_REAL d = (p - query).squaredNorm(); - if(d > maxD) maxD = d; - - if(ps2) - { - const Vector3d& v = ps2[index]; - FCL_REAL d = (v - query).squaredNorm(); - if(d > maxD) maxD = d; - } - } - - return std::sqrt(maxD); -} - -FCL_REAL maximumDistance(Vector3d* ps, Vector3d* ps2, Triangle* ts, unsigned int* indices, int n, const Vector3d& query) -{ - if(ts) - return maximumDistance_mesh(ps, ps2, ts, indices, n, query); - else - return maximumDistance_pointcloud(ps, ps2, indices, n, query); -} - - -} diff --git a/src/BVH/BV_fitter.cpp b/src/BVH/BV_fitter.cpp deleted file mode 100644 index 1e242b16d..000000000 --- a/src/BVH/BV_fitter.cpp +++ /dev/null @@ -1,658 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#include "fcl/BVH/BV_fitter.h" -#include "fcl/BVH/BVH_utility.h" -#include - -namespace fcl -{ - - -static const double kIOS_RATIO = 1.5; -static const double invSinA = 2; -static const double invCosA = 2.0 / sqrt(3.0); -// static const double sinA = 0.5; -static const double cosA = sqrt(3.0) / 2.0; - -static inline void axisFromEigen(const Matrix3d& eigenV, const Vector3d& eigenS, Matrix3d& axis) -{ - int min, mid, max; - - if(eigenS[0] > eigenS[1]) - { - max = 0; - min = 1; - } - else - { - min = 0; - max = 1; - } - - if(eigenS[2] < eigenS[min]) - { - mid = min; - min = 2; - } - else if(eigenS[2] > eigenS[max]) - { - mid = max; - max = 2; - } - else - { - mid = 2; - } - - axis.col(0) = eigenV.row(max); - axis.col(1) = eigenV.row(mid); - axis.col(2) = axis.col(0).cross(axis.col(1)); -} - -namespace OBB_fit_functions -{ - -void fit1(Vector3d* ps, OBB& bv) -{ - bv.To = ps[0]; - bv.axis.setIdentity(); - bv.extent.setConstant(0); -} - -void fit2(Vector3d* ps, OBB& bv) -{ - const Vector3d& p1 = ps[0]; - const Vector3d& p2 = ps[1]; - Vector3d p1p2 = p1 - p2; - FCL_REAL len_p1p2 = p1p2.norm(); - p1p2.normalize(); - - bv.axis.col(0) = p1p2; - generateCoordinateSystem(bv.axis); - - bv.extent << len_p1p2 * 0.5, 0, 0; - bv.To = 0.5 * (p1 + p2); -} - -void fit3(Vector3d* ps, OBB& bv) -{ - const Vector3d& p1 = ps[0]; - const Vector3d& p2 = ps[1]; - const Vector3d& p3 = ps[2]; - Vector3d e[3]; - e[0] = p1 - p2; - e[1] = p2 - p3; - e[2] = p3 - p1; - FCL_REAL len[3]; - len[0] = e[0].squaredNorm(); - len[1] = e[1].squaredNorm(); - len[2] = e[2].squaredNorm(); - - int imax = 0; - if(len[1] > len[0]) imax = 1; - if(len[2] > len[imax]) imax = 2; - - bv.axis.col(2) = e[0].cross(e[1]); - bv.axis.col(2).normalize(); - bv.axis.col(0) = e[imax]; - bv.axis.col(0).normalize(); - bv.axis.col(1) = bv.axis.col(2).cross(bv.axis.col(0)); - - getExtentAndCenter(ps, NULL, NULL, NULL, 3, bv.axis, bv.To, bv.extent); -} - -void fit6(Vector3d* ps, OBB& bv) -{ - OBB bv1, bv2; - fit3(ps, bv1); - fit3(ps + 3, bv2); - bv = bv1 + bv2; -} - - -void fitn(Vector3d* ps, int n, OBB& bv) -{ - Matrix3d M; - Matrix3d E; - Vector3d s = Vector3d::Zero(); // three eigen values - - getCovariance(ps, NULL, NULL, NULL, n, M); - eigen(M, s, E); - axisFromEigen(E, s, bv.axis); - - // set obb centers and extensions - getExtentAndCenter(ps, NULL, NULL, NULL, n, bv.axis, bv.To, bv.extent); -} - -} - - -namespace RSS_fit_functions -{ -void fit1(Vector3d* ps, RSS& bv) -{ - bv.Tr = ps[0]; - bv.axis.setIdentity(); - bv.l[0] = 0; - bv.l[1] = 0; - bv.r = 0; -} - -void fit2(Vector3d* ps, RSS& bv) -{ - const Vector3d& p1 = ps[0]; - const Vector3d& p2 = ps[1]; - Vector3d p1p2 = p1 - p2; - FCL_REAL len_p1p2 = p1p2.norm(); - p1p2.normalize(); - - bv.axis.col(0) = p1p2; - generateCoordinateSystem(bv.axis); - bv.l[0] = len_p1p2; - bv.l[1] = 0; - - bv.Tr = p2; - bv.r = 0; -} - -void fit3(Vector3d* ps, RSS& bv) -{ - const Vector3d& p1 = ps[0]; - const Vector3d& p2 = ps[1]; - const Vector3d& p3 = ps[2]; - Vector3d e[3]; - e[0] = p1 - p2; - e[1] = p2 - p3; - e[2] = p3 - p1; - FCL_REAL len[3]; - len[0] = e[0].squaredNorm(); - len[1] = e[1].squaredNorm(); - len[2] = e[2].squaredNorm(); - - int imax = 0; - if(len[1] > len[0]) imax = 1; - if(len[2] > len[imax]) imax = 2; - - bv.axis.col(2) = e[0].cross(e[1]).normalized(); - bv.axis.col(0) = e[imax].normalized(); - bv.axis.col(1) = bv.axis.col(2).cross(bv.axis.col(0)); - - getRadiusAndOriginAndRectangleSize(ps, NULL, NULL, NULL, 3, bv.axis, bv.Tr, bv.l, bv.r); -} - -void fit6(Vector3d* ps, RSS& bv) -{ - RSS bv1, bv2; - fit3(ps, bv1); - fit3(ps + 3, bv2); - bv = bv1 + bv2; -} - -void fitn(Vector3d* ps, int n, RSS& bv) -{ - Matrix3d M; // row first matrix - Matrix3d E; // row first eigen-vectors - Vector3d s = Vector3d::Zero(); - - getCovariance(ps, NULL, NULL, NULL, n, M); - eigen(M, s, E); - axisFromEigen(E, s, bv.axis); - - // set rss origin, rectangle size and radius - getRadiusAndOriginAndRectangleSize(ps, NULL, NULL, NULL, n, bv.axis, bv.Tr, bv.l, bv.r); -} - -} - -namespace kIOS_fit_functions -{ - -void fit1(Vector3d* ps, kIOS& bv) -{ - bv.num_spheres = 1; - bv.spheres[0].o = ps[0]; - bv.spheres[0].r = 0; - - bv.obb.axis.setIdentity(); - bv.obb.extent.setZero(); - bv.obb.To = ps[0]; -} - -void fit2(Vector3d* ps, kIOS& bv) -{ - bv.num_spheres = 5; - - const Vector3d& p1 = ps[0]; - const Vector3d& p2 = ps[1]; - Vector3d p1p2 = p1 - p2; - FCL_REAL len_p1p2 = p1p2.norm(); - p1p2.normalize(); - - bv.obb.axis.col(0) = p1p2; - generateCoordinateSystem(bv.obb.axis); - - FCL_REAL r0 = len_p1p2 * 0.5; - bv.obb.extent << r0, 0, 0; - bv.obb.To = (p1 + p2) * 0.5; - - bv.spheres[0].o = bv.obb.To; - bv.spheres[0].r = r0; - - FCL_REAL r1 = r0 * invSinA; - FCL_REAL r1cosA = r1 * cosA; - bv.spheres[1].r = r1; - bv.spheres[2].r = r1; - Vector3d delta = bv.obb.axis.col(1) * r1cosA; - bv.spheres[1].o = bv.spheres[0].o - delta; - bv.spheres[2].o = bv.spheres[0].o + delta; - - bv.spheres[3].r = r1; - bv.spheres[4].r = r1; - delta = bv.obb.axis.col(2) * r1cosA; - bv.spheres[3].o = bv.spheres[0].o - delta; - bv.spheres[4].o = bv.spheres[0].o + delta; -} - -void fit3(Vector3d* ps, kIOS& bv) -{ - bv.num_spheres = 3; - - const Vector3d& p1 = ps[0]; - const Vector3d& p2 = ps[1]; - const Vector3d& p3 = ps[2]; - Vector3d e[3]; - e[0] = p1 - p2; - e[1] = p2 - p3; - e[2] = p3 - p1; - FCL_REAL len[3]; - len[0] = e[0].squaredNorm(); - len[1] = e[1].squaredNorm(); - len[2] = e[2].squaredNorm(); - - int imax = 0; - if(len[1] > len[0]) imax = 1; - if(len[2] > len[imax]) imax = 2; - - bv.obb.axis.col(2) = e[0].cross(e[1]).normalized(); - bv.obb.axis.col(0) = e[imax].normalized(); - bv.obb.axis.col(1) = bv.obb.axis.col(2).cross(bv.obb.axis.col(0)); - - getExtentAndCenter(ps, NULL, NULL, NULL, 3, bv.obb.axis, bv.obb.To, bv.obb.extent); - - // compute radius and center - FCL_REAL r0; - Vector3d center; - circumCircleComputation(p1, p2, p3, center, r0); - - bv.spheres[0].o = center; - bv.spheres[0].r = r0; - - FCL_REAL r1 = r0 * invSinA; - Vector3d delta = bv.obb.axis.col(2) * (r1 * cosA); - - bv.spheres[1].r = r1; - bv.spheres[1].o = center - delta; - bv.spheres[2].r = r1; - bv.spheres[2].o = center + delta; -} - -void fitn(Vector3d* ps, int n, kIOS& bv) -{ - Matrix3d M; - Matrix3d E; - Vector3d s = Vector3d::Zero(); // three eigen values; - - getCovariance(ps, NULL, NULL, NULL, n, M); - eigen(M, s, E); - - axisFromEigen(E, s, bv.obb.axis); - - getExtentAndCenter(ps, NULL, NULL, NULL, n, bv.obb.axis, bv.obb.To, bv.obb.extent); - - // get center and extension - const Vector3d& center = bv.obb.To; - const Vector3d& extent = bv.obb.extent; - FCL_REAL r0 = maximumDistance(ps, NULL, NULL, NULL, n, center); - - // decide the k in kIOS - if(extent[0] > kIOS_RATIO * extent[2]) - { - if(extent[0] > kIOS_RATIO * extent[1]) bv.num_spheres = 5; - else bv.num_spheres = 3; - } - else bv.num_spheres = 1; - - - bv.spheres[0].o = center; - bv.spheres[0].r = r0; - - if(bv.num_spheres >= 3) - { - FCL_REAL r10 = sqrt(r0 * r0 - extent[2] * extent[2]) * invSinA; - Vector3d delta = bv.obb.axis.col(2) * (r10 * cosA - extent[2]); - bv.spheres[1].o = center - delta; - bv.spheres[2].o = center + delta; - - FCL_REAL r11 = 0, r12 = 0; - r11 = maximumDistance(ps, NULL, NULL, NULL, n, bv.spheres[1].o); - r12 = maximumDistance(ps, NULL, NULL, NULL, n, bv.spheres[2].o); - bv.spheres[1].o += bv.obb.axis.col(2) * (-r10 + r11); - bv.spheres[2].o += bv.obb.axis.col(2) * (r10 - r12); - - bv.spheres[1].r = r10; - bv.spheres[2].r = r10; - } - - if(bv.num_spheres >= 5) - { - FCL_REAL r10 = bv.spheres[1].r; - Vector3d delta = bv.obb.axis.col(1) * (sqrt(r10 * r10 - extent[0] * extent[0] - extent[2] * extent[2]) - extent[1]); - bv.spheres[3].o = bv.spheres[0].o - delta; - bv.spheres[4].o = bv.spheres[0].o + delta; - - FCL_REAL r21 = 0, r22 = 0; - r21 = maximumDistance(ps, NULL, NULL, NULL, n, bv.spheres[3].o); - r22 = maximumDistance(ps, NULL, NULL, NULL, n, bv.spheres[4].o); - - bv.spheres[3].o += bv.obb.axis.col(1) * (-r10 + r21); - bv.spheres[4].o += bv.obb.axis.col(1) * (r10 - r22); - - bv.spheres[3].r = r10; - bv.spheres[4].r = r10; - } -} - -} - -namespace OBBRSS_fit_functions -{ -void fit1(Vector3d* ps, OBBRSS& bv) -{ - OBB_fit_functions::fit1(ps, bv.obb); - RSS_fit_functions::fit1(ps, bv.rss); -} - -void fit2(Vector3d* ps, OBBRSS& bv) -{ - OBB_fit_functions::fit2(ps, bv.obb); - RSS_fit_functions::fit2(ps, bv.rss); -} - -void fit3(Vector3d* ps, OBBRSS& bv) -{ - OBB_fit_functions::fit3(ps, bv.obb); - RSS_fit_functions::fit3(ps, bv.rss); -} - -void fitn(Vector3d* ps, int n, OBBRSS& bv) -{ - OBB_fit_functions::fitn(ps, n, bv.obb); - RSS_fit_functions::fitn(ps, n, bv.rss); -} - -} - - - -template<> -void fit(Vector3d* ps, int n, OBB& bv) -{ - switch(n) - { - case 1: - OBB_fit_functions::fit1(ps, bv); - break; - case 2: - OBB_fit_functions::fit2(ps, bv); - break; - case 3: - OBB_fit_functions::fit3(ps, bv); - break; - case 6: - OBB_fit_functions::fit6(ps, bv); - break; - default: - OBB_fit_functions::fitn(ps, n, bv); - } -} - - -template<> -void fit(Vector3d* ps, int n, RSS& bv) -{ - switch(n) - { - case 1: - RSS_fit_functions::fit1(ps, bv); - break; - case 2: - RSS_fit_functions::fit2(ps, bv); - break; - case 3: - RSS_fit_functions::fit3(ps, bv); - break; - default: - RSS_fit_functions::fitn(ps, n, bv); - } -} - -template<> -void fit(Vector3d* ps, int n, kIOS& bv) -{ - switch(n) - { - case 1: - kIOS_fit_functions::fit1(ps, bv); - break; - case 2: - kIOS_fit_functions::fit2(ps, bv); - break; - case 3: - kIOS_fit_functions::fit3(ps, bv); - break; - default: - kIOS_fit_functions::fitn(ps, n, bv); - } -} - -template<> -void fit(Vector3d* ps, int n, OBBRSS& bv) -{ - switch(n) - { - case 1: - OBBRSS_fit_functions::fit1(ps, bv); - break; - case 2: - OBBRSS_fit_functions::fit2(ps, bv); - break; - case 3: - OBBRSS_fit_functions::fit3(ps, bv); - break; - default: - OBBRSS_fit_functions::fitn(ps, n, bv); - } -} - - -OBB BVFitter::fit(unsigned int* primitive_indices, int num_primitives) -{ - OBB bv; - - Matrix3d M; // row first matrix - Matrix3d E; // row first eigen-vectors - Vector3d s; // three eigen values - - getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M); - eigen(M, s, E); - - axisFromEigen(E, s, bv.axis); - - // set obb centers and extensions - getExtentAndCenter(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.axis, bv.To, bv.extent); - - return bv; -} - -OBBRSS BVFitter::fit(unsigned int* primitive_indices, int num_primitives) -{ - OBBRSS bv; - Matrix3d M; - Matrix3d E; - Vector3d s; - - getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M); - eigen(M, s, E); - - axisFromEigen(E, s, bv.obb.axis); - bv.rss.axis = bv.obb.axis; - - getExtentAndCenter(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.obb.axis, bv.obb.To, bv.obb.extent); - - Vector3d origin; - FCL_REAL l[2]; - FCL_REAL r; - getRadiusAndOriginAndRectangleSize(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.rss.axis, origin, l, r); - - bv.rss.Tr = origin; - bv.rss.l[0] = l[0]; - bv.rss.l[1] = l[1]; - bv.rss.r = r; - - return bv; -} - -RSS BVFitter::fit(unsigned int* primitive_indices, int num_primitives) -{ - RSS bv; - - Matrix3d M; // row first matrix - Matrix3d E; // row first eigen-vectors - Vector3d s; // three eigen values - getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M); - eigen(M, s, E); - axisFromEigen(E, s, bv.axis); - - // set rss origin, rectangle size and radius - - Vector3d origin; - FCL_REAL l[2]; - FCL_REAL r; - getRadiusAndOriginAndRectangleSize(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.axis, origin, l, r); - - bv.Tr = origin; - bv.l[0] = l[0]; - bv.l[1] = l[1]; - bv.r = r; - - - return bv; -} - - -kIOS BVFitter::fit(unsigned int* primitive_indices, int num_primitives) -{ - kIOS bv; - - Matrix3d M; // row first matrix - Matrix3d E; // row first eigen-vectors - Vector3d s; - - getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M); - eigen(M, s, E); - - axisFromEigen(E, s, bv.obb.axis); - - // get centers and extensions - getExtentAndCenter(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.obb.axis, bv.obb.To, bv.obb.extent); - - const Vector3d& center = bv.obb.To; - const Vector3d& extent = bv.obb.extent; - FCL_REAL r0 = maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, center); - - // decide k in kIOS - if(extent[0] > kIOS_RATIO * extent[2]) - { - if(extent[0] > kIOS_RATIO * extent[1]) bv.num_spheres = 5; - else bv.num_spheres = 3; - } - else bv.num_spheres = 1; - - bv.spheres[0].o = center; - bv.spheres[0].r = r0; - - if(bv.num_spheres >= 3) - { - FCL_REAL r10 = sqrt(r0 * r0 - extent[2] * extent[2]) * invSinA; - Vector3d delta = bv.obb.axis.col(2) * (r10 * cosA - extent[2]); - bv.spheres[1].o = center - delta; - bv.spheres[2].o = center + delta; - - FCL_REAL r11 = maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.spheres[1].o); - FCL_REAL r12 = maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.spheres[2].o); - - bv.spheres[1].o += bv.obb.axis.col(2) * (-r10 + r11); - bv.spheres[2].o += bv.obb.axis.col(2) * (r10 - r12); - - bv.spheres[1].r = r10; - bv.spheres[2].r = r10; - } - - if(bv.num_spheres >= 5) - { - FCL_REAL r10 = bv.spheres[1].r; - Vector3d delta = bv.obb.axis.col(1) * (sqrt(r10 * r10 - extent[0] * extent[0] - extent[2] * extent[2]) - extent[1]); - bv.spheres[3].o = bv.spheres[0].o - delta; - bv.spheres[4].o = bv.spheres[0].o + delta; - - FCL_REAL r21 = 0, r22 = 0; - r21 = maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.spheres[3].o); - r22 = maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.spheres[4].o); - - bv.spheres[3].o += bv.obb.axis.col(1) * (-r10 + r21); - bv.spheres[4].o += bv.obb.axis.col(1) * (r10 - r22); - - bv.spheres[3].r = r10; - bv.spheres[4].r = r10; - } - - return bv; -} - - -} diff --git a/src/BVH/BV_splitter.cpp b/src/BVH/BV_splitter.cpp deleted file mode 100644 index 8dd8864fe..000000000 --- a/src/BVH/BV_splitter.cpp +++ /dev/null @@ -1,288 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#include "fcl/BVH/BV_splitter.h" - -namespace fcl -{ - - -template -void computeSplitVector(const BV& bv, Vector3d& split_vector) -{ - split_vector = bv.axis.col(0); -} - -template<> -void computeSplitVector(const kIOS& bv, Vector3d& split_vector) -{ - /* - switch(bv.num_spheres) - { - case 1: - split_vector = Vector3d(1, 0, 0); - break; - case 3: - { - Vector3d v[3]; - v[0] = bv.spheres[1].o - bv.spheres[0].o; - v[0].normalize(); - generateCoordinateSystem(v[0], v[1], v[2]); - split_vector = v[1]; - } - break; - case 5: - { - Vector3d v[2]; - v[0] = bv.spheres[1].o - bv.spheres[0].o; - v[1] = bv.spheres[3].o - bv.spheres[0].o; - split_vector = v[0].cross(v[1]); - split_vector.normalize(); - } - break; - default: - ; - } - */ - split_vector = bv.obb.axis.col(0); -} - -template<> -void computeSplitVector(const OBBRSS& bv, Vector3d& split_vector) -{ - split_vector = bv.obb.axis.col(0); -} - -template -void computeSplitValue_bvcenter(const BV& bv, FCL_REAL& split_value) -{ - Vector3d center = bv.center(); - split_value = center[0]; -} - -template -void computeSplitValue_mean(const BV& bv, Vector3d* vertices, Triangle* triangles, unsigned int* primitive_indices, int num_primitives, BVHModelType type, const Vector3d& split_vector, FCL_REAL& split_value) -{ - FCL_REAL sum = 0.0; - if(type == BVH_MODEL_TRIANGLES) - { - FCL_REAL c[3] = {0.0, 0.0, 0.0}; - - for(int i = 0; i < num_primitives; ++i) - { - const Triangle& t = triangles[primitive_indices[i]]; - const Vector3d& p1 = vertices[t[0]]; - const Vector3d& p2 = vertices[t[1]]; - const Vector3d& p3 = vertices[t[2]]; - - c[0] += (p1[0] + p2[0] + p3[0]); - c[1] += (p1[1] + p2[1] + p3[1]); - c[2] += (p1[2] + p2[2] + p3[2]); - } - split_value = (c[0] * split_vector[0] + c[1] * split_vector[1] + c[2] * split_vector[2]) / (3 * num_primitives); - } - else if(type == BVH_MODEL_POINTCLOUD) - { - for(int i = 0; i < num_primitives; ++i) - { - const Vector3d& p = vertices[primitive_indices[i]]; - Vector3d v(p[0], p[1], p[2]); - sum += v.dot(split_vector); - } - - split_value = sum / num_primitives; - } -} - -template -void computeSplitValue_median(const BV& bv, Vector3d* vertices, Triangle* triangles, unsigned int* primitive_indices, int num_primitives, BVHModelType type, const Vector3d& split_vector, FCL_REAL& split_value) -{ - std::vector proj(num_primitives); - - if(type == BVH_MODEL_TRIANGLES) - { - for(int i = 0; i < num_primitives; ++i) - { - const Triangle& t = triangles[primitive_indices[i]]; - const Vector3d& p1 = vertices[t[0]]; - const Vector3d& p2 = vertices[t[1]]; - const Vector3d& p3 = vertices[t[2]]; - Vector3d centroid3(p1[0] + p2[0] + p3[0], - p1[1] + p2[1] + p3[1], - p1[2] + p2[2] + p3[2]); - - proj[i] = centroid3.dot(split_vector) / 3; - } - } - else if(type == BVH_MODEL_POINTCLOUD) - { - for(int i = 0; i < num_primitives; ++i) - { - const Vector3d& p = vertices[primitive_indices[i]]; - Vector3d v(p[0], p[1], p[2]); - proj[i] = v.dot(split_vector); - } - } - - std::sort(proj.begin(), proj.end()); - - if(num_primitives % 2 == 1) - { - split_value = proj[(num_primitives - 1) / 2]; - } - else - { - split_value = (proj[num_primitives / 2] + proj[num_primitives / 2 - 1]) / 2; - } -} - -template<> -void BVSplitter::computeRule_bvcenter(const OBB& bv, unsigned int* primitive_indices, int num_primitives) -{ - computeSplitVector(bv, split_vector); - computeSplitValue_bvcenter(bv, split_value); -} - -template<> -void BVSplitter::computeRule_mean(const OBB& bv, unsigned int* primitive_indices, int num_primitives) -{ - computeSplitVector(bv, split_vector); - computeSplitValue_mean(bv, vertices, tri_indices, primitive_indices, num_primitives, type, split_vector, split_value); -} - -template<> -void BVSplitter::computeRule_median(const OBB& bv, unsigned int* primitive_indices, int num_primitives) -{ - computeSplitVector(bv, split_vector); - computeSplitValue_median(bv, vertices, tri_indices, primitive_indices, num_primitives, type, split_vector, split_value); -} - -template<> -void BVSplitter::computeRule_bvcenter(const RSS& bv, unsigned int* primitive_indices, int num_primitives) -{ - computeSplitVector(bv, split_vector); - computeSplitValue_bvcenter(bv, split_value); -} - -template<> -void BVSplitter::computeRule_mean(const RSS& bv, unsigned int* primitive_indices, int num_primitives) -{ - computeSplitVector(bv, split_vector); - computeSplitValue_mean(bv, vertices, tri_indices, primitive_indices, num_primitives, type, split_vector, split_value); -} - -template<> -void BVSplitter::computeRule_median(const RSS& bv, unsigned int* primitive_indices, int num_primitives) -{ - computeSplitVector(bv, split_vector); - computeSplitValue_median(bv, vertices, tri_indices, primitive_indices, num_primitives, type, split_vector, split_value); -} - -template<> -void BVSplitter::computeRule_bvcenter(const kIOS& bv, unsigned int* primitive_indices, int num_primitives) -{ - computeSplitVector(bv, split_vector); - computeSplitValue_bvcenter(bv, split_value); -} - -template<> -void BVSplitter::computeRule_mean(const kIOS& bv, unsigned int* primitive_indices, int num_primitives) -{ - computeSplitVector(bv, split_vector); - computeSplitValue_mean(bv, vertices, tri_indices, primitive_indices, num_primitives, type, split_vector, split_value); -} - -template<> -void BVSplitter::computeRule_median(const kIOS& bv, unsigned int* primitive_indices, int num_primitives) -{ - computeSplitVector(bv, split_vector); - computeSplitValue_median(bv, vertices, tri_indices, primitive_indices, num_primitives, type, split_vector, split_value); -} - -template<> -void BVSplitter::computeRule_bvcenter(const OBBRSS& bv, unsigned int* primitive_indices, int num_primitives) -{ - computeSplitVector(bv, split_vector); - computeSplitValue_bvcenter(bv, split_value); -} - -template<> -void BVSplitter::computeRule_mean(const OBBRSS& bv, unsigned int* primitive_indices, int num_primitives) -{ - computeSplitVector(bv, split_vector); - computeSplitValue_mean(bv, vertices, tri_indices, primitive_indices, num_primitives, type, split_vector, split_value); -} - -template<> -void BVSplitter::computeRule_median(const OBBRSS& bv, unsigned int* primitive_indices, int num_primitives) -{ - computeSplitVector(bv, split_vector); - computeSplitValue_median(bv, vertices, tri_indices, primitive_indices, num_primitives, type, split_vector, split_value); -} - - -template<> -bool BVSplitter::apply(const Vector3d& q) const -{ - return split_vector.dot(q) > split_value; -} - -template<> -bool BVSplitter::apply(const Vector3d& q) const -{ - return split_vector.dot(q) > split_value; -} - -template<> -bool BVSplitter::apply(const Vector3d& q) const -{ - return split_vector.dot(q) > split_value; -} - -template<> -bool BVSplitter::apply(const Vector3d& q) const -{ - return split_vector.dot(q) > split_value; -} - - -template class BVSplitter; -template class BVSplitter; -template class BVSplitter; -template class BVSplitter; - -} diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index a1c382baf..2201e214b 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -11,7 +11,7 @@ set_target_properties(${PROJECT_NAME} PROPERTIES target_link_libraries(${PROJECT_NAME} PUBLIC ${OCTOMAP_LIBRARIES} - PRIVATE ${CCD_LIBRARIES}) + PUBLIC ${CCD_LIBRARIES}) target_include_directories(${PROJECT_NAME} INTERFACE $ diff --git a/src/articulated_model/joint.cpp b/src/articulated_model/joint.cpp deleted file mode 100644 index 578d098b0..000000000 --- a/src/articulated_model/joint.cpp +++ /dev/null @@ -1,205 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Dalibor Matura, Jia Pan */ - -#include "fcl/articulated_model/joint.h" -#include "fcl/articulated_model/link.h" -#include "fcl/articulated_model/joint_config.h" - -namespace fcl -{ - -Joint::Joint(const std::shared_ptr& link_parent, const std::shared_ptr& link_child, - const Transform3d& transform_to_parent, - const std::string& name) : - link_parent_(link_parent), link_child_(link_child), - name_(name), - transform_to_parent_(transform_to_parent) -{} - -Joint::Joint(const std::string& name) : - name_(name) -{ -} - -const std::string& Joint::getName() const -{ - return name_; -} - -void Joint::setName(const std::string& name) -{ - name_ = name; -} - -std::shared_ptr Joint::getJointConfig() const -{ - return joint_cfg_; -} - -void Joint::setJointConfig(const std::shared_ptr& joint_cfg) -{ - joint_cfg_ = joint_cfg; -} - -std::shared_ptr Joint::getParentLink() const -{ - return link_parent_.lock(); -} - -std::shared_ptr Joint::getChildLink() const -{ - return link_child_.lock(); -} - -void Joint::setParentLink(const std::shared_ptr& link) -{ - link_parent_ = link; -} - -void Joint::setChildLink(const std::shared_ptr& link) -{ - link_child_ = link; -} - -JointType Joint::getJointType() const -{ - return type_; -} - -const Transform3d& Joint::getTransformToParent() const -{ - return transform_to_parent_; -} - -void Joint::setTransformToParent(const Transform3d& t) -{ - transform_to_parent_ = t; -} - - -PrismaticJoint::PrismaticJoint(const std::shared_ptr& link_parent, const std::shared_ptr& link_child, - const Transform3d& transform_to_parent, - const std::string& name, - const Vector3d& axis) : - Joint(link_parent, link_child, transform_to_parent, name), - axis_(axis) -{ - type_ = JT_PRISMATIC; -} - -const Vector3d& PrismaticJoint::getAxis() const -{ - return axis_; -} - -std::size_t PrismaticJoint::getNumDofs() const -{ - return 1; -} - -Transform3d PrismaticJoint::getLocalTransform() const -{ - const Quaternion3d quat(transform_to_parent_.linear()); - const Vector3d& transl = transform_to_parent_.translation(); - - Transform3d tf = Transform3d::Identity(); - tf.linear() = quat.toRotationMatrix(); - tf.translation() = quat * (axis_ * (*joint_cfg_)[0]) + transl; - - return tf; -} - - -RevoluteJoint::RevoluteJoint(const std::shared_ptr& link_parent, const std::shared_ptr& link_child, - const Transform3d& transform_to_parent, - const std::string& name, - const Vector3d& axis) : - Joint(link_parent, link_child, transform_to_parent, name), - axis_(axis) -{ - type_ = JT_REVOLUTE; -} - -const Vector3d& RevoluteJoint::getAxis() const -{ - return axis_; -} - -std::size_t RevoluteJoint::getNumDofs() const -{ - return 1; -} - -Transform3d RevoluteJoint::getLocalTransform() const -{ - Transform3d tf = Transform3d::Identity(); - tf.linear() = transform_to_parent_.linear() * Eigen::AngleAxisd((*joint_cfg_)[0], axis_); - tf.translation() = transform_to_parent_.translation(); - - return tf; -} - - -BallEulerJoint::BallEulerJoint(const std::shared_ptr& link_parent, const std::shared_ptr& link_child, - const Transform3d& transform_to_parent, - const std::string& name) : - Joint(link_parent, link_child, transform_to_parent, name) -{} - -std::size_t BallEulerJoint::getNumDofs() const -{ - return 3; -} - -Transform3d BallEulerJoint::getLocalTransform() const -{ - Matrix3d rot( - Eigen::AngleAxisd((*joint_cfg_)[0], Eigen::Vector3d::UnitX()) - * Eigen::AngleAxisd((*joint_cfg_)[1], Eigen::Vector3d::UnitY()) - * Eigen::AngleAxisd((*joint_cfg_)[2], Eigen::Vector3d::UnitZ())); - - Transform3d tf = Transform3d::Identity(); - tf.linear() = rot; - - return transform_to_parent_ * tf; -} - - - - - -} diff --git a/src/articulated_model/model.cpp b/src/articulated_model/model.cpp deleted file mode 100644 index 840952651..000000000 --- a/src/articulated_model/model.cpp +++ /dev/null @@ -1,158 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Dalibor Matura, Jia Pan */ - -#include "fcl/articulated_model/model.h" -#include "fcl/articulated_model/model_config.h" - -namespace fcl -{ - - -std::shared_ptr Model::getRoot() const -{ - return root_link_; -} - -std::shared_ptr Model::getLink(const std::string& name) const -{ - std::shared_ptr ptr; - std::map >::const_iterator it = links_.find(name); - if(it == links_.end()) - ptr.reset(); - else - ptr = it->second; - return ptr; -} - -std::shared_ptr Model::getJoint(const std::string& name) const -{ - std::shared_ptr ptr; - std::map >::const_iterator it = joints_.find(name); - if(it == joints_.end()) - ptr.reset(); - else - ptr = it->second; - return ptr; -} - -const std::string& Model::getName() const -{ - return name_; -} - -std::vector > Model::getLinks() const -{ - std::vector > links; - for(std::map >::const_iterator it = links_.begin(); it != links_.end(); ++it) - { - links.push_back(it->second); - } - - return links; -} - -std::size_t Model::getNumLinks() const -{ - return links_.size(); -} - -std::size_t Model::getNumJoints() const -{ - return joints_.size(); -} - -std::size_t Model::getNumDofs() const -{ - std::size_t dof = 0; - for(std::map >::const_iterator it = joints_.begin(); it != joints_.end(); ++it) - { - dof += it->second->getNumDofs(); - } - - return dof; -} - -void Model::addLink(const std::shared_ptr& link) -{ - links_[link->getName()] = link; -} - -void Model::addJoint(const std::shared_ptr& joint) -{ - joints_[joint->getName()] = joint; -} - -void Model::initRoot(const std::map& link_parent_tree) -{ - root_link_.reset(); - - /// find the links that have no parent in the tree - for(std::map >::const_iterator it = links_.begin(); it != links_.end(); ++it) - { - std::map::const_iterator parent = link_parent_tree.find(it->first); - if(parent == link_parent_tree.end()) - { - if(!root_link_) - { - root_link_ = getLink(it->first); - } - else - { - throw ModelParseError("Two root links found: [" + root_link_->getName() + "] and [" + it->first + "]"); - } - } - } - - if(!root_link_) - throw ModelParseError("No root link found."); -} - -void Model::initTree(std::map& link_parent_tree) -{ - for(std::map >::iterator it = joints_.begin(); it != joints_.end(); ++it) - { - std::string parent_link_name = it->second->getParentLink()->getName(); - std::string child_link_name = it->second->getChildLink()->getName(); - - it->second->getParentLink()->addChildJoint(it->second); - it->second->getChildLink()->setParentJoint(it->second); - - link_parent_tree[child_link_name] = parent_link_name; - } -} - -} diff --git a/src/articulated_model/model_config.cpp b/src/articulated_model/model_config.cpp deleted file mode 100644 index 1c246f301..000000000 --- a/src/articulated_model/model_config.cpp +++ /dev/null @@ -1,86 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Dalibor Matura, Jia Pan */ - -#include "fcl/articulated_model/model_config.h" -#include "fcl/articulated_model/joint.h" -#include -#include - -namespace fcl -{ - -ModelConfig::ModelConfig() {} - -ModelConfig::ModelConfig(const ModelConfig& model_cfg) : - joint_cfgs_map_(model_cfg.joint_cfgs_map_) -{} - -ModelConfig::ModelConfig(std::map > joints_map) -{ - std::map >::iterator it; - for(it = joints_map.begin(); it != joints_map.end(); ++it) - joint_cfgs_map_[it->first] = JointConfig(it->second); -} - -JointConfig ModelConfig::getJointConfigByJointName(const std::string& joint_name) const -{ - std::map::const_iterator it = joint_cfgs_map_.find(joint_name); - assert(it != joint_cfgs_map_.end()); - - return it->second; -} - -JointConfig& ModelConfig::getJointConfigByJointName(const std::string& joint_name) -{ - std::map::iterator it = joint_cfgs_map_.find(joint_name); - assert(it != joint_cfgs_map_.end()); - - return it->second; -} - -JointConfig ModelConfig::getJointConfigByJoint(std::shared_ptr joint) const -{ - return getJointConfigByJointName(joint->getName()); -} - -JointConfig& ModelConfig::getJointConfigByJoint(std::shared_ptr joint) -{ - return getJointConfigByJointName(joint->getName()); -} - - -} diff --git a/src/broadphase/broadphase_SSaP.cpp b/src/broadphase/broadphase_SSaP.cpp deleted file mode 100644 index 78bb66589..000000000 --- a/src/broadphase/broadphase_SSaP.cpp +++ /dev/null @@ -1,505 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#include "fcl/broadphase/broadphase_SSaP.h" -#include -#include - -namespace fcl -{ - -/** \brief Functor sorting objects according to the AABB lower x bound */ -struct SortByXLow -{ - bool operator()(const CollisionObject* a, const CollisionObject* b) const - { - if(a->getAABB().min_[0] < b->getAABB().min_[0]) - return true; - return false; - } -}; - -/** \brief Functor sorting objects according to the AABB lower y bound */ -struct SortByYLow -{ - bool operator()(const CollisionObject* a, const CollisionObject* b) const - { - if(a->getAABB().min_[1] < b->getAABB().min_[1]) - return true; - return false; - } -}; - -/** \brief Functor sorting objects according to the AABB lower z bound */ -struct SortByZLow -{ - bool operator()(const CollisionObject* a, const CollisionObject* b) const - { - if(a->getAABB().min_[2] < b->getAABB().min_[2]) - return true; - return false; - } -}; - -/** \brief Dummy collision object with a point AABB */ -class DummyCollisionObject : public CollisionObject -{ -public: - DummyCollisionObject(const AABB& aabb_) : CollisionObject(std::shared_ptr()) - { - aabb = aabb_; - } - - void computeLocalAABB() {} -}; - - -void SSaPCollisionManager::unregisterObject(CollisionObject* obj) -{ - setup(); - - DummyCollisionObject dummyHigh(AABB(obj->getAABB().max_)); - - std::vector::iterator pos_start1 = objs_x.begin(); - std::vector::iterator pos_end1 = std::upper_bound(pos_start1, objs_x.end(), &dummyHigh, SortByXLow()); - - while(pos_start1 < pos_end1) - { - if(*pos_start1 == obj) - { - objs_x.erase(pos_start1); - break; - } - ++pos_start1; - } - - std::vector::iterator pos_start2 = objs_y.begin(); - std::vector::iterator pos_end2 = std::upper_bound(pos_start2, objs_y.end(), &dummyHigh, SortByYLow()); - - while(pos_start2 < pos_end2) - { - if(*pos_start2 == obj) - { - objs_y.erase(pos_start2); - break; - } - ++pos_start2; - } - - std::vector::iterator pos_start3 = objs_z.begin(); - std::vector::iterator pos_end3 = std::upper_bound(pos_start3, objs_z.end(), &dummyHigh, SortByZLow()); - - while(pos_start3 < pos_end3) - { - if(*pos_start3 == obj) - { - objs_z.erase(pos_start3); - break; - } - ++pos_start3; - } -} - - -void SSaPCollisionManager::registerObject(CollisionObject* obj) -{ - objs_x.push_back(obj); - objs_y.push_back(obj); - objs_z.push_back(obj); - setup_ = false; -} - -void SSaPCollisionManager::setup() -{ - if(!setup_) - { - std::sort(objs_x.begin(), objs_x.end(), SortByXLow()); - std::sort(objs_y.begin(), objs_y.end(), SortByYLow()); - std::sort(objs_z.begin(), objs_z.end(), SortByZLow()); - setup_ = true; - } -} - -void SSaPCollisionManager::update() -{ - setup_ = false; - setup(); -} - -void SSaPCollisionManager::clear() -{ - objs_x.clear(); - objs_y.clear(); - objs_z.clear(); - setup_ = false; -} - -void SSaPCollisionManager::getObjects(std::vector& objs) const -{ - objs.resize(objs_x.size()); - std::copy(objs_x.begin(), objs_x.end(), objs.begin()); -} - -bool SSaPCollisionManager::checkColl(std::vector::const_iterator pos_start, std::vector::const_iterator pos_end, - CollisionObject* obj, void* cdata, CollisionCallBack callback) const -{ - while(pos_start < pos_end) - { - if(*pos_start != obj) // no collision between the same object - { - if((*pos_start)->getAABB().overlap(obj->getAABB())) - { - if(callback(*pos_start, obj, cdata)) - return true; - } - } - pos_start++; - } - return false; -} - -bool SSaPCollisionManager::checkDis(std::vector::const_iterator pos_start, std::vector::const_iterator pos_end, CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const -{ - while(pos_start < pos_end) - { - if(*pos_start != obj) // no distance between the same object - { - if((*pos_start)->getAABB().distance(obj->getAABB()) < min_dist) - { - if(callback(*pos_start, obj, cdata, min_dist)) - return true; - } - } - pos_start++; - } - - return false; -} - - - -void SSaPCollisionManager::collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const -{ - if(size() == 0) return; - - collide_(obj, cdata, callback); -} - -bool SSaPCollisionManager::collide_(CollisionObject* obj, void* cdata, CollisionCallBack callback) const -{ - static const unsigned int CUTOFF = 100; - - DummyCollisionObject dummyHigh(AABB(obj->getAABB().max_)); - bool coll_res = false; - - std::vector::const_iterator pos_start1 = objs_x.begin(); - std::vector::const_iterator pos_end1 = std::upper_bound(pos_start1, objs_x.end(), &dummyHigh, SortByXLow()); - unsigned int d1 = pos_end1 - pos_start1; - - if(d1 > CUTOFF) - { - std::vector::const_iterator pos_start2 = objs_y.begin(); - std::vector::const_iterator pos_end2 = std::upper_bound(pos_start2, objs_y.end(), &dummyHigh, SortByYLow()); - unsigned int d2 = pos_end2 - pos_start2; - - if(d2 > CUTOFF) - { - std::vector::const_iterator pos_start3 = objs_z.begin(); - std::vector::const_iterator pos_end3 = std::upper_bound(pos_start3, objs_z.end(), &dummyHigh, SortByZLow()); - unsigned int d3 = pos_end3 - pos_start3; - - if(d3 > CUTOFF) - { - if(d3 <= d2 && d3 <= d1) - coll_res = checkColl(pos_start3, pos_end3, obj, cdata, callback); - else - { - if(d2 <= d3 && d2 <= d1) - coll_res = checkColl(pos_start2, pos_end2, obj, cdata, callback); - else - coll_res = checkColl(pos_start1, pos_end1, obj, cdata, callback); - } - } - else - coll_res = checkColl(pos_start3, pos_end3, obj, cdata, callback); - } - else - coll_res = checkColl(pos_start2, pos_end2, obj, cdata, callback); - } - else - coll_res = checkColl(pos_start1, pos_end1, obj, cdata, callback); - - return coll_res; -} - - -void SSaPCollisionManager::distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const -{ - if(size() == 0) return; - - FCL_REAL min_dist = std::numeric_limits::max(); - distance_(obj, cdata, callback, min_dist); -} - -bool SSaPCollisionManager::distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const -{ - static const unsigned int CUTOFF = 100; - Vector3d delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5; - Vector3d dummy_vector = obj->getAABB().max_; - if(min_dist < std::numeric_limits::max()) - dummy_vector += Vector3d(min_dist, min_dist, min_dist); - - std::vector::const_iterator pos_start1 = objs_x.begin(); - std::vector::const_iterator pos_start2 = objs_y.begin(); - std::vector::const_iterator pos_start3 = objs_z.begin(); - std::vector::const_iterator pos_end1 = objs_x.end(); - std::vector::const_iterator pos_end2 = objs_y.end(); - std::vector::const_iterator pos_end3 = objs_z.end(); - - int status = 1; - FCL_REAL old_min_distance; - - while(1) - { - old_min_distance = min_dist; - DummyCollisionObject dummyHigh((AABB(dummy_vector))); - - pos_end1 = std::upper_bound(pos_start1, objs_x.end(), &dummyHigh, SortByXLow()); - unsigned int d1 = pos_end1 - pos_start1; - - bool dist_res = false; - - if(d1 > CUTOFF) - { - pos_end2 = std::upper_bound(pos_start2, objs_y.end(), &dummyHigh, SortByYLow()); - unsigned int d2 = pos_end2 - pos_start2; - - if(d2 > CUTOFF) - { - pos_end3 = std::upper_bound(pos_start3, objs_z.end(), &dummyHigh, SortByZLow()); - unsigned int d3 = pos_end3 - pos_start3; - - if(d3 > CUTOFF) - { - if(d3 <= d2 && d3 <= d1) - dist_res = checkDis(pos_start3, pos_end3, obj, cdata, callback, min_dist); - else - { - if(d2 <= d3 && d2 <= d1) - dist_res = checkDis(pos_start2, pos_end2, obj, cdata, callback, min_dist); - else - dist_res = checkDis(pos_start1, pos_end1, obj, cdata, callback, min_dist); - } - } - else - dist_res = checkDis(pos_start3, pos_end3, obj, cdata, callback, min_dist); - } - else - dist_res = checkDis(pos_start2, pos_end2, obj, cdata, callback, min_dist); - } - else - { - dist_res = checkDis(pos_start1, pos_end1, obj, cdata, callback, min_dist); - } - - if(dist_res) return true; - - if(status == 1) - { - if(old_min_distance < std::numeric_limits::max()) - break; - else - { - // from infinity to a finite one, only need one additional loop - // to check the possible missed ones to the right of the objs array - if(min_dist < old_min_distance) - { - dummy_vector = obj->getAABB().max_ + Vector3d(min_dist, min_dist, min_dist); - status = 0; - } - else // need more loop - { - if(dummy_vector.isApprox(obj->getAABB().max_, std::numeric_limits::epsilon() * 100)) - dummy_vector = dummy_vector + delta; - else - dummy_vector = dummy_vector * 2 - obj->getAABB().max_; - } - } - - // yes, following is wrong, will result in too large distance. - // if(pos_end1 != objs_x.end()) pos_start1 = pos_end1; - // if(pos_end2 != objs_y.end()) pos_start2 = pos_end2; - // if(pos_end3 != objs_z.end()) pos_start3 = pos_end3; - } - else if(status == 0) - break; - } - - return false; -} - -void SSaPCollisionManager::collide(void* cdata, CollisionCallBack callback) const -{ - if(size() == 0) return; - - std::vector::const_iterator pos, run_pos, pos_end; - size_t axis = selectOptimalAxis(objs_x, objs_y, objs_z, - pos, pos_end); - size_t axis2 = (axis + 1 > 2) ? 0 : (axis + 1); - size_t axis3 = (axis2 + 1 > 2) ? 0 : (axis2 + 1); - - run_pos = pos; - - while((run_pos < pos_end) && (pos < pos_end)) - { - CollisionObject* obj = *(pos++); - - while(1) - { - if((*run_pos)->getAABB().min_[axis] < obj->getAABB().min_[axis]) - { - run_pos++; - if(run_pos == pos_end) break; - continue; - } - else - { - run_pos++; - break; - } - } - - if(run_pos < pos_end) - { - std::vector::const_iterator run_pos2 = run_pos; - - while((*run_pos2)->getAABB().min_[axis] <= obj->getAABB().max_[axis]) - { - CollisionObject* obj2 = *run_pos2; - run_pos2++; - - if((obj->getAABB().max_[axis2] >= obj2->getAABB().min_[axis2]) && (obj2->getAABB().max_[axis2] >= obj->getAABB().min_[axis2])) - { - if((obj->getAABB().max_[axis3] >= obj2->getAABB().min_[axis3]) && (obj2->getAABB().max_[axis3] >= obj->getAABB().min_[axis3])) - { - if(callback(obj, obj2, cdata)) - return; - } - } - - if(run_pos2 == pos_end) break; - } - } - } -} - - -void SSaPCollisionManager::distance(void* cdata, DistanceCallBack callback) const -{ - if(size() == 0) return; - - std::vector::const_iterator it, it_end; - selectOptimalAxis(objs_x, objs_y, objs_z, it, it_end); - - FCL_REAL min_dist = std::numeric_limits::max(); - for(; it != it_end; ++it) - { - if(distance_(*it, cdata, callback, min_dist)) - return; - } -} - -void SSaPCollisionManager::collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const -{ - SSaPCollisionManager* other_manager = static_cast(other_manager_); - - if((size() == 0) || (other_manager->size() == 0)) return; - - if(this == other_manager) - { - collide(cdata, callback); - return; - } - - - std::vector::const_iterator it, end; - if(this->size() < other_manager->size()) - { - for(it = objs_x.begin(), end = objs_x.end(); it != end; ++it) - if(other_manager->collide_(*it, cdata, callback)) return; - } - else - { - for(it = other_manager->objs_x.begin(), end = other_manager->objs_x.end(); it != end; ++it) - if(collide_(*it, cdata, callback)) return; - } -} - -void SSaPCollisionManager::distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const -{ - SSaPCollisionManager* other_manager = static_cast(other_manager_); - - if((size() == 0) || (other_manager->size() == 0)) return; - - if(this == other_manager) - { - distance(cdata, callback); - return; - } - - FCL_REAL min_dist = std::numeric_limits::max(); - std::vector::const_iterator it, end; - if(this->size() < other_manager->size()) - { - for(it = objs_x.begin(), end = objs_x.end(); it != end; ++it) - if(other_manager->distance_(*it, cdata, callback, min_dist)) return; - } - else - { - for(it = other_manager->objs_x.begin(), end = other_manager->objs_x.end(); it != end; ++it) - if(distance_(*it, cdata, callback, min_dist)) return; - } -} - -bool SSaPCollisionManager::empty() const -{ - return objs_x.empty(); -} - - - -} diff --git a/src/broadphase/broadphase_SaP.cpp b/src/broadphase/broadphase_SaP.cpp deleted file mode 100644 index ba904d531..000000000 --- a/src/broadphase/broadphase_SaP.cpp +++ /dev/null @@ -1,767 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#include "fcl/broadphase/broadphase_SaP.h" -#include -#include -#include - -namespace fcl -{ - -void SaPCollisionManager::unregisterObject(CollisionObject* obj) -{ - std::list::iterator it = AABB_arr.begin(); - for(std::list::iterator end = AABB_arr.end(); it != end; ++it) - { - if((*it)->obj == obj) - break; - } - - AABB_arr.erase(it); - obj_aabb_map.erase(obj); - - if(it == AABB_arr.end()) - return; - - SaPAABB* curr = *it; - *it = NULL; - - for(int coord = 0; coord < 3; ++coord) - { - //first delete the lo endpoint of the interval. - if(curr->lo->prev[coord] == NULL) - elist[coord] = curr->lo->next[coord]; - else - curr->lo->prev[coord]->next[coord] = curr->lo->next[coord]; - - curr->lo->next[coord]->prev[coord] = curr->lo->prev[coord]; - - //then, delete the "hi" endpoint. - if(curr->hi->prev[coord] == NULL) - elist[coord] = curr->hi->next[coord]; - else - curr->hi->prev[coord]->next[coord] = curr->hi->next[coord]; - - if(curr->hi->next[coord] != NULL) - curr->hi->next[coord]->prev[coord] = curr->hi->prev[coord]; - } - - delete curr->lo; - delete curr->hi; - delete curr; - - overlap_pairs.remove_if(isUnregistered(obj)); -} - -void SaPCollisionManager::registerObjects(const std::vector& other_objs) -{ - if(other_objs.empty()) return; - - if(size() > 0) - BroadPhaseCollisionManager::registerObjects(other_objs); - else - { - std::vector endpoints(2 * other_objs.size()); - - for(size_t i = 0; i < other_objs.size(); ++i) - { - SaPAABB* sapaabb = new SaPAABB(); - sapaabb->obj = other_objs[i]; - sapaabb->lo = new EndPoint(); - sapaabb->hi = new EndPoint(); - sapaabb->cached = other_objs[i]->getAABB(); - endpoints[2 * i] = sapaabb->lo; - endpoints[2 * i + 1] = sapaabb->hi; - sapaabb->lo->minmax = 0; - sapaabb->hi->minmax = 1; - sapaabb->lo->aabb = sapaabb; - sapaabb->hi->aabb = sapaabb; - AABB_arr.push_back(sapaabb); - obj_aabb_map[other_objs[i]] = sapaabb; - } - - - FCL_REAL scale[3]; - for(size_t coord = 0; coord < 3; ++coord) - { - std::sort(endpoints.begin(), endpoints.end(), - std::bind(std::less(), - std::bind(static_cast(&EndPoint::getVal), std::placeholders::_1, coord), - std::bind(static_cast(&EndPoint::getVal), std::placeholders::_2, coord))); - - endpoints[0]->prev[coord] = NULL; - endpoints[0]->next[coord] = endpoints[1]; - for(size_t i = 1; i < endpoints.size() - 1; ++i) - { - endpoints[i]->prev[coord] = endpoints[i-1]; - endpoints[i]->next[coord] = endpoints[i+1]; - } - endpoints[endpoints.size() - 1]->prev[coord] = endpoints[endpoints.size() - 2]; - endpoints[endpoints.size() - 1]->next[coord] = NULL; - - elist[coord] = endpoints[0]; - - scale[coord] = endpoints.back()->aabb->cached.max_[coord] - endpoints[0]->aabb->cached.min_[coord]; - } - - int axis = 0; - if(scale[axis] < scale[1]) axis = 1; - if(scale[axis] < scale[2]) axis = 2; - - EndPoint* pos = elist[axis]; - - while(pos != NULL) - { - EndPoint* pos_next = NULL; - SaPAABB* aabb = pos->aabb; - EndPoint* pos_it = pos->next[axis]; - - while(pos_it != NULL) - { - if(pos_it->aabb == aabb) - { - if(pos_next == NULL) pos_next = pos_it; - break; - } - - if(pos_it->minmax == 0) - { - if(pos_next == NULL) pos_next = pos_it; - if(pos_it->aabb->cached.overlap(aabb->cached)) - overlap_pairs.push_back(SaPPair(pos_it->aabb->obj, aabb->obj)); - } - pos_it = pos_it->next[axis]; - } - - pos = pos_next; - } - } - - updateVelist(); -} - -void SaPCollisionManager::registerObject(CollisionObject* obj) -{ - SaPAABB* curr = new SaPAABB; - curr->cached = obj->getAABB(); - curr->obj = obj; - curr->lo = new EndPoint; - curr->lo->minmax = 0; - curr->lo->aabb = curr; - - curr->hi = new EndPoint; - curr->hi->minmax = 1; - curr->hi->aabb = curr; - - for(int coord = 0; coord < 3; ++coord) - { - EndPoint* current = elist[coord]; - - // first insert the lo end point - if(current == NULL) // empty list - { - elist[coord] = curr->lo; - curr->lo->prev[coord] = curr->lo->next[coord] = NULL; - } - else // otherwise, find the correct location in the list and insert - { - EndPoint* curr_lo = curr->lo; - FCL_REAL curr_lo_val = curr_lo->getVal()[coord]; - while((current->getVal()[coord] < curr_lo_val) && (current->next[coord] != NULL)) - current = current->next[coord]; - - if(current->getVal()[coord] >= curr_lo_val) - { - curr_lo->prev[coord] = current->prev[coord]; - curr_lo->next[coord] = current; - if(current->prev[coord] == NULL) - elist[coord] = curr_lo; - else - current->prev[coord]->next[coord] = curr_lo; - - current->prev[coord] = curr_lo; - } - else - { - curr_lo->prev[coord] = current; - curr_lo->next[coord] = NULL; - current->next[coord] = curr_lo; - } - } - - // now insert hi end point - current = curr->lo; - - EndPoint* curr_hi = curr->hi; - FCL_REAL curr_hi_val = curr_hi->getVal()[coord]; - - if(coord == 0) - { - while((current->getVal()[coord] < curr_hi_val) && (current->next[coord] != NULL)) - { - if(current != curr->lo) - if(current->aabb->cached.overlap(curr->cached)) - overlap_pairs.push_back(SaPPair(current->aabb->obj, obj)); - - current = current->next[coord]; - } - } - else - { - while((current->getVal()[coord] < curr_hi_val) && (current->next[coord] != NULL)) - current = current->next[coord]; - } - - if(current->getVal()[coord] >= curr_hi_val) - { - curr_hi->prev[coord] = current->prev[coord]; - curr_hi->next[coord] = current; - if(current->prev[coord] == NULL) - elist[coord] = curr_hi; - else - current->prev[coord]->next[coord] = curr_hi; - - current->prev[coord] = curr_hi; - } - else - { - curr_hi->prev[coord] = current; - curr_hi->next[coord] = NULL; - current->next[coord] = curr_hi; - } - } - - AABB_arr.push_back(curr); - - obj_aabb_map[obj] = curr; - - updateVelist(); -} - -void SaPCollisionManager::setup() -{ - if(size() == 0) return; - - FCL_REAL scale[3]; - scale[0] = (velist[0].back())->getVal(0) - velist[0][0]->getVal(0); - scale[1] = (velist[1].back())->getVal(1) - velist[1][0]->getVal(1);; - scale[2] = (velist[2].back())->getVal(2) - velist[2][0]->getVal(2); - size_t axis = 0; - if(scale[axis] < scale[1]) axis = 1; - if(scale[axis] < scale[2]) axis = 2; - optimal_axis = axis; -} - -void SaPCollisionManager::update_(SaPAABB* updated_aabb) -{ - if(updated_aabb->cached.equal(updated_aabb->obj->getAABB())) - return; - - SaPAABB* current = updated_aabb; - - Vector3d new_min = current->obj->getAABB().min_; - Vector3d new_max = current->obj->getAABB().max_; - - SaPAABB dummy; - dummy.cached = current->obj->getAABB(); - - for(int coord = 0; coord < 3; ++coord) - { - int direction; // -1 reverse, 0 nochange, 1 forward - EndPoint* temp; - - if(current->lo->getVal(coord) > new_min[coord]) - direction = -1; - else if(current->lo->getVal(coord) < new_min[coord]) - direction = 1; - else direction = 0; - - if(direction == -1) - { - //first update the "lo" endpoint of the interval - if(current->lo->prev[coord] != NULL) - { - temp = current->lo; - while((temp != NULL) && (temp->getVal(coord) > new_min[coord])) - { - if(temp->minmax == 1) - if(temp->aabb->cached.overlap(dummy.cached)) - addToOverlapPairs(SaPPair(temp->aabb->obj, current->obj)); - temp = temp->prev[coord]; - } - - if(temp == NULL) - { - current->lo->prev[coord]->next[coord] = current->lo->next[coord]; - current->lo->next[coord]->prev[coord] = current->lo->prev[coord]; - current->lo->prev[coord] = NULL; - current->lo->next[coord] = elist[coord]; - elist[coord]->prev[coord] = current->lo; - elist[coord] = current->lo; - } - else - { - current->lo->prev[coord]->next[coord] = current->lo->next[coord]; - current->lo->next[coord]->prev[coord] = current->lo->prev[coord]; - current->lo->prev[coord] = temp; - current->lo->next[coord] = temp->next[coord]; - temp->next[coord]->prev[coord] = current->lo; - temp->next[coord] = current->lo; - } - } - - current->lo->getVal(coord) = new_min[coord]; - - // update hi end point - temp = current->hi; - while(temp->getVal(coord) > new_max[coord]) - { - if((temp->minmax == 0) && (temp->aabb->cached.overlap(current->cached))) - removeFromOverlapPairs(SaPPair(temp->aabb->obj, current->obj)); - temp = temp->prev[coord]; - } - - current->hi->prev[coord]->next[coord] = current->hi->next[coord]; - if(current->hi->next[coord] != NULL) - current->hi->next[coord]->prev[coord] = current->hi->prev[coord]; - current->hi->prev[coord] = temp; - current->hi->next[coord] = temp->next[coord]; - if(temp->next[coord] != NULL) - temp->next[coord]->prev[coord] = current->hi; - temp->next[coord] = current->hi; - - current->hi->getVal(coord) = new_max[coord]; - } - else if(direction == 1) - { - //here, we first update the "hi" endpoint. - if(current->hi->next[coord] != NULL) - { - temp = current->hi; - while((temp->next[coord] != NULL) && (temp->getVal(coord) < new_max[coord])) - { - if(temp->minmax == 0) - if(temp->aabb->cached.overlap(dummy.cached)) - addToOverlapPairs(SaPPair(temp->aabb->obj, current->obj)); - temp = temp->next[coord]; - } - - if(temp->getVal(coord) < new_max[coord]) - { - current->hi->prev[coord]->next[coord] = current->hi->next[coord]; - current->hi->next[coord]->prev[coord] = current->hi->prev[coord]; - current->hi->prev[coord] = temp; - current->hi->next[coord] = NULL; - temp->next[coord] = current->hi; - } - else - { - current->hi->prev[coord]->next[coord] = current->hi->next[coord]; - current->hi->next[coord]->prev[coord] = current->hi->prev[coord]; - current->hi->prev[coord] = temp->prev[coord]; - current->hi->next[coord] = temp; - temp->prev[coord]->next[coord] = current->hi; - temp->prev[coord] = current->hi; - } - } - - current->hi->getVal(coord) = new_max[coord]; - - //then, update the "lo" endpoint of the interval. - temp = current->lo; - - while(temp->getVal(coord) < new_min[coord]) - { - if((temp->minmax == 1) && (temp->aabb->cached.overlap(current->cached))) - removeFromOverlapPairs(SaPPair(temp->aabb->obj, current->obj)); - temp = temp->next[coord]; - } - - if(current->lo->prev[coord] != NULL) - current->lo->prev[coord]->next[coord] = current->lo->next[coord]; - else - elist[coord] = current->lo->next[coord]; - current->lo->next[coord]->prev[coord] = current->lo->prev[coord]; - current->lo->prev[coord] = temp->prev[coord]; - current->lo->next[coord] = temp; - if(temp->prev[coord] != NULL) - temp->prev[coord]->next[coord] = current->lo; - else - elist[coord] = current->lo; - temp->prev[coord] = current->lo; - current->lo->getVal(coord) = new_min[coord]; - } - } -} - -void SaPCollisionManager::update(CollisionObject* updated_obj) -{ - update_(obj_aabb_map[updated_obj]); - - updateVelist(); - - setup(); -} - -void SaPCollisionManager::update(const std::vector& updated_objs) -{ - for(size_t i = 0; i < updated_objs.size(); ++i) - update_(obj_aabb_map[updated_objs[i]]); - - updateVelist(); - - setup(); -} - -void SaPCollisionManager::update() -{ - for(std::list::const_iterator it = AABB_arr.begin(), end = AABB_arr.end(); it != end; ++it) - { - update_(*it); - } - - updateVelist(); - - setup(); -} - - -void SaPCollisionManager::clear() -{ - for(std::list::iterator it = AABB_arr.begin(), end = AABB_arr.end(); - it != end; - ++it) - { - delete (*it)->hi; - delete (*it)->lo; - delete *it; - *it = NULL; - } - - AABB_arr.clear(); - overlap_pairs.clear(); - - elist[0] = NULL; - elist[1] = NULL; - elist[2] = NULL; - - velist[0].clear(); - velist[1].clear(); - velist[2].clear(); - - obj_aabb_map.clear(); -} - -void SaPCollisionManager::getObjects(std::vector& objs) const -{ - objs.resize(AABB_arr.size()); - int i = 0; - for(std::list::const_iterator it = AABB_arr.begin(), end = AABB_arr.end(); it != end; ++it, ++i) - { - objs[i] = (*it)->obj; - } -} - -bool SaPCollisionManager::collide_(CollisionObject* obj, void* cdata, CollisionCallBack callback) const -{ - size_t axis = optimal_axis; - const AABB& obj_aabb = obj->getAABB(); - - FCL_REAL min_val = obj_aabb.min_[axis]; - // FCL_REAL max_val = obj_aabb.max_[axis]; - - EndPoint dummy; - SaPAABB dummy_aabb; - dummy_aabb.cached = obj_aabb; - dummy.minmax = 1; - dummy.aabb = &dummy_aabb; - - // compute stop_pos by binary search, this is cheaper than check it in while iteration linearly - std::vector::const_iterator res_it = std::upper_bound(velist[axis].begin(), velist[axis].end(), &dummy, - std::bind(std::less(), - std::bind(static_cast(&EndPoint::getVal), std::placeholders::_1, axis), - std::bind(static_cast(&EndPoint::getVal), std::placeholders::_2, axis))); - - EndPoint* end_pos = NULL; - if(res_it != velist[axis].end()) - end_pos = *res_it; - - EndPoint* pos = elist[axis]; - - while(pos != end_pos) - { - if(pos->aabb->obj != obj) - { - if((pos->minmax == 0) && (pos->aabb->hi->getVal(axis) >= min_val)) - { - if(pos->aabb->cached.overlap(obj->getAABB())) - if(callback(obj, pos->aabb->obj, cdata)) - return true; - } - } - pos = pos->next[axis]; - } - - return false; -} - -void SaPCollisionManager::collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const -{ - if(size() == 0) return; - - collide_(obj, cdata, callback); -} - -bool SaPCollisionManager::distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const -{ - Vector3d delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5; - AABB aabb = obj->getAABB(); - - if(min_dist < std::numeric_limits::max()) - { - Vector3d min_dist_delta(min_dist, min_dist, min_dist); - aabb.expand(min_dist_delta); - } - - size_t axis = optimal_axis; - - int status = 1; - FCL_REAL old_min_distance; - - EndPoint* start_pos = elist[axis]; - - while(1) - { - old_min_distance = min_dist; - FCL_REAL min_val = aabb.min_[axis]; - // FCL_REAL max_val = aabb.max_[axis]; - - EndPoint dummy; - SaPAABB dummy_aabb; - dummy_aabb.cached = aabb; - dummy.minmax = 1; - dummy.aabb = &dummy_aabb; - - - std::vector::const_iterator res_it = std::upper_bound(velist[axis].begin(), velist[axis].end(), &dummy, - std::bind(std::less(), - std::bind(static_cast(&EndPoint::getVal), std::placeholders::_1, axis), - std::bind(static_cast(&EndPoint::getVal), std::placeholders::_2, axis))); - - EndPoint* end_pos = NULL; - if(res_it != velist[axis].end()) - end_pos = *res_it; - - EndPoint* pos = start_pos; - - while(pos != end_pos) - { - // can change to pos->aabb->hi->getVal(axis) >= min_val - min_dist, and then update start_pos to end_pos. - // but this seems slower. - if((pos->minmax == 0) && (pos->aabb->hi->getVal(axis) >= min_val)) - { - CollisionObject* curr_obj = pos->aabb->obj; - if(curr_obj != obj) - { - if(!enable_tested_set_) - { - if(pos->aabb->cached.distance(obj->getAABB()) < min_dist) - { - if(callback(curr_obj, obj, cdata, min_dist)) - return true; - } - } - else - { - if(!inTestedSet(curr_obj, obj)) - { - if(pos->aabb->cached.distance(obj->getAABB()) < min_dist) - { - if(callback(curr_obj, obj, cdata, min_dist)) - return true; - } - - insertTestedSet(curr_obj, obj); - } - } - } - } - - pos = pos->next[axis]; - } - - if(status == 1) - { - if(old_min_distance < std::numeric_limits::max()) - break; - else - { - if(min_dist < old_min_distance) - { - Vector3d min_dist_delta(min_dist, min_dist, min_dist); - aabb = AABB(obj->getAABB(), min_dist_delta); - status = 0; - } - else - { - if(aabb.equal(obj->getAABB())) - aabb.expand(delta); - else - aabb.expand(obj->getAABB(), 2.0); - } - } - } - else if(status == 0) - break; - } - - return false; -} - -void SaPCollisionManager::distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const -{ - if(size() == 0) return; - - FCL_REAL min_dist = std::numeric_limits::max(); - - distance_(obj, cdata, callback, min_dist); -} - -void SaPCollisionManager::collide(void* cdata, CollisionCallBack callback) const -{ - if(size() == 0) return; - - for(std::list::const_iterator it = overlap_pairs.begin(), end = overlap_pairs.end(); it != end; ++it) - { - CollisionObject* obj1 = it->obj1; - CollisionObject* obj2 = it->obj2; - - if(callback(obj1, obj2, cdata)) - return; - } -} - -void SaPCollisionManager::distance(void* cdata, DistanceCallBack callback) const -{ - if(size() == 0) return; - - enable_tested_set_ = true; - tested_set.clear(); - - FCL_REAL min_dist = std::numeric_limits::max(); - - for(std::list::const_iterator it = AABB_arr.begin(), end = AABB_arr.end(); it != end; ++it) - { - if(distance_((*it)->obj, cdata, callback, min_dist)) - break; - } - - enable_tested_set_ = false; - tested_set.clear(); -} - -void SaPCollisionManager::collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const -{ - SaPCollisionManager* other_manager = static_cast(other_manager_); - - if((size() == 0) || (other_manager->size() == 0)) return; - - if(this == other_manager) - { - collide(cdata, callback); - return; - } - - if(this->size() < other_manager->size()) - { - for(std::list::const_iterator it = AABB_arr.begin(); it != AABB_arr.end(); ++it) - { - if(other_manager->collide_((*it)->obj, cdata, callback)) - return; - } - } - else - { - for(std::list::const_iterator it = other_manager->AABB_arr.begin(), end = other_manager->AABB_arr.end(); it != end; ++it) - { - if(collide_((*it)->obj, cdata, callback)) - return; - } - } -} - -void SaPCollisionManager::distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const -{ - SaPCollisionManager* other_manager = static_cast(other_manager_); - - if((size() == 0) || (other_manager->size() == 0)) return; - - if(this == other_manager) - { - distance(cdata, callback); - return; - } - - FCL_REAL min_dist = std::numeric_limits::max(); - - if(this->size() < other_manager->size()) - { - for(std::list::const_iterator it = AABB_arr.begin(), end = AABB_arr.end(); it != end; ++it) - { - if(other_manager->distance_((*it)->obj, cdata, callback, min_dist)) - return; - } - } - else - { - for(std::list::const_iterator it = other_manager->AABB_arr.begin(), end = other_manager->AABB_arr.end(); it != end; ++it) - { - if(distance_((*it)->obj, cdata, callback, min_dist)) - return; - } - } -} - -bool SaPCollisionManager::empty() const -{ - return AABB_arr.size(); -} - - - -} diff --git a/src/broadphase/broadphase_bruteforce.cpp b/src/broadphase/broadphase_bruteforce.cpp deleted file mode 100644 index b60593de2..000000000 --- a/src/broadphase/broadphase_bruteforce.cpp +++ /dev/null @@ -1,199 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#include "fcl/broadphase/broadphase_bruteforce.h" -#include -#include - -namespace fcl -{ - -void NaiveCollisionManager::registerObjects(const std::vector& other_objs) -{ - std::copy(other_objs.begin(), other_objs.end(), std::back_inserter(objs)); -} - -void NaiveCollisionManager::unregisterObject(CollisionObject* obj) -{ - objs.remove(obj); -} - -void NaiveCollisionManager::registerObject(CollisionObject* obj) -{ - objs.push_back(obj); -} - -void NaiveCollisionManager::setup() -{ - -} - -void NaiveCollisionManager::update() -{ - -} - -void NaiveCollisionManager::clear() -{ - objs.clear(); -} - -void NaiveCollisionManager::getObjects(std::vector& objs_) const -{ - objs_.resize(objs.size()); - std::copy(objs.begin(), objs.end(), objs_.begin()); -} - -void NaiveCollisionManager::collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const -{ - if(size() == 0) return; - - for(std::list::const_iterator it = objs.begin(), end = objs.end(); it != end; ++it) - { - if(callback(obj, *it, cdata)) - return; - } -} - -void NaiveCollisionManager::distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const -{ - if(size() == 0) return; - - FCL_REAL min_dist = std::numeric_limits::max(); - for(std::list::const_iterator it = objs.begin(), end = objs.end(); - it != end; ++it) - { - if(obj->getAABB().distance((*it)->getAABB()) < min_dist) - { - if(callback(obj, *it, cdata, min_dist)) - return; - } - } -} - -void NaiveCollisionManager::collide(void* cdata, CollisionCallBack callback) const -{ - if(size() == 0) return; - - for(std::list::const_iterator it1 = objs.begin(), end = objs.end(); - it1 != end; ++it1) - { - std::list::const_iterator it2 = it1; it2++; - for(; it2 != end; ++it2) - { - if((*it1)->getAABB().overlap((*it2)->getAABB())) - if(callback(*it1, *it2, cdata)) - return; - } - } -} - -void NaiveCollisionManager::distance(void* cdata, DistanceCallBack callback) const -{ - if(size() == 0) return; - - FCL_REAL min_dist = std::numeric_limits::max(); - for(std::list::const_iterator it1 = objs.begin(), end = objs.end(); it1 != end; ++it1) - { - std::list::const_iterator it2 = it1; it2++; - for(; it2 != end; ++it2) - { - if((*it1)->getAABB().distance((*it2)->getAABB()) < min_dist) - { - if(callback(*it1, *it2, cdata, min_dist)) - return; - } - } - } -} - -void NaiveCollisionManager::collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const -{ - NaiveCollisionManager* other_manager = static_cast(other_manager_); - - if((size() == 0) || (other_manager->size() == 0)) return; - - if(this == other_manager) - { - collide(cdata, callback); - return; - } - - for(std::list::const_iterator it1 = objs.begin(), end1 = objs.end(); it1 != end1; ++it1) - { - for(std::list::const_iterator it2 = other_manager->objs.begin(), end2 = other_manager->objs.end(); it2 != end2; ++it2) - { - if((*it1)->getAABB().overlap((*it2)->getAABB())) - if(callback((*it1), (*it2), cdata)) - return; - } - } -} - -void NaiveCollisionManager::distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const -{ - NaiveCollisionManager* other_manager = static_cast(other_manager_); - - if((size() == 0) || (other_manager->size() == 0)) return; - - if(this == other_manager) - { - distance(cdata, callback); - return; - } - - FCL_REAL min_dist = std::numeric_limits::max(); - for(std::list::const_iterator it1 = objs.begin(), end1 = objs.end(); it1 != end1; ++it1) - { - for(std::list::const_iterator it2 = other_manager->objs.begin(), end2 = other_manager->objs.end(); it2 != end2; ++it2) - { - if((*it1)->getAABB().distance((*it2)->getAABB()) < min_dist) - { - if(callback(*it1, *it2, cdata, min_dist)) - return; - } - } - } -} - -bool NaiveCollisionManager::empty() const -{ - return objs.empty(); -} - - -} diff --git a/src/broadphase/broadphase_dynamic_AABB_tree.cpp b/src/broadphase/broadphase_dynamic_AABB_tree.cpp deleted file mode 100644 index 8c848f4a8..000000000 --- a/src/broadphase/broadphase_dynamic_AABB_tree.cpp +++ /dev/null @@ -1,823 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - - -#include "fcl/broadphase/broadphase_dynamic_AABB_tree.h" - -#if FCL_HAVE_OCTOMAP -#include "fcl/octree.h" -#endif - -namespace fcl -{ - -namespace details -{ - -namespace dynamic_AABB_tree -{ - -#if FCL_HAVE_OCTOMAP -bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3d& tf2, void* cdata, CollisionCallBack callback) -{ - if(!root2) - { - if(root1->isLeaf()) - { - CollisionObject* obj1 = static_cast(root1->data); - - if(!obj1->isFree()) - { - OBB obb1, obb2; - convertBV(root1->bv, Transform3d::Identity(), obb1); - convertBV(root2_bv, tf2, obb2); - - if(obb1.overlap(obb2)) - { - Box* box = new Box(); - Transform3d box_tf; - constructBox(root2_bv, tf2, *box, box_tf); - - box->cost_density = tree2->getDefaultOccupancy(); - - CollisionObject obj2(std::shared_ptr(box), box_tf); - return callback(obj1, &obj2, cdata); - } - } - } - else - { - if(collisionRecurse_(root1->children[0], tree2, NULL, root2_bv, tf2, cdata, callback)) - return true; - if(collisionRecurse_(root1->children[1], tree2, NULL, root2_bv, tf2, cdata, callback)) - return true; - } - - return false; - } - else if(root1->isLeaf() && !tree2->nodeHasChildren(root2)) - { - CollisionObject* obj1 = static_cast(root1->data); - - if(!tree2->isNodeFree(root2) && !obj1->isFree()) - { - OBB obb1, obb2; - convertBV(root1->bv, Transform3d::Identity(), obb1); - convertBV(root2_bv, tf2, obb2); - - if(obb1.overlap(obb2)) - { - Box* box = new Box(); - Transform3d box_tf; - constructBox(root2_bv, tf2, *box, box_tf); - - box->cost_density = root2->getOccupancy(); - box->threshold_occupied = tree2->getOccupancyThres(); - - CollisionObject obj2(std::shared_ptr(box), box_tf); - return callback(obj1, &obj2, cdata); - } - else return false; - } - else return false; - } - - OBB obb1, obb2; - convertBV(root1->bv, Transform3d::Identity(), obb1); - convertBV(root2_bv, tf2, obb2); - - if(tree2->isNodeFree(root2) || !obb1.overlap(obb2)) return false; - - if(!tree2->nodeHasChildren(root2) || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) - { - if(collisionRecurse_(root1->children[0], tree2, root2, root2_bv, tf2, cdata, callback)) - return true; - if(collisionRecurse_(root1->children[1], tree2, root2, root2_bv, tf2, cdata, callback)) - return true; - } - else - { - for(unsigned int i = 0; i < 8; ++i) - { - if(tree2->nodeChildExists(root2, i)) - { - const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); - AABB child_bv; - computeChildBV(root2_bv, i, child_bv); - - if(collisionRecurse_(root1, tree2, child, child_bv, tf2, cdata, callback)) - return true; - } - else - { - AABB child_bv; - computeChildBV(root2_bv, i, child_bv); - if(collisionRecurse_(root1, tree2, NULL, child_bv, tf2, cdata, callback)) - return true; - } - } - } - return false; -} - -bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Vector3d& translation2, void* cdata, CollisionCallBack callback) -{ - if(!root2) - { - if(root1->isLeaf()) - { - CollisionObject* obj1 = static_cast(root1->data); - - if(!obj1->isFree()) - { - const AABB& root2_bv_t = translate(root2_bv, translation2); - if(root1->bv.overlap(root2_bv_t)) - { - Box* box = new Box(); - Transform3d box_tf; - Transform3d tf2 = Transform3d::Identity(); - tf2.translation() = translation2; - constructBox(root2_bv, tf2, *box, box_tf); - - box->cost_density = tree2->getOccupancyThres(); // thresholds are 0, 1, so uncertain - - CollisionObject obj2(std::shared_ptr(box), box_tf); - return callback(obj1, &obj2, cdata); - } - } - } - else - { - if(collisionRecurse_(root1->children[0], tree2, NULL, root2_bv, translation2, cdata, callback)) - return true; - if(collisionRecurse_(root1->children[1], tree2, NULL, root2_bv, translation2, cdata, callback)) - return true; - } - - return false; - } - else if(root1->isLeaf() && !tree2->nodeHasChildren(root2)) - { - CollisionObject* obj1 = static_cast(root1->data); - - if(!tree2->isNodeFree(root2) && !obj1->isFree()) - { - const AABB& root2_bv_t = translate(root2_bv, translation2); - if(root1->bv.overlap(root2_bv_t)) - { - Box* box = new Box(); - Transform3d box_tf; - Transform3d tf2 = Transform3d::Identity(); - tf2.translation() = translation2; - constructBox(root2_bv, tf2, *box, box_tf); - - box->cost_density = root2->getOccupancy(); - box->threshold_occupied = tree2->getOccupancyThres(); - - CollisionObject obj2(std::shared_ptr(box), box_tf); - return callback(obj1, &obj2, cdata); - } - else return false; - } - else return false; - } - - const AABB& root2_bv_t = translate(root2_bv, translation2); - if(tree2->isNodeFree(root2) || !root1->bv.overlap(root2_bv_t)) return false; - - if(!tree2->nodeHasChildren(root2) || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) - { - if(collisionRecurse_(root1->children[0], tree2, root2, root2_bv, translation2, cdata, callback)) - return true; - if(collisionRecurse_(root1->children[1], tree2, root2, root2_bv, translation2, cdata, callback)) - return true; - } - else - { - for(unsigned int i = 0; i < 8; ++i) - { - if(tree2->nodeChildExists(root2, i)) - { - const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); - AABB child_bv; - computeChildBV(root2_bv, i, child_bv); - - if(collisionRecurse_(root1, tree2, child, child_bv, translation2, cdata, callback)) - return true; - } - else - { - AABB child_bv; - computeChildBV(root2_bv, i, child_bv); - if(collisionRecurse_(root1, tree2, NULL, child_bv, translation2, cdata, callback)) - return true; - } - } - } - return false; -} - - -bool distanceRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3d& tf2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) -{ - if(root1->isLeaf() && !tree2->nodeHasChildren(root2)) - { - if(tree2->isNodeOccupied(root2)) - { - Box* box = new Box(); - Transform3d box_tf; - constructBox(root2_bv, tf2, *box, box_tf); - CollisionObject obj(std::shared_ptr(box), box_tf); - return callback(static_cast(root1->data), &obj, cdata, min_dist); - } - else return false; - } - - if(!tree2->isNodeOccupied(root2)) return false; - - if(!tree2->nodeHasChildren(root2) || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) - { - AABB aabb2; - convertBV(root2_bv, tf2, aabb2); - - FCL_REAL d1 = aabb2.distance(root1->children[0]->bv); - FCL_REAL d2 = aabb2.distance(root1->children[1]->bv); - - if(d2 < d1) - { - if(d2 < min_dist) - { - if(distanceRecurse_(root1->children[1], tree2, root2, root2_bv, tf2, cdata, callback, min_dist)) - return true; - } - - if(d1 < min_dist) - { - if(distanceRecurse_(root1->children[0], tree2, root2, root2_bv, tf2, cdata, callback, min_dist)) - return true; - } - } - else - { - if(d1 < min_dist) - { - if(distanceRecurse_(root1->children[0], tree2, root2, root2_bv, tf2, cdata, callback, min_dist)) - return true; - } - - if(d2 < min_dist) - { - if(distanceRecurse_(root1->children[1], tree2, root2, root2_bv, tf2, cdata, callback, min_dist)) - return true; - } - } - } - else - { - for(unsigned int i = 0; i < 8; ++i) - { - if(tree2->nodeChildExists(root2, i)) - { - const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); - AABB child_bv; - computeChildBV(root2_bv, i, child_bv); - - AABB aabb2; - convertBV(child_bv, tf2, aabb2); - FCL_REAL d = root1->bv.distance(aabb2); - - if(d < min_dist) - { - if(distanceRecurse_(root1, tree2, child, child_bv, tf2, cdata, callback, min_dist)) - return true; - } - } - } - } - - return false; -} - -bool collisionRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3d& tf2, void* cdata, CollisionCallBack callback) -{ - if(tf2.linear().isIdentity()) - return collisionRecurse_(root1, tree2, root2, root2_bv, tf2.translation(), cdata, callback); - else // has rotation - return collisionRecurse_(root1, tree2, root2, root2_bv, tf2, cdata, callback); -} - - -bool distanceRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Vector3d& translation2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) -{ - if(root1->isLeaf() && !tree2->nodeHasChildren(root2)) - { - if(tree2->isNodeOccupied(root2)) - { - Box* box = new Box(); - Transform3d box_tf; - Transform3d tf2 = Transform3d::Identity(); - tf2.translation() = translation2; - constructBox(root2_bv, tf2, *box, box_tf); - CollisionObject obj(std::shared_ptr(box), box_tf); - return callback(static_cast(root1->data), &obj, cdata, min_dist); - } - else return false; - } - - if(!tree2->isNodeOccupied(root2)) return false; - - if(!tree2->nodeHasChildren(root2) || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) - { - const AABB& aabb2 = translate(root2_bv, translation2); - FCL_REAL d1 = aabb2.distance(root1->children[0]->bv); - FCL_REAL d2 = aabb2.distance(root1->children[1]->bv); - - if(d2 < d1) - { - if(d2 < min_dist) - { - if(distanceRecurse_(root1->children[1], tree2, root2, root2_bv, translation2, cdata, callback, min_dist)) - return true; - } - - if(d1 < min_dist) - { - if(distanceRecurse_(root1->children[0], tree2, root2, root2_bv, translation2, cdata, callback, min_dist)) - return true; - } - } - else - { - if(d1 < min_dist) - { - if(distanceRecurse_(root1->children[0], tree2, root2, root2_bv, translation2, cdata, callback, min_dist)) - return true; - } - - if(d2 < min_dist) - { - if(distanceRecurse_(root1->children[1], tree2, root2, root2_bv, translation2, cdata, callback, min_dist)) - return true; - } - } - } - else - { - for(unsigned int i = 0; i < 8; ++i) - { - if(tree2->nodeChildExists(root2, i)) - { - const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); - AABB child_bv; - computeChildBV(root2_bv, i, child_bv); - const AABB& aabb2 = translate(child_bv, translation2); - - FCL_REAL d = root1->bv.distance(aabb2); - - if(d < min_dist) - { - if(distanceRecurse_(root1, tree2, child, child_bv, translation2, cdata, callback, min_dist)) - return true; - } - } - } - } - - return false; -} - - -bool distanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3d& tf2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) -{ - if(tf2.linear().isIdentity()) - return distanceRecurse_(root1, tree2, root2, root2_bv, tf2.translation(), cdata, callback, min_dist); - else - return distanceRecurse_(root1, tree2, root2, root2_bv, tf2, cdata, callback, min_dist); -} - -#endif - -bool collisionRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, DynamicAABBTreeCollisionManager::DynamicAABBNode* root2, void* cdata, CollisionCallBack callback) -{ - if(root1->isLeaf() && root2->isLeaf()) - { - if(!root1->bv.overlap(root2->bv)) return false; - return callback(static_cast(root1->data), static_cast(root2->data), cdata); - } - - if(!root1->bv.overlap(root2->bv)) return false; - - if(root2->isLeaf() || (!root1->isLeaf() && (root1->bv.size() > root2->bv.size()))) - { - if(collisionRecurse(root1->children[0], root2, cdata, callback)) - return true; - if(collisionRecurse(root1->children[1], root2, cdata, callback)) - return true; - } - else - { - if(collisionRecurse(root1, root2->children[0], cdata, callback)) - return true; - if(collisionRecurse(root1, root2->children[1], cdata, callback)) - return true; - } - return false; -} - -bool collisionRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root, CollisionObject* query, void* cdata, CollisionCallBack callback) -{ - if(root->isLeaf()) - { - if(!root->bv.overlap(query->getAABB())) return false; - return callback(static_cast(root->data), query, cdata); - } - - if(!root->bv.overlap(query->getAABB())) return false; - - int select_res = select(query->getAABB(), *(root->children[0]), *(root->children[1])); - - if(collisionRecurse(root->children[select_res], query, cdata, callback)) - return true; - - if(collisionRecurse(root->children[1-select_res], query, cdata, callback)) - return true; - - return false; -} - -bool selfCollisionRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root, void* cdata, CollisionCallBack callback) -{ - if(root->isLeaf()) return false; - - if(selfCollisionRecurse(root->children[0], cdata, callback)) - return true; - - if(selfCollisionRecurse(root->children[1], cdata, callback)) - return true; - - if(collisionRecurse(root->children[0], root->children[1], cdata, callback)) - return true; - - return false; -} - -bool distanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, DynamicAABBTreeCollisionManager::DynamicAABBNode* root2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) -{ - if(root1->isLeaf() && root2->isLeaf()) - { - CollisionObject* root1_obj = static_cast(root1->data); - CollisionObject* root2_obj = static_cast(root2->data); - return callback(root1_obj, root2_obj, cdata, min_dist); - } - - if(root2->isLeaf() || (!root1->isLeaf() && (root1->bv.size() > root2->bv.size()))) - { - FCL_REAL d1 = root2->bv.distance(root1->children[0]->bv); - FCL_REAL d2 = root2->bv.distance(root1->children[1]->bv); - - if(d2 < d1) - { - if(d2 < min_dist) - { - if(distanceRecurse(root1->children[1], root2, cdata, callback, min_dist)) - return true; - } - - if(d1 < min_dist) - { - if(distanceRecurse(root1->children[0], root2, cdata, callback, min_dist)) - return true; - } - } - else - { - if(d1 < min_dist) - { - if(distanceRecurse(root1->children[0], root2, cdata, callback, min_dist)) - return true; - } - - if(d2 < min_dist) - { - if(distanceRecurse(root1->children[1], root2, cdata, callback, min_dist)) - return true; - } - } - } - else - { - FCL_REAL d1 = root1->bv.distance(root2->children[0]->bv); - FCL_REAL d2 = root1->bv.distance(root2->children[1]->bv); - - if(d2 < d1) - { - if(d2 < min_dist) - { - if(distanceRecurse(root1, root2->children[1], cdata, callback, min_dist)) - return true; - } - - if(d1 < min_dist) - { - if(distanceRecurse(root1, root2->children[0], cdata, callback, min_dist)) - return true; - } - } - else - { - if(d1 < min_dist) - { - if(distanceRecurse(root1, root2->children[0], cdata, callback, min_dist)) - return true; - } - - if(d2 < min_dist) - { - if(distanceRecurse(root1, root2->children[1], cdata, callback, min_dist)) - return true; - } - } - } - - return false; -} - -bool distanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root, CollisionObject* query, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) -{ - if(root->isLeaf()) - { - CollisionObject* root_obj = static_cast(root->data); - return callback(root_obj, query, cdata, min_dist); - } - - FCL_REAL d1 = query->getAABB().distance(root->children[0]->bv); - FCL_REAL d2 = query->getAABB().distance(root->children[1]->bv); - - if(d2 < d1) - { - if(d2 < min_dist) - { - if(distanceRecurse(root->children[1], query, cdata, callback, min_dist)) - return true; - } - - if(d1 < min_dist) - { - if(distanceRecurse(root->children[0], query, cdata, callback, min_dist)) - return true; - } - } - else - { - if(d1 < min_dist) - { - if(distanceRecurse(root->children[0], query, cdata, callback, min_dist)) - return true; - } - - if(d2 < min_dist) - { - if(distanceRecurse(root->children[1], query, cdata, callback, min_dist)) - return true; - } - } - - return false; -} - -bool selfDistanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) -{ - if(root->isLeaf()) return false; - - if(selfDistanceRecurse(root->children[0], cdata, callback, min_dist)) - return true; - - if(selfDistanceRecurse(root->children[1], cdata, callback, min_dist)) - return true; - - if(distanceRecurse(root->children[0], root->children[1], cdata, callback, min_dist)) - return true; - - return false; -} - -} // dynamic_AABB_tree - -} // details - -void DynamicAABBTreeCollisionManager::registerObjects(const std::vector& other_objs) -{ - if(other_objs.empty()) return; - - if(size() > 0) - { - BroadPhaseCollisionManager::registerObjects(other_objs); - } - else - { - std::vector leaves(other_objs.size()); - table.rehash(other_objs.size()); - for(size_t i = 0, size = other_objs.size(); i < size; ++i) - { - DynamicAABBNode* node = new DynamicAABBNode; // node will be managed by the dtree - node->bv = other_objs[i]->getAABB(); - node->parent = NULL; - node->children[1] = NULL; - node->data = other_objs[i]; - table[other_objs[i]] = node; - leaves[i] = node; - } - - dtree.init(leaves, tree_init_level); - - setup_ = true; - } -} - -void DynamicAABBTreeCollisionManager::registerObject(CollisionObject* obj) -{ - DynamicAABBNode* node = dtree.insert(obj->getAABB(), obj); - table[obj] = node; -} - -void DynamicAABBTreeCollisionManager::unregisterObject(CollisionObject* obj) -{ - DynamicAABBNode* node = table[obj]; - table.erase(obj); - dtree.remove(node); -} - -void DynamicAABBTreeCollisionManager::setup() -{ - if(!setup_) - { - int num = dtree.size(); - if(num == 0) - { - setup_ = true; - return; - } - - int height = dtree.getMaxHeight(); - - - if(height - std::log((FCL_REAL)num) / std::log(2.0) < max_tree_nonbalanced_level) - dtree.balanceIncremental(tree_incremental_balance_pass); - else - dtree.balanceTopdown(); - - setup_ = true; - } -} - - -void DynamicAABBTreeCollisionManager::update() -{ - for(DynamicAABBTable::const_iterator it = table.begin(); it != table.end(); ++it) - { - CollisionObject* obj = it->first; - DynamicAABBNode* node = it->second; - node->bv = obj->getAABB(); - } - - dtree.refit(); - setup_ = false; - - setup(); -} - -void DynamicAABBTreeCollisionManager::update_(CollisionObject* updated_obj) -{ - DynamicAABBTable::const_iterator it = table.find(updated_obj); - if(it != table.end()) - { - DynamicAABBNode* node = it->second; - if(!node->bv.equal(updated_obj->getAABB())) - dtree.update(node, updated_obj->getAABB()); - } - setup_ = false; -} - -void DynamicAABBTreeCollisionManager::update(CollisionObject* updated_obj) -{ - update_(updated_obj); - setup(); -} - -void DynamicAABBTreeCollisionManager::update(const std::vector& updated_objs) -{ - for(size_t i = 0, size = updated_objs.size(); i < size; ++i) - update_(updated_objs[i]); - setup(); -} - -void DynamicAABBTreeCollisionManager::collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const -{ - if(size() == 0) return; - switch(obj->collisionGeometry()->getNodeType()) - { -#if FCL_HAVE_OCTOMAP - case GEOM_OCTREE: - { - if(!octree_as_geometry_collide) - { - const OcTree* octree = static_cast(obj->collisionGeometry().get()); - details::dynamic_AABB_tree::collisionRecurse(dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(), obj->getTransform(), cdata, callback); - } - else - details::dynamic_AABB_tree::collisionRecurse(dtree.getRoot(), obj, cdata, callback); - } - break; -#endif - default: - details::dynamic_AABB_tree::collisionRecurse(dtree.getRoot(), obj, cdata, callback); - } -} - -void DynamicAABBTreeCollisionManager::distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const -{ - if(size() == 0) return; - FCL_REAL min_dist = std::numeric_limits::max(); - switch(obj->collisionGeometry()->getNodeType()) - { -#if FCL_HAVE_OCTOMAP - case GEOM_OCTREE: - { - if(!octree_as_geometry_distance) - { - const OcTree* octree = static_cast(obj->collisionGeometry().get()); - details::dynamic_AABB_tree::distanceRecurse(dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(), obj->getTransform(), cdata, callback, min_dist); - } - else - details::dynamic_AABB_tree::distanceRecurse(dtree.getRoot(), obj, cdata, callback, min_dist); - } - break; -#endif - default: - details::dynamic_AABB_tree::distanceRecurse(dtree.getRoot(), obj, cdata, callback, min_dist); - } -} - - -void DynamicAABBTreeCollisionManager::collide(void* cdata, CollisionCallBack callback) const -{ - if(size() == 0) return; - details::dynamic_AABB_tree::selfCollisionRecurse(dtree.getRoot(), cdata, callback); -} - -void DynamicAABBTreeCollisionManager::distance(void* cdata, DistanceCallBack callback) const -{ - if(size() == 0) return; - FCL_REAL min_dist = std::numeric_limits::max(); - details::dynamic_AABB_tree::selfDistanceRecurse(dtree.getRoot(), cdata, callback, min_dist); -} - -void DynamicAABBTreeCollisionManager::collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const -{ - DynamicAABBTreeCollisionManager* other_manager = static_cast(other_manager_); - if((size() == 0) || (other_manager->size() == 0)) return; - details::dynamic_AABB_tree::collisionRecurse(dtree.getRoot(), other_manager->dtree.getRoot(), cdata, callback); -} - -void DynamicAABBTreeCollisionManager::distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const -{ - DynamicAABBTreeCollisionManager* other_manager = static_cast(other_manager_); - if((size() == 0) || (other_manager->size() == 0)) return; - FCL_REAL min_dist = std::numeric_limits::max(); - details::dynamic_AABB_tree::distanceRecurse(dtree.getRoot(), other_manager->dtree.getRoot(), cdata, callback, min_dist); -} -} diff --git a/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp b/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp deleted file mode 100644 index 8ac695b35..000000000 --- a/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp +++ /dev/null @@ -1,851 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#include "fcl/broadphase/broadphase_dynamic_AABB_tree_array.h" - -#if FCL_HAVE_OCTOMAP -#include "fcl/octree.h" -#endif - -namespace fcl -{ - -namespace details -{ - -namespace dynamic_AABB_tree_array -{ - -#if FCL_HAVE_OCTOMAP -bool collisionRecurse_(DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3d& tf2, void* cdata, CollisionCallBack callback) -{ - DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* root1 = nodes1 + root1_id; - if(!root2) - { - if(root1->isLeaf()) - { - CollisionObject* obj1 = static_cast(root1->data); - - if(!obj1->isFree()) - { - OBB obb1, obb2; - convertBV(root1->bv, Transform3d::Identity(), obb1); - convertBV(root2_bv, tf2, obb2); - - if(obb1.overlap(obb2)) - { - Box* box = new Box(); - Transform3d box_tf; - constructBox(root2_bv, tf2, *box, box_tf); - - box->cost_density = tree2->getDefaultOccupancy(); - - CollisionObject obj2(std::shared_ptr(box), box_tf); - return callback(obj1, &obj2, cdata); - } - } - } - else - { - if(collisionRecurse_(nodes1, root1->children[0], tree2, NULL, root2_bv, tf2, cdata, callback)) - return true; - if(collisionRecurse_(nodes1, root1->children[1], tree2, NULL, root2_bv, tf2, cdata, callback)) - return true; - } - - return false; - } - else if(root1->isLeaf() && !tree2->nodeHasChildren(root2)) - { - CollisionObject* obj1 = static_cast(root1->data); - if(!tree2->isNodeFree(root2) && !obj1->isFree()) - { - OBB obb1, obb2; - convertBV(root1->bv, Transform3d::Identity(), obb1); - convertBV(root2_bv, tf2, obb2); - - if(obb1.overlap(obb2)) - { - Box* box = new Box(); - Transform3d box_tf; - constructBox(root2_bv, tf2, *box, box_tf); - - box->cost_density = root2->getOccupancy(); - box->threshold_occupied = tree2->getOccupancyThres(); - - CollisionObject obj2(std::shared_ptr(box), box_tf); - return callback(obj1, &obj2, cdata); - } - else return false; - } - else return false; - } - - OBB obb1, obb2; - convertBV(root1->bv, Transform3d::Identity(), obb1); - convertBV(root2_bv, tf2, obb2); - - if(tree2->isNodeFree(root2) || !obb1.overlap(obb2)) return false; - - if(!tree2->nodeHasChildren(root2) || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) - { - if(collisionRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv, tf2, cdata, callback)) - return true; - if(collisionRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv, tf2, cdata, callback)) - return true; - } - else - { - for(unsigned int i = 0; i < 8; ++i) - { - if(tree2->nodeChildExists(root2, i)) - { - const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); - AABB child_bv; - computeChildBV(root2_bv, i, child_bv); - - if(collisionRecurse_(nodes1, root1_id, tree2, child, child_bv, tf2, cdata, callback)) - return true; - } - else - { - AABB child_bv; - computeChildBV(root2_bv, i, child_bv); - if(collisionRecurse_(nodes1, root1_id, tree2, NULL, child_bv, tf2, cdata, callback)) - return true; - } - } - } - - return false; -} - - -bool collisionRecurse_(DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Vector3d& translation2, void* cdata, CollisionCallBack callback) -{ - DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* root1 = nodes1 + root1_id; - if(!root2) - { - if(root1->isLeaf()) - { - CollisionObject* obj1 = static_cast(root1->data); - - if(!obj1->isFree()) - { - const AABB& root_bv_t = translate(root2_bv, translation2); - if(root1->bv.overlap(root_bv_t)) - { - Box* box = new Box(); - Transform3d box_tf; - Transform3d tf2 = Transform3d::Identity(); - tf2.translation() = translation2; - constructBox(root2_bv, tf2, *box, box_tf); - - box->cost_density = tree2->getDefaultOccupancy(); - - CollisionObject obj2(std::shared_ptr(box), box_tf); - return callback(obj1, &obj2, cdata); - } - } - } - else - { - if(collisionRecurse_(nodes1, root1->children[0], tree2, NULL, root2_bv, translation2, cdata, callback)) - return true; - if(collisionRecurse_(nodes1, root1->children[1], tree2, NULL, root2_bv, translation2, cdata, callback)) - return true; - } - - return false; - } - else if(root1->isLeaf() && !tree2->nodeHasChildren(root2)) - { - CollisionObject* obj1 = static_cast(root1->data); - if(!tree2->isNodeFree(root2) && !obj1->isFree()) - { - const AABB& root_bv_t = translate(root2_bv, translation2); - if(root1->bv.overlap(root_bv_t)) - { - Box* box = new Box(); - Transform3d box_tf; - Transform3d tf2 = Transform3d::Identity(); - tf2.translation() = translation2; - constructBox(root2_bv, tf2, *box, box_tf); - - box->cost_density = root2->getOccupancy(); - box->threshold_occupied = tree2->getOccupancyThres(); - - CollisionObject obj2(std::shared_ptr(box), box_tf); - return callback(obj1, &obj2, cdata); - } - else return false; - } - else return false; - } - - const AABB& root_bv_t = translate(root2_bv, translation2); - if(tree2->isNodeFree(root2) || !root1->bv.overlap(root_bv_t)) return false; - - if(!tree2->nodeHasChildren(root2) || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) - { - if(collisionRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv, translation2, cdata, callback)) - return true; - if(collisionRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv, translation2, cdata, callback)) - return true; - } - else - { - for(unsigned int i = 0; i < 8; ++i) - { - if(tree2->nodeChildExists(root2, i)) - { - const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); - AABB child_bv; - computeChildBV(root2_bv, i, child_bv); - - if(collisionRecurse_(nodes1, root1_id, tree2, child, child_bv, translation2, cdata, callback)) - return true; - } - else - { - AABB child_bv; - computeChildBV(root2_bv, i, child_bv); - if(collisionRecurse_(nodes1, root1_id, tree2, NULL, child_bv, translation2, cdata, callback)) - return true; - } - } - } - - return false; -} - - -bool distanceRecurse_(DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3d& tf2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) -{ - DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* root1 = nodes1 + root1_id; - if(root1->isLeaf() && !tree2->nodeHasChildren(root2)) - { - if(tree2->isNodeOccupied(root2)) - { - Box* box = new Box(); - Transform3d box_tf; - constructBox(root2_bv, tf2, *box, box_tf); - CollisionObject obj(std::shared_ptr(box), box_tf); - return callback(static_cast(root1->data), &obj, cdata, min_dist); - } - else return false; - } - - if(!tree2->isNodeOccupied(root2)) return false; - - if(!tree2->nodeHasChildren(root2) || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) - { - AABB aabb2; - convertBV(root2_bv, tf2, aabb2); - - FCL_REAL d1 = aabb2.distance((nodes1 + root1->children[0])->bv); - FCL_REAL d2 = aabb2.distance((nodes1 + root1->children[1])->bv); - - if(d2 < d1) - { - if(d2 < min_dist) - { - if(distanceRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv, tf2, cdata, callback, min_dist)) - return true; - } - - if(d1 < min_dist) - { - if(distanceRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv, tf2, cdata, callback, min_dist)) - return true; - } - } - else - { - if(d1 < min_dist) - { - if(distanceRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv, tf2, cdata, callback, min_dist)) - return true; - } - - if(d2 < min_dist) - { - if(distanceRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv, tf2, cdata, callback, min_dist)) - return true; - } - } - } - else - { - for(unsigned int i = 0; i < 8; ++i) - { - if(tree2->nodeChildExists(root2, i)) - { - const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); - AABB child_bv; - computeChildBV(root2_bv, i, child_bv); - - AABB aabb2; - convertBV(child_bv, tf2, aabb2); - FCL_REAL d = root1->bv.distance(aabb2); - - if(d < min_dist) - { - if(distanceRecurse_(nodes1, root1_id, tree2, child, child_bv, tf2, cdata, callback, min_dist)) - return true; - } - } - } - } - - return false; -} - - -bool distanceRecurse_(DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Vector3d& translation2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) -{ - DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* root1 = nodes1 + root1_id; - if(root1->isLeaf() && !tree2->nodeHasChildren(root2)) - { - if(tree2->isNodeOccupied(root2)) - { - Box* box = new Box(); - Transform3d box_tf; - Transform3d tf2 = Transform3d::Identity(); - tf2.translation() = translation2; - constructBox(root2_bv, tf2, *box, box_tf); - CollisionObject obj(std::shared_ptr(box), box_tf); - return callback(static_cast(root1->data), &obj, cdata, min_dist); - } - else return false; - } - - if(!tree2->isNodeOccupied(root2)) return false; - - if(!tree2->nodeHasChildren(root2) || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) - { - const AABB& aabb2 = translate(root2_bv, translation2); - - FCL_REAL d1 = aabb2.distance((nodes1 + root1->children[0])->bv); - FCL_REAL d2 = aabb2.distance((nodes1 + root1->children[1])->bv); - - if(d2 < d1) - { - if(d2 < min_dist) - { - if(distanceRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv, translation2, cdata, callback, min_dist)) - return true; - } - - if(d1 < min_dist) - { - if(distanceRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv, translation2, cdata, callback, min_dist)) - return true; - } - } - else - { - if(d1 < min_dist) - { - if(distanceRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv, translation2, cdata, callback, min_dist)) - return true; - } - - if(d2 < min_dist) - { - if(distanceRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv, translation2, cdata, callback, min_dist)) - return true; - } - } - } - else - { - for(unsigned int i = 0; i < 8; ++i) - { - if(tree2->nodeChildExists(root2, i)) - { - const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); - AABB child_bv; - computeChildBV(root2_bv, i, child_bv); - - const AABB& aabb2 = translate(child_bv, translation2); - FCL_REAL d = root1->bv.distance(aabb2); - - if(d < min_dist) - { - if(distanceRecurse_(nodes1, root1_id, tree2, child, child_bv, translation2, cdata, callback, min_dist)) - return true; - } - } - } - } - - return false; -} - - -#endif - -bool collisionRecurse(DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes1, size_t root1_id, - DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes2, size_t root2_id, - void* cdata, CollisionCallBack callback) -{ - DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* root1 = nodes1 + root1_id; - DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* root2 = nodes2 + root2_id; - if(root1->isLeaf() && root2->isLeaf()) - { - if(!root1->bv.overlap(root2->bv)) return false; - return callback(static_cast(root1->data), static_cast(root2->data), cdata); - } - - if(!root1->bv.overlap(root2->bv)) return false; - - if(root2->isLeaf() || (!root1->isLeaf() && (root1->bv.size() > root2->bv.size()))) - { - if(collisionRecurse(nodes1, root1->children[0], nodes2, root2_id, cdata, callback)) - return true; - if(collisionRecurse(nodes1, root1->children[1], nodes2, root2_id, cdata, callback)) - return true; - } - else - { - if(collisionRecurse(nodes1, root1_id, nodes2, root2->children[0], cdata, callback)) - return true; - if(collisionRecurse(nodes1, root1_id, nodes2, root2->children[1], cdata, callback)) - return true; - } - return false; -} - -bool collisionRecurse(DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes, size_t root_id, CollisionObject* query, void* cdata, CollisionCallBack callback) -{ - DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* root = nodes + root_id; - if(root->isLeaf()) - { - if(!root->bv.overlap(query->getAABB())) return false; - return callback(static_cast(root->data), query, cdata); - } - - if(!root->bv.overlap(query->getAABB())) return false; - - int select_res = implementation_array::select(query->getAABB(), root->children[0], root->children[1], nodes); - - if(collisionRecurse(nodes, root->children[select_res], query, cdata, callback)) - return true; - - if(collisionRecurse(nodes, root->children[1-select_res], query, cdata, callback)) - return true; - - return false; -} - -bool selfCollisionRecurse(DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes, size_t root_id, void* cdata, CollisionCallBack callback) -{ - DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* root = nodes + root_id; - if(root->isLeaf()) return false; - - if(selfCollisionRecurse(nodes, root->children[0], cdata, callback)) - return true; - - if(selfCollisionRecurse(nodes, root->children[1], cdata, callback)) - return true; - - if(collisionRecurse(nodes, root->children[0], nodes, root->children[1], cdata, callback)) - return true; - - return false; -} - -bool distanceRecurse(DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes1, size_t root1_id, - DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes2, size_t root2_id, - void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) -{ - DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* root1 = nodes1 + root1_id; - DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* root2 = nodes2 + root2_id; - if(root1->isLeaf() && root2->isLeaf()) - { - CollisionObject* root1_obj = static_cast(root1->data); - CollisionObject* root2_obj = static_cast(root2->data); - return callback(root1_obj, root2_obj, cdata, min_dist); - } - - if(root2->isLeaf() || (!root1->isLeaf() && (root1->bv.size() > root2->bv.size()))) - { - FCL_REAL d1 = root2->bv.distance((nodes1 + root1->children[0])->bv); - FCL_REAL d2 = root2->bv.distance((nodes1 + root1->children[1])->bv); - - if(d2 < d1) - { - if(d2 < min_dist) - { - if(distanceRecurse(nodes1, root1->children[1], nodes2, root2_id, cdata, callback, min_dist)) - return true; - } - - if(d1 < min_dist) - { - if(distanceRecurse(nodes1, root1->children[0], nodes2, root2_id, cdata, callback, min_dist)) - return true; - } - } - else - { - if(d1 < min_dist) - { - if(distanceRecurse(nodes1, root1->children[0], nodes2, root2_id, cdata, callback, min_dist)) - return true; - } - - if(d2 < min_dist) - { - if(distanceRecurse(nodes1, root1->children[1], nodes2, root2_id, cdata, callback, min_dist)) - return true; - } - } - } - else - { - FCL_REAL d1 = root1->bv.distance((nodes2 + root2->children[0])->bv); - FCL_REAL d2 = root1->bv.distance((nodes2 + root2->children[1])->bv); - - if(d2 < d1) - { - if(d2 < min_dist) - { - if(distanceRecurse(nodes1, root1_id, nodes2, root2->children[1], cdata, callback, min_dist)) - return true; - } - - if(d1 < min_dist) - { - if(distanceRecurse(nodes1, root1_id, nodes2, root2->children[0], cdata, callback, min_dist)) - return true; - } - } - else - { - if(d1 < min_dist) - { - if(distanceRecurse(nodes1, root1_id, nodes2, root2->children[0], cdata, callback, min_dist)) - return true; - } - - if(d2 < min_dist) - { - if(distanceRecurse(nodes1, root1_id, nodes2, root2->children[1], cdata, callback, min_dist)) - return true; - } - } - } - - return false; -} - -bool distanceRecurse(DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes, size_t root_id, CollisionObject* query, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) -{ - DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* root = nodes + root_id; - if(root->isLeaf()) - { - CollisionObject* root_obj = static_cast(root->data); - return callback(root_obj, query, cdata, min_dist); - } - - FCL_REAL d1 = query->getAABB().distance((nodes + root->children[0])->bv); - FCL_REAL d2 = query->getAABB().distance((nodes + root->children[1])->bv); - - if(d2 < d1) - { - if(d2 < min_dist) - { - if(distanceRecurse(nodes, root->children[1], query, cdata, callback, min_dist)) - return true; - } - - if(d1 < min_dist) - { - if(distanceRecurse(nodes, root->children[0], query, cdata, callback, min_dist)) - return true; - } - } - else - { - if(d1 < min_dist) - { - if(distanceRecurse(nodes, root->children[0], query, cdata, callback, min_dist)) - return true; - } - - if(d2 < min_dist) - { - if(distanceRecurse(nodes, root->children[1], query, cdata, callback, min_dist)) - return true; - } - } - - return false; -} - -bool selfDistanceRecurse(DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes, size_t root_id, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) -{ - DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* root = nodes + root_id; - if(root->isLeaf()) return false; - - if(selfDistanceRecurse(nodes, root->children[0], cdata, callback, min_dist)) - return true; - - if(selfDistanceRecurse(nodes, root->children[1], cdata, callback, min_dist)) - return true; - - if(distanceRecurse(nodes, root->children[0], nodes, root->children[1], cdata, callback, min_dist)) - return true; - - return false; -} - - -#if FCL_HAVE_OCTOMAP -bool collisionRecurse(DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3d& tf2, void* cdata, CollisionCallBack callback) -{ - if(tf2.linear().isIdentity()) - return collisionRecurse_(nodes1, root1_id, tree2, root2, root2_bv, tf2.translation(), cdata, callback); - else - return collisionRecurse_(nodes1, root1_id, tree2, root2, root2_bv, tf2, cdata, callback); -} - - -bool distanceRecurse(DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3d& tf2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) -{ - if(tf2.linear().isIdentity()) - return distanceRecurse_(nodes1, root1_id, tree2, root2, root2_bv, tf2.translation(), cdata, callback, min_dist); - else - return distanceRecurse_(nodes1, root1_id, tree2, root2, root2_bv, tf2, cdata, callback, min_dist); -} - -#endif - -} // dynamic_AABB_tree_array - -} // details - - -void DynamicAABBTreeCollisionManager_Array::registerObjects(const std::vector& other_objs) -{ - if(other_objs.empty()) return; - - if(size() > 0) - { - BroadPhaseCollisionManager::registerObjects(other_objs); - } - else - { - DynamicAABBNode* leaves = new DynamicAABBNode[other_objs.size()]; - table.rehash(other_objs.size()); - for(size_t i = 0, size = other_objs.size(); i < size; ++i) - { - leaves[i].bv = other_objs[i]->getAABB(); - leaves[i].parent = dtree.NULL_NODE; - leaves[i].children[1] = dtree.NULL_NODE; - leaves[i].data = other_objs[i]; - table[other_objs[i]] = i; - } - - int n_leaves = other_objs.size(); - - dtree.init(leaves, n_leaves, tree_init_level); - - setup_ = true; - } -} - -void DynamicAABBTreeCollisionManager_Array::registerObject(CollisionObject* obj) -{ - size_t node = dtree.insert(obj->getAABB(), obj); - table[obj] = node; -} - -void DynamicAABBTreeCollisionManager_Array::unregisterObject(CollisionObject* obj) -{ - size_t node = table[obj]; - table.erase(obj); - dtree.remove(node); -} - -void DynamicAABBTreeCollisionManager_Array::setup() -{ - if(!setup_) - { - int num = dtree.size(); - if(num == 0) - { - setup_ = true; - return; - } - - int height = dtree.getMaxHeight(); - - - if(height - std::log((FCL_REAL)num) / std::log(2.0) < max_tree_nonbalanced_level) - dtree.balanceIncremental(tree_incremental_balance_pass); - else - dtree.balanceTopdown(); - - setup_ = true; - } -} - - -void DynamicAABBTreeCollisionManager_Array::update() -{ - for(DynamicAABBTable::const_iterator it = table.begin(), end = table.end(); it != end; ++it) - { - CollisionObject* obj = it->first; - size_t node = it->second; - dtree.getNodes()[node].bv = obj->getAABB(); - } - - dtree.refit(); - setup_ = false; - - setup(); -} - -void DynamicAABBTreeCollisionManager_Array::update_(CollisionObject* updated_obj) -{ - DynamicAABBTable::const_iterator it = table.find(updated_obj); - if(it != table.end()) - { - size_t node = it->second; - if(!dtree.getNodes()[node].bv.equal(updated_obj->getAABB())) - dtree.update(node, updated_obj->getAABB()); - } - setup_ = false; -} - -void DynamicAABBTreeCollisionManager_Array::update(CollisionObject* updated_obj) -{ - update_(updated_obj); - setup(); -} - -void DynamicAABBTreeCollisionManager_Array::update(const std::vector& updated_objs) -{ - for(size_t i = 0, size = updated_objs.size(); i < size; ++i) - update_(updated_objs[i]); - setup(); -} - - - -void DynamicAABBTreeCollisionManager_Array::collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const -{ - if(size() == 0) return; - switch(obj->collisionGeometry()->getNodeType()) - { -#if FCL_HAVE_OCTOMAP - case GEOM_OCTREE: - { - if(!octree_as_geometry_collide) - { - const OcTree* octree = static_cast(obj->collisionGeometry().get()); - details::dynamic_AABB_tree_array::collisionRecurse(dtree.getNodes(), dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(), obj->getTransform(), cdata, callback); - } - else - details::dynamic_AABB_tree_array::collisionRecurse(dtree.getNodes(), dtree.getRoot(), obj, cdata, callback); - } - break; -#endif - default: - details::dynamic_AABB_tree_array::collisionRecurse(dtree.getNodes(), dtree.getRoot(), obj, cdata, callback); - } -} - -void DynamicAABBTreeCollisionManager_Array::distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const -{ - if(size() == 0) return; - FCL_REAL min_dist = std::numeric_limits::max(); - switch(obj->collisionGeometry()->getNodeType()) - { -#if FCL_HAVE_OCTOMAP - case GEOM_OCTREE: - { - if(!octree_as_geometry_distance) - { - const OcTree* octree = static_cast(obj->collisionGeometry().get()); - details::dynamic_AABB_tree_array::distanceRecurse(dtree.getNodes(), dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(), obj->getTransform(), cdata, callback, min_dist); - } - else - details::dynamic_AABB_tree_array::distanceRecurse(dtree.getNodes(), dtree.getRoot(), obj, cdata, callback, min_dist); - } - break; -#endif - default: - details::dynamic_AABB_tree_array::distanceRecurse(dtree.getNodes(), dtree.getRoot(), obj, cdata, callback, min_dist); - } -} - -void DynamicAABBTreeCollisionManager_Array::collide(void* cdata, CollisionCallBack callback) const -{ - if(size() == 0) return; - details::dynamic_AABB_tree_array::selfCollisionRecurse(dtree.getNodes(), dtree.getRoot(), cdata, callback); -} - -void DynamicAABBTreeCollisionManager_Array::distance(void* cdata, DistanceCallBack callback) const -{ - if(size() == 0) return; - FCL_REAL min_dist = std::numeric_limits::max(); - details::dynamic_AABB_tree_array::selfDistanceRecurse(dtree.getNodes(), dtree.getRoot(), cdata, callback, min_dist); -} - -void DynamicAABBTreeCollisionManager_Array::collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const -{ - DynamicAABBTreeCollisionManager_Array* other_manager = static_cast(other_manager_); - if((size() == 0) || (other_manager->size() == 0)) return; - details::dynamic_AABB_tree_array::collisionRecurse(dtree.getNodes(), dtree.getRoot(), other_manager->dtree.getNodes(), other_manager->dtree.getRoot(), cdata, callback); -} - -void DynamicAABBTreeCollisionManager_Array::distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const -{ - DynamicAABBTreeCollisionManager_Array* other_manager = static_cast(other_manager_); - if((size() == 0) || (other_manager->size() == 0)) return; - FCL_REAL min_dist = std::numeric_limits::max(); - details::dynamic_AABB_tree_array::distanceRecurse(dtree.getNodes(), dtree.getRoot(), other_manager->dtree.getNodes(), other_manager->dtree.getRoot(), cdata, callback, min_dist); -} - - - - -} diff --git a/src/broadphase/broadphase_interval_tree.cpp b/src/broadphase/broadphase_interval_tree.cpp deleted file mode 100644 index 631d553fc..000000000 --- a/src/broadphase/broadphase_interval_tree.cpp +++ /dev/null @@ -1,656 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#include "fcl/broadphase/broadphase_interval_tree.h" -#include -#include -#include - - -namespace fcl -{ - -void IntervalTreeCollisionManager::unregisterObject(CollisionObject* obj) -{ - // must sorted before - setup(); - - EndPoint p; - p.value = obj->getAABB().min_[0]; - std::vector::iterator start1 = std::lower_bound(endpoints[0].begin(), endpoints[0].end(), p); - p.value = obj->getAABB().max_[0]; - std::vector::iterator end1 = std::upper_bound(start1, endpoints[0].end(), p); - - if(start1 < end1) - { - unsigned int start_id = start1 - endpoints[0].begin(); - unsigned int end_id = end1 - endpoints[0].begin(); - unsigned int cur_id = start_id; - for(unsigned int i = start_id; i < end_id; ++i) - { - if(endpoints[0][i].obj != obj) - { - if(i == cur_id) cur_id++; - else - { - endpoints[0][cur_id] = endpoints[0][i]; - cur_id++; - } - } - } - if(cur_id < end_id) - endpoints[0].resize(endpoints[0].size() - 2); - } - - p.value = obj->getAABB().min_[1]; - std::vector::iterator start2 = std::lower_bound(endpoints[1].begin(), endpoints[1].end(), p); - p.value = obj->getAABB().max_[1]; - std::vector::iterator end2 = std::upper_bound(start2, endpoints[1].end(), p); - - if(start2 < end2) - { - unsigned int start_id = start2 - endpoints[1].begin(); - unsigned int end_id = end2 - endpoints[1].begin(); - unsigned int cur_id = start_id; - for(unsigned int i = start_id; i < end_id; ++i) - { - if(endpoints[1][i].obj != obj) - { - if(i == cur_id) cur_id++; - else - { - endpoints[1][cur_id] = endpoints[1][i]; - cur_id++; - } - } - } - if(cur_id < end_id) - endpoints[1].resize(endpoints[1].size() - 2); - } - - - p.value = obj->getAABB().min_[2]; - std::vector::iterator start3 = std::lower_bound(endpoints[2].begin(), endpoints[2].end(), p); - p.value = obj->getAABB().max_[2]; - std::vector::iterator end3 = std::upper_bound(start3, endpoints[2].end(), p); - - if(start3 < end3) - { - unsigned int start_id = start3 - endpoints[2].begin(); - unsigned int end_id = end3 - endpoints[2].begin(); - unsigned int cur_id = start_id; - for(unsigned int i = start_id; i < end_id; ++i) - { - if(endpoints[2][i].obj != obj) - { - if(i == cur_id) cur_id++; - else - { - endpoints[2][cur_id] = endpoints[2][i]; - cur_id++; - } - } - } - if(cur_id < end_id) - endpoints[2].resize(endpoints[2].size() - 2); - } - - // update the interval tree - if(obj_interval_maps[0].find(obj) != obj_interval_maps[0].end()) - { - SAPInterval* ivl1 = obj_interval_maps[0][obj]; - SAPInterval* ivl2 = obj_interval_maps[1][obj]; - SAPInterval* ivl3 = obj_interval_maps[2][obj]; - - interval_trees[0]->deleteNode(ivl1); - interval_trees[1]->deleteNode(ivl2); - interval_trees[2]->deleteNode(ivl3); - - delete ivl1; - delete ivl2; - delete ivl3; - - obj_interval_maps[0].erase(obj); - obj_interval_maps[1].erase(obj); - obj_interval_maps[2].erase(obj); - } -} - -void IntervalTreeCollisionManager::registerObject(CollisionObject* obj) -{ - EndPoint p, q; - - p.obj = obj; - q.obj = obj; - p.minmax = 0; - q.minmax = 1; - p.value = obj->getAABB().min_[0]; - q.value = obj->getAABB().max_[0]; - endpoints[0].push_back(p); - endpoints[0].push_back(q); - - p.value = obj->getAABB().min_[1]; - q.value = obj->getAABB().max_[1]; - endpoints[1].push_back(p); - endpoints[1].push_back(q); - - p.value = obj->getAABB().min_[2]; - q.value = obj->getAABB().max_[2]; - endpoints[2].push_back(p); - endpoints[2].push_back(q); - setup_ = false; -} - -void IntervalTreeCollisionManager::setup() -{ - if(!setup_) - { - std::sort(endpoints[0].begin(), endpoints[0].end()); - std::sort(endpoints[1].begin(), endpoints[1].end()); - std::sort(endpoints[2].begin(), endpoints[2].end()); - - for(int i = 0; i < 3; ++i) - delete interval_trees[i]; - - for(int i = 0; i < 3; ++i) - interval_trees[i] = new IntervalTree; - - for(unsigned int i = 0, size = endpoints[0].size(); i < size; ++i) - { - EndPoint p = endpoints[0][i]; - CollisionObject* obj = p.obj; - if(p.minmax == 0) - { - SAPInterval* ivl1 = new SAPInterval(obj->getAABB().min_[0], obj->getAABB().max_[0], obj); - SAPInterval* ivl2 = new SAPInterval(obj->getAABB().min_[1], obj->getAABB().max_[1], obj); - SAPInterval* ivl3 = new SAPInterval(obj->getAABB().min_[2], obj->getAABB().max_[2], obj); - - interval_trees[0]->insert(ivl1); - interval_trees[1]->insert(ivl2); - interval_trees[2]->insert(ivl3); - - obj_interval_maps[0][obj] = ivl1; - obj_interval_maps[1][obj] = ivl2; - obj_interval_maps[2][obj] = ivl3; - } - } - - setup_ = true; - } -} - -void IntervalTreeCollisionManager::update() -{ - setup_ = false; - - for(unsigned int i = 0, size = endpoints[0].size(); i < size; ++i) - { - if(endpoints[0][i].minmax == 0) - endpoints[0][i].value = endpoints[0][i].obj->getAABB().min_[0]; - else - endpoints[0][i].value = endpoints[0][i].obj->getAABB().max_[0]; - } - - for(unsigned int i = 0, size = endpoints[1].size(); i < size; ++i) - { - if(endpoints[1][i].minmax == 0) - endpoints[1][i].value = endpoints[1][i].obj->getAABB().min_[1]; - else - endpoints[1][i].value = endpoints[1][i].obj->getAABB().max_[1]; - } - - for(unsigned int i = 0, size = endpoints[2].size(); i < size; ++i) - { - if(endpoints[2][i].minmax == 0) - endpoints[2][i].value = endpoints[2][i].obj->getAABB().min_[2]; - else - endpoints[2][i].value = endpoints[2][i].obj->getAABB().max_[2]; - } - - setup(); - -} - - -void IntervalTreeCollisionManager::update(CollisionObject* updated_obj) -{ - AABB old_aabb; - const AABB& new_aabb = updated_obj->getAABB(); - for(int i = 0; i < 3; ++i) - { - std::map::const_iterator it = obj_interval_maps[i].find(updated_obj); - interval_trees[i]->deleteNode(it->second); - old_aabb.min_[i] = it->second->low; - old_aabb.max_[i] = it->second->high; - it->second->low = new_aabb.min_[i]; - it->second->high = new_aabb.max_[i]; - interval_trees[i]->insert(it->second); - } - - EndPoint dummy; - std::vector::iterator it; - for(int i = 0; i < 3; ++i) - { - dummy.value = old_aabb.min_[i]; - it = std::lower_bound(endpoints[i].begin(), endpoints[i].end(), dummy); - for(; it != endpoints[i].end(); ++it) - { - if(it->obj == updated_obj && it->minmax == 0) - { - it->value = new_aabb.min_[i]; - break; - } - } - - dummy.value = old_aabb.max_[i]; - it = std::lower_bound(endpoints[i].begin(), endpoints[i].end(), dummy); - for(; it != endpoints[i].end(); ++it) - { - if(it->obj == updated_obj && it->minmax == 0) - { - it->value = new_aabb.max_[i]; - break; - } - } - - std::sort(endpoints[i].begin(), endpoints[i].end()); - } -} - -void IntervalTreeCollisionManager::update(const std::vector& updated_objs) -{ - for(size_t i = 0; i < updated_objs.size(); ++i) - update(updated_objs[i]); -} - -void IntervalTreeCollisionManager::clear() -{ - endpoints[0].clear(); - endpoints[1].clear(); - endpoints[2].clear(); - - delete interval_trees[0]; interval_trees[0] = NULL; - delete interval_trees[1]; interval_trees[1] = NULL; - delete interval_trees[2]; interval_trees[2] = NULL; - - for(int i = 0; i < 3; ++i) - { - for(std::map::const_iterator it = obj_interval_maps[i].begin(), end = obj_interval_maps[i].end(); - it != end; ++it) - { - delete it->second; - } - } - - for(int i = 0; i < 3; ++i) - obj_interval_maps[i].clear(); - - setup_ = false; -} - -void IntervalTreeCollisionManager::getObjects(std::vector& objs) const -{ - objs.resize(endpoints[0].size() / 2); - unsigned int j = 0; - for(unsigned int i = 0, size = endpoints[0].size(); i < size; ++i) - { - if(endpoints[0][i].minmax == 0) - { - objs[j] = endpoints[0][i].obj; j++; - } - } -} - -void IntervalTreeCollisionManager::collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const -{ - if(size() == 0) return; - collide_(obj, cdata, callback); -} - -bool IntervalTreeCollisionManager::collide_(CollisionObject* obj, void* cdata, CollisionCallBack callback) const -{ - static const unsigned int CUTOFF = 100; - - std::deque results0, results1, results2; - - results0 = interval_trees[0]->query(obj->getAABB().min_[0], obj->getAABB().max_[0]); - if(results0.size() > CUTOFF) - { - results1 = interval_trees[1]->query(obj->getAABB().min_[1], obj->getAABB().max_[1]); - if(results1.size() > CUTOFF) - { - results2 = interval_trees[2]->query(obj->getAABB().min_[2], obj->getAABB().max_[2]); - if(results2.size() > CUTOFF) - { - int d1 = results0.size(); - int d2 = results1.size(); - int d3 = results2.size(); - - if(d1 >= d2 && d1 >= d3) - return checkColl(results0.begin(), results0.end(), obj, cdata, callback); - else if(d2 >= d1 && d2 >= d3) - return checkColl(results1.begin(), results1.end(), obj, cdata, callback); - else - return checkColl(results2.begin(), results2.end(), obj, cdata, callback); - } - else - return checkColl(results2.begin(), results2.end(), obj, cdata, callback); - } - else - return checkColl(results1.begin(), results1.end(), obj, cdata, callback); - } - else - return checkColl(results0.begin(), results0.end(), obj, cdata, callback); -} - -void IntervalTreeCollisionManager::distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const -{ - if(size() == 0) return; - FCL_REAL min_dist = std::numeric_limits::max(); - distance_(obj, cdata, callback, min_dist); -} - -bool IntervalTreeCollisionManager::distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const -{ - static const unsigned int CUTOFF = 100; - - Vector3d delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5; - AABB aabb = obj->getAABB(); - if(min_dist < std::numeric_limits::max()) - { - Vector3d min_dist_delta(min_dist, min_dist, min_dist); - aabb.expand(min_dist_delta); - } - - int status = 1; - FCL_REAL old_min_distance; - - while(1) - { - bool dist_res = false; - - old_min_distance = min_dist; - - std::deque results0, results1, results2; - - results0 = interval_trees[0]->query(aabb.min_[0], aabb.max_[0]); - if(results0.size() > CUTOFF) - { - results1 = interval_trees[1]->query(aabb.min_[1], aabb.max_[1]); - if(results1.size() > CUTOFF) - { - results2 = interval_trees[2]->query(aabb.min_[2], aabb.max_[2]); - if(results2.size() > CUTOFF) - { - int d1 = results0.size(); - int d2 = results1.size(); - int d3 = results2.size(); - - if(d1 >= d2 && d1 >= d3) - dist_res = checkDist(results0.begin(), results0.end(), obj, cdata, callback, min_dist); - else if(d2 >= d1 && d2 >= d3) - dist_res = checkDist(results1.begin(), results1.end(), obj, cdata, callback, min_dist); - else - dist_res = checkDist(results2.begin(), results2.end(), obj, cdata, callback, min_dist); - } - else - dist_res = checkDist(results2.begin(), results2.end(), obj, cdata, callback, min_dist); - } - else - dist_res = checkDist(results1.begin(), results1.end(), obj, cdata, callback, min_dist); - } - else - dist_res = checkDist(results0.begin(), results0.end(), obj, cdata, callback, min_dist); - - if(dist_res) return true; - - results0.clear(); - results1.clear(); - results2.clear(); - - if(status == 1) - { - if(old_min_distance < std::numeric_limits::max()) - break; - else - { - if(min_dist < old_min_distance) - { - Vector3d min_dist_delta(min_dist, min_dist, min_dist); - aabb = AABB(obj->getAABB(), min_dist_delta); - status = 0; - } - else - { - if(aabb.equal(obj->getAABB())) - aabb.expand(delta); - else - aabb.expand(obj->getAABB(), 2.0); - } - } - } - else if(status == 0) - break; - } - - return false; -} - -void IntervalTreeCollisionManager::collide(void* cdata, CollisionCallBack callback) const -{ - if(size() == 0) return; - - std::set active; - std::set > overlap; - unsigned int n = endpoints[0].size(); - double diff_x = endpoints[0][0].value - endpoints[0][n-1].value; - double diff_y = endpoints[1][0].value - endpoints[1][n-1].value; - double diff_z = endpoints[2][0].value - endpoints[2][n-1].value; - - int axis = 0; - if(diff_y > diff_x && diff_y > diff_z) - axis = 1; - else if(diff_z > diff_y && diff_z > diff_x) - axis = 2; - - for(unsigned int i = 0; i < n; ++i) - { - const EndPoint& endpoint = endpoints[axis][i]; - CollisionObject* index = endpoint.obj; - if(endpoint.minmax == 0) - { - std::set::iterator iter = active.begin(); - std::set::iterator end = active.end(); - for(; iter != end; ++iter) - { - CollisionObject* active_index = *iter; - const AABB& b0 = active_index->getAABB(); - const AABB& b1 = index->getAABB(); - - int axis2 = (axis + 1) % 3; - int axis3 = (axis + 2) % 3; - - if(b0.axisOverlap(b1, axis2) && b0.axisOverlap(b1, axis3)) - { - std::pair >::iterator, bool> insert_res; - if(active_index < index) - insert_res = overlap.insert(std::make_pair(active_index, index)); - else - insert_res = overlap.insert(std::make_pair(index, active_index)); - - if(insert_res.second) - { - if(callback(active_index, index, cdata)) - return; - } - } - } - active.insert(index); - } - else - active.erase(index); - } - -} - -void IntervalTreeCollisionManager::distance(void* cdata, DistanceCallBack callback) const -{ - if(size() == 0) return; - - enable_tested_set_ = true; - tested_set.clear(); - FCL_REAL min_dist = std::numeric_limits::max(); - - for(size_t i = 0; i < endpoints[0].size(); ++i) - if(distance_(endpoints[0][i].obj, cdata, callback, min_dist)) break; - - enable_tested_set_ = false; - tested_set.clear(); -} - -void IntervalTreeCollisionManager::collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const -{ - IntervalTreeCollisionManager* other_manager = static_cast(other_manager_); - - if((size() == 0) || (other_manager->size() == 0)) return; - - if(this == other_manager) - { - collide(cdata, callback); - return; - } - - if(this->size() < other_manager->size()) - { - for(size_t i = 0, size = endpoints[0].size(); i < size; ++i) - if(other_manager->collide_(endpoints[0][i].obj, cdata, callback)) return; - } - else - { - for(size_t i = 0, size = other_manager->endpoints[0].size(); i < size; ++i) - if(collide_(other_manager->endpoints[0][i].obj, cdata, callback)) return; - } -} - -void IntervalTreeCollisionManager::distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const -{ - IntervalTreeCollisionManager* other_manager = static_cast(other_manager_); - - if((size() == 0) || (other_manager->size() == 0)) return; - - if(this == other_manager) - { - distance(cdata, callback); - return; - } - - FCL_REAL min_dist = std::numeric_limits::max(); - - if(this->size() < other_manager->size()) - { - for(size_t i = 0, size = endpoints[0].size(); i < size; ++i) - if(other_manager->distance_(endpoints[0][i].obj, cdata, callback, min_dist)) return; - } - else - { - for(size_t i = 0, size = other_manager->endpoints[0].size(); i < size; ++i) - if(distance_(other_manager->endpoints[0][i].obj, cdata, callback, min_dist)) return; - } -} - -bool IntervalTreeCollisionManager::empty() const -{ - return endpoints[0].empty(); -} - -bool IntervalTreeCollisionManager::checkColl(std::deque::const_iterator pos_start, std::deque::const_iterator pos_end, CollisionObject* obj, void* cdata, CollisionCallBack callback) const -{ - while(pos_start < pos_end) - { - SAPInterval* ivl = static_cast(*pos_start); - if(ivl->obj != obj) - { - if(ivl->obj->getAABB().overlap(obj->getAABB())) - { - if(callback(ivl->obj, obj, cdata)) - return true; - } - } - - pos_start++; - } - - return false; -} - -bool IntervalTreeCollisionManager::checkDist(std::deque::const_iterator pos_start, std::deque::const_iterator pos_end, CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const -{ - while(pos_start < pos_end) - { - SAPInterval* ivl = static_cast(*pos_start); - if(ivl->obj != obj) - { - if(!enable_tested_set_) - { - if(ivl->obj->getAABB().distance(obj->getAABB()) < min_dist) - { - if(callback(ivl->obj, obj, cdata, min_dist)) - return true; - } - } - else - { - if(!inTestedSet(ivl->obj, obj)) - { - if(ivl->obj->getAABB().distance(obj->getAABB()) < min_dist) - { - if(callback(ivl->obj, obj, cdata, min_dist)) - return true; - } - - insertTestedSet(ivl->obj, obj); - } - } - } - - pos_start++; - } - - return false; -} - -} diff --git a/src/broadphase/hierarchy_tree.cpp b/src/broadphase/hierarchy_tree.cpp deleted file mode 100644 index 1981543c8..000000000 --- a/src/broadphase/hierarchy_tree.cpp +++ /dev/null @@ -1,136 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#include "fcl/broadphase/hierarchy_tree.h" - -namespace fcl -{ - -template<> -size_t select(const NodeBase& node, const NodeBase& node1, const NodeBase& node2) -{ - const AABB& bv = node.bv; - const AABB& bv1 = node1.bv; - const AABB& bv2 = node2.bv; - Vector3d v = bv.min_ + bv.max_; - Vector3d v1 = v - (bv1.min_ + bv1.max_); - Vector3d v2 = v - (bv2.min_ + bv2.max_); - FCL_REAL d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]); - FCL_REAL d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]); - return (d1 < d2) ? 0 : 1; -} - -template<> -size_t select(const AABB& query, const NodeBase& node1, const NodeBase& node2) -{ - const AABB& bv = query; - const AABB& bv1 = node1.bv; - const AABB& bv2 = node2.bv; - Vector3d v = bv.min_ + bv.max_; - Vector3d v1 = v - (bv1.min_ + bv1.max_); - Vector3d v2 = v - (bv2.min_ + bv2.max_); - FCL_REAL d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]); - FCL_REAL d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]); - return (d1 < d2) ? 0 : 1; -} - -template<> -bool HierarchyTree::update(NodeBase* leaf, const AABB& bv_, const Vector3d& vel, FCL_REAL margin) -{ - AABB bv(bv_); - if(leaf->bv.contain(bv)) return false; - Vector3d marginv = Vector3d::Constant(margin); - bv.min_ -= marginv; - bv.max_ += marginv; - if(vel[0] > 0) bv.max_[0] += vel[0]; - else bv.min_[0] += vel[0]; - if(vel[1] > 0) bv.max_[1] += vel[1]; - else bv.min_[1] += vel[1]; - if(vel[2] > 0) bv.max_[2] += vel[2]; - else bv.max_[2] += vel[2]; - update(leaf, bv); - return true; -} - -template<> -bool HierarchyTree::update(NodeBase* leaf, const AABB& bv_, const Vector3d& vel) -{ - AABB bv(bv_); - if(leaf->bv.contain(bv)) return false; - if(vel[0] > 0) bv.max_[0] += vel[0]; - else bv.min_[0] += vel[0]; - if(vel[1] > 0) bv.max_[1] += vel[1]; - else bv.min_[1] += vel[1]; - if(vel[2] > 0) bv.max_[2] += vel[2]; - else bv.max_[2] += vel[2]; - update(leaf, bv); - return true; -} - -namespace implementation_array -{ -template<> -size_t select(size_t query, size_t node1, size_t node2, NodeBase* nodes) -{ - const AABB& bv = nodes[query].bv; - const AABB& bv1 = nodes[node1].bv; - const AABB& bv2 = nodes[node2].bv; - Vector3d v = bv.min_ + bv.max_; - Vector3d v1 = v - (bv1.min_ + bv1.max_); - Vector3d v2 = v - (bv2.min_ + bv2.max_); - FCL_REAL d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]); - FCL_REAL d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]); - return (d1 < d2) ? 0 : 1; -} - -template<> -size_t select(const AABB& query, size_t node1, size_t node2, NodeBase* nodes) -{ - const AABB& bv = query; - const AABB& bv1 = nodes[node1].bv; - const AABB& bv2 = nodes[node2].bv; - Vector3d v = bv.min_ + bv.max_; - Vector3d v1 = v - (bv1.min_ + bv1.max_); - Vector3d v2 = v - (bv2.min_ + bv2.max_); - FCL_REAL d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]); - FCL_REAL d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]); - return (d1 < d2) ? 0 : 1; -} - -} - -} diff --git a/src/broadphase/interval_tree.cpp b/src/broadphase/interval_tree.cpp deleted file mode 100644 index 7eba6b026..000000000 --- a/src/broadphase/interval_tree.cpp +++ /dev/null @@ -1,567 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#include "fcl/broadphase/interval_tree.h" -#include -#include -#include - - -namespace fcl -{ - -IntervalTreeNode::IntervalTreeNode(){} - -IntervalTreeNode::IntervalTreeNode(SimpleInterval* new_interval) : - stored_interval (new_interval), - key(new_interval->low), - high(new_interval->high), - max_high(high) {} - -IntervalTreeNode::~IntervalTreeNode() {} - - -/// @brief Class describes the information needed when we take the -/// right branch in searching for intervals but possibly come back -/// and check the left branch as well. -struct it_recursion_node -{ -public: - IntervalTreeNode* start_node; - - unsigned int parent_index; - - bool try_right_branch; -}; - - -IntervalTree::IntervalTree() -{ - nil = new IntervalTreeNode; - nil->left = nil->right = nil->parent = nil; - nil->red = false; - nil->key = nil->high = nil->max_high = -std::numeric_limits::max(); - nil->stored_interval = NULL; - - root = new IntervalTreeNode; - root->parent = root->left = root->right = nil; - root->key = root->high = root->max_high = std::numeric_limits::max(); - root->red = false; - root->stored_interval = NULL; - - /// the following are used for the query function - recursion_node_stack_size = 128; - recursion_node_stack = (it_recursion_node*)malloc(recursion_node_stack_size*sizeof(it_recursion_node)); - recursion_node_stack_top = 1; - recursion_node_stack[0].start_node = NULL; -} - -IntervalTree::~IntervalTree() -{ - IntervalTreeNode* x = root->left; - std::deque nodes_to_free; - - if(x != nil) - { - if(x->left != nil) - { - nodes_to_free.push_back(x->left); - } - if(x->right != nil) - { - nodes_to_free.push_back(x->right); - } - - delete x; - while( nodes_to_free.size() > 0) - { - x = nodes_to_free.back(); - nodes_to_free.pop_back(); - if(x->left != nil) - { - nodes_to_free.push_back(x->left); - } - if(x->right != nil) - { - nodes_to_free.push_back(x->right); - } - delete x; - } - } - delete nil; - delete root; - free(recursion_node_stack); -} - - -void IntervalTree::leftRotate(IntervalTreeNode* x) -{ - IntervalTreeNode* y; - - y = x->right; - x->right = y->left; - - if(y->left != nil) y->left->parent = x; - - y->parent = x->parent; - - if(x == x->parent->left) - x->parent->left = y; - else - x->parent->right = y; - - y->left = x; - x->parent = y; - - x->max_high = std::max(x->left->max_high, std::max(x->right->max_high, x->high)); - y->max_high = std::max(x->max_high, std::max(y->right->max_high, y->high)); -} - - -void IntervalTree::rightRotate(IntervalTreeNode* y) -{ - IntervalTreeNode* x; - - x = y->left; - y->left = x->right; - - if(nil != x->right) x->right->parent = y; - - x->parent = y->parent; - if(y == y->parent->left) - y->parent->left = x; - else - y->parent->right = x; - - x->right = y; - y->parent = x; - - y->max_high = std::max(y->left->max_high, std::max(y->right->max_high, y->high)); - x->max_high = std::max(x->left->max_high, std::max(y->max_high, x->high)); -} - - - -/// @brief Inserts z into the tree as if it were a regular binary tree -void IntervalTree::recursiveInsert(IntervalTreeNode* z) -{ - IntervalTreeNode* x; - IntervalTreeNode* y; - - z->left = z->right = nil; - y = root; - x = root->left; - while(x != nil) - { - y = x; - if(x->key > z->key) - x = x->left; - else - x = x->right; - } - z->parent = y; - if((y == root) || (y->key > z->key)) - y->left = z; - else - y->right = z; -} - - -void IntervalTree::fixupMaxHigh(IntervalTreeNode* x) -{ - while(x != root) - { - x->max_high = std::max(x->high, std::max(x->left->max_high, x->right->max_high)); - x = x->parent; - } -} - -IntervalTreeNode* IntervalTree::insert(SimpleInterval* new_interval) -{ - IntervalTreeNode* y; - IntervalTreeNode* x; - IntervalTreeNode* new_node; - - x = new IntervalTreeNode(new_interval); - recursiveInsert(x); - fixupMaxHigh(x->parent); - new_node = x; - x->red = true; - while(x->parent->red) - { - /// use sentinel instead of checking for root - if(x->parent == x->parent->parent->left) - { - y = x->parent->parent->right; - if(y->red) - { - x->parent->red = true; - y->red = true; - x->parent->parent->red = true; - x = x->parent->parent; - } - else - { - if(x == x->parent->right) - { - x = x->parent; - leftRotate(x); - } - x->parent->red = false; - x->parent->parent->red = true; - rightRotate(x->parent->parent); - } - } - else - { - y = x->parent->parent->left; - if(y->red) - { - x->parent->red = false; - y->red = false; - x->parent->parent->red = true; - x = x->parent->parent; - } - else - { - if(x == x->parent->left) - { - x = x->parent; - rightRotate(x); - } - x->parent->red = false; - x->parent->parent->red = true; - leftRotate(x->parent->parent); - } - } - } - root->left->red = false; - return new_node; -} - -IntervalTreeNode* IntervalTree::getSuccessor(IntervalTreeNode* x) const -{ - IntervalTreeNode* y; - - if(nil != (y = x->right)) - { - while(y->left != nil) - y = y->left; - return y; - } - else - { - y = x->parent; - while(x == y->right) - { - x = y; - y = y->parent; - } - if(y == root) return nil; - return y; - } -} - - -IntervalTreeNode* IntervalTree::getPredecessor(IntervalTreeNode* x) const -{ - IntervalTreeNode* y; - - if(nil != (y = x->left)) - { - while(y->right != nil) - y = y->right; - return y; - } - else - { - y = x->parent; - while(x == y->left) - { - if(y == root) return nil; - x = y; - y = y->parent; - } - return y; - } -} - -void IntervalTreeNode::print(IntervalTreeNode* nil, IntervalTreeNode* root) const -{ - stored_interval->print(); - std::cout << ", k = " << key << ", h = " << high << ", mH = " << max_high; - std::cout << " l->key = "; - if(left == nil) std::cout << "NULL"; else std::cout << left->key; - std::cout << " r->key = "; - if(right == nil) std::cout << "NULL"; else std::cout << right->key; - std::cout << " p->key = "; - if(parent == root) std::cout << "NULL"; else std::cout << parent->key; - std::cout << " red = " << (int)red << std::endl; -} - -void IntervalTree::recursivePrint(IntervalTreeNode* x) const -{ - if(x != nil) - { - recursivePrint(x->left); - x->print(nil,root); - recursivePrint(x->right); - } -} - - -void IntervalTree::print() const -{ - recursivePrint(root->left); -} - -void IntervalTree::deleteFixup(IntervalTreeNode* x) -{ - IntervalTreeNode* w; - IntervalTreeNode* root_left_node = root->left; - - while((!x->red) && (root_left_node != x)) - { - if(x == x->parent->left) - { - w = x->parent->right; - if(w->red) - { - w->red = false; - x->parent->red = true; - leftRotate(x->parent); - w = x->parent->right; - } - if((!w->right->red) && (!w->left->red)) - { - w->red = true; - x = x->parent; - } - else - { - if(!w->right->red) - { - w->left->red = false; - w->red = true; - rightRotate(w); - w = x->parent->right; - } - w->red = x->parent->red; - x->parent->red = false; - w->right->red = false; - leftRotate(x->parent); - x = root_left_node; - } - } - else - { - w = x->parent->left; - if(w->red) - { - w->red = false; - x->parent->red = true; - rightRotate(x->parent); - w = x->parent->left; - } - if((!w->right->red) && (!w->left->red)) - { - w->red = true; - x = x->parent; - } - else - { - if(!w->left->red) - { - w->right->red = false; - w->red = true; - leftRotate(w); - w = x->parent->left; - } - w->red = x->parent->red; - x->parent->red = false; - w->left->red = false; - rightRotate(x->parent); - x = root_left_node; - } - } - } - x->red = false; -} - -void IntervalTree::deleteNode(SimpleInterval* ivl) -{ - IntervalTreeNode* node = recursiveSearch(root, ivl); - if(node) - deleteNode(node); -} - -IntervalTreeNode* IntervalTree::recursiveSearch(IntervalTreeNode* node, SimpleInterval* ivl) const -{ - if(node != nil) - { - if(node->stored_interval == ivl) - return node; - - IntervalTreeNode* left = recursiveSearch(node->left, ivl); - if(left != nil) return left; - IntervalTreeNode* right = recursiveSearch(node->right, ivl); - if(right != nil) return right; - } - - return nil; -} - -SimpleInterval* IntervalTree::deleteNode(IntervalTreeNode* z) -{ - IntervalTreeNode* y; - IntervalTreeNode* x; - SimpleInterval* node_to_delete = z->stored_interval; - - y= ((z->left == nil) || (z->right == nil)) ? z : getSuccessor(z); - x= (y->left == nil) ? y->right : y->left; - if(root == (x->parent = y->parent)) - { - root->left = x; - } - else - { - if(y == y->parent->left) - { - y->parent->left = x; - } - else - { - y->parent->right = x; - } - } - - /// @brief y should not be nil in this case - /// y is the node to splice out and x is its child - if(y != z) - { - y->max_high = -std::numeric_limits::max(); - y->left = z->left; - y->right = z->right; - y->parent = z->parent; - z->left->parent = z->right->parent = y; - if(z == z->parent->left) - z->parent->left = y; - else - z->parent->right = y; - - fixupMaxHigh(x->parent); - if(!(y->red)) - { - y->red = z->red; - deleteFixup(x); - } - else - y->red = z->red; - delete z; - } - else - { - fixupMaxHigh(x->parent); - if(!(y->red)) deleteFixup(x); - delete y; - } - - return node_to_delete; -} - -/// @brief returns 1 if the intervals overlap, and 0 otherwise -bool overlap(double a1, double a2, double b1, double b2) -{ - if(a1 <= b1) - { - return (b1 <= a2); - } - else - { - return (a1 <= b2); - } -} - -std::deque IntervalTree::query(double low, double high) -{ - std::deque result_stack; - IntervalTreeNode* x = root->left; - bool run = (x != nil); - - current_parent = 0; - - while(run) - { - if(overlap(low,high,x->key,x->high)) - { - result_stack.push_back(x->stored_interval); - recursion_node_stack[current_parent].try_right_branch = true; - } - if(x->left->max_high >= low) - { - if(recursion_node_stack_top == recursion_node_stack_size) - { - recursion_node_stack_size *= 2; - recursion_node_stack = (it_recursion_node *)realloc(recursion_node_stack, recursion_node_stack_size * sizeof(it_recursion_node)); - if(recursion_node_stack == NULL) - exit(1); - } - recursion_node_stack[recursion_node_stack_top].start_node = x; - recursion_node_stack[recursion_node_stack_top].try_right_branch = false; - recursion_node_stack[recursion_node_stack_top].parent_index = current_parent; - current_parent = recursion_node_stack_top++; - x = x->left; - } - else - x = x->right; - - run = (x != nil); - while((!run) && (recursion_node_stack_top > 1)) - { - if(recursion_node_stack[--recursion_node_stack_top].try_right_branch) - { - x=recursion_node_stack[recursion_node_stack_top].start_node->right; - current_parent=recursion_node_stack[recursion_node_stack_top].parent_index; - recursion_node_stack[current_parent].try_right_branch = true; - run = (x != nil); - } - } - } - return result_stack; -} - -} diff --git a/src/ccd/conservative_advancement.cpp b/src/ccd/conservative_advancement.cpp deleted file mode 100644 index e271201d9..000000000 --- a/src/ccd/conservative_advancement.cpp +++ /dev/null @@ -1,962 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#include "fcl/ccd/conservative_advancement.h" -#include "fcl/ccd/motion.h" -#include "fcl/collision_node.h" -#include "fcl/traversal/traversal_node_bvhs.h" -#include "fcl/traversal/traversal_node_setup.h" -#include "fcl/traversal/traversal_recurse.h" -#include "fcl/collision.h" - - -namespace fcl -{ - - - - -template -bool conservativeAdvancement(const BVHModel& o1, - const MotionBase* motion1, - const BVHModel& o2, - const MotionBase* motion2, - const CollisionRequest& request, - CollisionResult& result, - FCL_REAL& toc) -{ - Transform3d tf1, tf2; - motion1->getCurrentTransform(tf1); - motion2->getCurrentTransform(tf2); - - // whether the first start configuration is in collision - if(collide(&o1, tf1, &o2, tf2, request, result)) - { - toc = 0; - return true; - } - - - BVHModel* o1_tmp = new BVHModel(o1); - BVHModel* o2_tmp = new BVHModel(o2); - - - MeshConservativeAdvancementTraversalNode node; - - node.motion1 = motion1; - node.motion2 = motion2; - - do - { - // repeatedly update mesh to global coordinate, so time consuming - initialize(node, *o1_tmp, tf1, *o2_tmp, tf2); - - node.delta_t = 1; - node.min_distance = std::numeric_limits::max(); - - distanceRecurse(&node, 0, 0, NULL); - - if(node.delta_t <= node.t_err) - { - // std::cout << node.delta_t << " " << node.t_err << std::endl; - break; - } - - node.toc += node.delta_t; - if(node.toc > 1) - { - node.toc = 1; - break; - } - - node.motion1->integrate(node.toc); - node.motion2->integrate(node.toc); - - motion1->getCurrentTransform(tf1); - motion2->getCurrentTransform(tf2); - } - while(1); - - delete o1_tmp; - delete o2_tmp; - - toc = node.toc; - - if(node.toc < 1) - return true; - - return false; -} - -namespace details -{ - -template -bool conservativeAdvancementMeshOriented(const BVHModel& o1, - const MotionBase* motion1, - const BVHModel& o2, - const MotionBase* motion2, - const CollisionRequest& request, - CollisionResult& result, - FCL_REAL& toc) -{ - Transform3d tf1, tf2; - motion1->getCurrentTransform(tf1); - motion2->getCurrentTransform(tf2); - - // whether the first start configuration is in collision - if(collide(&o1, tf1, &o2, tf2, request, result)) - { - toc = 0; - return true; - } - - - ConservativeAdvancementOrientedNode node; - - initialize(node, o1, tf1, o2, tf2); - - node.motion1 = motion1; - node.motion2 = motion2; - - do - { - node.motion1->getCurrentTransform(tf1); - node.motion2->getCurrentTransform(tf2); - - // compute the transformation from 1 to 2 - Transform3d tf = tf1.inverse() * tf2; - node.R = tf.linear(); - node.T = tf.translation(); - - node.delta_t = 1; - node.min_distance = std::numeric_limits::max(); - - distanceRecurse(&node, 0, 0, NULL); - - if(node.delta_t <= node.t_err) - { - // std::cout << node.delta_t << " " << node.t_err << std::endl; - break; - } - - node.toc += node.delta_t; - if(node.toc > 1) - { - node.toc = 1; - break; - } - - node.motion1->integrate(node.toc); - node.motion2->integrate(node.toc); - } - while(1); - - toc = node.toc; - - if(node.toc < 1) - return true; - - return false; -} - - -} - -template<> -bool conservativeAdvancement(const BVHModel& o1, - const MotionBase* motion1, - const BVHModel& o2, - const MotionBase* motion2, - const CollisionRequest& request, - CollisionResult& result, - FCL_REAL& toc); - - -template<> -bool conservativeAdvancement(const BVHModel& o1, - const MotionBase* motion1, - const BVHModel& o2, - const MotionBase* motion2, - const CollisionRequest& request, - CollisionResult& result, - FCL_REAL& toc); - -template -bool conservativeAdvancement(const S1& o1, - const MotionBase* motion1, - const S2& o2, - const MotionBase* motion2, - const NarrowPhaseSolver* solver, - const CollisionRequest& request, - CollisionResult& result, - FCL_REAL& toc) -{ - Transform3d tf1, tf2; - motion1->getCurrentTransform(tf1); - motion2->getCurrentTransform(tf2); - - // whether the first start configuration is in collision - if(collide(&o1, tf1, &o2, tf2, request, result)) - { - toc = 0; - return true; - } - - ShapeConservativeAdvancementTraversalNode node; - - initialize(node, o1, tf1, o2, tf2, solver); - - node.motion1 = motion1; - node.motion2 = motion2; - - do - { - motion1->getCurrentTransform(tf1); - motion2->getCurrentTransform(tf2); - node.tf1 = tf1; - node.tf2 = tf2; - - node.delta_t = 1; - node.min_distance = std::numeric_limits::max(); - - distanceRecurse(&node, 0, 0, NULL); - - if(node.delta_t <= node.t_err) - { - // std::cout << node.delta_t << " " << node.t_err << std::endl; - break; - } - - node.toc += node.delta_t; - if(node.toc > 1) - { - node.toc = 1; - break; - } - - node.motion1->integrate(node.toc); - node.motion2->integrate(node.toc); - } - while(1); - - toc = node.toc; - - if(node.toc < 1) - return true; - - return false; -} - -template -bool conservativeAdvancement(const BVHModel& o1, - const MotionBase* motion1, - const S& o2, - const MotionBase* motion2, - const NarrowPhaseSolver* nsolver, - const CollisionRequest& request, - CollisionResult& result, - FCL_REAL& toc) -{ - Transform3d tf1, tf2; - motion1->getCurrentTransform(tf1); - motion2->getCurrentTransform(tf2); - - if(collide(&o1, tf1, &o2, tf2, request, result)) - { - toc = 0; - return true; - } - - BVHModel* o1_tmp = new BVHModel(o1); - - MeshShapeConservativeAdvancementTraversalNode node; - - node.motion1 = motion1; - node.motion2 = motion2; - - do - { - // initialize update mesh to global coordinate, so time consuming - initialize(node, *o1_tmp, tf1, o2, tf2, nsolver); - - node.delta_t = 1; - node.min_distance = std::numeric_limits::max(); - - distanceRecurse(&node, 0, 0, NULL); - - if(node.delta_t <= node.t_err) - { - break; - } - - node.toc += node.delta_t; - if(node.toc > 1) - { - node.toc = 1; - break; - } - - node.motion1->integrate(node.toc); - node.motion2->integrate(node.toc); - - motion1->getCurrentTransform(tf1); - motion2->getCurrentTransform(tf2); - } - while(1); - - delete o1_tmp; - - toc = node.toc; - - if(node.toc < 1) - return true; - - return false; -} - -namespace details -{ - -template -bool conservativeAdvancementMeshShapeOriented(const BVHModel& o1, - const MotionBase* motion1, - const S& o2, - const MotionBase* motion2, - const NarrowPhaseSolver* nsolver, - const CollisionRequest& request, - CollisionResult& result, - FCL_REAL& toc) -{ - Transform3d tf1, tf2; - motion1->getCurrentTransform(tf1); - motion2->getCurrentTransform(tf2); - - if(collide(&o1, tf1, &o2, tf2, request, result)) - { - toc = 0; - return true; - } - - ConservativeAdvancementOrientedNode node; - - initialize(node, o1, tf1, o2, tf2, nsolver); - - node.motion1 = motion1; - node.motion2 = motion2; - - do - { - node.motion1->getCurrentTransform(tf1); - node.motion2->getCurrentTransform(tf2); - - node.tf1 = tf1; - node.tf2 = tf2; - - node.delta_t = 1; - node.min_distance = std::numeric_limits::max(); - - distanceRecurse(&node, 0, 0, NULL); - - if(node.delta_t <= node.t_err) - { - break; - } - - node.toc += node.delta_t; - if(node.toc > 1) - { - node.toc = 1; - break; - } - - node.motion1->integrate(node.toc); - node.motion2->integrate(node.toc); - } - while(1); - - toc = node.toc; - - if(node.toc < 1) - return true; - - return false; -} - -} - - -template -bool conservativeAdvancement(const BVHModel& o1, - const MotionBase* motion1, - const S& o2, - const MotionBase* motion2, - const NarrowPhaseSolver* nsolver, - const CollisionRequest& request, - CollisionResult& result, - FCL_REAL& toc) -{ - return details::conservativeAdvancementMeshShapeOriented >(o1, motion1, o2, motion2, nsolver, request, result, toc); -} - -template -bool conservativeAdvancement(const BVHModel& o1, - const MotionBase* motion1, - const S& o2, - const MotionBase* motion2, - const NarrowPhaseSolver* nsolver, - const CollisionRequest& request, - CollisionResult& result, - FCL_REAL& toc) -{ - return details::conservativeAdvancementMeshShapeOriented >(o1, motion1, o2, motion2, nsolver, request, result, toc); -} - -template -bool conservativeAdvancement(const S& o1, - const MotionBase* motion1, - const BVHModel& o2, - const MotionBase* motion2, - const NarrowPhaseSolver* nsolver, - const CollisionRequest& request, - CollisionResult& result, - FCL_REAL& toc) -{ - Transform3d tf1, tf2; - motion1->getCurrentTransform(tf1); - motion2->getCurrentTransform(tf2); - - if(collide(&o1, tf1, &o2, tf2, request, result)) - { - toc = 0; - return true; - } - - BVHModel* o2_tmp = new BVHModel(o2); - - ShapeMeshConservativeAdvancementTraversalNode node; - - node.motion1 = motion1; - node.motion2 = motion2; - - do - { - // initialize update mesh to global coordinate, so time consuming - initialize(node, o1, tf1, *o2_tmp, tf2, nsolver); - - node.delta_t = 1; - node.min_distance = std::numeric_limits::max(); - - distanceRecurse(&node, 0, 0, NULL); - - if(node.delta_t <= node.t_err) - { - break; - } - - node.toc += node.delta_t; - if(node.toc > 1) - { - node.toc = 1; - break; - } - - node.motion1->integrate(node.toc); - node.motion2->integrate(node.toc); - - motion1->getCurrentTransform(tf1); - motion2->getCurrentTransform(tf2); - } - while(1); - - delete o2_tmp; - - toc = node.toc; - - if(node.toc < 1) - return true; - - return false; -} - -namespace details -{ - -template -bool conservativeAdvancementShapeMeshOriented(const S& o1, - const MotionBase* motion1, - const BVHModel& o2, - const MotionBase* motion2, - const NarrowPhaseSolver* nsolver, - const CollisionRequest& request, - CollisionResult& result, - FCL_REAL& toc) -{ - Transform3d tf1, tf2; - motion1->getCurrentTransform(tf1); - motion2->getCurrentTransform(tf2); - - if(collide(&o1, tf1, &o2, tf2, request, result)) - { - toc = 0; - return true; - } - - ConservativeAdvancementOrientedNode node; - - initialize(node, o1, tf1, o2, tf2, nsolver); - - node.motion1 = motion1; - node.motion2 = motion2; - - do - { - node.motion1->getCurrentTransform(tf1); - node.motion2->getCurrentTransform(tf2); - - node.tf1 = tf1; - node.tf2 = tf2; - - node.delta_t = 1; - node.min_distance = std::numeric_limits::max(); - - distanceRecurse(&node, 0, 0, NULL); - - if(node.delta_t <= node.t_err) - { - break; - } - - node.toc += node.delta_t; - if(node.toc > 1) - { - node.toc = 1; - break; - } - - node.motion1->integrate(node.toc); - node.motion2->integrate(node.toc); - } - while(1); - - toc = node.toc; - - if(node.toc < 1) - return true; - - return false; -} - -} - -template -bool conservativeAdvancement(const S& o1, - const MotionBase* motion1, - const BVHModel& o2, - const MotionBase* motion2, - const NarrowPhaseSolver* nsolver, - const CollisionRequest& request, - CollisionResult& result, - FCL_REAL& toc) -{ - return details::conservativeAdvancementShapeMeshOriented >(o1, motion1, o2, motion2, nsolver, request, result, toc); -} - - -template -bool conservativeAdvancement(const S& o1, - const MotionBase* motion1, - const BVHModel& o2, - const MotionBase* motion2, - const NarrowPhaseSolver* nsolver, - const CollisionRequest& request, - CollisionResult& result, - FCL_REAL& toc) -{ - return details::conservativeAdvancementShapeMeshOriented >(o1, motion1, o2, motion2, nsolver, request, result, toc); -} - - - -template<> -bool conservativeAdvancement(const BVHModel& o1, - const MotionBase* motion1, - const BVHModel& o2, - const MotionBase* motion2, - const CollisionRequest& request, - CollisionResult& result, - FCL_REAL& toc) -{ - return details::conservativeAdvancementMeshOriented(o1, motion1, o2, motion2, request, result, toc); -} - -template<> -bool conservativeAdvancement(const BVHModel& o1, - const MotionBase* motion1, - const BVHModel& o2, - const MotionBase* motion2, - const CollisionRequest& request, - CollisionResult& result, - FCL_REAL& toc) -{ - return details::conservativeAdvancementMeshOriented(o1, motion1, o2, motion2, request, result, toc); -} - - -template -FCL_REAL BVHConservativeAdvancement(const CollisionGeometry* o1, const MotionBase* motion1, const CollisionGeometry* o2, const MotionBase* motion2, const NarrowPhaseSolver* nsolver, const ContinuousCollisionRequest& request, ContinuousCollisionResult& result) -{ - const BVHModel* obj1 = static_cast*>(o1); - const BVHModel* obj2 = static_cast*>(o2); - - CollisionRequest c_request; - CollisionResult c_result; - FCL_REAL toc; - bool is_collide = conservativeAdvancement(*obj1, motion1, *obj2, motion2, c_request, c_result, toc); - - result.is_collide = is_collide; - result.time_of_contact = toc; - - return toc; -} - -template -FCL_REAL ShapeConservativeAdvancement(const CollisionGeometry* o1, const MotionBase* motion1, const CollisionGeometry* o2, const MotionBase* motion2, const NarrowPhaseSolver* nsolver, const ContinuousCollisionRequest& request, ContinuousCollisionResult& result) -{ - const S1* obj1 = static_cast(o1); - const S2* obj2 = static_cast(o2); - - CollisionRequest c_request; - CollisionResult c_result; - FCL_REAL toc; - bool is_collide = conservativeAdvancement(*obj1, motion1, *obj2, motion2, nsolver, c_request, c_result, toc); - - result.is_collide = is_collide; - result.time_of_contact = toc; - - return toc; -} - -template -FCL_REAL ShapeBVHConservativeAdvancement(const CollisionGeometry* o1, const MotionBase* motion1, const CollisionGeometry* o2, const MotionBase* motion2, const NarrowPhaseSolver* nsolver, const ContinuousCollisionRequest& request, ContinuousCollisionResult& result) -{ - const S* obj1 = static_cast(o1); - const BVHModel* obj2 = static_cast*>(o2); - - CollisionRequest c_request; - CollisionResult c_result; - FCL_REAL toc; - - bool is_collide = conservativeAdvancement(*obj1, motion1, *obj2, motion2, nsolver, c_request, c_result, toc); - - result.is_collide = is_collide; - result.time_of_contact = toc; - - return toc; -} - -template -FCL_REAL BVHShapeConservativeAdvancement(const CollisionGeometry* o1, const MotionBase* motion1, const CollisionGeometry* o2, const MotionBase* motion2, const NarrowPhaseSolver* nsolver, const ContinuousCollisionRequest& request, ContinuousCollisionResult& result) -{ - const BVHModel* obj1 = static_cast*>(o1); - const S* obj2 = static_cast(o2); - - CollisionRequest c_request; - CollisionResult c_result; - FCL_REAL toc; - - bool is_collide = conservativeAdvancement(*obj1, motion1, *obj2, motion2, nsolver, c_request, c_result, toc); - - result.is_collide = is_collide; - result.time_of_contact = toc; - - return toc; -} - -template -ConservativeAdvancementFunctionMatrix::ConservativeAdvancementFunctionMatrix() -{ - for(int i = 0; i < NODE_COUNT; ++i) - { - for(int j = 0; j < NODE_COUNT; ++j) - conservative_advancement_matrix[i][j] = NULL; - } - - - conservative_advancement_matrix[GEOM_BOX][GEOM_BOX] = &ShapeConservativeAdvancement; - conservative_advancement_matrix[GEOM_BOX][GEOM_SPHERE] = &ShapeConservativeAdvancement; - conservative_advancement_matrix[GEOM_BOX][GEOM_CAPSULE] = &ShapeConservativeAdvancement; - conservative_advancement_matrix[GEOM_BOX][GEOM_CONE] = &ShapeConservativeAdvancement; - conservative_advancement_matrix[GEOM_BOX][GEOM_CYLINDER] = &ShapeConservativeAdvancement; - conservative_advancement_matrix[GEOM_BOX][GEOM_CONVEX] = &ShapeConservativeAdvancement; - conservative_advancement_matrix[GEOM_BOX][GEOM_PLANE] = &ShapeConservativeAdvancement; - conservative_advancement_matrix[GEOM_BOX][GEOM_HALFSPACE] = &ShapeConservativeAdvancement; - - conservative_advancement_matrix[GEOM_SPHERE][GEOM_BOX] = &ShapeConservativeAdvancement; - conservative_advancement_matrix[GEOM_SPHERE][GEOM_SPHERE] = &ShapeConservativeAdvancement; - conservative_advancement_matrix[GEOM_SPHERE][GEOM_CAPSULE] = &ShapeConservativeAdvancement; - conservative_advancement_matrix[GEOM_SPHERE][GEOM_CONE] = &ShapeConservativeAdvancement; - conservative_advancement_matrix[GEOM_SPHERE][GEOM_CYLINDER] = &ShapeConservativeAdvancement; - conservative_advancement_matrix[GEOM_SPHERE][GEOM_CONVEX] = &ShapeConservativeAdvancement; - conservative_advancement_matrix[GEOM_SPHERE][GEOM_PLANE] = &ShapeConservativeAdvancement; - conservative_advancement_matrix[GEOM_SPHERE][GEOM_HALFSPACE] = &ShapeConservativeAdvancement; - - conservative_advancement_matrix[GEOM_CAPSULE][GEOM_BOX] = &ShapeConservativeAdvancement; - conservative_advancement_matrix[GEOM_CAPSULE][GEOM_SPHERE] = &ShapeConservativeAdvancement; - conservative_advancement_matrix[GEOM_CAPSULE][GEOM_CAPSULE] = &ShapeConservativeAdvancement; - conservative_advancement_matrix[GEOM_CAPSULE][GEOM_CONE] = &ShapeConservativeAdvancement; - conservative_advancement_matrix[GEOM_CAPSULE][GEOM_CYLINDER] = &ShapeConservativeAdvancement; - conservative_advancement_matrix[GEOM_CAPSULE][GEOM_CONVEX] = &ShapeConservativeAdvancement; - conservative_advancement_matrix[GEOM_CAPSULE][GEOM_PLANE] = &ShapeConservativeAdvancement; - conservative_advancement_matrix[GEOM_CAPSULE][GEOM_HALFSPACE] = &ShapeConservativeAdvancement; - - conservative_advancement_matrix[GEOM_CONE][GEOM_BOX] = &ShapeConservativeAdvancement; - conservative_advancement_matrix[GEOM_CONE][GEOM_SPHERE] = &ShapeConservativeAdvancement; - conservative_advancement_matrix[GEOM_CONE][GEOM_CAPSULE] = &ShapeConservativeAdvancement; - conservative_advancement_matrix[GEOM_CONE][GEOM_CONE] = &ShapeConservativeAdvancement; - conservative_advancement_matrix[GEOM_CONE][GEOM_CYLINDER] = &ShapeConservativeAdvancement; - conservative_advancement_matrix[GEOM_CONE][GEOM_CONVEX] = &ShapeConservativeAdvancement; - conservative_advancement_matrix[GEOM_CONE][GEOM_PLANE] = &ShapeConservativeAdvancement; - conservative_advancement_matrix[GEOM_CONE][GEOM_HALFSPACE] = &ShapeConservativeAdvancement; - - conservative_advancement_matrix[GEOM_CYLINDER][GEOM_BOX] = &ShapeConservativeAdvancement; - conservative_advancement_matrix[GEOM_CYLINDER][GEOM_SPHERE] = &ShapeConservativeAdvancement; - conservative_advancement_matrix[GEOM_CYLINDER][GEOM_CAPSULE] = &ShapeConservativeAdvancement; - conservative_advancement_matrix[GEOM_CYLINDER][GEOM_CONE] = &ShapeConservativeAdvancement; - conservative_advancement_matrix[GEOM_CYLINDER][GEOM_CYLINDER] = &ShapeConservativeAdvancement; - conservative_advancement_matrix[GEOM_CYLINDER][GEOM_CONVEX] = &ShapeConservativeAdvancement; - conservative_advancement_matrix[GEOM_CYLINDER][GEOM_PLANE] = &ShapeConservativeAdvancement; - conservative_advancement_matrix[GEOM_CYLINDER][GEOM_HALFSPACE] = &ShapeConservativeAdvancement; - - conservative_advancement_matrix[GEOM_CONVEX][GEOM_BOX] = &ShapeConservativeAdvancement; - conservative_advancement_matrix[GEOM_CONVEX][GEOM_SPHERE] = &ShapeConservativeAdvancement; - conservative_advancement_matrix[GEOM_CONVEX][GEOM_CAPSULE] = &ShapeConservativeAdvancement; - conservative_advancement_matrix[GEOM_CONVEX][GEOM_CONE] = &ShapeConservativeAdvancement; - conservative_advancement_matrix[GEOM_CONVEX][GEOM_CYLINDER] = &ShapeConservativeAdvancement; - conservative_advancement_matrix[GEOM_CONVEX][GEOM_CONVEX] = &ShapeConservativeAdvancement; - conservative_advancement_matrix[GEOM_CONVEX][GEOM_PLANE] = &ShapeConservativeAdvancement; - conservative_advancement_matrix[GEOM_CONVEX][GEOM_HALFSPACE] = &ShapeConservativeAdvancement; - - conservative_advancement_matrix[GEOM_PLANE][GEOM_BOX] = &ShapeConservativeAdvancement; - conservative_advancement_matrix[GEOM_PLANE][GEOM_SPHERE] = &ShapeConservativeAdvancement; - conservative_advancement_matrix[GEOM_PLANE][GEOM_CAPSULE] = &ShapeConservativeAdvancement; - conservative_advancement_matrix[GEOM_PLANE][GEOM_CONE] = &ShapeConservativeAdvancement; - conservative_advancement_matrix[GEOM_PLANE][GEOM_CYLINDER] = &ShapeConservativeAdvancement; - conservative_advancement_matrix[GEOM_PLANE][GEOM_CONVEX] = &ShapeConservativeAdvancement; - conservative_advancement_matrix[GEOM_PLANE][GEOM_PLANE] = &ShapeConservativeAdvancement; - conservative_advancement_matrix[GEOM_PLANE][GEOM_HALFSPACE] = &ShapeConservativeAdvancement; - - conservative_advancement_matrix[GEOM_HALFSPACE][GEOM_BOX] = &ShapeConservativeAdvancement; - conservative_advancement_matrix[GEOM_HALFSPACE][GEOM_SPHERE] = &ShapeConservativeAdvancement; - conservative_advancement_matrix[GEOM_HALFSPACE][GEOM_CAPSULE] = &ShapeConservativeAdvancement; - conservative_advancement_matrix[GEOM_HALFSPACE][GEOM_CONE] = &ShapeConservativeAdvancement; - conservative_advancement_matrix[GEOM_HALFSPACE][GEOM_CYLINDER] = &ShapeConservativeAdvancement; - conservative_advancement_matrix[GEOM_HALFSPACE][GEOM_CONVEX] = &ShapeConservativeAdvancement; - conservative_advancement_matrix[GEOM_HALFSPACE][GEOM_PLANE] = &ShapeConservativeAdvancement; - conservative_advancement_matrix[GEOM_HALFSPACE][GEOM_HALFSPACE] = &ShapeConservativeAdvancement; - - conservative_advancement_matrix[BV_AABB][GEOM_BOX] = &BVHShapeConservativeAdvancement; - conservative_advancement_matrix[BV_AABB][GEOM_SPHERE] = &BVHShapeConservativeAdvancement; - conservative_advancement_matrix[BV_AABB][GEOM_CAPSULE] = &BVHShapeConservativeAdvancement; - conservative_advancement_matrix[BV_AABB][GEOM_CONE] = &BVHShapeConservativeAdvancement; - conservative_advancement_matrix[BV_AABB][GEOM_CYLINDER] = &BVHShapeConservativeAdvancement; - conservative_advancement_matrix[BV_AABB][GEOM_CONVEX] = &BVHShapeConservativeAdvancement; - conservative_advancement_matrix[BV_AABB][GEOM_PLANE] = &BVHShapeConservativeAdvancement; - conservative_advancement_matrix[BV_AABB][GEOM_HALFSPACE] = &BVHShapeConservativeAdvancement; - - conservative_advancement_matrix[BV_OBB][GEOM_BOX] = &BVHShapeConservativeAdvancement; - conservative_advancement_matrix[BV_OBB][GEOM_SPHERE] = &BVHShapeConservativeAdvancement; - conservative_advancement_matrix[BV_OBB][GEOM_CAPSULE] = &BVHShapeConservativeAdvancement; - conservative_advancement_matrix[BV_OBB][GEOM_CONE] = &BVHShapeConservativeAdvancement; - conservative_advancement_matrix[BV_OBB][GEOM_CYLINDER] = &BVHShapeConservativeAdvancement; - conservative_advancement_matrix[BV_OBB][GEOM_CONVEX] = &BVHShapeConservativeAdvancement; - conservative_advancement_matrix[BV_OBB][GEOM_PLANE] = &BVHShapeConservativeAdvancement; - conservative_advancement_matrix[BV_OBB][GEOM_HALFSPACE] = &BVHShapeConservativeAdvancement; - - conservative_advancement_matrix[BV_OBBRSS][GEOM_BOX] = &BVHShapeConservativeAdvancement; - conservative_advancement_matrix[BV_OBBRSS][GEOM_SPHERE] = &BVHShapeConservativeAdvancement; - conservative_advancement_matrix[BV_OBBRSS][GEOM_CAPSULE] = &BVHShapeConservativeAdvancement; - conservative_advancement_matrix[BV_OBBRSS][GEOM_CONE] = &BVHShapeConservativeAdvancement; - conservative_advancement_matrix[BV_OBBRSS][GEOM_CYLINDER] = &BVHShapeConservativeAdvancement; - conservative_advancement_matrix[BV_OBBRSS][GEOM_CONVEX] = &BVHShapeConservativeAdvancement; - conservative_advancement_matrix[BV_OBBRSS][GEOM_PLANE] = &BVHShapeConservativeAdvancement; - conservative_advancement_matrix[BV_OBBRSS][GEOM_HALFSPACE] = &BVHShapeConservativeAdvancement; - - conservative_advancement_matrix[BV_RSS][GEOM_BOX] = &BVHShapeConservativeAdvancement; - conservative_advancement_matrix[BV_RSS][GEOM_SPHERE] = &BVHShapeConservativeAdvancement; - conservative_advancement_matrix[BV_RSS][GEOM_CAPSULE] = &BVHShapeConservativeAdvancement; - conservative_advancement_matrix[BV_RSS][GEOM_CONE] = &BVHShapeConservativeAdvancement; - conservative_advancement_matrix[BV_RSS][GEOM_CYLINDER] = &BVHShapeConservativeAdvancement; - conservative_advancement_matrix[BV_RSS][GEOM_CONVEX] = &BVHShapeConservativeAdvancement; - conservative_advancement_matrix[BV_RSS][GEOM_PLANE] = &BVHShapeConservativeAdvancement; - conservative_advancement_matrix[BV_RSS][GEOM_HALFSPACE] = &BVHShapeConservativeAdvancement; - - conservative_advancement_matrix[BV_KDOP16][GEOM_BOX] = &BVHShapeConservativeAdvancement, Box, NarrowPhaseSolver>; - conservative_advancement_matrix[BV_KDOP16][GEOM_SPHERE] = &BVHShapeConservativeAdvancement, Sphere, NarrowPhaseSolver>; - conservative_advancement_matrix[BV_KDOP16][GEOM_CAPSULE] = &BVHShapeConservativeAdvancement, Capsule, NarrowPhaseSolver>; - conservative_advancement_matrix[BV_KDOP16][GEOM_CONE] = &BVHShapeConservativeAdvancement, Cone, NarrowPhaseSolver>; - conservative_advancement_matrix[BV_KDOP16][GEOM_CYLINDER] = &BVHShapeConservativeAdvancement, Cylinder, NarrowPhaseSolver>; - conservative_advancement_matrix[BV_KDOP16][GEOM_CONVEX] = &BVHShapeConservativeAdvancement, Convex, NarrowPhaseSolver>; - conservative_advancement_matrix[BV_KDOP16][GEOM_PLANE] = &BVHShapeConservativeAdvancement, Plane, NarrowPhaseSolver>; - conservative_advancement_matrix[BV_KDOP16][GEOM_HALFSPACE] = &BVHShapeConservativeAdvancement, Halfspace, NarrowPhaseSolver>; - - conservative_advancement_matrix[BV_KDOP18][GEOM_BOX] = &BVHShapeConservativeAdvancement, Box, NarrowPhaseSolver>; - conservative_advancement_matrix[BV_KDOP18][GEOM_SPHERE] = &BVHShapeConservativeAdvancement, Sphere, NarrowPhaseSolver>; - conservative_advancement_matrix[BV_KDOP18][GEOM_CAPSULE] = &BVHShapeConservativeAdvancement, Capsule, NarrowPhaseSolver>; - conservative_advancement_matrix[BV_KDOP18][GEOM_CONE] = &BVHShapeConservativeAdvancement, Cone, NarrowPhaseSolver>; - conservative_advancement_matrix[BV_KDOP18][GEOM_CYLINDER] = &BVHShapeConservativeAdvancement, Cylinder, NarrowPhaseSolver>; - conservative_advancement_matrix[BV_KDOP18][GEOM_CONVEX] = &BVHShapeConservativeAdvancement, Convex, NarrowPhaseSolver>; - conservative_advancement_matrix[BV_KDOP18][GEOM_PLANE] = &BVHShapeConservativeAdvancement, Plane, NarrowPhaseSolver>; - conservative_advancement_matrix[BV_KDOP18][GEOM_HALFSPACE] = &BVHShapeConservativeAdvancement, Halfspace, NarrowPhaseSolver>; - - conservative_advancement_matrix[BV_KDOP24][GEOM_BOX] = &BVHShapeConservativeAdvancement, Box, NarrowPhaseSolver>; - conservative_advancement_matrix[BV_KDOP24][GEOM_SPHERE] = &BVHShapeConservativeAdvancement, Sphere, NarrowPhaseSolver>; - conservative_advancement_matrix[BV_KDOP24][GEOM_CAPSULE] = &BVHShapeConservativeAdvancement, Capsule, NarrowPhaseSolver>; - conservative_advancement_matrix[BV_KDOP24][GEOM_CONE] = &BVHShapeConservativeAdvancement, Cone, NarrowPhaseSolver>; - conservative_advancement_matrix[BV_KDOP24][GEOM_CYLINDER] = &BVHShapeConservativeAdvancement, Cylinder, NarrowPhaseSolver>; - conservative_advancement_matrix[BV_KDOP24][GEOM_CONVEX] = &BVHShapeConservativeAdvancement, Convex, NarrowPhaseSolver>; - conservative_advancement_matrix[BV_KDOP24][GEOM_PLANE] = &BVHShapeConservativeAdvancement, Plane, NarrowPhaseSolver>; - conservative_advancement_matrix[BV_KDOP24][GEOM_HALFSPACE] = &BVHShapeConservativeAdvancement, Halfspace, NarrowPhaseSolver>; - - conservative_advancement_matrix[BV_kIOS][GEOM_BOX] = &BVHShapeConservativeAdvancement; - conservative_advancement_matrix[BV_kIOS][GEOM_SPHERE] = &BVHShapeConservativeAdvancement; - conservative_advancement_matrix[BV_kIOS][GEOM_CAPSULE] = &BVHShapeConservativeAdvancement; - conservative_advancement_matrix[BV_kIOS][GEOM_CONE] = &BVHShapeConservativeAdvancement; - conservative_advancement_matrix[BV_kIOS][GEOM_CYLINDER] = &BVHShapeConservativeAdvancement; - conservative_advancement_matrix[BV_kIOS][GEOM_CONVEX] = &BVHShapeConservativeAdvancement; - conservative_advancement_matrix[BV_kIOS][GEOM_PLANE] = &BVHShapeConservativeAdvancement; - conservative_advancement_matrix[BV_kIOS][GEOM_HALFSPACE] = &BVHShapeConservativeAdvancement; - - - conservative_advancement_matrix[GEOM_BOX][BV_AABB] = &ShapeBVHConservativeAdvancement; - conservative_advancement_matrix[GEOM_SPHERE][BV_AABB] = &ShapeBVHConservativeAdvancement; - conservative_advancement_matrix[GEOM_CAPSULE][BV_AABB] = &ShapeBVHConservativeAdvancement; - conservative_advancement_matrix[GEOM_CONE][BV_AABB] = &ShapeBVHConservativeAdvancement; - conservative_advancement_matrix[GEOM_CYLINDER][BV_AABB] = &ShapeBVHConservativeAdvancement; - conservative_advancement_matrix[GEOM_CONVEX][BV_AABB] = &ShapeBVHConservativeAdvancement; - conservative_advancement_matrix[GEOM_PLANE][BV_AABB] = &ShapeBVHConservativeAdvancement; - conservative_advancement_matrix[GEOM_HALFSPACE][BV_AABB] = &ShapeBVHConservativeAdvancement; - - conservative_advancement_matrix[GEOM_BOX][BV_OBB] = &ShapeBVHConservativeAdvancement; - conservative_advancement_matrix[GEOM_SPHERE][BV_OBB] = &ShapeBVHConservativeAdvancement; - conservative_advancement_matrix[GEOM_CAPSULE][BV_OBB] = &ShapeBVHConservativeAdvancement; - conservative_advancement_matrix[GEOM_CONE][BV_OBB] = &ShapeBVHConservativeAdvancement; - conservative_advancement_matrix[GEOM_CYLINDER][BV_OBB] = &ShapeBVHConservativeAdvancement; - conservative_advancement_matrix[GEOM_CONVEX][BV_OBB] = &ShapeBVHConservativeAdvancement; - conservative_advancement_matrix[GEOM_PLANE][BV_OBB] = &ShapeBVHConservativeAdvancement; - conservative_advancement_matrix[GEOM_HALFSPACE][BV_OBB] = &ShapeBVHConservativeAdvancement; - - conservative_advancement_matrix[GEOM_BOX][BV_RSS] = &ShapeBVHConservativeAdvancement; - conservative_advancement_matrix[GEOM_SPHERE][BV_RSS] = &ShapeBVHConservativeAdvancement; - conservative_advancement_matrix[GEOM_CAPSULE][BV_RSS] = &ShapeBVHConservativeAdvancement; - conservative_advancement_matrix[GEOM_CONE][BV_RSS] = &ShapeBVHConservativeAdvancement; - conservative_advancement_matrix[GEOM_CYLINDER][BV_RSS] = &ShapeBVHConservativeAdvancement; - conservative_advancement_matrix[GEOM_CONVEX][BV_RSS] = &ShapeBVHConservativeAdvancement; - conservative_advancement_matrix[GEOM_PLANE][BV_RSS] = &ShapeBVHConservativeAdvancement; - conservative_advancement_matrix[GEOM_HALFSPACE][BV_RSS] = &ShapeBVHConservativeAdvancement; - - conservative_advancement_matrix[GEOM_BOX][BV_OBBRSS] = &ShapeBVHConservativeAdvancement; - conservative_advancement_matrix[GEOM_SPHERE][BV_OBBRSS] = &ShapeBVHConservativeAdvancement; - conservative_advancement_matrix[GEOM_CAPSULE][BV_OBBRSS] = &ShapeBVHConservativeAdvancement; - conservative_advancement_matrix[GEOM_CONE][BV_OBBRSS] = &ShapeBVHConservativeAdvancement; - conservative_advancement_matrix[GEOM_CYLINDER][BV_OBBRSS] = &ShapeBVHConservativeAdvancement; - conservative_advancement_matrix[GEOM_CONVEX][BV_OBBRSS] = &ShapeBVHConservativeAdvancement; - conservative_advancement_matrix[GEOM_PLANE][BV_OBBRSS] = &ShapeBVHConservativeAdvancement; - conservative_advancement_matrix[GEOM_HALFSPACE][BV_OBBRSS] = &ShapeBVHConservativeAdvancement; - - conservative_advancement_matrix[GEOM_BOX][BV_KDOP16] = &ShapeBVHConservativeAdvancement, NarrowPhaseSolver>; - conservative_advancement_matrix[GEOM_SPHERE][BV_KDOP16] = &ShapeBVHConservativeAdvancement, NarrowPhaseSolver>; - conservative_advancement_matrix[GEOM_CAPSULE][BV_KDOP16] = &ShapeBVHConservativeAdvancement, NarrowPhaseSolver>; - conservative_advancement_matrix[GEOM_CONE][BV_KDOP16] = &ShapeBVHConservativeAdvancement, NarrowPhaseSolver>; - conservative_advancement_matrix[GEOM_CYLINDER][BV_KDOP16] = &ShapeBVHConservativeAdvancement, NarrowPhaseSolver>; - conservative_advancement_matrix[GEOM_CONVEX][BV_KDOP16] = &ShapeBVHConservativeAdvancement, NarrowPhaseSolver>; - conservative_advancement_matrix[GEOM_PLANE][BV_KDOP16] = &ShapeBVHConservativeAdvancement, NarrowPhaseSolver>; - conservative_advancement_matrix[GEOM_HALFSPACE][BV_KDOP16] = &ShapeBVHConservativeAdvancement, NarrowPhaseSolver>; - - conservative_advancement_matrix[GEOM_BOX][BV_KDOP18] = &ShapeBVHConservativeAdvancement, NarrowPhaseSolver>; - conservative_advancement_matrix[GEOM_SPHERE][BV_KDOP18] = &ShapeBVHConservativeAdvancement, NarrowPhaseSolver>; - conservative_advancement_matrix[GEOM_CAPSULE][BV_KDOP18] = &ShapeBVHConservativeAdvancement, NarrowPhaseSolver>; - conservative_advancement_matrix[GEOM_CONE][BV_KDOP18] = &ShapeBVHConservativeAdvancement, NarrowPhaseSolver>; - conservative_advancement_matrix[GEOM_CYLINDER][BV_KDOP18] = &ShapeBVHConservativeAdvancement, NarrowPhaseSolver>; - conservative_advancement_matrix[GEOM_CONVEX][BV_KDOP18] = &ShapeBVHConservativeAdvancement, NarrowPhaseSolver>; - conservative_advancement_matrix[GEOM_PLANE][BV_KDOP18] = &ShapeBVHConservativeAdvancement, NarrowPhaseSolver>; - conservative_advancement_matrix[GEOM_HALFSPACE][BV_KDOP18] = &ShapeBVHConservativeAdvancement, NarrowPhaseSolver>; - - conservative_advancement_matrix[GEOM_BOX][BV_KDOP24] = &ShapeBVHConservativeAdvancement, NarrowPhaseSolver>; - conservative_advancement_matrix[GEOM_SPHERE][BV_KDOP24] = &ShapeBVHConservativeAdvancement, NarrowPhaseSolver>; - conservative_advancement_matrix[GEOM_CAPSULE][BV_KDOP24] = &ShapeBVHConservativeAdvancement, NarrowPhaseSolver>; - conservative_advancement_matrix[GEOM_CONE][BV_KDOP24] = &ShapeBVHConservativeAdvancement, NarrowPhaseSolver>; - conservative_advancement_matrix[GEOM_CYLINDER][BV_KDOP24] = &ShapeBVHConservativeAdvancement, NarrowPhaseSolver>; - conservative_advancement_matrix[GEOM_CONVEX][BV_KDOP24] = &ShapeBVHConservativeAdvancement, NarrowPhaseSolver>; - conservative_advancement_matrix[GEOM_PLANE][BV_KDOP24] = &ShapeBVHConservativeAdvancement, NarrowPhaseSolver>; - conservative_advancement_matrix[GEOM_HALFSPACE][BV_KDOP24] = &ShapeBVHConservativeAdvancement, NarrowPhaseSolver>; - - conservative_advancement_matrix[GEOM_BOX][BV_kIOS] = &ShapeBVHConservativeAdvancement; - conservative_advancement_matrix[GEOM_SPHERE][BV_kIOS] = &ShapeBVHConservativeAdvancement; - conservative_advancement_matrix[GEOM_CAPSULE][BV_kIOS] = &ShapeBVHConservativeAdvancement; - conservative_advancement_matrix[GEOM_CONE][BV_kIOS] = &ShapeBVHConservativeAdvancement; - conservative_advancement_matrix[GEOM_CYLINDER][BV_kIOS] = &ShapeBVHConservativeAdvancement; - conservative_advancement_matrix[GEOM_CONVEX][BV_kIOS] = &ShapeBVHConservativeAdvancement; - conservative_advancement_matrix[GEOM_PLANE][BV_kIOS] = &ShapeBVHConservativeAdvancement; - conservative_advancement_matrix[GEOM_HALFSPACE][BV_kIOS] = &ShapeBVHConservativeAdvancement; - - conservative_advancement_matrix[BV_AABB][BV_AABB] = &BVHConservativeAdvancement; - conservative_advancement_matrix[BV_OBB][BV_OBB] = &BVHConservativeAdvancement; - conservative_advancement_matrix[BV_RSS][BV_RSS] = &BVHConservativeAdvancement; - conservative_advancement_matrix[BV_OBBRSS][BV_OBBRSS] = &BVHConservativeAdvancement; - conservative_advancement_matrix[BV_KDOP16][BV_KDOP16] = &BVHConservativeAdvancement, NarrowPhaseSolver>; - conservative_advancement_matrix[BV_KDOP18][BV_KDOP18] = &BVHConservativeAdvancement, NarrowPhaseSolver>; - conservative_advancement_matrix[BV_KDOP24][BV_KDOP24] = &BVHConservativeAdvancement, NarrowPhaseSolver>; - conservative_advancement_matrix[BV_kIOS][BV_kIOS] = &BVHConservativeAdvancement; - -} - - -template struct ConservativeAdvancementFunctionMatrix; -template struct ConservativeAdvancementFunctionMatrix; - - - - - - - - - -} - - diff --git a/src/ccd/interval.cpp b/src/ccd/interval.cpp deleted file mode 100644 index a6841d22c..000000000 --- a/src/ccd/interval.cpp +++ /dev/null @@ -1,204 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#include "fcl/ccd/interval.h" -#include - -namespace fcl -{ - -Interval bound(const Interval& i, FCL_REAL v) -{ - Interval res = i; - if(v < res.i_[0]) res.i_[0] = v; - if(v > res.i_[1]) res.i_[1] = v; - return res; -} - -Interval bound(const Interval& i, const Interval& other) -{ - Interval res = i; - if(other.i_[0] < res.i_[0]) res.i_[0] = other.i_[0]; - if(other.i_[1] > res.i_[1]) res.i_[1] = other.i_[1]; - return res; -} - -Interval Interval::operator * (const Interval& other) const -{ - if(other.i_[0] >= 0) - { - if(i_[0] >= 0) return Interval(i_[0] * other.i_[0], i_[1] * other.i_[1]); - if(i_[1] <= 0) return Interval(i_[0] * other.i_[1], i_[1] * other.i_[0]); - return Interval(i_[0] * other.i_[1], i_[1] * other.i_[1]); - } - if(other.i_[1] <= 0) - { - if(i_[0] >= 0) return Interval(i_[1] * other.i_[0], i_[0] * other.i_[1]); - if(i_[1] <= 0) return Interval(i_[1] * other.i_[1], i_[0] * other.i_[0]); - return Interval(i_[1] * other.i_[0], i_[0] * other.i_[0]); - } - - if(i_[0] >= 0) return Interval(i_[1] * other.i_[0], i_[1] * other.i_[1]); - if(i_[1] <= 0) return Interval(i_[0] * other.i_[1], i_[0] * other.i_[0]); - - FCL_REAL v00 = i_[0] * other.i_[0]; - FCL_REAL v11 = i_[1] * other.i_[1]; - if(v00 <= v11) - { - FCL_REAL v01 = i_[0] * other.i_[1]; - FCL_REAL v10 = i_[1] * other.i_[0]; - if(v01 < v10) return Interval(v01, v11); - return Interval(v10, v11); - } - - FCL_REAL v01 = i_[0] * other.i_[1]; - FCL_REAL v10 = i_[1] * other.i_[0]; - if(v01 < v10) return Interval(v01, v00); - return Interval(v10, v00); -} - -Interval& Interval::operator *= (const Interval& other) -{ - if(other.i_[0] >= 0) - { - if(i_[0] >= 0) - { - i_[0] *= other.i_[0]; - i_[1] *= other.i_[1]; - } - else if(i_[1] <= 0) - { - i_[0] *= other.i_[1]; - i_[1] *= other.i_[0]; - } - else - { - i_[0] *= other.i_[1]; - i_[1] *= other.i_[1]; - } - return *this; - } - - if(other.i_[1] <= 0) - { - if(i_[0] >= 0) - { - FCL_REAL tmp = i_[0]; - i_[0] = i_[1] * other.i_[0]; - i_[1] = tmp * other.i_[1]; - } - else if(i_[1] <= 0) - { - FCL_REAL tmp = i_[0]; - i_[0] = i_[1] * other.i_[1]; - i_[1] = tmp * other.i_[0]; - } - else - { - FCL_REAL tmp = i_[0]; - i_[0] = i_[1] * other.i_[0]; - i_[1] = tmp * other.i_[0]; - } - return *this; - } - - if(i_[0] >= 0) - { - i_[0] = i_[1] * other.i_[0]; - i_[1] *= other.i_[1]; - return *this; - } - - if(i_[1] <= 0) - { - i_[1] = i_[0] * other.i_[0]; - i_[0] *= other.i_[1]; - return *this; - } - - FCL_REAL v00 = i_[0] * other.i_[0]; - FCL_REAL v11 = i_[1] * other.i_[1]; - if(v00 <= v11) - { - FCL_REAL v01 = i_[0] * other.i_[1]; - FCL_REAL v10 = i_[1] * other.i_[0]; - if(v01 < v10) - { - i_[0] = v01; - i_[1] = v11; - } - else - { - i_[0] = v10; - i_[1] = v11; - } - return *this; - } - - FCL_REAL v01 = i_[0] * other.i_[1]; - FCL_REAL v10 = i_[1] * other.i_[0]; - if(v01 < v10) - { - i_[0] = v01; - i_[1] = v00; - } - else - { - i_[0] = v10; - i_[1] = v00; - } - - return *this; -} - -Interval Interval::operator / (const Interval& other) const -{ - return *this * Interval(1.0 / other.i_[1], 1.0 / other.i_[0]); -} - -Interval& Interval::operator /= (const Interval& other) -{ - *this *= Interval(1.0 / other.i_[1], 1.0 / other.i_[0]); - return *this; -} - -void Interval::print() const -{ - std::cout << "[" << i_[0] << ", " << i_[1] << "]" << std::endl; -} - -} diff --git a/src/ccd/interval_matrix.cpp b/src/ccd/interval_matrix.cpp deleted file mode 100644 index 7212ced16..000000000 --- a/src/ccd/interval_matrix.cpp +++ /dev/null @@ -1,260 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#include "fcl/ccd/interval_matrix.h" -#include - -namespace fcl -{ - -IMatrix3::IMatrix3() {} - -IMatrix3::IMatrix3(FCL_REAL v) -{ - v_[0].setValue(v); - v_[1].setValue(v); - v_[2].setValue(v); -} - -IMatrix3::IMatrix3(const Matrix3d& m) -{ - v_[0].setValue(m.row(0)[0], m.row(0)[1], m.row(0)[2]); - v_[1].setValue(m.row(1)[0], m.row(1)[1], m.row(1)[2]); - v_[2].setValue(m.row(2)[0], m.row(2)[1], m.row(2)[2]); -} - -IMatrix3::IMatrix3(FCL_REAL m[3][3][2]) -{ - v_[0].setValue(m[0]); - v_[1].setValue(m[1]); - v_[2].setValue(m[2]); -} - -IMatrix3::IMatrix3(FCL_REAL m[3][3]) -{ - v_[0].setValue(m[0]); - v_[1].setValue(m[1]); - v_[2].setValue(m[2]); -} - -IMatrix3::IMatrix3(Interval m[3][3]) -{ - v_[0].setValue(m[0]); - v_[1].setValue(m[1]); - v_[2].setValue(m[2]); -} - -IMatrix3::IMatrix3(const IVector3& v1, const IVector3& v2, const IVector3& v3) -{ - v_[0] = v1; - v_[1] = v2; - v_[2] = v3; -} - -void IMatrix3::setIdentity() -{ - v_[0].setValue(1, 0, 0); - v_[1].setValue(0, 1, 0); - v_[2].setValue(0, 0, 1); -} - -IVector3 IMatrix3::getColumn(size_t i) const -{ - return IVector3(v_[0][i], v_[1][i], v_[2][i]); -} - -const IVector3& IMatrix3::getRow(size_t i) const -{ - return v_[i]; -} - -Vector3d IMatrix3::getColumnLow(size_t i) const -{ - return Vector3d(v_[0][i][0], v_[1][i][0], v_[2][i][0]); -} - -Vector3d IMatrix3::getRowLow(size_t i) const -{ - return Vector3d(v_[i][0][0], v_[i][1][0], v_[i][2][0]); -} - -Vector3d IMatrix3::getColumnHigh(size_t i) const -{ - return Vector3d(v_[0][i][1], v_[1][i][1], v_[2][i][1]); -} - -Vector3d IMatrix3::getRowHigh(size_t i) const -{ - return Vector3d(v_[i][0][1], v_[i][1][1], v_[i][2][1]); -} - -Matrix3d IMatrix3::getLow() const -{ - Matrix3d m; - m << v_[0][0][1], v_[0][1][1], v_[0][2][1], - v_[1][0][1], v_[1][1][1], v_[1][2][1], - v_[2][0][1], v_[2][1][1], v_[2][2][1]; - return m; -} - -Matrix3d IMatrix3::getHigh() const -{ - Matrix3d m; - m << v_[0][0][1], v_[0][1][1], v_[0][2][1], - v_[1][0][1], v_[1][1][1], v_[1][2][1], - v_[2][0][1], v_[2][1][1], v_[2][2][1]; - return m; -} - -IMatrix3 IMatrix3::operator * (const Matrix3d& m) const -{ - return IMatrix3(IVector3(v_[0].dot(m.col(0)), v_[0].dot(m.col(1)), v_[0].dot(m.col(2))), - IVector3(v_[1].dot(m.col(0)), v_[1].dot(m.col(1)), v_[1].dot(m.col(2))), - IVector3(v_[2].dot(m.col(0)), v_[2].dot(m.col(1)), v_[2].dot(m.col(2)))); -} - -IVector3 IMatrix3::operator * (const Vector3d& v) const -{ - return IVector3(v_[0].dot(v), v_[1].dot(v), v_[2].dot(v)); -} - -IVector3 IMatrix3::operator * (const IVector3& v) const -{ - return IVector3(v_[0].dot(v), v_[1].dot(v), v_[2].dot(v)); -} - -IMatrix3 IMatrix3::operator * (const IMatrix3& m) const -{ - const IVector3& mc0 = m.getColumn(0); - const IVector3& mc1 = m.getColumn(1); - const IVector3& mc2 = m.getColumn(2); - - return IMatrix3(IVector3(v_[0].dot(mc0), v_[0].dot(mc1), v_[0].dot(mc2)), - IVector3(v_[1].dot(mc0), v_[1].dot(mc1), v_[1].dot(mc2)), - IVector3(v_[2].dot(mc0), v_[2].dot(mc1), v_[2].dot(mc2))); -} - -IMatrix3& IMatrix3::operator *= (const Matrix3d& m) -{ - v_[0].setValue(v_[0].dot(m.col(0)), v_[0].dot(m.col(1)), v_[0].dot(m.col(2))); - v_[1].setValue(v_[1].dot(m.col(0)), v_[1].dot(m.col(1)), v_[1].dot(m.col(2))); - v_[2].setValue(v_[2].dot(m.col(0)), v_[2].dot(m.col(1)), v_[2].dot(m.col(2))); - return *this; -} - - -IMatrix3& IMatrix3::operator *= (const IMatrix3& m) -{ - const IVector3& mc0 = m.getColumn(0); - const IVector3& mc1 = m.getColumn(1); - const IVector3& mc2 = m.getColumn(2); - - v_[0].setValue(v_[0].dot(mc0), v_[0].dot(mc1), v_[0].dot(mc2)); - v_[1].setValue(v_[1].dot(mc0), v_[1].dot(mc1), v_[1].dot(mc2)); - v_[2].setValue(v_[2].dot(mc0), v_[2].dot(mc1), v_[2].dot(mc2)); - return *this; -} - -IMatrix3 IMatrix3::operator + (const IMatrix3& m) const -{ - return IMatrix3(v_[0] + m.v_[0], v_[1] + m.v_[1], v_[2] + m.v_[2]); -} - -IMatrix3& IMatrix3::operator += (const IMatrix3& m) -{ - v_[0] += m.v_[0]; - v_[1] += m.v_[1]; - v_[2] += m.v_[2]; - return *this; -} - -IMatrix3 IMatrix3::operator - (const IMatrix3& m) const -{ - return IMatrix3(v_[0] - m.v_[0], v_[1] - m.v_[1], v_[2] - m.v_[2]); -} - -IMatrix3& IMatrix3::operator -= (const IMatrix3& m) -{ - v_[0] -= m.v_[0]; - v_[1] -= m.v_[1]; - v_[2] -= m.v_[2]; - return *this; -} - -IMatrix3& IMatrix3::rotationConstrain() -{ - for(std::size_t i = 0; i < 3; ++i) - { - for(std::size_t j = 0; j < 3; ++j) - { - if(v_[i][j][0] < -1) v_[i][j][0] = -1; - else if(v_[i][j][0] > 1) v_[i][j][0] = 1; - - if(v_[i][j][1] < -1) v_[i][j][1] = -1; - else if(v_[i][j][1] > 1) v_[i][j][1] = 1; - } - } - - return *this; -} - -void IMatrix3::print() const -{ - std::cout << "[" << v_[0][0][0] << "," << v_[0][0][1] << "]" << " [" << v_[0][1][0] << "," << v_[0][1][1] << "]" << " [" << v_[0][2][0] << "," << v_[0][2][1] << "]" << std::endl; - std::cout << "[" << v_[1][0][0] << "," << v_[1][0][1] << "]" << " [" << v_[1][1][0] << "," << v_[1][1][1] << "]" << " [" << v_[1][2][0] << "," << v_[1][2][1] << "]" << std::endl; - std::cout << "[" << v_[2][0][0] << "," << v_[2][0][1] << "]" << " [" << v_[2][1][0] << "," << v_[2][1][1] << "]" << " [" << v_[2][2][0] << "," << v_[2][2][1] << "]" << std::endl; -} - -IMatrix3 rotationConstrain(const IMatrix3& m) -{ - IMatrix3 res; - for(std::size_t i = 0; i < 3; ++i) - { - for(std::size_t j = 0; j < 3; ++j) - { - if(m(i, j)[0] < -1) res(i, j)[0] = -1; - else if(m(i, j)[0] > 1) res(i, j)[0] = 1; - - if(m(i, j)[1] < -1) res(i, j)[1] = -1; - else if(m(i, j)[1] > 1) res(i, j)[1] = 1; - } - } - - return res; -} - -} diff --git a/src/ccd/interval_vector.cpp b/src/ccd/interval_vector.cpp deleted file mode 100644 index f1a450f18..000000000 --- a/src/ccd/interval_vector.cpp +++ /dev/null @@ -1,212 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#include "fcl/ccd/interval_vector.h" -#include - -namespace fcl -{ - - -IVector3::IVector3() {} - -IVector3::IVector3(FCL_REAL v) { setValue(v); } - -IVector3::IVector3(FCL_REAL x, FCL_REAL y, FCL_REAL z) { setValue(x, y, z); } - -IVector3::IVector3(FCL_REAL xl, FCL_REAL xu, FCL_REAL yl, FCL_REAL yu, FCL_REAL zl, FCL_REAL zu) -{ - setValue(xl, xu, yl, yu, zl, zu); -} - -IVector3::IVector3(FCL_REAL v[3][2]) { setValue(v); } - -IVector3::IVector3(Interval v[3]) { setValue(v); } - -IVector3::IVector3(const Interval& v1, const Interval& v2, const Interval& v3) { setValue(v1, v2, v3); } - -IVector3::IVector3(const Vector3d& v) { setValue(v); } - -void IVector3::setZero() { setValue((FCL_REAL)0.0); } - -IVector3 IVector3::operator + (const IVector3& other) const -{ - return IVector3(i_[0] + other.i_[0], i_[1] + other.i_[1], i_[2] + other.i_[2]); -} - -IVector3& IVector3::operator += (const IVector3& other) -{ - i_[0] += other[0]; - i_[1] += other[1]; - i_[2] += other[2]; - return *this; -} - -IVector3 IVector3::operator - (const IVector3& other) const -{ - return IVector3(i_[0] - other.i_[0], i_[1] - other.i_[1], i_[2] - other.i_[2]); -} - -IVector3& IVector3::operator -= (const IVector3& other) -{ - i_[0] -= other[0]; - i_[1] -= other[1]; - i_[2] -= other[2]; - return *this; -} - -Interval IVector3::dot(const IVector3& other) const -{ - return i_[0] * other.i_[0] + i_[1] * other.i_[1] + i_[2] * other.i_[2]; -} - -IVector3 IVector3::cross(const IVector3& other) const -{ - return IVector3(i_[1] * other.i_[2] - i_[2] * other.i_[1], - i_[2] * other.i_[0] - i_[0] * other.i_[2], - i_[0] * other.i_[1] - i_[1] * other.i_[0]); -} - -Interval IVector3::dot(const Vector3d& other) const -{ - return i_[0] * other[0] + i_[1] * other[1] + i_[2] * other[2]; -} - -IVector3 IVector3::cross(const Vector3d& other) const -{ - return IVector3(i_[1] * other[2] - i_[2] * other[1], - i_[2] * other[0] - i_[0] * other[2], - i_[0] * other[1] - i_[1] * other[0]); -} - -FCL_REAL IVector3::volumn() const -{ - return i_[0].diameter() * i_[1].diameter() * i_[2].diameter(); -} - -void IVector3::print() const -{ - std::cout << "[" << i_[0][0] << "," << i_[0][1] << "]" << std::endl; - std::cout << "[" << i_[1][0] << "," << i_[1][1] << "]" << std::endl; - std::cout << "[" << i_[2][0] << "," << i_[2][1] << "]" << std::endl; -} - -Vector3d IVector3::center() const -{ - return Vector3d(i_[0].center(), i_[1].center(), i_[2].center()); -} - -void IVector3::bound(const IVector3& v) -{ - if(v[0][0] < i_[0][0]) i_[0][0] = v[0][0]; - if(v[1][0] < i_[1][0]) i_[1][0] = v[1][0]; - if(v[2][0] < i_[2][0]) i_[2][0] = v[2][0]; - - if(v[0][1] > i_[0][1]) i_[0][1] = v[0][1]; - if(v[1][1] > i_[1][1]) i_[1][1] = v[1][1]; - if(v[2][1] > i_[2][1]) i_[2][1] = v[2][1]; -} - -void IVector3::bound(const Vector3d& v) -{ - if(v[0] < i_[0][0]) i_[0][0] = v[0]; - if(v[1] < i_[1][0]) i_[1][0] = v[1]; - if(v[2] < i_[2][0]) i_[2][0] = v[2]; - - if(v[0] > i_[0][1]) i_[0][1] = v[0]; - if(v[1] > i_[1][1]) i_[1][1] = v[1]; - if(v[2] > i_[2][1]) i_[2][1] = v[2]; -} - - -IVector3 bound(const IVector3& i, const IVector3& v) -{ - IVector3 res(i); - if(v[0][0] < res.i_[0][0]) res.i_[0][0] = v[0][0]; - if(v[1][0] < res.i_[1][0]) res.i_[1][0] = v[1][0]; - if(v[2][0] < res.i_[2][0]) res.i_[2][0] = v[2][0]; - - if(v[0][1] > res.i_[0][1]) res.i_[0][1] = v[0][1]; - if(v[1][1] > res.i_[1][1]) res.i_[1][1] = v[1][1]; - if(v[2][1] > res.i_[2][1]) res.i_[2][1] = v[2][1]; - - return res; -} - -IVector3 bound(const IVector3& i, const Vector3d& v) -{ - IVector3 res(i); - if(v[0] < res.i_[0][0]) res.i_[0][0] = v[0]; - if(v[1] < res.i_[1][0]) res.i_[1][0] = v[1]; - if(v[2] < res.i_[2][0]) res.i_[2][0] = v[2]; - - if(v[0] > res.i_[0][1]) res.i_[0][1] = v[0]; - if(v[1] > res.i_[1][1]) res.i_[1][1] = v[1]; - if(v[2] > res.i_[2][1]) res.i_[2][1] = v[2]; - - return res; -} - -bool IVector3::overlap(const IVector3& v) const -{ - if(v[0][1] < i_[0][0]) return false; - if(v[1][1] < i_[1][0]) return false; - if(v[2][1] < i_[2][0]) return false; - - if(v[0][0] > i_[0][1]) return false; - if(v[1][0] > i_[1][1]) return false; - if(v[2][0] > i_[2][1]) return false; - - return true; -} - -bool IVector3::contain(const IVector3& v) const -{ - if(v[0][0] < i_[0][0]) return false; - if(v[1][0] < i_[1][0]) return false; - if(v[2][0] < i_[2][0]) return false; - - if(v[0][1] > i_[0][1]) return false; - if(v[1][1] > i_[1][1]) return false; - if(v[2][1] > i_[2][1]) return false; - - return true; -} - - - -} diff --git a/src/ccd/motion.cpp b/src/ccd/motion.cpp deleted file mode 100644 index 1f9c46a9d..000000000 --- a/src/ccd/motion.cpp +++ /dev/null @@ -1,557 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - - -#include "fcl/ccd/motion.h" - -namespace fcl -{ - -template<> -FCL_REAL TBVMotionBoundVisitor::visit(const SplineMotion& motion) const -{ - FCL_REAL T_bound = motion.computeTBound(n); - FCL_REAL tf_t = motion.getCurrentTime(); - - Vector3d c1 = bv.Tr; - Vector3d c2 = bv.Tr + bv.axis.col(0) * bv.l[0]; - Vector3d c3 = bv.Tr + bv.axis.col(1) * bv.l[1]; - Vector3d c4 = bv.Tr + bv.axis.col(0) * bv.l[0] + bv.axis.col(1) * bv.l[1]; - - FCL_REAL tmp; - // max_i |c_i * n| - FCL_REAL cn_max = std::abs(c1.dot(n)); - tmp = std::abs(c2.dot(n)); - if(tmp > cn_max) cn_max = tmp; - tmp = std::abs(c3.dot(n)); - if(tmp > cn_max) cn_max = tmp; - tmp = std::abs(c4.dot(n)); - if(tmp > cn_max) cn_max = tmp; - - // max_i ||c_i|| - FCL_REAL cmax = c1.squaredNorm(); - tmp = c2.squaredNorm(); - if(tmp > cmax) cmax = tmp; - tmp = c3.squaredNorm(); - if(tmp > cmax) cmax = tmp; - tmp = c4.squaredNorm(); - if(tmp > cmax) cmax = tmp; - cmax = sqrt(cmax); - - // max_i ||c_i x n|| - FCL_REAL cxn_max = (c1.cross(n)).squaredNorm(); - tmp = (c2.cross(n)).squaredNorm(); - if(tmp > cxn_max) cxn_max = tmp; - tmp = (c3.cross(n)).squaredNorm(); - if(tmp > cxn_max) cxn_max = tmp; - tmp = (c4.cross(n)).squaredNorm(); - if(tmp > cxn_max) cxn_max = tmp; - cxn_max = sqrt(cxn_max); - - FCL_REAL dWdW_max = motion.computeDWMax(); - FCL_REAL ratio = std::min(1 - tf_t, dWdW_max); - - FCL_REAL R_bound = 2 * (cn_max + cmax + cxn_max + 3 * bv.r) * ratio; - - - // std::cout << R_bound << " " << T_bound << std::endl; - - return R_bound + T_bound; -} - - -FCL_REAL TriangleMotionBoundVisitor::visit(const SplineMotion& motion) const -{ - FCL_REAL T_bound = motion.computeTBound(n); - FCL_REAL tf_t = motion.getCurrentTime(); - - FCL_REAL R_bound = std::abs(a.dot(n)) + a.norm() + (a.cross(n)).norm(); - FCL_REAL R_bound_tmp = std::abs(b.dot(n)) + b.norm() + (b.cross(n)).norm(); - if(R_bound_tmp > R_bound) R_bound = R_bound_tmp; - R_bound_tmp = std::abs(c.dot(n)) + c.norm() + (c.cross(n)).norm(); - if(R_bound_tmp > R_bound) R_bound = R_bound_tmp; - - FCL_REAL dWdW_max = motion.computeDWMax(); - FCL_REAL ratio = std::min(1 - tf_t, dWdW_max); - - R_bound *= 2 * ratio; - - // std::cout << R_bound << " " << T_bound << std::endl; - - return R_bound + T_bound; -} - -SplineMotion::SplineMotion(const Vector3d& Td0, const Vector3d& Td1, const Vector3d& Td2, const Vector3d& Td3, - const Vector3d& Rd0, const Vector3d& Rd1, const Vector3d& Rd2, const Vector3d& Rd3) : MotionBase() -{ - Td[0] = Td0; - Td[1] = Td1; - Td[2] = Td2; - Td[3] = Td3; - - Rd[0] = Rd0; - Rd[1] = Rd1; - Rd[2] = Rd2; - Rd[3] = Rd3; - - Rd0Rd0 = Rd[0].dot(Rd[0]); - Rd0Rd1 = Rd[0].dot(Rd[1]); - Rd0Rd2 = Rd[0].dot(Rd[2]); - Rd0Rd3 = Rd[0].dot(Rd[3]); - Rd1Rd1 = Rd[1].dot(Rd[1]); - Rd1Rd2 = Rd[1].dot(Rd[2]); - Rd1Rd3 = Rd[1].dot(Rd[3]); - Rd2Rd2 = Rd[2].dot(Rd[2]); - Rd2Rd3 = Rd[2].dot(Rd[3]); - Rd3Rd3 = Rd[3].dot(Rd[3]); - - TA = Td[1] * 3 - Td[2] * 3 + Td[3] - Td[0]; - TB = (Td[0] - Td[1] * 2 + Td[2]) * 3; - TC = (Td[2] - Td[0]) * 3; - - RA = Rd[1] * 3 - Rd[2] * 3 + Rd[3] - Rd[0]; - RB = (Rd[0] - Rd[1] * 2 + Rd[2]) * 3; - RC = (Rd[2] - Rd[0]) * 3; - - integrate(0.0); -} - -bool SplineMotion::integrate(double dt) const -{ - if(dt > 1) dt = 1; - - Vector3d cur_T = Td[0] * getWeight0(dt) + Td[1] * getWeight1(dt) + Td[2] * getWeight2(dt) + Td[3] * getWeight3(dt); - Vector3d cur_w = Rd[0] * getWeight0(dt) + Rd[1] * getWeight1(dt) + Rd[2] * getWeight2(dt) + Rd[3] * getWeight3(dt); - FCL_REAL cur_angle = cur_w.norm(); - cur_w.normalize(); - - tf.linear() = Eigen::AngleAxisd(cur_angle, cur_w).toRotationMatrix(); - tf.translation() = cur_T; - - tf_t = dt; - - return true; -} - - -FCL_REAL SplineMotion::computeTBound(const Vector3d& n) const -{ - FCL_REAL Ta = TA.dot(n); - FCL_REAL Tb = TB.dot(n); - FCL_REAL Tc = TC.dot(n); - - std::vector T_potential; - T_potential.push_back(tf_t); - T_potential.push_back(1); - if(Tb * Tb - 3 * Ta * Tc >= 0) - { - if(Ta == 0) - { - if(Tb != 0) - { - FCL_REAL tmp = -Tc / (2 * Tb); - if(tmp < 1 && tmp > tf_t) - T_potential.push_back(tmp); - } - } - else - { - FCL_REAL tmp_delta = sqrt(Tb * Tb - 3 * Ta * Tc); - FCL_REAL tmp1 = (-Tb + tmp_delta) / (3 * Ta); - FCL_REAL tmp2 = (-Tb - tmp_delta) / (3 * Ta); - if(tmp1 < 1 && tmp1 > tf_t) - T_potential.push_back(tmp1); - if(tmp2 < 1 && tmp2 > tf_t) - T_potential.push_back(tmp2); - } - } - - FCL_REAL T_bound = Ta * T_potential[0] * T_potential[0] * T_potential[0] + Tb * T_potential[0] * T_potential[0] + Tc * T_potential[0]; - for(unsigned int i = 1; i < T_potential.size(); ++i) - { - FCL_REAL T_bound_tmp = Ta * T_potential[i] * T_potential[i] * T_potential[i] + Tb * T_potential[i] * T_potential[i] + Tc * T_potential[i]; - if(T_bound_tmp > T_bound) T_bound = T_bound_tmp; - } - - - FCL_REAL cur_delta = Ta * tf_t * tf_t * tf_t + Tb * tf_t * tf_t + Tc * tf_t; - - T_bound -= cur_delta; - T_bound /= 6.0; - - return T_bound; -} - - -FCL_REAL SplineMotion::computeDWMax() const -{ - // first compute ||w'|| - int a00[5] = {1,-4,6,-4,1}; - int a01[5] = {-3,10,-11,4,0}; - int a02[5] = {3,-8,6,0,-1}; - int a03[5] = {-1,2,-1,0,0}; - int a11[5] = {9,-24,16,0,0}; - int a12[5] = {-9,18,-5,-4,0}; - int a13[5] = {3,-4,0,0,0}; - int a22[5] = {9,-12,-2,4,1}; - int a23[5] = {-3,2,1,0,0}; - int a33[5] = {1,0,0,0,0}; - - FCL_REAL a[5]; - - for(int i = 0; i < 5; ++i) - { - a[i] = Rd0Rd0 * a00[i] + Rd0Rd1 * a01[i] + Rd0Rd2 * a02[i] + Rd0Rd3 * a03[i] - + Rd0Rd1 * a01[i] + Rd1Rd1 * a11[i] + Rd1Rd2 * a12[i] + Rd1Rd3 * a13[i] - + Rd0Rd2 * a02[i] + Rd1Rd2 * a12[i] + Rd2Rd2 * a22[i] + Rd2Rd3 * a23[i] - + Rd0Rd3 * a03[i] + Rd1Rd3 * a13[i] + Rd2Rd3 * a23[i] + Rd3Rd3 * a33[i]; - a[i] /= 4.0; - } - - // compute polynomial for ||w'||' - int da00[4] = {4,-12,12,-4}; - int da01[4] = {-12,30,-22,4}; - int da02[4] = {12,-24,12,0}; - int da03[4] = {-4,6,-2,0}; - int da11[4] = {36,-72,32,0}; - int da12[4] = {-36,54,-10,-4}; - int da13[4] = {12,-12,0,0}; - int da22[4] = {36,-36,-4,4}; - int da23[4] = {-12,6,2,0}; - int da33[4] = {4,0,0,0}; - - FCL_REAL da[4]; - for(int i = 0; i < 4; ++i) - { - da[i] = Rd0Rd0 * da00[i] + Rd0Rd1 * da01[i] + Rd0Rd2 * da02[i] + Rd0Rd3 * da03[i] - + Rd0Rd1 * da01[i] + Rd1Rd1 * da11[i] + Rd1Rd2 * da12[i] + Rd1Rd3 * da13[i] - + Rd0Rd2 * da02[i] + Rd1Rd2 * da12[i] + Rd2Rd2 * da22[i] + Rd2Rd3 * da23[i] - + Rd0Rd3 * da03[i] + Rd1Rd3 * da13[i] + Rd2Rd3 * da23[i] + Rd3Rd3 * da33[i]; - da[i] /= 4.0; - } - - FCL_REAL roots[3]; - - int root_num = PolySolver::solveCubic(da, roots); - - FCL_REAL dWdW_max = a[0] * tf_t * tf_t * tf_t + a[1] * tf_t * tf_t * tf_t + a[2] * tf_t * tf_t + a[3] * tf_t + a[4]; - FCL_REAL dWdW_1 = a[0] + a[1] + a[2] + a[3] + a[4]; - if(dWdW_max < dWdW_1) dWdW_max = dWdW_1; - for(int i = 0; i < root_num; ++i) - { - FCL_REAL v = roots[i]; - - if(v >= tf_t && v <= 1) - { - FCL_REAL value = a[0] * v * v * v * v + a[1] * v * v * v + a[2] * v * v + a[3] * v + a[4]; - if(value > dWdW_max) dWdW_max = value; - } - } - - return sqrt(dWdW_max); -} - -FCL_REAL SplineMotion::getWeight0(FCL_REAL t) const -{ - return (1 - 3 * t + 3 * t * t - t * t * t) / 6.0; -} - -FCL_REAL SplineMotion::getWeight1(FCL_REAL t) const -{ - return (4 - 6 * t * t + 3 * t * t * t) / 6.0; -} - -FCL_REAL SplineMotion::getWeight2(FCL_REAL t) const -{ - return (1 + 3 * t + 3 * t * t - 3 * t * t * t) / 6.0; -} - -FCL_REAL SplineMotion::getWeight3(FCL_REAL t) const -{ - return t * t * t / 6.0; -} - - - - -/// @brief Compute the motion bound for a bounding volume along a given direction n -/// according to mu < |v * n| + ||w x n||(r + max(||ci*||)) where ||ci*|| = ||R0(ci) x w||. w is the angular axis (normalized) -/// and ci are the endpoints of the generator primitives of RSS. -/// Notice that all bv parameters are in the local frame of the object, but n should be in the global frame (the reason is that the motion (t1, t2 and t) is in global frame) -template<> -FCL_REAL TBVMotionBoundVisitor::visit(const ScrewMotion& motion) const -{ - Transform3d tf; - motion.getCurrentTransform(tf); - - const Vector3d& axis = motion.getAxis(); - FCL_REAL linear_vel = motion.getLinearVelocity(); - FCL_REAL angular_vel = motion.getAngularVelocity(); - const Vector3d& p = motion.getAxisOrigin(); - - FCL_REAL c_proj_max = ((tf.linear() * bv.Tr).cross(axis)).squaredNorm(); - FCL_REAL tmp; - tmp = ((tf.linear() * (bv.Tr + bv.axis.col(0) * bv.l[0])).cross(axis)).squaredNorm(); - if(tmp > c_proj_max) c_proj_max = tmp; - tmp = ((tf.linear() * (bv.Tr + bv.axis.col(1) * bv.l[1])).cross(axis)).squaredNorm(); - if(tmp > c_proj_max) c_proj_max = tmp; - tmp = ((tf.linear() * (bv.Tr + bv.axis.col(0) * bv.l[0] + bv.axis.col(1) * bv.l[1])).cross(axis)).squaredNorm(); - if(tmp > c_proj_max) c_proj_max = tmp; - - c_proj_max = sqrt(c_proj_max); - - FCL_REAL v_dot_n = axis.dot(n) * linear_vel; - FCL_REAL w_cross_n = (axis.cross(n)).norm() * angular_vel; - FCL_REAL origin_proj = ((tf.translation() - p).cross(axis)).norm(); - - FCL_REAL mu = v_dot_n + w_cross_n * (c_proj_max + bv.r + origin_proj); - - return mu; -} - -FCL_REAL TriangleMotionBoundVisitor::visit(const ScrewMotion& motion) const -{ - Transform3d tf; - motion.getCurrentTransform(tf); - - const Vector3d& axis = motion.getAxis(); - FCL_REAL linear_vel = motion.getLinearVelocity(); - FCL_REAL angular_vel = motion.getAngularVelocity(); - const Vector3d& p = motion.getAxisOrigin(); - - FCL_REAL proj_max = ((tf.linear() * a + tf.translation() - p).cross(axis)).squaredNorm(); - FCL_REAL tmp; - tmp = ((tf.linear() * b + tf.translation() - p).cross(axis)).squaredNorm(); - if(tmp > proj_max) proj_max = tmp; - tmp = ((tf.linear() * c + tf.translation() - p).cross(axis)).squaredNorm(); - if(tmp > proj_max) proj_max = tmp; - - proj_max = std::sqrt(proj_max); - - FCL_REAL v_dot_n = axis.dot(n) * linear_vel; - FCL_REAL w_cross_n = (axis.cross(n)).norm() * angular_vel; - FCL_REAL mu = v_dot_n + w_cross_n * proj_max; - - return mu; -} - - - - - -/// @brief Compute the motion bound for a bounding volume along a given direction n -/// according to mu < |v * n| + ||w x n||(r + max(||ci*||)) where ||ci*|| = ||R0(ci) x w||. w is the angular axis (normalized) -/// and ci are the endpoints of the generator primitives of RSS. -/// Notice that all bv parameters are in the local frame of the object, but n should be in the global frame (the reason is that the motion (t1, t2 and t) is in global frame) -template<> -FCL_REAL TBVMotionBoundVisitor::visit(const InterpMotion& motion) const -{ - Transform3d tf; - motion.getCurrentTransform(tf); - - const Vector3d& reference_p = motion.getReferencePoint(); - const Vector3d& angular_axis = motion.getAngularAxis(); - FCL_REAL angular_vel = motion.getAngularVelocity(); - const Vector3d& linear_vel = motion.getLinearVelocity(); - - FCL_REAL c_proj_max = ((tf.linear() * (bv.Tr - reference_p)).cross(angular_axis)).squaredNorm(); - FCL_REAL tmp; - tmp = ((tf.linear() * (bv.Tr + bv.axis.col(0) * bv.l[0] - reference_p)).cross(angular_axis)).squaredNorm(); - if(tmp > c_proj_max) c_proj_max = tmp; - tmp = ((tf.linear() * (bv.Tr + bv.axis.col(1) * bv.l[1] - reference_p)).cross(angular_axis)).squaredNorm(); - if(tmp > c_proj_max) c_proj_max = tmp; - tmp = ((tf.linear() * (bv.Tr + bv.axis.col(0) * bv.l[0] + bv.axis.col(1) * bv.l[1] - reference_p)).cross(angular_axis)).squaredNorm(); - if(tmp > c_proj_max) c_proj_max = tmp; - - c_proj_max = std::sqrt(c_proj_max); - - FCL_REAL v_dot_n = linear_vel.dot(n); - FCL_REAL w_cross_n = (angular_axis.cross(n)).norm() * angular_vel; - FCL_REAL mu = v_dot_n + w_cross_n * (bv.r + c_proj_max); - - return mu; -} - -/// @brief Compute the motion bound for a triangle along a given direction n -/// according to mu < |v * n| + ||w x n||(max||ci*||) where ||ci*|| = ||R0(ci) x w|| / \|w\|. w is the angular velocity -/// and ci are the triangle vertex coordinates. -/// Notice that the triangle is in the local frame of the object, but n should be in the global frame (the reason is that the motion (t1, t2 and t) is in global frame) -FCL_REAL TriangleMotionBoundVisitor::visit(const InterpMotion& motion) const -{ - Transform3d tf; - motion.getCurrentTransform(tf); - - const Vector3d& reference_p = motion.getReferencePoint(); - const Vector3d& angular_axis = motion.getAngularAxis(); - FCL_REAL angular_vel = motion.getAngularVelocity(); - const Vector3d& linear_vel = motion.getLinearVelocity(); - - FCL_REAL proj_max = ((tf.linear() * (a - reference_p)).cross(angular_axis)).squaredNorm(); - FCL_REAL tmp; - tmp = ((tf.linear() * (b - reference_p)).cross(angular_axis)).squaredNorm(); - if(tmp > proj_max) proj_max = tmp; - tmp = ((tf.linear() * (c - reference_p)).cross(angular_axis)).squaredNorm(); - if(tmp > proj_max) proj_max = tmp; - - proj_max = std::sqrt(proj_max); - - FCL_REAL v_dot_n = linear_vel.dot(n); - FCL_REAL w_cross_n = (angular_axis.cross(n)).norm() * angular_vel; - FCL_REAL mu = v_dot_n + w_cross_n * proj_max; - - return mu; -} - -InterpMotion::InterpMotion() : MotionBase(), angular_axis(Vector3d::UnitX()) -{ - // Default angular velocity is zero - angular_vel = 0; - - // Default reference point is local zero point - - // Default linear velocity is zero -} - -InterpMotion::InterpMotion(const Matrix3d& R1, const Vector3d& T1, - const Matrix3d& R2, const Vector3d& T2) : MotionBase(), - tf1(Transform3d::Identity()), - tf2(Transform3d::Identity()) -{ - tf1.linear() = R1; - tf1.translation() = T1; - - tf2.linear() = R2; - tf2.translation() = T2; - - tf = tf1; - - // Compute the velocities for the motion - computeVelocity(); -} - - -InterpMotion::InterpMotion(const Transform3d& tf1_, const Transform3d& tf2_) : MotionBase(), - tf1(tf1_), - tf2(tf2_), - tf(tf1) -{ - // Compute the velocities for the motion - computeVelocity(); -} - -InterpMotion::InterpMotion(const Matrix3d& R1, const Vector3d& T1, - const Matrix3d& R2, const Vector3d& T2, - const Vector3d& O) : MotionBase(), - tf1(Transform3d::Identity()), - tf2(Transform3d::Identity()), - reference_p(O) -{ - tf1.linear() = R1; - tf1.translation() = T1; - - tf2.linear() = R2; - tf2.translation() = T2; - - tf = tf1; - - // Compute the velocities for the motion - computeVelocity(); -} - -InterpMotion::InterpMotion(const Transform3d& tf1_, const Transform3d& tf2_, const Vector3d& O) : MotionBase(), - tf1(tf1_), - tf2(tf2_), - tf(tf1), - reference_p(O) -{ -} - -bool InterpMotion::integrate(double dt) const -{ - if(dt > 1) dt = 1; - - tf.linear() = absoluteRotation(dt).toRotationMatrix(); - tf.translation() = linear_vel * dt + tf1 * reference_p - tf.linear() * reference_p; - - return true; -} - - -void InterpMotion::computeVelocity() -{ - linear_vel = tf2 * reference_p - tf1 * reference_p; - - const Eigen::AngleAxisd aa(tf2.linear() * tf1.linear().transpose()); - angular_axis = aa.axis(); - angular_vel = aa.angle(); - - if(angular_vel < 0) - { - angular_vel = -angular_vel; - angular_axis = -angular_axis; - } -} - - -Quaternion3d InterpMotion::deltaRotation(FCL_REAL dt) const -{ - return Quaternion3d(Eigen::AngleAxisd((FCL_REAL)(dt * angular_vel), angular_axis)); -} - -Quaternion3d InterpMotion::absoluteRotation(FCL_REAL dt) const -{ - Quaternion3d delta_t = deltaRotation(dt); - return delta_t * Quaternion3d(tf1.linear()); -} - - -/// @brief Compute the motion bound for a bounding volume along a given direction n -template<> -FCL_REAL TBVMotionBoundVisitor::visit(const TranslationMotion& motion) const -{ - return motion.getVelocity().dot(n); -} - -/// @brief Compute the motion bound for a triangle along a given direction n -FCL_REAL TriangleMotionBoundVisitor::visit(const TranslationMotion& motion) const -{ - return motion.getVelocity().dot(n); -} - -template class TBVMotionBoundVisitor; - -} diff --git a/src/ccd/taylor_matrix.cpp b/src/ccd/taylor_matrix.cpp deleted file mode 100644 index 25e9f8cf0..000000000 --- a/src/ccd/taylor_matrix.cpp +++ /dev/null @@ -1,440 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#include "fcl/ccd/taylor_matrix.h" - -namespace fcl -{ - -TMatrix3::TMatrix3() -{ -} - -TMatrix3::TMatrix3(const std::shared_ptr& time_interval) -{ - setTimeInterval(time_interval); -} - -TMatrix3::TMatrix3(TaylorModel m[3][3]) -{ - v_[0] = TVector3(m[0]); - v_[1] = TVector3(m[1]); - v_[2] = TVector3(m[2]); -} - -TMatrix3::TMatrix3(const TVector3& v1, const TVector3& v2, const TVector3& v3) -{ - v_[0] = v1; - v_[1] = v2; - v_[2] = v3; -} - - -TMatrix3::TMatrix3(const Matrix3d& m, const std::shared_ptr& time_interval) -{ - v_[0] = TVector3(m.row(0), time_interval); - v_[1] = TVector3(m.row(1), time_interval); - v_[2] = TVector3(m.row(2), time_interval); -} - -void TMatrix3::setIdentity() -{ - setZero(); - v_[0][0].coeff(0) = 1; - v_[1][1].coeff(0) = 1; - v_[2][2].coeff(0) = 1; - -} - -void TMatrix3::setZero() -{ - v_[0].setZero(); - v_[1].setZero(); - v_[2].setZero(); -} - -TVector3 TMatrix3::getColumn(size_t i) const -{ - return TVector3(v_[0][i], v_[1][i], v_[2][i]); -} - -const TVector3& TMatrix3::getRow(size_t i) const -{ - return v_[i]; -} - -const TaylorModel& TMatrix3::operator () (size_t i, size_t j) const -{ - return v_[i][j]; -} - -TaylorModel& TMatrix3::operator () (size_t i, size_t j) -{ - return v_[i][j]; -} - -TMatrix3 TMatrix3::operator * (const Matrix3d& m) const -{ - const Vector3d& mc0 = m.col(0); - const Vector3d& mc1 = m.col(1); - const Vector3d& mc2 = m.col(2); - - return TMatrix3(TVector3(v_[0].dot(mc0), v_[0].dot(mc1), v_[0].dot(mc2)), - TVector3(v_[1].dot(mc0), v_[1].dot(mc1), v_[1].dot(mc2)), - TVector3(v_[2].dot(mc0), v_[2].dot(mc1), v_[2].dot(mc2))); -} - - -TVector3 TMatrix3::operator * (const Vector3d& v) const -{ - return TVector3(v_[0].dot(v), v_[1].dot(v), v_[2].dot(v)); -} - -TVector3 TMatrix3::operator * (const TVector3& v) const -{ - return TVector3(v_[0].dot(v), v_[1].dot(v), v_[2].dot(v)); -} - -TMatrix3 TMatrix3::operator * (const TMatrix3& m) const -{ - const TVector3& mc0 = m.getColumn(0); - const TVector3& mc1 = m.getColumn(1); - const TVector3& mc2 = m.getColumn(2); - - return TMatrix3(TVector3(v_[0].dot(mc0), v_[0].dot(mc1), v_[0].dot(mc2)), - TVector3(v_[1].dot(mc0), v_[1].dot(mc1), v_[1].dot(mc2)), - TVector3(v_[2].dot(mc0), v_[2].dot(mc1), v_[2].dot(mc2))); -} - -TMatrix3 TMatrix3::operator * (const TaylorModel& d) const -{ - return TMatrix3(v_[0] * d, v_[1] * d, v_[2] * d); -} - -TMatrix3 TMatrix3::operator * (FCL_REAL d) const -{ - return TMatrix3(v_[0] * d, v_[1] * d, v_[2] * d); -} - - -TMatrix3& TMatrix3::operator *= (const Matrix3d& m) -{ - const Vector3d& mc0 = m.col(0); - const Vector3d& mc1 = m.col(1); - const Vector3d& mc2 = m.col(2); - - v_[0] = TVector3(v_[0].dot(mc0), v_[0].dot(mc1), v_[0].dot(mc2)); - v_[1] = TVector3(v_[1].dot(mc0), v_[1].dot(mc1), v_[1].dot(mc2)); - v_[2] = TVector3(v_[2].dot(mc0), v_[2].dot(mc1), v_[2].dot(mc2)); - return *this; -} - -TMatrix3& TMatrix3::operator *= (const TMatrix3& m) -{ - const TVector3& mc0 = m.getColumn(0); - const TVector3& mc1 = m.getColumn(1); - const TVector3& mc2 = m.getColumn(2); - - v_[0] = TVector3(v_[0].dot(mc0), v_[0].dot(mc1), v_[0].dot(mc2)); - v_[1] = TVector3(v_[1].dot(mc0), v_[1].dot(mc1), v_[1].dot(mc2)); - v_[2] = TVector3(v_[2].dot(mc0), v_[2].dot(mc1), v_[2].dot(mc2)); - - return *this; -} - -TMatrix3& TMatrix3::operator *= (const TaylorModel& d) -{ - v_[0] *= d; - v_[1] *= d; - v_[2] *= d; - return *this; -} - -TMatrix3& TMatrix3::operator *= (FCL_REAL d) -{ - v_[0] *= d; - v_[1] *= d; - v_[2] *= d; - return *this; -} - -TMatrix3 TMatrix3::operator + (const TMatrix3& m) const -{ - return TMatrix3(v_[0] + m.v_[0], v_[1] + m.v_[1], v_[2] + m.v_[2]); -} - -TMatrix3& TMatrix3::operator += (const TMatrix3& m) -{ - v_[0] += m.v_[0]; - v_[1] += m.v_[1]; - v_[2] += m.v_[2]; - return *this; -} - -TMatrix3 TMatrix3::operator + (const Matrix3d& m) const -{ - TMatrix3 res = *this; - res += m; - return res; -} - -TMatrix3& TMatrix3::operator += (const Matrix3d& m) -{ - for(std::size_t i = 0; i < 3; ++i) - { - for(std::size_t j = 0; j < 3; ++j) - v_[i][j] += m(i, j); - } - - return *this; -} - -TMatrix3 TMatrix3::operator - (const TMatrix3& m) const -{ - return TMatrix3(v_[0] - m.v_[0], v_[1] - m.v_[1], v_[2] - m.v_[2]); -} - -TMatrix3& TMatrix3::operator -= (const TMatrix3& m) -{ - v_[0] -= m.v_[0]; - v_[1] -= m.v_[1]; - v_[2] -= m.v_[2]; - return *this; -} - -TMatrix3 TMatrix3::operator - (const Matrix3d& m) const -{ - TMatrix3 res = *this; - res -= m; - return res; -} - -TMatrix3& TMatrix3::operator -= (const Matrix3d& m) -{ - for(std::size_t i = 0; i < 3; ++i) - { - for(std::size_t j = 0; j < 3; ++j) - v_[i][j] -= m(i, j); - } - - return *this; -} - -TMatrix3 TMatrix3::operator - () const -{ - return TMatrix3(-v_[0], -v_[1], -v_[2]); -} - - -void TMatrix3::print() const -{ - getColumn(0).print(); - getColumn(1).print(); - getColumn(2).print(); -} - -IMatrix3 TMatrix3::getBound() const -{ - return IMatrix3(v_[0].getBound(), v_[1].getBound(), v_[2].getBound()); -} - -IMatrix3 TMatrix3::getBound(FCL_REAL l, FCL_REAL r) const -{ - return IMatrix3(v_[0].getBound(l, r), v_[1].getBound(l, r), v_[2].getBound(l, r)); -} - -IMatrix3 TMatrix3::getBound(FCL_REAL t) const -{ - return IMatrix3(v_[0].getBound(t), v_[1].getBound(t), v_[2].getBound(t)); -} - -IMatrix3 TMatrix3::getTightBound() const -{ - return IMatrix3(v_[0].getTightBound(), v_[1].getTightBound(), v_[2].getTightBound()); -} - -IMatrix3 TMatrix3::getTightBound(FCL_REAL l, FCL_REAL r) const -{ - return IMatrix3(v_[0].getTightBound(l, r), v_[1].getTightBound(l, r), v_[2].getTightBound(l, r)); -} - - -FCL_REAL TMatrix3::diameter() const -{ - FCL_REAL d = 0; - - FCL_REAL tmp = v_[0][0].remainder().diameter(); - if(tmp > d) d = tmp; - tmp = v_[0][1].remainder().diameter(); - if(tmp > d) d = tmp; - tmp = v_[0][2].remainder().diameter(); - if(tmp > d) d = tmp; - - tmp = v_[1][0].remainder().diameter(); - if(tmp > d) d = tmp; - tmp = v_[1][1].remainder().diameter(); - if(tmp > d) d = tmp; - tmp = v_[1][2].remainder().diameter(); - if(tmp > d) d = tmp; - - tmp = v_[2][0].remainder().diameter(); - if(tmp > d) d = tmp; - tmp = v_[2][1].remainder().diameter(); - if(tmp > d) d = tmp; - tmp = v_[2][2].remainder().diameter(); - if(tmp > d) d = tmp; - - return d; -} - -void TMatrix3::setTimeInterval(const std::shared_ptr& time_interval) -{ - v_[0].setTimeInterval(time_interval); - v_[1].setTimeInterval(time_interval); - v_[2].setTimeInterval(time_interval); -} - -void TMatrix3::setTimeInterval(FCL_REAL l, FCL_REAL r) -{ - v_[0].setTimeInterval(l, r); - v_[1].setTimeInterval(l, r); - v_[2].setTimeInterval(l, r); -} - -const std::shared_ptr& TMatrix3::getTimeInterval() const -{ - return v_[0].getTimeInterval(); -} - -TMatrix3& TMatrix3::rotationConstrain() -{ - for(std::size_t i = 0; i < 3; ++i) - { - for(std::size_t j = 0; j < 3; ++j) - { - if(v_[i][j].remainder()[0] < -1) v_[i][j].remainder()[0] = -1; - else if(v_[i][j].remainder()[0] > 1) v_[i][j].remainder()[0] = 1; - - if(v_[i][j].remainder()[1] < -1) v_[i][j].remainder()[1] = -1; - else if(v_[i][j].remainder()[1] > 1) v_[i][j].remainder()[1] = 1; - - if((v_[i][j].remainder()[0] == -1) && (v_[i][j].remainder()[1] == 1)) - { - v_[i][j].coeff(0) = 0; - v_[i][j].coeff(1) = 0; - v_[i][j].coeff(2) = 0; - v_[i][j].coeff(3) = 0; - } - } - } - - return *this; -} - - -TMatrix3 rotationConstrain(const TMatrix3& m) -{ - TMatrix3 res; - - for(std::size_t i = 0; i < 3; ++i) - { - for(std::size_t j = 0; j < 3; ++j) - { - if(m(i, j).remainder()[0] < -1) res(i, j).remainder()[0] = -1; - else if(m(i, j).remainder()[0] > 1) res(i, j).remainder()[0] = 1; - - if(m(i, j).remainder()[1] < -1) res(i, j).remainder()[1] = -1; - else if(m(i, j).remainder()[1] > 1) res(i, j).remainder()[1] = 1; - - if((m(i, j).remainder()[0] == -1) && (m(i, j).remainder()[1] == 1)) - { - res(i, j).coeff(0) = 0; - res(i, j).coeff(1) = 0; - res(i, j).coeff(2) = 0; - res(i, j).coeff(3) = 0; - } - } - } - - return res; -} - -TMatrix3 operator * (const Matrix3d& m, const TaylorModel& a) -{ - TMatrix3 res(a.getTimeInterval()); - res(0, 0) = a * m(0, 0); - res(0, 1) = a * m(0, 1); - res(0, 1) = a * m(0, 2); - - res(1, 0) = a * m(1, 0); - res(1, 1) = a * m(1, 1); - res(1, 1) = a * m(1, 2); - - res(2, 0) = a * m(2, 0); - res(2, 1) = a * m(2, 1); - res(2, 1) = a * m(2, 2); - - return res; -} - -TMatrix3 operator * (const TaylorModel& a, const Matrix3d& m) -{ - return m * a; -} - -TMatrix3 operator * (const TaylorModel& a, const TMatrix3& m) -{ - return m * a; -} - -TMatrix3 operator * (FCL_REAL d, const TMatrix3& m) -{ - return m * d; -} - -TMatrix3 operator + (const Matrix3d& m1, const TMatrix3& m2) -{ - return m2 + m1; -} - -TMatrix3 operator - (const Matrix3d& m1, const TMatrix3& m2) -{ - return -m2 + m1; -} - - -} diff --git a/src/ccd/taylor_model.cpp b/src/ccd/taylor_model.cpp deleted file mode 100644 index 7526abca1..000000000 --- a/src/ccd/taylor_model.cpp +++ /dev/null @@ -1,505 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#include "fcl/math/constants.h" -#include "fcl/ccd/taylor_model.h" -#include -#include -#include - -namespace fcl -{ - -TaylorModel::TaylorModel() -{ - coeffs_[0] = coeffs_[1] = coeffs_[2] = coeffs_[3] = 0; -} - -TaylorModel::TaylorModel(const std::shared_ptr& time_interval) : time_interval_(time_interval) -{ - coeffs_[0] = coeffs_[1] = coeffs_[2] = coeffs_[3] = 0; -} - -TaylorModel::TaylorModel(FCL_REAL coeff, const std::shared_ptr& time_interval) : time_interval_(time_interval) -{ - coeffs_[0] = coeff; - coeffs_[1] = coeffs_[2] = coeffs_[3] = r_[0] = r_[1] = 0; -} - -TaylorModel::TaylorModel(FCL_REAL coeffs[3], const Interval& r, const std::shared_ptr& time_interval) : time_interval_(time_interval) -{ - coeffs_[0] = coeffs[0]; - coeffs_[1] = coeffs[1]; - coeffs_[2] = coeffs[2]; - coeffs_[3] = coeffs[3]; - - r_ = r; -} - -TaylorModel::TaylorModel(FCL_REAL c0, FCL_REAL c1, FCL_REAL c2, FCL_REAL c3, const Interval& r, const std::shared_ptr& time_interval) : time_interval_(time_interval) -{ - coeffs_[0] = c0; - coeffs_[1] = c1; - coeffs_[2] = c2; - coeffs_[3] = c3; - - r_ = r; -} - -TaylorModel TaylorModel::operator + (FCL_REAL d) const -{ - return TaylorModel(coeffs_[0] + d, coeffs_[1], coeffs_[2], coeffs_[3], r_, time_interval_); -} - -TaylorModel& TaylorModel::operator += (FCL_REAL d) -{ - coeffs_[0] += d; - return *this; -} - -TaylorModel TaylorModel::operator - (FCL_REAL d) const -{ - return TaylorModel(coeffs_[0] - d, coeffs_[1], coeffs_[2], coeffs_[3], r_, time_interval_); -} - -TaylorModel& TaylorModel::operator -= (FCL_REAL d) -{ - coeffs_[0] -= d; - return *this; -} - - -TaylorModel TaylorModel::operator + (const TaylorModel& other) const -{ - assert(other.time_interval_ == time_interval_); - return TaylorModel(coeffs_[0] + other.coeffs_[0], coeffs_[1] + other.coeffs_[1], coeffs_[2] + other.coeffs_[2], coeffs_[3] + other.coeffs_[3], r_ + other.r_, time_interval_); -} - -TaylorModel TaylorModel::operator - (const TaylorModel& other) const -{ - assert(other.time_interval_ == time_interval_); - return TaylorModel(coeffs_[0] - other.coeffs_[0], coeffs_[1] - other.coeffs_[1], coeffs_[2] - other.coeffs_[2], coeffs_[3] - other.coeffs_[3], r_ - other.r_, time_interval_); -} -TaylorModel& TaylorModel::operator += (const TaylorModel& other) -{ - assert(other.time_interval_ == time_interval_); - coeffs_[0] += other.coeffs_[0]; - coeffs_[1] += other.coeffs_[1]; - coeffs_[2] += other.coeffs_[2]; - coeffs_[3] += other.coeffs_[3]; - r_ += other.r_; - return *this; -} - -TaylorModel& TaylorModel::operator -= (const TaylorModel& other) -{ - assert(other.time_interval_ == time_interval_); - coeffs_[0] -= other.coeffs_[0]; - coeffs_[1] -= other.coeffs_[1]; - coeffs_[2] -= other.coeffs_[2]; - coeffs_[3] -= other.coeffs_[3]; - r_ -= other.r_; - return *this; -} - -/// @brief Taylor model multiplication: -/// f(t) = c0+c1*t+c2*t^2+c3*t^3+[a,b] -/// g(t) = c0'+c1'*t+c2'*t^2+c3'*t^2+[c,d] -/// f(t)g(t)= c0c0'+ -/// (c0c1'+c1c0')t+ -/// (c0c2'+c1c1'+c2c0')t^2+ -/// (c0c3'+c1c2'+c2c1'+c3c0')t^3+ -/// [a,b][c,d]+ -/// (c1c3'+c2c2'+c3c1')t^4+ -/// (c2c3'+c3c2')t^5+ -/// (c3c3')t^6+ -/// (c0+c1*t+c2*t^2+c3*t^3)[c,d]+ -/// (c0'+c1'*t+c2'*t^2+c3'*c^3)[a,b] -TaylorModel TaylorModel::operator * (const TaylorModel& other) const -{ - TaylorModel res(*this); - res *= other; - return res; -} - -TaylorModel TaylorModel::operator * (FCL_REAL d) const -{ - return TaylorModel(coeffs_[0] * d, coeffs_[1] * d, coeffs_[2] * d, coeffs_[3] * d, r_ * d, time_interval_); -} - -TaylorModel& TaylorModel::operator *= (const TaylorModel& other) -{ - assert(other.time_interval_ == time_interval_); - register FCL_REAL c0, c1, c2, c3; - register FCL_REAL c0b = other.coeffs_[0], c1b = other.coeffs_[1], c2b = other.coeffs_[2], c3b = other.coeffs_[3]; - - const Interval& rb = other.r_; - - c0 = coeffs_[0] * c0b; - c1 = coeffs_[0] * c1b + coeffs_[1] * c0b; - c2 = coeffs_[0] * c2b + coeffs_[1] * c1b + coeffs_[2] * c0b; - c3 = coeffs_[0] * c3b + coeffs_[1] * c2b + coeffs_[2] * c1b + coeffs_[3] * c0b; - - Interval remainder(r_ * rb); - register FCL_REAL tempVal = coeffs_[1] * c3b + coeffs_[2] * c2b + coeffs_[3] * c1b; - remainder += time_interval_->t4_ * tempVal; - - tempVal = coeffs_[2] * c3b + coeffs_[3] * c2b; - remainder += time_interval_->t5_ * tempVal; - - tempVal = coeffs_[3] * c3b; - remainder += time_interval_->t6_ * tempVal; - - remainder += ((Interval(coeffs_[0]) + time_interval_->t_ * coeffs_[1] + time_interval_->t2_ * coeffs_[2] + time_interval_->t3_ * coeffs_[3]) * rb + - (Interval(c0b) + time_interval_->t_ * c1b + time_interval_->t2_ * c2b + time_interval_->t3_ * c3b) * r_); - - coeffs_[0] = c0; - coeffs_[1] = c1; - coeffs_[2] = c2; - coeffs_[3] = c3; - - r_ = remainder; - - return *this; -} - -TaylorModel& TaylorModel::operator *= (FCL_REAL d) -{ - coeffs_[0] *= d; - coeffs_[1] *= d; - coeffs_[2] *= d; - coeffs_[3] *= d; - r_ *= d; - return *this; -} - - -TaylorModel TaylorModel::operator - () const -{ - return TaylorModel(-coeffs_[0], -coeffs_[1], -coeffs_[2], -coeffs_[3], -r_, time_interval_); -} - -void TaylorModel::print() const -{ - std::cout << coeffs_[0] << "+" << coeffs_[1] << "*t+" << coeffs_[2] << "*t^2+" << coeffs_[3] << "*t^3+[" << r_[0] << "," << r_[1] << "]" << std::endl; -} - -Interval TaylorModel::getBound(FCL_REAL t) const -{ - return Interval(coeffs_[0] + t * (coeffs_[1] + t * (coeffs_[2] + t * coeffs_[3]))) + r_; -} - -Interval TaylorModel::getBound(FCL_REAL t0, FCL_REAL t1) const -{ - Interval t(t0, t1); - Interval t2(t0 * t0, t1 * t1); - Interval t3(t0 * t2[0], t1 * t2[1]); - - return Interval(coeffs_[0]) + t * coeffs_[1] + t2 * coeffs_[2] + t3 * coeffs_[3] + r_; -} - -Interval TaylorModel::getBound() const -{ - return Interval(coeffs_[0] + r_[0], coeffs_[1] + r_[1]) + time_interval_->t_ * coeffs_[1] + time_interval_->t2_ * coeffs_[2] + time_interval_->t3_ * coeffs_[3]; -} - -Interval TaylorModel::getTightBound(FCL_REAL t0, FCL_REAL t1) const -{ - if(t0 < time_interval_->t_[0]) t0 = time_interval_->t_[0]; - if(t1 > time_interval_->t_[1]) t1 = time_interval_->t_[1]; - - if(coeffs_[3] == 0) - { - register FCL_REAL a = -coeffs_[1] / (2 * coeffs_[2]); - Interval polybounds; - if(a <= t1 && a >= t0) - { - FCL_REAL AQ = coeffs_[0] + a * (coeffs_[1] + a * coeffs_[2]); - register FCL_REAL t = t0; - FCL_REAL LQ = coeffs_[0] + t * (coeffs_[1] + t * coeffs_[2]); - t = t1; - FCL_REAL RQ = coeffs_[0] + t * (coeffs_[1] + t * coeffs_[2]); - - FCL_REAL minQ = LQ, maxQ = RQ; - if(LQ > RQ) - { - minQ = RQ; - maxQ = LQ; - } - - if(minQ > AQ) minQ = AQ; - if(maxQ < AQ) maxQ = AQ; - - polybounds.setValue(minQ, maxQ); - } - else - { - register FCL_REAL t = t0; - FCL_REAL LQ = coeffs_[0] + t * (coeffs_[1] + t * coeffs_[2]); - t = t1; - FCL_REAL RQ = coeffs_[0] + t * (coeffs_[1] + t * coeffs_[2]); - - if(LQ > RQ) polybounds.setValue(RQ, LQ); - else polybounds.setValue(LQ, RQ); - } - - return polybounds + r_; - } - else - { - register FCL_REAL t = t0; - FCL_REAL LQ = coeffs_[0] + t * (coeffs_[1] + t * (coeffs_[2] + t * coeffs_[3])); - t = t1; - FCL_REAL RQ = coeffs_[0] + t * (coeffs_[1] + t * (coeffs_[2] + t * coeffs_[3])); - - if(LQ > RQ) - { - FCL_REAL tmp = LQ; - LQ = RQ; - RQ = tmp; - } - - // derivative: c1+2*c2*t+3*c3*t^2 - - FCL_REAL delta = coeffs_[2] * coeffs_[2] - 3 * coeffs_[1] * coeffs_[3]; - if(delta < 0) - return Interval(LQ, RQ) + r_; - - FCL_REAL r1 = (-coeffs_[2]-sqrt(delta))/(3*coeffs_[3]); - FCL_REAL r2 = (-coeffs_[2]+sqrt(delta))/(3*coeffs_[3]); - - if(r1 <= t1 && r1 >= t0) - { - FCL_REAL Q = coeffs_[0] + r1 * (coeffs_[1] + r1 * (coeffs_[2] + r1 * coeffs_[3])); - if(Q < LQ) LQ = Q; - else if(Q > RQ) RQ = Q; - } - - if(r2 <= t1 && r2 >= t0) - { - FCL_REAL Q = coeffs_[0] + r2 * (coeffs_[1] + r2 * (coeffs_[2] + r2 * coeffs_[3])); - if(Q < LQ) LQ = Q; - else if(Q > RQ) RQ = Q; - } - - return Interval(LQ, RQ) + r_; - } -} - -Interval TaylorModel::getTightBound() const -{ - return getTightBound(time_interval_->t_[0], time_interval_->t_[1]); -} - -void TaylorModel::setZero() -{ - coeffs_[0] = coeffs_[1] = coeffs_[2] = coeffs_[3] = 0; - r_.setValue(0); -} - -TaylorModel operator * (FCL_REAL d, const TaylorModel& a) -{ - TaylorModel res(a); - res.coeff(0) *= d; - res.coeff(1) *= d; - res.coeff(2) *= d; - res.coeff(3) *= d; - res.remainder() *= d; - return res; -} - -TaylorModel operator + (FCL_REAL d, const TaylorModel& a) -{ - return a + d; -} - -TaylorModel operator - (FCL_REAL d, const TaylorModel& a) -{ - return -a + d; -} - - -void generateTaylorModelForCosFunc(TaylorModel& tm, FCL_REAL w, FCL_REAL q0) -{ - FCL_REAL a = tm.getTimeInterval()->t_.center(); - FCL_REAL t = w * a + q0; - FCL_REAL w2 = w * w; - FCL_REAL fa = cos(t); - FCL_REAL fda = -w*sin(t); - FCL_REAL fdda = -w2*fa; - FCL_REAL fddda = -w2*fda; - - tm.coeff(0) = fa-a*(fda-0.5*a*(fdda-1.0/3.0*a*fddda)); - tm.coeff(1) = fda-a*fdda+0.5*a*a*fddda; - tm.coeff(2) = 0.5*(fdda-a*fddda); - tm.coeff(3) = 1.0/6.0*fddda; - - // compute bounds for w^3 cos(wt+q0)/16, t \in [t0, t1] - Interval fddddBounds; - if(w == 0) fddddBounds.setValue(0); - else - { - FCL_REAL cosQL = cos(tm.getTimeInterval()->t_[0] * w + q0); - FCL_REAL cosQR = cos(tm.getTimeInterval()->t_[1] * w + q0); - - if(cosQL < cosQR) fddddBounds.setValue(cosQL, cosQR); - else fddddBounds.setValue(cosQR, cosQL); - - // enlarge to handle round-off errors - fddddBounds[0] -= 1e-15; - fddddBounds[1] += 1e-15; - - // cos reaches maximum if there exists an integer k in [(w*t0+q0)/2pi, (w*t1+q0)/2pi]; - // cos reaches minimum if there exists an integer k in [(w*t0+q0-pi)/2pi, (w*t1+q0-pi)/2pi] - - FCL_REAL k1 = (tm.getTimeInterval()->t_[0] * w + q0) / (2 * constants::pi); - FCL_REAL k2 = (tm.getTimeInterval()->t_[1] * w + q0) / (2 * constants::pi); - - - if(w > 0) - { - if(ceil(k2) - floor(k1) > 1) fddddBounds[1] = 1; - k1 -= 0.5; - k2 -= 0.5; - if(ceil(k2) - floor(k1) > 1) fddddBounds[0] = -1; - } - else - { - if(ceil(k1) - floor(k2) > 1) fddddBounds[1] = 1; - k1 -= 0.5; - k2 -= 0.5; - if(ceil(k1) - floor(k2) > 1) fddddBounds[0] = -1; - } - } - - FCL_REAL w4 = w2 * w2; - fddddBounds *= w4; - - FCL_REAL midSize = 0.5 * (tm.getTimeInterval()->t_[1] - tm.getTimeInterval()->t_[0]); - FCL_REAL midSize2 = midSize * midSize; - FCL_REAL midSize4 = midSize2 * midSize2; - - // [0, midSize4] * fdddBounds - if(fddddBounds[0] > 0) - tm.remainder().setValue(0, fddddBounds[1] * midSize4 * (1.0 / 24)); - else if(fddddBounds[0] < 0) - tm.remainder().setValue(fddddBounds[0] * midSize4 * (1.0 / 24), 0); - else - tm.remainder().setValue(fddddBounds[0] * midSize4 * (1.0 / 24), fddddBounds[1] * midSize4 * (1.0 / 24)); -} - -void generateTaylorModelForSinFunc(TaylorModel& tm, FCL_REAL w, FCL_REAL q0) -{ - FCL_REAL a = tm.getTimeInterval()->t_.center(); - FCL_REAL t = w * a + q0; - FCL_REAL w2 = w * w; - FCL_REAL fa = sin(t); - FCL_REAL fda = w*cos(t); - FCL_REAL fdda = -w2*fa; - FCL_REAL fddda = -w2*fda; - - tm.coeff(0) = fa-a*(fda-0.5*a*(fdda-1.0/3.0*a*fddda)); - tm.coeff(1) = fda-a*fdda+0.5*a*a*fddda; - tm.coeff(2) = 0.5*(fdda-a*fddda); - tm.coeff(3) = 1.0/6.0*fddda; - - // compute bounds for w^3 sin(wt+q0)/16, t \in [t0, t1] - - Interval fddddBounds; - - if(w == 0) fddddBounds.setValue(0); - else - { - FCL_REAL sinQL = sin(w * tm.getTimeInterval()->t_[0] + q0); - FCL_REAL sinQR = sin(w * tm.getTimeInterval()->t_[1] + q0); - - if(sinQL < sinQR) fddddBounds.setValue(sinQL, sinQR); - else fddddBounds.setValue(sinQR, sinQL); - - // enlarge to handle round-off errors - fddddBounds[0] -= 1e-15; - fddddBounds[1] += 1e-15; - - // sin reaches maximum if there exists an integer k in [(w*t0+q0-pi/2)/2pi, (w*t1+q0-pi/2)/2pi]; - // sin reaches minimum if there exists an integer k in [(w*t0+q0-pi-pi/2)/2pi, (w*t1+q0-pi-pi/2)/2pi] - - FCL_REAL k1 = (tm.getTimeInterval()->t_[0] * w + q0) / (2 * constants::pi) - 0.25; - FCL_REAL k2 = (tm.getTimeInterval()->t_[1] * w + q0) / (2 * constants::pi) - 0.25; - - if(w > 0) - { - if(ceil(k2) - floor(k1) > 1) fddddBounds[1] = 1; - k1 -= 0.5; - k2 -= 0.5; - if(ceil(k2) - floor(k1) > 1) fddddBounds[0] = -1; - } - else - { - if(ceil(k1) - floor(k2) > 1) fddddBounds[1] = 1; - k1 -= 0.5; - k2 -= 0.5; - if(ceil(k1) - floor(k2) > 1) fddddBounds[0] = -1; - } - - FCL_REAL w4 = w2 * w2; - fddddBounds *= w4; - - FCL_REAL midSize = 0.5 * (tm.getTimeInterval()->t_[1] - tm.getTimeInterval()->t_[0]); - FCL_REAL midSize2 = midSize * midSize; - FCL_REAL midSize4 = midSize2 * midSize2; - - // [0, midSize4] * fdddBounds - if(fddddBounds[0] > 0) - tm.remainder().setValue(0, fddddBounds[1] * midSize4 * (1.0 / 24)); - else if(fddddBounds[0] < 0) - tm.remainder().setValue(fddddBounds[0] * midSize4 * (1.0 / 24), 0); - else - tm.remainder().setValue(fddddBounds[0] * midSize4 * (1.0 / 24), fddddBounds[1] * midSize4 * (1.0 / 24)); - } -} - -void generateTaylorModelForLinearFunc(TaylorModel& tm, FCL_REAL p, FCL_REAL v) -{ - tm.coeff(0) = p; - tm.coeff(1) = v; - tm.coeff(2) = 0; - tm.coeff(3) = 0; - tm.remainder()[0] = 0; - tm.remainder()[1] = 0; -} - -} diff --git a/src/ccd/taylor_vector.cpp b/src/ccd/taylor_vector.cpp deleted file mode 100644 index 7fdf4f815..000000000 --- a/src/ccd/taylor_vector.cpp +++ /dev/null @@ -1,291 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#include "fcl/ccd/taylor_vector.h" - -namespace fcl -{ - -TVector3::TVector3() -{ -} - -TVector3::TVector3(const std::shared_ptr& time_interval) -{ - setTimeInterval(time_interval); -} - -TVector3::TVector3(TaylorModel v[3]) -{ - i_[0] = v[0]; - i_[1] = v[1]; - i_[2] = v[2]; -} - -TVector3::TVector3(const TaylorModel& v1, const TaylorModel& v2, const TaylorModel& v3) -{ - i_[0] = v1; - i_[1] = v2; - i_[2] = v3; -} - -TVector3::TVector3(const Vector3d& v, const std::shared_ptr& time_interval) -{ - i_[0] = TaylorModel(v[0], time_interval); - i_[1] = TaylorModel(v[1], time_interval); - i_[2] = TaylorModel(v[2], time_interval); -} - -void TVector3::setZero() -{ - i_[0].setZero(); - i_[1].setZero(); - i_[2].setZero(); -} - -TVector3 TVector3::operator + (const TVector3& other) const -{ - return TVector3(i_[0] + other.i_[0], i_[1] + other.i_[1], i_[2] + other.i_[2]); -} - -TVector3 TVector3::operator - (const TVector3& other) const -{ - return TVector3(i_[0] - other.i_[0], i_[1] - other.i_[1], i_[2] - other.i_[2]); -} - -TVector3 TVector3::operator - () const -{ - return TVector3(-i_[0], -i_[1], -i_[2]); -} - -TVector3& TVector3::operator += (const TVector3& other) -{ - i_[0] += other.i_[0]; - i_[1] += other.i_[1]; - i_[2] += other.i_[2]; - return *this; -} - -TVector3& TVector3::operator -= (const TVector3& other) -{ - i_[0] -= other.i_[0]; - i_[1] -= other.i_[1]; - i_[2] -= other.i_[2]; - return *this; -} - -TVector3 TVector3::operator + (const Vector3d& other) const -{ - return TVector3(i_[0] + other[0], i_[1] + other[1], i_[2] + other[2]); -} - -TVector3& TVector3::operator += (const Vector3d& other) -{ - i_[0] += other[0]; - i_[1] += other[1]; - i_[2] += other[2]; - return *this; -} - -TVector3 TVector3::operator - (const Vector3d& other) const -{ - return TVector3(i_[0] - other[0], i_[1] - other[1], i_[2] - other[2]); -} - -TVector3& TVector3::operator -= (const Vector3d& other) -{ - i_[0] -= other[0]; - i_[1] -= other[1]; - i_[2] -= other[2]; - return *this; -} - - -TVector3 TVector3::operator * (const TaylorModel& d) const -{ - return TVector3(i_[0] * d, i_[1] * d, i_[2] * d); -} - -TVector3& TVector3::operator *= (const TaylorModel& d) -{ - i_[0] *= d; - i_[1] *= d; - i_[2] *= d; - return *this; -} - -TVector3 TVector3::operator * (FCL_REAL d) const -{ - return TVector3(i_[0] * d, i_[1] * d, i_[2] * d); -} - -TVector3& TVector3::operator *= (FCL_REAL d) -{ - i_[0] *= d; - i_[1] *= d; - i_[2] *= d; - return *this; -} - -const TaylorModel& TVector3::operator [] (size_t i) const -{ - return i_[i]; -} - -TaylorModel& TVector3::operator [] (size_t i) -{ - return i_[i]; -} - -TaylorModel TVector3::dot(const TVector3& other) const -{ - return i_[0] * other.i_[0] + i_[1] * other.i_[1] + i_[2] * other.i_[2]; -} - -TVector3 TVector3::cross(const TVector3& other) const -{ - return TVector3(i_[1] * other.i_[2] - i_[2] * other.i_[1], - i_[2] * other.i_[0] - i_[0] * other.i_[2], - i_[0] * other.i_[1] - i_[1] * other.i_[0]); -} - -TaylorModel TVector3::dot(const Vector3d& other) const -{ - return i_[0] * other[0] + i_[1] * other[1] + i_[2] * other[2]; -} - -TVector3 TVector3::cross(const Vector3d& other) const -{ - return TVector3(i_[1] * other[2] - i_[2] * other[1], - i_[2] * other[0] - i_[0] * other[2], - i_[0] * other[1] - i_[1] * other[0]); -} - -FCL_REAL TVector3::volumn() const -{ - return i_[0].getBound().diameter() * i_[1].getBound().diameter() * i_[2].getBound().diameter(); -} - -IVector3 TVector3::getBound() const -{ - return IVector3(i_[0].getBound(), i_[1].getBound(), i_[2].getBound()); -} - -IVector3 TVector3::getBound(FCL_REAL l, FCL_REAL r) const -{ - return IVector3(i_[0].getBound(l, r), i_[1].getBound(l, r), i_[2].getBound(l, r)); -} - -IVector3 TVector3::getBound(FCL_REAL t) const -{ - return IVector3(i_[0].getBound(t), i_[1].getBound(t), i_[2].getBound(t)); -} - -IVector3 TVector3::getTightBound() const -{ - return IVector3(i_[0].getTightBound(), i_[1].getTightBound(), i_[2].getTightBound()); -} - -IVector3 TVector3::getTightBound(FCL_REAL l, FCL_REAL r) const -{ - return IVector3(i_[0].getTightBound(l, r), i_[1].getTightBound(l, r), i_[2].getTightBound(l, r)); -} - - -void TVector3::print() const -{ - i_[0].print(); - i_[1].print(); - i_[2].print(); -} - - -TaylorModel TVector3::squareLength() const -{ - return i_[0] * i_[0] + i_[1] * i_[1] + i_[2] * i_[2]; -} - -void TVector3::setTimeInterval(const std::shared_ptr& time_interval) -{ - i_[0].setTimeInterval(time_interval); - i_[1].setTimeInterval(time_interval); - i_[2].setTimeInterval(time_interval); -} - -void TVector3::setTimeInterval(FCL_REAL l, FCL_REAL r) -{ - i_[0].setTimeInterval(l, r); - i_[1].setTimeInterval(l, r); - i_[2].setTimeInterval(l, r); -} - -const std::shared_ptr& TVector3::getTimeInterval() const -{ - return i_[0].getTimeInterval(); -} - -void generateTVector3ForLinearFunc(TVector3& v, const Vector3d& position, const Vector3d& velocity) -{ - generateTaylorModelForLinearFunc(v[0], position[0], velocity[0]); - generateTaylorModelForLinearFunc(v[1], position[1], velocity[1]); - generateTaylorModelForLinearFunc(v[2], position[2], velocity[2]); -} - - -TVector3 operator * (const Vector3d& v, const TaylorModel& a) -{ - TVector3 res(a.getTimeInterval()); - res[0] = a * v[0]; - res[1] = a * v[1]; - res[2] = a * v[2]; - - return res; -} - -TVector3 operator + (const Vector3d& v1, const TVector3& v2) -{ - return v2 + v1; -} - -TVector3 operator - (const Vector3d& v1, const TVector3& v2) -{ - return -v2 + v1; -} - - - -} diff --git a/src/collision.cpp b/src/collision.cpp deleted file mode 100644 index 36e288132..000000000 --- a/src/collision.cpp +++ /dev/null @@ -1,162 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - - -#include "fcl/collision.h" -#include "fcl/collision_func_matrix.h" -#include "fcl/narrowphase/narrowphase.h" - -#include - -namespace fcl -{ - -template -CollisionFunctionMatrix& getCollisionFunctionLookTable() -{ - static CollisionFunctionMatrix table; - return table; -}; - -template -std::size_t collide(const CollisionObject* o1, const CollisionObject* o2, - const NarrowPhaseSolver* nsolver, - const CollisionRequest& request, - CollisionResult& result) -{ - return collide(o1->collisionGeometry().get(), o1->getTransform(), o2->collisionGeometry().get(), o2->getTransform(), - nsolver, request, result); -} - -template -std::size_t collide(const CollisionGeometry* o1, const Transform3d& tf1, - const CollisionGeometry* o2, const Transform3d& tf2, - const NarrowPhaseSolver* nsolver_, - const CollisionRequest& request, - CollisionResult& result) -{ - const NarrowPhaseSolver* nsolver = nsolver_; - if(!nsolver_) - nsolver = new NarrowPhaseSolver(); - - const CollisionFunctionMatrix& looktable = getCollisionFunctionLookTable(); - - std::size_t res; - if(request.num_max_contacts == 0) - { - std::cerr << "Warning: should stop early as num_max_contact is " << request.num_max_contacts << " !" << std::endl; - res = 0; - } - else - { - OBJECT_TYPE object_type1 = o1->getObjectType(); - OBJECT_TYPE object_type2 = o2->getObjectType(); - NODE_TYPE node_type1 = o1->getNodeType(); - NODE_TYPE node_type2 = o2->getNodeType(); - - if(object_type1 == OT_GEOM && object_type2 == OT_BVH) - { - if(!looktable.collision_matrix[node_type2][node_type1]) - { - std::cerr << "Warning: collision function between node type " << node_type1 << " and node type " << node_type2 << " is not supported"<< std::endl; - res = 0; - } - else - res = looktable.collision_matrix[node_type2][node_type1](o2, tf2, o1, tf1, nsolver, request, result); - } - else - { - if(!looktable.collision_matrix[node_type1][node_type2]) - { - std::cerr << "Warning: collision function between node type " << node_type1 << " and node type " << node_type2 << " is not supported"<< std::endl; - res = 0; - } - else - res = looktable.collision_matrix[node_type1][node_type2](o1, tf1, o2, tf2, nsolver, request, result); - } - } - - if(!nsolver_) - delete nsolver; - - return res; -} - -std::size_t collide(const CollisionObject* o1, const CollisionObject* o2, - const CollisionRequest& request, CollisionResult& result) -{ - switch(request.gjk_solver_type) - { - case GST_LIBCCD: - { - GJKSolver_libccd solver; - return collide(o1, o2, &solver, request, result); - } - case GST_INDEP: - { - GJKSolver_indep solver; - return collide(o1, o2, &solver, request, result); - } - default: - return -1; // error - } -} - -std::size_t collide(const CollisionGeometry* o1, const Transform3d& tf1, - const CollisionGeometry* o2, const Transform3d& tf2, - const CollisionRequest& request, CollisionResult& result) -{ - switch(request.gjk_solver_type) - { - case GST_LIBCCD: - { - GJKSolver_libccd solver; - return collide(o1, tf1, o2, tf2, &solver, request, result); - } - case GST_INDEP: - { - GJKSolver_indep solver; - return collide(o1, tf1, o2, tf2, &solver, request, result); - } - default: - std::cerr << "Warning! Invalid GJK solver" << std::endl; - return -1; // error - } -} - -} - diff --git a/src/collision_func_matrix.cpp b/src/collision_func_matrix.cpp deleted file mode 100755 index d938e4a67..000000000 --- a/src/collision_func_matrix.cpp +++ /dev/null @@ -1,681 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - - -#include "fcl/collision_func_matrix.h" - -#include "fcl/traversal/traversal_node_setup.h" -#include "fcl/collision_node.h" -#include "fcl/narrowphase/narrowphase.h" - -namespace fcl -{ - -#if FCL_HAVE_OCTOMAP -template -std::size_t ShapeOcTreeCollide(const CollisionGeometry* o1, const Transform3d& tf1, const CollisionGeometry* o2, const Transform3d& tf2, - const NarrowPhaseSolver* nsolver, - const CollisionRequest& request, CollisionResult& result) -{ - if(request.isSatisfied(result)) return result.numContacts(); - - ShapeOcTreeCollisionTraversalNode node; - const T_SH* obj1 = static_cast(o1); - const OcTree* obj2 = static_cast(o2); - OcTreeSolver otsolver(nsolver); - - initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result); - collide(&node); - - return result.numContacts(); -} - -template -std::size_t OcTreeShapeCollide(const CollisionGeometry* o1, const Transform3d& tf1, const CollisionGeometry* o2, const Transform3d& tf2, - const NarrowPhaseSolver* nsolver, - const CollisionRequest& request, CollisionResult& result) -{ - if(request.isSatisfied(result)) return result.numContacts(); - - OcTreeShapeCollisionTraversalNode node; - const OcTree* obj1 = static_cast(o1); - const T_SH* obj2 = static_cast(o2); - OcTreeSolver otsolver(nsolver); - - initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result); - collide(&node); - - return result.numContacts(); -} - -template -std::size_t OcTreeCollide(const CollisionGeometry* o1, const Transform3d& tf1, const CollisionGeometry* o2, const Transform3d& tf2, - const NarrowPhaseSolver* nsolver, - const CollisionRequest& request, CollisionResult& result) -{ - if(request.isSatisfied(result)) return result.numContacts(); - - OcTreeCollisionTraversalNode node; - const OcTree* obj1 = static_cast(o1); - const OcTree* obj2 = static_cast(o2); - OcTreeSolver otsolver(nsolver); - - initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result); - collide(&node); - - return result.numContacts(); -} - -template -std::size_t OcTreeBVHCollide(const CollisionGeometry* o1, const Transform3d& tf1, const CollisionGeometry* o2, const Transform3d& tf2, - const NarrowPhaseSolver* nsolver, - const CollisionRequest& request, CollisionResult& result) -{ - if(request.isSatisfied(result)) return result.numContacts(); - - if(request.enable_cost && request.use_approximate_cost) - { - CollisionRequest no_cost_request(request); // request remove cost to avoid the exact but expensive cost computation between mesh and octree - no_cost_request.enable_cost = false; // disable cost computation - - OcTreeMeshCollisionTraversalNode node; - const OcTree* obj1 = static_cast(o1); - const BVHModel* obj2 = static_cast*>(o2); - OcTreeSolver otsolver(nsolver); - - initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, no_cost_request, result); - collide(&node); - - Box box; - Transform3d box_tf; - constructBox(obj2->getBV(0).bv, tf2, box, box_tf); // compute the box for BVH's root node - - box.cost_density = obj2->cost_density; - box.threshold_occupied = obj2->threshold_occupied; - box.threshold_free = obj2->threshold_free; - - CollisionRequest only_cost_request(result.numContacts(), false, request.num_max_cost_sources, true, false); // additional cost request, no contacts - OcTreeShapeCollide(o1, tf1, &box, box_tf, nsolver, only_cost_request, result); - } - else - { - OcTreeMeshCollisionTraversalNode node; - const OcTree* obj1 = static_cast(o1); - const BVHModel* obj2 = static_cast*>(o2); - OcTreeSolver otsolver(nsolver); - - initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result); - collide(&node); - } - - return result.numContacts(); -} - -template -std::size_t BVHOcTreeCollide(const CollisionGeometry* o1, const Transform3d& tf1, const CollisionGeometry* o2, const Transform3d& tf2, - const NarrowPhaseSolver* nsolver, - const CollisionRequest& request, CollisionResult& result) -{ - if(request.isSatisfied(result)) return result.numContacts(); - - if(request.enable_cost && request.use_approximate_cost) - { - CollisionRequest no_cost_request(request); // request remove cost to avoid the exact but expensive cost computation between mesh and octree - no_cost_request.enable_cost = false; // disable cost computation - - MeshOcTreeCollisionTraversalNode node; - const BVHModel* obj1 = static_cast*>(o1); - const OcTree* obj2 = static_cast(o2); - OcTreeSolver otsolver(nsolver); - - initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, no_cost_request, result); - collide(&node); - - Box box; - Transform3d box_tf; - constructBox(obj1->getBV(0).bv, tf1, box, box_tf); - - box.cost_density = obj1->cost_density; - box.threshold_occupied = obj1->threshold_occupied; - box.threshold_free = obj1->threshold_free; - - CollisionRequest only_cost_request(result.numContacts(), false, request.num_max_cost_sources, true, false); - ShapeOcTreeCollide(&box, box_tf, o2, tf2, nsolver, only_cost_request, result); - } - else - { - MeshOcTreeCollisionTraversalNode node; - const BVHModel* obj1 = static_cast*>(o1); - const OcTree* obj2 = static_cast(o2); - OcTreeSolver otsolver(nsolver); - - initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result); - collide(&node); - } - - return result.numContacts(); -} - -#endif - -template -std::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3d& tf1, const CollisionGeometry* o2, const Transform3d& tf2, - const NarrowPhaseSolver* nsolver, - const CollisionRequest& request, CollisionResult& result) -{ - if(request.isSatisfied(result)) return result.numContacts(); - - ShapeCollisionTraversalNode node; - const T_SH1* obj1 = static_cast(o1); - const T_SH2* obj2 = static_cast(o2); - - if(request.enable_cached_gjk_guess) - { - nsolver->enableCachedGuess(true); - nsolver->setCachedGuess(request.cached_gjk_guess); - } - else - { - nsolver->enableCachedGuess(true); - } - - initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result); - collide(&node); - - if(request.enable_cached_gjk_guess) - result.cached_gjk_guess = nsolver->getCachedGuess(); - - return result.numContacts(); -} - -template -struct BVHShapeCollider -{ - static std::size_t collide(const CollisionGeometry* o1, const Transform3d& tf1, const CollisionGeometry* o2, const Transform3d& tf2, - const NarrowPhaseSolver* nsolver, - const CollisionRequest& request, CollisionResult& result) - { - if(request.isSatisfied(result)) return result.numContacts(); - - if(request.enable_cost && request.use_approximate_cost) - { - CollisionRequest no_cost_request(request); - no_cost_request.enable_cost = false; - - MeshShapeCollisionTraversalNode node; - const BVHModel* obj1 = static_cast* >(o1); - BVHModel* obj1_tmp = new BVHModel(*obj1); - Transform3d tf1_tmp = tf1; - const T_SH* obj2 = static_cast(o2); - - initialize(node, *obj1_tmp, tf1_tmp, *obj2, tf2, nsolver, no_cost_request, result); - fcl::collide(&node); - - delete obj1_tmp; - - Box box; - Transform3d box_tf; - constructBox(obj1->getBV(0).bv, tf1, box, box_tf); - - box.cost_density = obj1->cost_density; - box.threshold_occupied = obj1->threshold_occupied; - box.threshold_free = obj1->threshold_free; - - CollisionRequest only_cost_request(result.numContacts(), false, request.num_max_cost_sources, true, false); - ShapeShapeCollide(&box, box_tf, o2, tf2, nsolver, only_cost_request, result); - } - else - { - MeshShapeCollisionTraversalNode node; - const BVHModel* obj1 = static_cast* >(o1); - BVHModel* obj1_tmp = new BVHModel(*obj1); - Transform3d tf1_tmp = tf1; - const T_SH* obj2 = static_cast(o2); - - initialize(node, *obj1_tmp, tf1_tmp, *obj2, tf2, nsolver, request, result); - fcl::collide(&node); - - delete obj1_tmp; - } - - return result.numContacts(); - } -}; - -namespace details -{ - -template -std::size_t orientedBVHShapeCollide(const CollisionGeometry* o1, const Transform3d& tf1, const CollisionGeometry* o2, const Transform3d& tf2, - const NarrowPhaseSolver* nsolver, - const CollisionRequest& request, CollisionResult& result) -{ - if(request.isSatisfied(result)) return result.numContacts(); - - if(request.enable_cost && request.use_approximate_cost) - { - CollisionRequest no_cost_request(request); - no_cost_request.enable_cost = false; - - OrientMeshShapeCollisionTraveralNode node; - const BVHModel* obj1 = static_cast* >(o1); - const T_SH* obj2 = static_cast(o2); - - initialize(node, *obj1, tf1, *obj2, tf2, nsolver, no_cost_request, result); - fcl::collide(&node); - - Box box; - Transform3d box_tf; - constructBox(obj1->getBV(0).bv, tf1, box, box_tf); - - box.cost_density = obj1->cost_density; - box.threshold_occupied = obj1->threshold_occupied; - box.threshold_free = obj1->threshold_free; - - CollisionRequest only_cost_request(result.numContacts(), false, request.num_max_cost_sources, true, false); - ShapeShapeCollide(&box, box_tf, o2, tf2, nsolver, only_cost_request, result); - } - else - { - OrientMeshShapeCollisionTraveralNode node; - const BVHModel* obj1 = static_cast* >(o1); - const T_SH* obj2 = static_cast(o2); - - initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result); - fcl::collide(&node); - } - - return result.numContacts(); -} - -} - - -template -struct BVHShapeCollider -{ - static std::size_t collide(const CollisionGeometry* o1, const Transform3d& tf1, const CollisionGeometry* o2, const Transform3d& tf2, - const NarrowPhaseSolver* nsolver, - const CollisionRequest& request, CollisionResult& result) - { - return details::orientedBVHShapeCollide, OBB, T_SH, NarrowPhaseSolver>(o1, tf1, o2, tf2, nsolver, request, result); - } -}; - - -template -struct BVHShapeCollider -{ - static std::size_t collide(const CollisionGeometry* o1, const Transform3d& tf1, const CollisionGeometry* o2, const Transform3d& tf2, - const NarrowPhaseSolver* nsolver, - const CollisionRequest& request, CollisionResult& result) - { - return details::orientedBVHShapeCollide, RSS, T_SH, NarrowPhaseSolver>(o1, tf1, o2, tf2, nsolver, request, result); - } -}; - - -template -struct BVHShapeCollider -{ - static std::size_t collide(const CollisionGeometry* o1, const Transform3d& tf1, const CollisionGeometry* o2, const Transform3d& tf2, - const NarrowPhaseSolver* nsolver, - const CollisionRequest& request, CollisionResult& result) - { - return details::orientedBVHShapeCollide, kIOS, T_SH, NarrowPhaseSolver>(o1, tf1, o2, tf2, nsolver, request, result); - } -}; - - -template -struct BVHShapeCollider -{ - static std::size_t collide(const CollisionGeometry* o1, const Transform3d& tf1, const CollisionGeometry* o2, const Transform3d& tf2, - const NarrowPhaseSolver* nsolver, - const CollisionRequest& request, CollisionResult& result) - { - return details::orientedBVHShapeCollide, OBBRSS, T_SH, NarrowPhaseSolver>(o1, tf1, o2, tf2, nsolver, request, result); - } -}; - - -template -std::size_t BVHCollide(const CollisionGeometry* o1, const Transform3d& tf1, const CollisionGeometry* o2, const Transform3d& tf2, const CollisionRequest& request, CollisionResult& result) -{ - if(request.isSatisfied(result)) return result.numContacts(); - - MeshCollisionTraversalNode node; - const BVHModel* obj1 = static_cast* >(o1); - const BVHModel* obj2 = static_cast* >(o2); - BVHModel* obj1_tmp = new BVHModel(*obj1); - Transform3d tf1_tmp = tf1; - BVHModel* obj2_tmp = new BVHModel(*obj2); - Transform3d tf2_tmp = tf2; - - initialize(node, *obj1_tmp, tf1_tmp, *obj2_tmp, tf2_tmp, request, result); - collide(&node); - - delete obj1_tmp; - delete obj2_tmp; - - return result.numContacts(); -} - -namespace details -{ -template -std::size_t orientedMeshCollide(const CollisionGeometry* o1, const Transform3d& tf1, const CollisionGeometry* o2, const Transform3d& tf2, const CollisionRequest& request, CollisionResult& result) -{ - if(request.isSatisfied(result)) return result.numContacts(); - - OrientedMeshCollisionTraversalNode node; - const BVHModel* obj1 = static_cast* >(o1); - const BVHModel* obj2 = static_cast* >(o2); - - initialize(node, *obj1, tf1, *obj2, tf2, request, result); - collide(&node); - - return result.numContacts(); -} - -} - -template<> -std::size_t BVHCollide(const CollisionGeometry* o1, const Transform3d& tf1, const CollisionGeometry* o2, const Transform3d& tf2, const CollisionRequest& request, CollisionResult& result) -{ - return details::orientedMeshCollide(o1, tf1, o2, tf2, request, result); -} - -template<> -std::size_t BVHCollide(const CollisionGeometry* o1, const Transform3d& tf1, const CollisionGeometry* o2, const Transform3d& tf2, const CollisionRequest& request, CollisionResult& result) -{ - return details::orientedMeshCollide(o1, tf1, o2, tf2, request, result); -} - - -template<> -std::size_t BVHCollide(const CollisionGeometry* o1, const Transform3d& tf1, const CollisionGeometry* o2, const Transform3d& tf2, const CollisionRequest& request, CollisionResult& result) -{ - return details::orientedMeshCollide(o1, tf1, o2, tf2, request, result); -} - - -template -std::size_t BVHCollide(const CollisionGeometry* o1, const Transform3d& tf1, const CollisionGeometry* o2, const Transform3d& tf2, - const NarrowPhaseSolver* nsolver, - const CollisionRequest& request, CollisionResult& result) -{ - return BVHCollide(o1, tf1, o2, tf2, request, result); -} - - -template -CollisionFunctionMatrix::CollisionFunctionMatrix() -{ - for(int i = 0; i < NODE_COUNT; ++i) - { - for(int j = 0; j < NODE_COUNT; ++j) - collision_matrix[i][j] = NULL; - } - - collision_matrix[GEOM_BOX][GEOM_BOX] = &ShapeShapeCollide; - collision_matrix[GEOM_BOX][GEOM_SPHERE] = &ShapeShapeCollide; - collision_matrix[GEOM_BOX][GEOM_ELLIPSOID] = &ShapeShapeCollide; - collision_matrix[GEOM_BOX][GEOM_CAPSULE] = &ShapeShapeCollide; - collision_matrix[GEOM_BOX][GEOM_CONE] = &ShapeShapeCollide; - collision_matrix[GEOM_BOX][GEOM_CYLINDER] = &ShapeShapeCollide; - collision_matrix[GEOM_BOX][GEOM_CONVEX] = &ShapeShapeCollide; - collision_matrix[GEOM_BOX][GEOM_PLANE] = &ShapeShapeCollide; - collision_matrix[GEOM_BOX][GEOM_HALFSPACE] = &ShapeShapeCollide; - - collision_matrix[GEOM_SPHERE][GEOM_BOX] = &ShapeShapeCollide; - collision_matrix[GEOM_SPHERE][GEOM_SPHERE] = &ShapeShapeCollide; - collision_matrix[GEOM_SPHERE][GEOM_ELLIPSOID] = &ShapeShapeCollide; - collision_matrix[GEOM_SPHERE][GEOM_CAPSULE] = &ShapeShapeCollide; - collision_matrix[GEOM_SPHERE][GEOM_CONE] = &ShapeShapeCollide; - collision_matrix[GEOM_SPHERE][GEOM_CYLINDER] = &ShapeShapeCollide; - collision_matrix[GEOM_SPHERE][GEOM_CONVEX] = &ShapeShapeCollide; - collision_matrix[GEOM_SPHERE][GEOM_PLANE] = &ShapeShapeCollide; - collision_matrix[GEOM_SPHERE][GEOM_HALFSPACE] = &ShapeShapeCollide; - - collision_matrix[GEOM_ELLIPSOID][GEOM_BOX] = &ShapeShapeCollide; - collision_matrix[GEOM_ELLIPSOID][GEOM_SPHERE] = &ShapeShapeCollide; - collision_matrix[GEOM_ELLIPSOID][GEOM_ELLIPSOID] = &ShapeShapeCollide; - collision_matrix[GEOM_ELLIPSOID][GEOM_CAPSULE] = &ShapeShapeCollide; - collision_matrix[GEOM_ELLIPSOID][GEOM_CONE] = &ShapeShapeCollide; - collision_matrix[GEOM_ELLIPSOID][GEOM_CYLINDER] = &ShapeShapeCollide; - collision_matrix[GEOM_ELLIPSOID][GEOM_CONVEX] = &ShapeShapeCollide; - collision_matrix[GEOM_ELLIPSOID][GEOM_PLANE] = &ShapeShapeCollide; - collision_matrix[GEOM_ELLIPSOID][GEOM_HALFSPACE] = &ShapeShapeCollide; - - collision_matrix[GEOM_CAPSULE][GEOM_BOX] = &ShapeShapeCollide; - collision_matrix[GEOM_CAPSULE][GEOM_SPHERE] = &ShapeShapeCollide; - collision_matrix[GEOM_CAPSULE][GEOM_ELLIPSOID] = &ShapeShapeCollide; - collision_matrix[GEOM_CAPSULE][GEOM_CAPSULE] = &ShapeShapeCollide; - collision_matrix[GEOM_CAPSULE][GEOM_CONE] = &ShapeShapeCollide; - collision_matrix[GEOM_CAPSULE][GEOM_CYLINDER] = &ShapeShapeCollide; - collision_matrix[GEOM_CAPSULE][GEOM_CONVEX] = &ShapeShapeCollide; - collision_matrix[GEOM_CAPSULE][GEOM_PLANE] = &ShapeShapeCollide; - collision_matrix[GEOM_CAPSULE][GEOM_HALFSPACE] = &ShapeShapeCollide; - - collision_matrix[GEOM_CONE][GEOM_BOX] = &ShapeShapeCollide; - collision_matrix[GEOM_CONE][GEOM_SPHERE] = &ShapeShapeCollide; - collision_matrix[GEOM_CONE][GEOM_ELLIPSOID] = &ShapeShapeCollide; - collision_matrix[GEOM_CONE][GEOM_CAPSULE] = &ShapeShapeCollide; - collision_matrix[GEOM_CONE][GEOM_CONE] = &ShapeShapeCollide; - collision_matrix[GEOM_CONE][GEOM_CYLINDER] = &ShapeShapeCollide; - collision_matrix[GEOM_CONE][GEOM_CONVEX] = &ShapeShapeCollide; - collision_matrix[GEOM_CONE][GEOM_PLANE] = &ShapeShapeCollide; - collision_matrix[GEOM_CONE][GEOM_HALFSPACE] = &ShapeShapeCollide; - - collision_matrix[GEOM_CYLINDER][GEOM_BOX] = &ShapeShapeCollide; - collision_matrix[GEOM_CYLINDER][GEOM_SPHERE] = &ShapeShapeCollide; - collision_matrix[GEOM_CYLINDER][GEOM_ELLIPSOID] = &ShapeShapeCollide; - collision_matrix[GEOM_CYLINDER][GEOM_CAPSULE] = &ShapeShapeCollide; - collision_matrix[GEOM_CYLINDER][GEOM_CONE] = &ShapeShapeCollide; - collision_matrix[GEOM_CYLINDER][GEOM_CYLINDER] = &ShapeShapeCollide; - collision_matrix[GEOM_CYLINDER][GEOM_CONVEX] = &ShapeShapeCollide; - collision_matrix[GEOM_CYLINDER][GEOM_PLANE] = &ShapeShapeCollide; - collision_matrix[GEOM_CYLINDER][GEOM_HALFSPACE] = &ShapeShapeCollide; - - collision_matrix[GEOM_CONVEX][GEOM_BOX] = &ShapeShapeCollide; - collision_matrix[GEOM_CONVEX][GEOM_SPHERE] = &ShapeShapeCollide; - collision_matrix[GEOM_CONVEX][GEOM_ELLIPSOID] = &ShapeShapeCollide; - collision_matrix[GEOM_CONVEX][GEOM_CAPSULE] = &ShapeShapeCollide; - collision_matrix[GEOM_CONVEX][GEOM_CONE] = &ShapeShapeCollide; - collision_matrix[GEOM_CONVEX][GEOM_CYLINDER] = &ShapeShapeCollide; - collision_matrix[GEOM_CONVEX][GEOM_CONVEX] = &ShapeShapeCollide; - collision_matrix[GEOM_CONVEX][GEOM_PLANE] = &ShapeShapeCollide; - collision_matrix[GEOM_CONVEX][GEOM_HALFSPACE] = &ShapeShapeCollide; - - collision_matrix[GEOM_PLANE][GEOM_BOX] = &ShapeShapeCollide; - collision_matrix[GEOM_PLANE][GEOM_SPHERE] = &ShapeShapeCollide; - collision_matrix[GEOM_PLANE][GEOM_ELLIPSOID] = &ShapeShapeCollide; - collision_matrix[GEOM_PLANE][GEOM_CAPSULE] = &ShapeShapeCollide; - collision_matrix[GEOM_PLANE][GEOM_CONE] = &ShapeShapeCollide; - collision_matrix[GEOM_PLANE][GEOM_CYLINDER] = &ShapeShapeCollide; - collision_matrix[GEOM_PLANE][GEOM_CONVEX] = &ShapeShapeCollide; - collision_matrix[GEOM_PLANE][GEOM_PLANE] = &ShapeShapeCollide; - collision_matrix[GEOM_PLANE][GEOM_HALFSPACE] = &ShapeShapeCollide; - - collision_matrix[GEOM_HALFSPACE][GEOM_BOX] = &ShapeShapeCollide; - collision_matrix[GEOM_HALFSPACE][GEOM_SPHERE] = &ShapeShapeCollide; - collision_matrix[GEOM_HALFSPACE][GEOM_CAPSULE] = &ShapeShapeCollide; - collision_matrix[GEOM_HALFSPACE][GEOM_CONE] = &ShapeShapeCollide; - collision_matrix[GEOM_HALFSPACE][GEOM_CYLINDER] = &ShapeShapeCollide; - collision_matrix[GEOM_HALFSPACE][GEOM_CONVEX] = &ShapeShapeCollide; - collision_matrix[GEOM_HALFSPACE][GEOM_PLANE] = &ShapeShapeCollide; - collision_matrix[GEOM_HALFSPACE][GEOM_HALFSPACE] = &ShapeShapeCollide; - - collision_matrix[BV_AABB][GEOM_BOX] = &BVHShapeCollider::collide; - collision_matrix[BV_AABB][GEOM_SPHERE] = &BVHShapeCollider::collide; - collision_matrix[BV_AABB][GEOM_ELLIPSOID] = &BVHShapeCollider::collide; - collision_matrix[BV_AABB][GEOM_CAPSULE] = &BVHShapeCollider::collide; - collision_matrix[BV_AABB][GEOM_CONE] = &BVHShapeCollider::collide; - collision_matrix[BV_AABB][GEOM_CYLINDER] = &BVHShapeCollider::collide; - collision_matrix[BV_AABB][GEOM_CONVEX] = &BVHShapeCollider::collide; - collision_matrix[BV_AABB][GEOM_PLANE] = &BVHShapeCollider::collide; - collision_matrix[BV_AABB][GEOM_HALFSPACE] = &BVHShapeCollider::collide; - - collision_matrix[BV_OBB][GEOM_BOX] = &BVHShapeCollider::collide; - collision_matrix[BV_OBB][GEOM_SPHERE] = &BVHShapeCollider::collide; - collision_matrix[BV_OBB][GEOM_ELLIPSOID] = &BVHShapeCollider::collide; - collision_matrix[BV_OBB][GEOM_CAPSULE] = &BVHShapeCollider::collide; - collision_matrix[BV_OBB][GEOM_CONE] = &BVHShapeCollider::collide; - collision_matrix[BV_OBB][GEOM_CYLINDER] = &BVHShapeCollider::collide; - collision_matrix[BV_OBB][GEOM_CONVEX] = &BVHShapeCollider::collide; - collision_matrix[BV_OBB][GEOM_PLANE] = &BVHShapeCollider::collide; - collision_matrix[BV_OBB][GEOM_HALFSPACE] = &BVHShapeCollider::collide; - - collision_matrix[BV_RSS][GEOM_BOX] = &BVHShapeCollider::collide; - collision_matrix[BV_RSS][GEOM_SPHERE] = &BVHShapeCollider::collide; - collision_matrix[BV_RSS][GEOM_ELLIPSOID] = &BVHShapeCollider::collide; - collision_matrix[BV_RSS][GEOM_CAPSULE] = &BVHShapeCollider::collide; - collision_matrix[BV_RSS][GEOM_CONE] = &BVHShapeCollider::collide; - collision_matrix[BV_RSS][GEOM_CYLINDER] = &BVHShapeCollider::collide; - collision_matrix[BV_RSS][GEOM_CONVEX] = &BVHShapeCollider::collide; - collision_matrix[BV_RSS][GEOM_PLANE] = &BVHShapeCollider::collide; - collision_matrix[BV_RSS][GEOM_HALFSPACE] = &BVHShapeCollider::collide; - - collision_matrix[BV_KDOP16][GEOM_BOX] = &BVHShapeCollider, Box, NarrowPhaseSolver>::collide; - collision_matrix[BV_KDOP16][GEOM_SPHERE] = &BVHShapeCollider, Sphere, NarrowPhaseSolver>::collide; - collision_matrix[BV_KDOP16][GEOM_ELLIPSOID] = &BVHShapeCollider, Ellipsoid, NarrowPhaseSolver>::collide; - collision_matrix[BV_KDOP16][GEOM_CAPSULE] = &BVHShapeCollider, Capsule, NarrowPhaseSolver>::collide; - collision_matrix[BV_KDOP16][GEOM_CONE] = &BVHShapeCollider, Cone, NarrowPhaseSolver>::collide; - collision_matrix[BV_KDOP16][GEOM_CYLINDER] = &BVHShapeCollider, Cylinder, NarrowPhaseSolver>::collide; - collision_matrix[BV_KDOP16][GEOM_CONVEX] = &BVHShapeCollider, Convex, NarrowPhaseSolver>::collide; - collision_matrix[BV_KDOP16][GEOM_PLANE] = &BVHShapeCollider, Plane, NarrowPhaseSolver>::collide; - collision_matrix[BV_KDOP16][GEOM_HALFSPACE] = &BVHShapeCollider, Halfspace, NarrowPhaseSolver>::collide; - - collision_matrix[BV_KDOP18][GEOM_BOX] = &BVHShapeCollider, Box, NarrowPhaseSolver>::collide; - collision_matrix[BV_KDOP18][GEOM_SPHERE] = &BVHShapeCollider, Sphere, NarrowPhaseSolver>::collide; - collision_matrix[BV_KDOP18][GEOM_ELLIPSOID] = &BVHShapeCollider, Ellipsoid, NarrowPhaseSolver>::collide; - collision_matrix[BV_KDOP18][GEOM_CAPSULE] = &BVHShapeCollider, Capsule, NarrowPhaseSolver>::collide; - collision_matrix[BV_KDOP18][GEOM_CONE] = &BVHShapeCollider, Cone, NarrowPhaseSolver>::collide; - collision_matrix[BV_KDOP18][GEOM_CYLINDER] = &BVHShapeCollider, Cylinder, NarrowPhaseSolver>::collide; - collision_matrix[BV_KDOP18][GEOM_CONVEX] = &BVHShapeCollider, Convex, NarrowPhaseSolver>::collide; - collision_matrix[BV_KDOP18][GEOM_PLANE] = &BVHShapeCollider, Plane, NarrowPhaseSolver>::collide; - collision_matrix[BV_KDOP18][GEOM_HALFSPACE] = &BVHShapeCollider, Halfspace, NarrowPhaseSolver>::collide; - - collision_matrix[BV_KDOP24][GEOM_BOX] = &BVHShapeCollider, Box, NarrowPhaseSolver>::collide; - collision_matrix[BV_KDOP24][GEOM_SPHERE] = &BVHShapeCollider, Sphere, NarrowPhaseSolver>::collide; - collision_matrix[BV_KDOP24][GEOM_ELLIPSOID] = &BVHShapeCollider, Ellipsoid, NarrowPhaseSolver>::collide; - collision_matrix[BV_KDOP24][GEOM_CAPSULE] = &BVHShapeCollider, Capsule, NarrowPhaseSolver>::collide; - collision_matrix[BV_KDOP24][GEOM_CONE] = &BVHShapeCollider, Cone, NarrowPhaseSolver>::collide; - collision_matrix[BV_KDOP24][GEOM_CYLINDER] = &BVHShapeCollider, Cylinder, NarrowPhaseSolver>::collide; - collision_matrix[BV_KDOP24][GEOM_CONVEX] = &BVHShapeCollider, Convex, NarrowPhaseSolver>::collide; - collision_matrix[BV_KDOP24][GEOM_PLANE] = &BVHShapeCollider, Plane, NarrowPhaseSolver>::collide; - collision_matrix[BV_KDOP24][GEOM_HALFSPACE] = &BVHShapeCollider, Halfspace, NarrowPhaseSolver>::collide; - - collision_matrix[BV_kIOS][GEOM_BOX] = &BVHShapeCollider::collide; - collision_matrix[BV_kIOS][GEOM_SPHERE] = &BVHShapeCollider::collide; - collision_matrix[BV_kIOS][GEOM_ELLIPSOID] = &BVHShapeCollider::collide; - collision_matrix[BV_kIOS][GEOM_CAPSULE] = &BVHShapeCollider::collide; - collision_matrix[BV_kIOS][GEOM_CONE] = &BVHShapeCollider::collide; - collision_matrix[BV_kIOS][GEOM_CYLINDER] = &BVHShapeCollider::collide; - collision_matrix[BV_kIOS][GEOM_CONVEX] = &BVHShapeCollider::collide; - collision_matrix[BV_kIOS][GEOM_PLANE] = &BVHShapeCollider::collide; - collision_matrix[BV_kIOS][GEOM_HALFSPACE] = &BVHShapeCollider::collide; - - collision_matrix[BV_OBBRSS][GEOM_BOX] = &BVHShapeCollider::collide; - collision_matrix[BV_OBBRSS][GEOM_SPHERE] = &BVHShapeCollider::collide; - collision_matrix[BV_OBBRSS][GEOM_ELLIPSOID] = &BVHShapeCollider::collide; - collision_matrix[BV_OBBRSS][GEOM_CAPSULE] = &BVHShapeCollider::collide; - collision_matrix[BV_OBBRSS][GEOM_CONE] = &BVHShapeCollider::collide; - collision_matrix[BV_OBBRSS][GEOM_CYLINDER] = &BVHShapeCollider::collide; - collision_matrix[BV_OBBRSS][GEOM_CONVEX] = &BVHShapeCollider::collide; - collision_matrix[BV_OBBRSS][GEOM_PLANE] = &BVHShapeCollider::collide; - collision_matrix[BV_OBBRSS][GEOM_HALFSPACE] = &BVHShapeCollider::collide; - - collision_matrix[BV_AABB][BV_AABB] = &BVHCollide; - collision_matrix[BV_OBB][BV_OBB] = &BVHCollide; - collision_matrix[BV_RSS][BV_RSS] = &BVHCollide; - collision_matrix[BV_KDOP16][BV_KDOP16] = &BVHCollide, NarrowPhaseSolver>; - collision_matrix[BV_KDOP18][BV_KDOP18] = &BVHCollide, NarrowPhaseSolver>; - collision_matrix[BV_KDOP24][BV_KDOP24] = &BVHCollide, NarrowPhaseSolver>; - collision_matrix[BV_kIOS][BV_kIOS] = &BVHCollide; - collision_matrix[BV_OBBRSS][BV_OBBRSS] = &BVHCollide; - -#if FCL_HAVE_OCTOMAP - collision_matrix[GEOM_OCTREE][GEOM_BOX] = &OcTreeShapeCollide; - collision_matrix[GEOM_OCTREE][GEOM_SPHERE] = &OcTreeShapeCollide; - collision_matrix[GEOM_OCTREE][GEOM_ELLIPSOID] = &OcTreeShapeCollide; - collision_matrix[GEOM_OCTREE][GEOM_CAPSULE] = &OcTreeShapeCollide; - collision_matrix[GEOM_OCTREE][GEOM_CONE] = &OcTreeShapeCollide; - collision_matrix[GEOM_OCTREE][GEOM_CYLINDER] = &OcTreeShapeCollide; - collision_matrix[GEOM_OCTREE][GEOM_CONVEX] = &OcTreeShapeCollide; - collision_matrix[GEOM_OCTREE][GEOM_PLANE] = &OcTreeShapeCollide; - collision_matrix[GEOM_OCTREE][GEOM_HALFSPACE] = &OcTreeShapeCollide; - - collision_matrix[GEOM_BOX][GEOM_OCTREE] = &ShapeOcTreeCollide; - collision_matrix[GEOM_SPHERE][GEOM_OCTREE] = &ShapeOcTreeCollide; - collision_matrix[GEOM_ELLIPSOID][GEOM_OCTREE] = &ShapeOcTreeCollide; - collision_matrix[GEOM_CAPSULE][GEOM_OCTREE] = &ShapeOcTreeCollide; - collision_matrix[GEOM_CONE][GEOM_OCTREE] = &ShapeOcTreeCollide; - collision_matrix[GEOM_CYLINDER][GEOM_OCTREE] = &ShapeOcTreeCollide; - collision_matrix[GEOM_CONVEX][GEOM_OCTREE] = &ShapeOcTreeCollide; - collision_matrix[GEOM_PLANE][GEOM_OCTREE] = &ShapeOcTreeCollide; - collision_matrix[GEOM_HALFSPACE][GEOM_OCTREE] = &ShapeOcTreeCollide; - - collision_matrix[GEOM_OCTREE][GEOM_OCTREE] = &OcTreeCollide; - - collision_matrix[GEOM_OCTREE][BV_AABB] = &OcTreeBVHCollide; - collision_matrix[GEOM_OCTREE][BV_OBB] = &OcTreeBVHCollide; - collision_matrix[GEOM_OCTREE][BV_RSS] = &OcTreeBVHCollide; - collision_matrix[GEOM_OCTREE][BV_OBBRSS] = &OcTreeBVHCollide; - collision_matrix[GEOM_OCTREE][BV_kIOS] = &OcTreeBVHCollide; - collision_matrix[GEOM_OCTREE][BV_KDOP16] = &OcTreeBVHCollide, NarrowPhaseSolver>; - collision_matrix[GEOM_OCTREE][BV_KDOP18] = &OcTreeBVHCollide, NarrowPhaseSolver>; - collision_matrix[GEOM_OCTREE][BV_KDOP24] = &OcTreeBVHCollide, NarrowPhaseSolver>; - - collision_matrix[BV_AABB][GEOM_OCTREE] = &BVHOcTreeCollide; - collision_matrix[BV_OBB][GEOM_OCTREE] = &BVHOcTreeCollide; - collision_matrix[BV_RSS][GEOM_OCTREE] = &BVHOcTreeCollide; - collision_matrix[BV_OBBRSS][GEOM_OCTREE] = &BVHOcTreeCollide; - collision_matrix[BV_kIOS][GEOM_OCTREE] = &BVHOcTreeCollide; - collision_matrix[BV_KDOP16][GEOM_OCTREE] = &BVHOcTreeCollide, NarrowPhaseSolver>; - collision_matrix[BV_KDOP18][GEOM_OCTREE] = &BVHOcTreeCollide, NarrowPhaseSolver>; - collision_matrix[BV_KDOP24][GEOM_OCTREE] = &BVHOcTreeCollide, NarrowPhaseSolver>; -#endif -} - - -template struct CollisionFunctionMatrix; -template struct CollisionFunctionMatrix; - - -} diff --git a/src/continuous_collision.cpp b/src/continuous_collision.cpp deleted file mode 100644 index 2aae18607..000000000 --- a/src/continuous_collision.cpp +++ /dev/null @@ -1,373 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2013-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#include "fcl/collision.h" -#include "fcl/continuous_collision.h" -#include "fcl/ccd/motion.h" -#include "fcl/BVH/BVH_model.h" -#include "fcl/traversal/traversal_node_bvhs.h" -#include "fcl/traversal/traversal_node_setup.h" -#include "fcl/collision_node.h" -#include "fcl/ccd/conservative_advancement.h" -#include - -namespace fcl -{ - -template -ConservativeAdvancementFunctionMatrix& getConservativeAdvancementFunctionLookTable() -{ - static ConservativeAdvancementFunctionMatrix table; - return table; -} - -MotionBasePtr getMotionBase(const Transform3d& tf_beg, const Transform3d& tf_end, CCDMotionType motion_type) -{ - switch(motion_type) - { - case CCDM_TRANS: - return MotionBasePtr(new TranslationMotion(tf_beg, tf_end)); - break; - case CCDM_LINEAR: - return MotionBasePtr(new InterpMotion(tf_beg, tf_end)); - break; - case CCDM_SCREW: - return MotionBasePtr(new ScrewMotion(tf_beg, tf_end)); - break; - case CCDM_SPLINE: - return MotionBasePtr(new SplineMotion(tf_beg, tf_end)); - break; - default: - return MotionBasePtr(); - } -} - - -FCL_REAL continuousCollideNaive(const CollisionGeometry* o1, const MotionBase* motion1, - const CollisionGeometry* o2, const MotionBase* motion2, - const ContinuousCollisionRequest& request, - ContinuousCollisionResult& result) -{ - std::size_t n_iter = std::min(request.num_max_iterations, (std::size_t)ceil(1 / request.toc_err)); - Transform3d cur_tf1, cur_tf2; - for(std::size_t i = 0; i < n_iter; ++i) - { - FCL_REAL t = i / (FCL_REAL) (n_iter - 1); - motion1->integrate(t); - motion2->integrate(t); - - motion1->getCurrentTransform(cur_tf1); - motion2->getCurrentTransform(cur_tf2); - - CollisionRequest c_request; - CollisionResult c_result; - - if(collide(o1, cur_tf1, o2, cur_tf2, c_request, c_result)) - { - result.is_collide = true; - result.time_of_contact = t; - result.contact_tf1 = cur_tf1; - result.contact_tf2 = cur_tf2; - return t; - } - } - - result.is_collide = false; - result.time_of_contact = FCL_REAL(1); - return result.time_of_contact; -} - -namespace details -{ -template -FCL_REAL continuousCollideBVHPolynomial(const CollisionGeometry* o1_, const TranslationMotion* motion1, - const CollisionGeometry* o2_, const TranslationMotion* motion2, - const ContinuousCollisionRequest& request, - ContinuousCollisionResult& result) -{ - const BVHModel* o1__ = static_cast*>(o1_); - const BVHModel* o2__ = static_cast*>(o2_); - - // ugly, but lets do it now. - BVHModel* o1 = const_cast*>(o1__); - BVHModel* o2 = const_cast*>(o2__); - std::vector new_v1(o1->num_vertices); - std::vector new_v2(o2->num_vertices); - - for(std::size_t i = 0; i < new_v1.size(); ++i) - new_v1[i] = o1->vertices[i] + motion1->getVelocity(); - for(std::size_t i = 0; i < new_v2.size(); ++i) - new_v2[i] = o2->vertices[i] + motion2->getVelocity(); - - o1->beginUpdateModel(); - o1->updateSubModel(new_v1); - o1->endUpdateModel(true, true); - - o2->beginUpdateModel(); - o2->updateSubModel(new_v2); - o2->endUpdateModel(true, true); - - MeshContinuousCollisionTraversalNode node; - CollisionRequest c_request; - - motion1->integrate(0); - motion2->integrate(0); - Transform3d tf1, tf2; - motion1->getCurrentTransform(tf1); - motion2->getCurrentTransform(tf2); - if(!initialize(node, *o1, tf1, *o2, tf2, c_request)) - return -1.0; - - collide(&node); - - result.is_collide = (node.pairs.size() > 0); - result.time_of_contact = node.time_of_contact; - - if(result.is_collide) - { - motion1->integrate(node.time_of_contact); - motion2->integrate(node.time_of_contact); - motion1->getCurrentTransform(tf1); - motion2->getCurrentTransform(tf2); - result.contact_tf1 = tf1; - result.contact_tf2 = tf2; - } - - return result.time_of_contact; -} - -} - -FCL_REAL continuousCollideBVHPolynomial(const CollisionGeometry* o1, const TranslationMotion* motion1, - const CollisionGeometry* o2, const TranslationMotion* motion2, - const ContinuousCollisionRequest& request, - ContinuousCollisionResult& result) -{ - switch(o1->getNodeType()) - { - case BV_AABB: - if(o2->getNodeType() == BV_AABB) - return details::continuousCollideBVHPolynomial(o1, motion1, o2, motion2, request, result); - break; - case BV_OBB: - if(o2->getNodeType() == BV_OBB) - return details::continuousCollideBVHPolynomial(o1, motion1, o2, motion2, request, result); - break; - case BV_RSS: - if(o2->getNodeType() == BV_RSS) - return details::continuousCollideBVHPolynomial(o1, motion1, o2, motion2, request, result); - break; - case BV_kIOS: - if(o2->getNodeType() == BV_kIOS) - return details::continuousCollideBVHPolynomial(o1, motion1, o2, motion2, request, result); - break; - case BV_OBBRSS: - if(o2->getNodeType() == BV_OBBRSS) - return details::continuousCollideBVHPolynomial(o1, motion1, o2, motion2, request, result); - break; - case BV_KDOP16: - if(o2->getNodeType() == BV_KDOP16) - return details::continuousCollideBVHPolynomial >(o1, motion1, o2, motion2, request, result); - break; - case BV_KDOP18: - if(o2->getNodeType() == BV_KDOP18) - return details::continuousCollideBVHPolynomial >(o1, motion1, o2, motion2, request, result); - break; - case BV_KDOP24: - if(o2->getNodeType() == BV_KDOP24) - return details::continuousCollideBVHPolynomial >(o1, motion1, o2, motion2, request, result); - break; - default: - ; - } - - std::cerr << "Warning: BV type not supported by polynomial solver CCD" << std::endl; - - return -1; -} - -namespace details -{ - -template -FCL_REAL continuousCollideConservativeAdvancement(const CollisionGeometry* o1, const MotionBase* motion1, - const CollisionGeometry* o2, const MotionBase* motion2, - const NarrowPhaseSolver* nsolver_, - const ContinuousCollisionRequest& request, - ContinuousCollisionResult& result) -{ - const NarrowPhaseSolver* nsolver = nsolver_; - if(!nsolver_) - nsolver = new NarrowPhaseSolver(); - - const ConservativeAdvancementFunctionMatrix& looktable = getConservativeAdvancementFunctionLookTable(); - - NODE_TYPE node_type1 = o1->getNodeType(); - NODE_TYPE node_type2 = o2->getNodeType(); - - FCL_REAL res = -1; - - if(!looktable.conservative_advancement_matrix[node_type1][node_type2]) - { - std::cerr << "Warning: collision function between node type " << node_type1 << " and node type " << node_type2 << " is not supported"<< std::endl; - } - else - { - res = looktable.conservative_advancement_matrix[node_type1][node_type2](o1, motion1, o2, motion2, nsolver, request, result); - } - - if(!nsolver_) - delete nsolver; - - if(result.is_collide) - { - motion1->integrate(result.time_of_contact); - motion2->integrate(result.time_of_contact); - - Transform3d tf1, tf2; - motion1->getCurrentTransform(tf1); - motion2->getCurrentTransform(tf2); - result.contact_tf1 = tf1; - result.contact_tf2 = tf2; - } - - return res; -} - -} - - -FCL_REAL continuousCollideConservativeAdvancement(const CollisionGeometry* o1, const MotionBase* motion1, - const CollisionGeometry* o2, const MotionBase* motion2, - const ContinuousCollisionRequest& request, - ContinuousCollisionResult& result) -{ - switch(request.gjk_solver_type) - { - case GST_LIBCCD: - { - GJKSolver_libccd solver; - return details::continuousCollideConservativeAdvancement(o1, motion1, o2, motion2, &solver, request, result); - } - case GST_INDEP: - { - GJKSolver_indep solver; - return details::continuousCollideConservativeAdvancement(o1, motion1, o2, motion2, &solver, request, result); - } - default: - return -1; - } -} - - -FCL_REAL continuousCollide(const CollisionGeometry* o1, const MotionBase* motion1, - const CollisionGeometry* o2, const MotionBase* motion2, - const ContinuousCollisionRequest& request, - ContinuousCollisionResult& result) -{ - switch(request.ccd_solver_type) - { - case CCDC_NAIVE: - return continuousCollideNaive(o1, motion1, - o2, motion2, - request, - result); - break; - case CCDC_CONSERVATIVE_ADVANCEMENT: - return continuousCollideConservativeAdvancement(o1, motion1, - o2, motion2, - request, - result); - break; - case CCDC_RAY_SHOOTING: - if(o1->getObjectType() == OT_GEOM && o2->getObjectType() == OT_GEOM && request.ccd_motion_type == CCDM_TRANS) - { - - } - else - std::cerr << "Warning! Invalid continuous collision setting" << std::endl; - break; - case CCDC_POLYNOMIAL_SOLVER: - if(o1->getObjectType() == OT_BVH && o2->getObjectType() == OT_BVH && request.ccd_motion_type == CCDM_TRANS) - { - return continuousCollideBVHPolynomial(o1, (const TranslationMotion*)motion1, - o2, (const TranslationMotion*)motion2, - request, result); - } - else - std::cerr << "Warning! Invalid continuous collision checking" << std::endl; - break; - default: - std::cerr << "Warning! Invalid continuous collision setting" << std::endl; - } - - return -1; -} - -FCL_REAL continuousCollide(const CollisionGeometry* o1, const Transform3d& tf1_beg, const Transform3d& tf1_end, - const CollisionGeometry* o2, const Transform3d& tf2_beg, const Transform3d& tf2_end, - const ContinuousCollisionRequest& request, - ContinuousCollisionResult& result) -{ - MotionBasePtr motion1 = getMotionBase(tf1_beg, tf1_end, request.ccd_motion_type); - MotionBasePtr motion2 = getMotionBase(tf2_beg, tf2_end, request.ccd_motion_type); - - return continuousCollide(o1, motion1.get(), o2, motion2.get(), request, result); -} - - -FCL_REAL continuousCollide(const CollisionObject* o1, const Transform3d& tf1_end, - const CollisionObject* o2, const Transform3d& tf2_end, - const ContinuousCollisionRequest& request, - ContinuousCollisionResult& result) -{ - return continuousCollide(o1->collisionGeometry().get(), o1->getTransform(), tf1_end, - o2->collisionGeometry().get(), o2->getTransform(), tf2_end, - request, result); -} - - -FCL_REAL collide(const ContinuousCollisionObject* o1, const ContinuousCollisionObject* o2, - const ContinuousCollisionRequest& request, - ContinuousCollisionResult& result) -{ - return continuousCollide(o1->collisionGeometry().get(), o1->getMotion(), - o2->collisionGeometry().get(), o2->getMotion(), - request, result); -} - -} diff --git a/src/distance.cpp b/src/distance.cpp deleted file mode 100644 index c89f8d64b..000000000 --- a/src/distance.cpp +++ /dev/null @@ -1,152 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#include "fcl/distance.h" -#include "fcl/distance_func_matrix.h" -#include "fcl/narrowphase/narrowphase.h" - -#include - -namespace fcl -{ - -template -DistanceFunctionMatrix& getDistanceFunctionLookTable() -{ - static DistanceFunctionMatrix table; - return table; -} - -template -FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2, const NarrowPhaseSolver* nsolver, - const DistanceRequest& request, DistanceResult& result) -{ - return distance(o1->collisionGeometry().get(), o1->getTransform(), o2->collisionGeometry().get(), o2->getTransform(), nsolver, - request, result); -} - -template -FCL_REAL distance(const CollisionGeometry* o1, const Transform3d& tf1, - const CollisionGeometry* o2, const Transform3d& tf2, - const NarrowPhaseSolver* nsolver_, - const DistanceRequest& request, DistanceResult& result) -{ - const NarrowPhaseSolver* nsolver = nsolver_; - if(!nsolver_) - nsolver = new NarrowPhaseSolver(); - - const DistanceFunctionMatrix& looktable = getDistanceFunctionLookTable(); - - OBJECT_TYPE object_type1 = o1->getObjectType(); - NODE_TYPE node_type1 = o1->getNodeType(); - OBJECT_TYPE object_type2 = o2->getObjectType(); - NODE_TYPE node_type2 = o2->getNodeType(); - - FCL_REAL res = std::numeric_limits::max(); - - if(object_type1 == OT_GEOM && object_type2 == OT_BVH) - { - if(!looktable.distance_matrix[node_type2][node_type1]) - { - std::cerr << "Warning: distance function between node type " << node_type1 << " and node type " << node_type2 << " is not supported" << std::endl; - } - else - { - res = looktable.distance_matrix[node_type2][node_type1](o2, tf2, o1, tf1, nsolver, request, result); - } - } - else - { - if(!looktable.distance_matrix[node_type1][node_type2]) - { - std::cerr << "Warning: distance function between node type " << node_type1 << " and node type " << node_type2 << " is not supported" << std::endl; - } - else - { - res = looktable.distance_matrix[node_type1][node_type2](o1, tf1, o2, tf2, nsolver, request, result); - } - } - - if(!nsolver_) - delete nsolver; - - return res; -} - - -FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2, const DistanceRequest& request, DistanceResult& result) -{ - switch(request.gjk_solver_type) - { - case GST_LIBCCD: - { - GJKSolver_libccd solver; - return distance(o1, o2, &solver, request, result); - } - case GST_INDEP: - { - GJKSolver_indep solver; - return distance(o1, o2, &solver, request, result); - } - default: - return -1; // error - } -} - -FCL_REAL distance(const CollisionGeometry* o1, const Transform3d& tf1, - const CollisionGeometry* o2, const Transform3d& tf2, - const DistanceRequest& request, DistanceResult& result) -{ - switch(request.gjk_solver_type) - { - case GST_LIBCCD: - { - GJKSolver_libccd solver; - return distance(o1, tf1, o2, tf2, &solver, request, result); - } - case GST_INDEP: - { - GJKSolver_indep solver; - return distance(o1, tf1, o2, tf2, &solver, request, result); - } - default: - return -1; - } -} - - -} diff --git a/src/distance_func_matrix.cpp b/src/distance_func_matrix.cpp deleted file mode 100755 index 0724469c0..000000000 --- a/src/distance_func_matrix.cpp +++ /dev/null @@ -1,527 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#include "fcl/distance_func_matrix.h" - -#include "fcl/collision_node.h" -#include "fcl/traversal/traversal_node_setup.h" -#include "fcl/narrowphase/narrowphase.h" - -namespace fcl -{ - -#if FCL_HAVE_OCTOMAP -template -FCL_REAL ShapeOcTreeDistance(const CollisionGeometry* o1, const Transform3d& tf1, const CollisionGeometry* o2, const Transform3d& tf2, const NarrowPhaseSolver* nsolver, - const DistanceRequest& request, DistanceResult& result) -{ - if(request.isSatisfied(result)) return result.min_distance; - ShapeOcTreeDistanceTraversalNode node; - const T_SH* obj1 = static_cast(o1); - const OcTree* obj2 = static_cast(o2); - OcTreeSolver otsolver(nsolver); - - initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result); - distance(&node); - - return result.min_distance; -} - -template -FCL_REAL OcTreeShapeDistance(const CollisionGeometry* o1, const Transform3d& tf1, const CollisionGeometry* o2, const Transform3d& tf2, const NarrowPhaseSolver* nsolver, - const DistanceRequest& request, DistanceResult& result) -{ - if(request.isSatisfied(result)) return result.min_distance; - OcTreeShapeDistanceTraversalNode node; - const OcTree* obj1 = static_cast(o1); - const T_SH* obj2 = static_cast(o2); - OcTreeSolver otsolver(nsolver); - - initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result); - distance(&node); - - return result.min_distance; -} - -template -FCL_REAL OcTreeDistance(const CollisionGeometry* o1, const Transform3d& tf1, const CollisionGeometry* o2, const Transform3d& tf2, const NarrowPhaseSolver* nsolver, - const DistanceRequest& request, DistanceResult& result) -{ - if(request.isSatisfied(result)) return result.min_distance; - OcTreeDistanceTraversalNode node; - const OcTree* obj1 = static_cast(o1); - const OcTree* obj2 = static_cast(o2); - OcTreeSolver otsolver(nsolver); - - initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result); - distance(&node); - - return result.min_distance; -} - -template -FCL_REAL BVHOcTreeDistance(const CollisionGeometry* o1, const Transform3d& tf1, const CollisionGeometry* o2, const Transform3d& tf2, const NarrowPhaseSolver* nsolver, - const DistanceRequest& request, DistanceResult& result) -{ - if(request.isSatisfied(result)) return result.min_distance; - MeshOcTreeDistanceTraversalNode node; - const BVHModel* obj1 = static_cast*>(o1); - const OcTree* obj2 = static_cast(o2); - OcTreeSolver otsolver(nsolver); - - initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result); - distance(&node); - - return result.min_distance; -} - -template -FCL_REAL OcTreeBVHDistance(const CollisionGeometry* o1, const Transform3d& tf1, const CollisionGeometry* o2, const Transform3d& tf2, const NarrowPhaseSolver* nsolver, - const DistanceRequest& request, DistanceResult& result) -{ - if(request.isSatisfied(result)) return result.min_distance; - OcTreeMeshDistanceTraversalNode node; - const OcTree* obj1 = static_cast(o1); - const BVHModel* obj2 = static_cast*>(o2); - OcTreeSolver otsolver(nsolver); - - initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result); - distance(&node); - - return result.min_distance; -} - -#endif - -template -FCL_REAL ShapeShapeDistance(const CollisionGeometry* o1, const Transform3d& tf1, const CollisionGeometry* o2, const Transform3d& tf2, const NarrowPhaseSolver* nsolver, - const DistanceRequest& request, DistanceResult& result) -{ - if(request.isSatisfied(result)) return result.min_distance; - ShapeDistanceTraversalNode node; - const T_SH1* obj1 = static_cast(o1); - const T_SH2* obj2 = static_cast(o2); - - initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result); - distance(&node); - - return result.min_distance; -} - -template -struct BVHShapeDistancer -{ - static FCL_REAL distance(const CollisionGeometry* o1, const Transform3d& tf1, const CollisionGeometry* o2, const Transform3d& tf2, const NarrowPhaseSolver* nsolver, - const DistanceRequest& request, DistanceResult& result) - { - if(request.isSatisfied(result)) return result.min_distance; - MeshShapeDistanceTraversalNode node; - const BVHModel* obj1 = static_cast* >(o1); - BVHModel* obj1_tmp = new BVHModel(*obj1); - Transform3d tf1_tmp = tf1; - const T_SH* obj2 = static_cast(o2); - - initialize(node, *obj1_tmp, tf1_tmp, *obj2, tf2, nsolver, request, result); - fcl::distance(&node); - - delete obj1_tmp; - return result.min_distance; - } -}; - -namespace details -{ - -template -FCL_REAL orientedBVHShapeDistance(const CollisionGeometry* o1, const Transform3d& tf1, const CollisionGeometry* o2, const Transform3d& tf2, const NarrowPhaseSolver* nsolver, - const DistanceRequest& request, DistanceResult& result) -{ - if(request.isSatisfied(result)) return result.min_distance; - OrientedMeshShapeDistanceTraversalNode node; - const BVHModel* obj1 = static_cast* >(o1); - const T_SH* obj2 = static_cast(o2); - - initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result); - fcl::distance(&node); - - return result.min_distance; -} - -} - -template -struct BVHShapeDistancer -{ - static FCL_REAL distance(const CollisionGeometry* o1, const Transform3d& tf1, const CollisionGeometry* o2, const Transform3d& tf2, const NarrowPhaseSolver* nsolver, - const DistanceRequest& request, DistanceResult& result) - { - return details::orientedBVHShapeDistance, RSS, T_SH, NarrowPhaseSolver>(o1, tf1, o2, tf2, nsolver, request, result); - } -}; - - -template -struct BVHShapeDistancer -{ - static FCL_REAL distance(const CollisionGeometry* o1, const Transform3d& tf1, const CollisionGeometry* o2, const Transform3d& tf2, const NarrowPhaseSolver* nsolver, - const DistanceRequest& request, DistanceResult& result) - { - return details::orientedBVHShapeDistance, kIOS, T_SH, NarrowPhaseSolver>(o1, tf1, o2, tf2, nsolver, request, result); - } -}; - -template -struct BVHShapeDistancer -{ - static FCL_REAL distance(const CollisionGeometry* o1, const Transform3d& tf1, const CollisionGeometry* o2, const Transform3d& tf2, const NarrowPhaseSolver* nsolver, - const DistanceRequest& request, DistanceResult& result) - { - return details::orientedBVHShapeDistance, OBBRSS, T_SH, NarrowPhaseSolver>(o1, tf1, o2, tf2, nsolver, request, result); - } -}; - - -template -FCL_REAL BVHDistance(const CollisionGeometry* o1, const Transform3d& tf1, const CollisionGeometry* o2, const Transform3d& tf2, - const DistanceRequest& request, DistanceResult& result) -{ - if(request.isSatisfied(result)) return result.min_distance; - MeshDistanceTraversalNode node; - const BVHModel* obj1 = static_cast* >(o1); - const BVHModel* obj2 = static_cast* >(o2); - BVHModel* obj1_tmp = new BVHModel(*obj1); - Transform3d tf1_tmp = tf1; - BVHModel* obj2_tmp = new BVHModel(*obj2); - Transform3d tf2_tmp = tf2; - - initialize(node, *obj1_tmp, tf1_tmp, *obj2_tmp, tf2_tmp, request, result); - distance(&node); - delete obj1_tmp; - delete obj2_tmp; - - return result.min_distance; -} - -namespace details -{ -template -FCL_REAL orientedMeshDistance(const CollisionGeometry* o1, const Transform3d& tf1, const CollisionGeometry* o2, const Transform3d& tf2, - const DistanceRequest& request, DistanceResult& result) -{ - if(request.isSatisfied(result)) return result.min_distance; - OrientedMeshDistanceTraversalNode node; - const BVHModel* obj1 = static_cast* >(o1); - const BVHModel* obj2 = static_cast* >(o2); - - initialize(node, *obj1, tf1, *obj2, tf2, request, result); - distance(&node); - - return result.min_distance; -} - -} - -template<> -FCL_REAL BVHDistance(const CollisionGeometry* o1, const Transform3d& tf1, const CollisionGeometry* o2, const Transform3d& tf2, - const DistanceRequest& request, DistanceResult& result) -{ - return details::orientedMeshDistance(o1, tf1, o2, tf2, request, result); -} - -template<> -FCL_REAL BVHDistance(const CollisionGeometry* o1, const Transform3d& tf1, const CollisionGeometry* o2, const Transform3d& tf2, - const DistanceRequest& request, DistanceResult& result) -{ - return details::orientedMeshDistance(o1, tf1, o2, tf2, request, result); -} - - -template<> -FCL_REAL BVHDistance(const CollisionGeometry* o1, const Transform3d& tf1, const CollisionGeometry* o2, const Transform3d& tf2, - const DistanceRequest& request, DistanceResult& result) -{ - return details::orientedMeshDistance(o1, tf1, o2, tf2, request, result); -} - - -template -FCL_REAL BVHDistance(const CollisionGeometry* o1, const Transform3d& tf1, const CollisionGeometry* o2, const Transform3d& tf2, - const NarrowPhaseSolver* nsolver, - const DistanceRequest& request, DistanceResult& result) -{ - return BVHDistance(o1, tf1, o2, tf2, request, result); -} - -template -DistanceFunctionMatrix::DistanceFunctionMatrix() -{ - for(int i = 0; i < NODE_COUNT; ++i) - { - for(int j = 0; j < NODE_COUNT; ++j) - distance_matrix[i][j] = NULL; - } - - distance_matrix[GEOM_BOX][GEOM_BOX] = &ShapeShapeDistance; - distance_matrix[GEOM_BOX][GEOM_SPHERE] = &ShapeShapeDistance; - distance_matrix[GEOM_BOX][GEOM_ELLIPSOID] = &ShapeShapeDistance; - distance_matrix[GEOM_BOX][GEOM_CAPSULE] = &ShapeShapeDistance; - distance_matrix[GEOM_BOX][GEOM_CONE] = &ShapeShapeDistance; - distance_matrix[GEOM_BOX][GEOM_CYLINDER] = &ShapeShapeDistance; - distance_matrix[GEOM_BOX][GEOM_CONVEX] = &ShapeShapeDistance; - distance_matrix[GEOM_BOX][GEOM_PLANE] = &ShapeShapeDistance; - distance_matrix[GEOM_BOX][GEOM_HALFSPACE] = &ShapeShapeDistance; - - distance_matrix[GEOM_SPHERE][GEOM_BOX] = &ShapeShapeDistance; - distance_matrix[GEOM_SPHERE][GEOM_SPHERE] = &ShapeShapeDistance; - distance_matrix[GEOM_SPHERE][GEOM_ELLIPSOID] = &ShapeShapeDistance; - distance_matrix[GEOM_SPHERE][GEOM_CAPSULE] = &ShapeShapeDistance; - distance_matrix[GEOM_SPHERE][GEOM_CONE] = &ShapeShapeDistance; - distance_matrix[GEOM_SPHERE][GEOM_CYLINDER] = &ShapeShapeDistance; - distance_matrix[GEOM_SPHERE][GEOM_CONVEX] = &ShapeShapeDistance; - distance_matrix[GEOM_SPHERE][GEOM_PLANE] = &ShapeShapeDistance; - distance_matrix[GEOM_SPHERE][GEOM_HALFSPACE] = &ShapeShapeDistance; - - distance_matrix[GEOM_ELLIPSOID][GEOM_BOX] = &ShapeShapeDistance; - distance_matrix[GEOM_ELLIPSOID][GEOM_SPHERE] = &ShapeShapeDistance; - distance_matrix[GEOM_ELLIPSOID][GEOM_ELLIPSOID] = &ShapeShapeDistance; - distance_matrix[GEOM_ELLIPSOID][GEOM_CAPSULE] = &ShapeShapeDistance; - distance_matrix[GEOM_ELLIPSOID][GEOM_CONE] = &ShapeShapeDistance; - distance_matrix[GEOM_ELLIPSOID][GEOM_CYLINDER] = &ShapeShapeDistance; - distance_matrix[GEOM_ELLIPSOID][GEOM_CONVEX] = &ShapeShapeDistance; - distance_matrix[GEOM_ELLIPSOID][GEOM_PLANE] = &ShapeShapeDistance; - distance_matrix[GEOM_ELLIPSOID][GEOM_HALFSPACE] = &ShapeShapeDistance; - - distance_matrix[GEOM_CAPSULE][GEOM_BOX] = &ShapeShapeDistance; - distance_matrix[GEOM_CAPSULE][GEOM_SPHERE] = &ShapeShapeDistance; - distance_matrix[GEOM_CAPSULE][GEOM_ELLIPSOID] = &ShapeShapeDistance; - distance_matrix[GEOM_CAPSULE][GEOM_CAPSULE] = &ShapeShapeDistance; - distance_matrix[GEOM_CAPSULE][GEOM_CONE] = &ShapeShapeDistance; - distance_matrix[GEOM_CAPSULE][GEOM_CYLINDER] = &ShapeShapeDistance; - distance_matrix[GEOM_CAPSULE][GEOM_CONVEX] = &ShapeShapeDistance; - distance_matrix[GEOM_CAPSULE][GEOM_PLANE] = &ShapeShapeDistance; - distance_matrix[GEOM_CAPSULE][GEOM_HALFSPACE] = &ShapeShapeDistance; - - distance_matrix[GEOM_CONE][GEOM_BOX] = &ShapeShapeDistance; - distance_matrix[GEOM_CONE][GEOM_SPHERE] = &ShapeShapeDistance; - distance_matrix[GEOM_CONE][GEOM_ELLIPSOID] = &ShapeShapeDistance; - distance_matrix[GEOM_CONE][GEOM_CAPSULE] = &ShapeShapeDistance; - distance_matrix[GEOM_CONE][GEOM_CONE] = &ShapeShapeDistance; - distance_matrix[GEOM_CONE][GEOM_CYLINDER] = &ShapeShapeDistance; - distance_matrix[GEOM_CONE][GEOM_CONVEX] = &ShapeShapeDistance; - distance_matrix[GEOM_CONE][GEOM_PLANE] = &ShapeShapeDistance; - distance_matrix[GEOM_CONE][GEOM_HALFSPACE] = &ShapeShapeDistance; - - distance_matrix[GEOM_CYLINDER][GEOM_BOX] = &ShapeShapeDistance; - distance_matrix[GEOM_CYLINDER][GEOM_SPHERE] = &ShapeShapeDistance; - distance_matrix[GEOM_CYLINDER][GEOM_ELLIPSOID] = &ShapeShapeDistance; - distance_matrix[GEOM_CYLINDER][GEOM_CAPSULE] = &ShapeShapeDistance; - distance_matrix[GEOM_CYLINDER][GEOM_CONE] = &ShapeShapeDistance; - distance_matrix[GEOM_CYLINDER][GEOM_CYLINDER] = &ShapeShapeDistance; - distance_matrix[GEOM_CYLINDER][GEOM_CONVEX] = &ShapeShapeDistance; - distance_matrix[GEOM_CYLINDER][GEOM_PLANE] = &ShapeShapeDistance; - distance_matrix[GEOM_CYLINDER][GEOM_HALFSPACE] = &ShapeShapeDistance; - - distance_matrix[GEOM_CONVEX][GEOM_BOX] = &ShapeShapeDistance; - distance_matrix[GEOM_CONVEX][GEOM_SPHERE] = &ShapeShapeDistance; - distance_matrix[GEOM_CONVEX][GEOM_ELLIPSOID] = &ShapeShapeDistance; - distance_matrix[GEOM_CONVEX][GEOM_CAPSULE] = &ShapeShapeDistance; - distance_matrix[GEOM_CONVEX][GEOM_CONE] = &ShapeShapeDistance; - distance_matrix[GEOM_CONVEX][GEOM_CYLINDER] = &ShapeShapeDistance; - distance_matrix[GEOM_CONVEX][GEOM_CONVEX] = &ShapeShapeDistance; - distance_matrix[GEOM_CONVEX][GEOM_PLANE] = &ShapeShapeDistance; - distance_matrix[GEOM_CONVEX][GEOM_HALFSPACE] = &ShapeShapeDistance; - - distance_matrix[GEOM_PLANE][GEOM_BOX] = &ShapeShapeDistance; - distance_matrix[GEOM_PLANE][GEOM_SPHERE] = &ShapeShapeDistance; - distance_matrix[GEOM_PLANE][GEOM_ELLIPSOID] = &ShapeShapeDistance; - distance_matrix[GEOM_PLANE][GEOM_CAPSULE] = &ShapeShapeDistance; - distance_matrix[GEOM_PLANE][GEOM_CONE] = &ShapeShapeDistance; - distance_matrix[GEOM_PLANE][GEOM_CYLINDER] = &ShapeShapeDistance; - distance_matrix[GEOM_PLANE][GEOM_CONVEX] = &ShapeShapeDistance; - distance_matrix[GEOM_PLANE][GEOM_PLANE] = &ShapeShapeDistance; - distance_matrix[GEOM_PLANE][GEOM_HALFSPACE] = &ShapeShapeDistance; - - distance_matrix[GEOM_HALFSPACE][GEOM_BOX] = &ShapeShapeDistance; - distance_matrix[GEOM_HALFSPACE][GEOM_SPHERE] = &ShapeShapeDistance; - distance_matrix[GEOM_HALFSPACE][GEOM_CAPSULE] = &ShapeShapeDistance; - distance_matrix[GEOM_HALFSPACE][GEOM_CONE] = &ShapeShapeDistance; - distance_matrix[GEOM_HALFSPACE][GEOM_CYLINDER] = &ShapeShapeDistance; - distance_matrix[GEOM_HALFSPACE][GEOM_CONVEX] = &ShapeShapeDistance; - distance_matrix[GEOM_HALFSPACE][GEOM_PLANE] = &ShapeShapeDistance; - distance_matrix[GEOM_HALFSPACE][GEOM_HALFSPACE] = &ShapeShapeDistance; - - /* AABB distance not implemented */ - /* - distance_matrix[BV_AABB][GEOM_BOX] = &BVHShapeDistancer::distance; - distance_matrix[BV_AABB][GEOM_SPHERE] = &BVHShapeDistancer::distance; - distance_matrix[BV_AABB][GEOM_ELLIPSOID] = &BVHShapeDistancer::distance; - distance_matrix[BV_AABB][GEOM_CAPSULE] = &BVHShapeDistancer::distance; - distance_matrix[BV_AABB][GEOM_CONE] = &BVHShapeDistancer::distance; - distance_matrix[BV_AABB][GEOM_CYLINDER] = &BVHShapeDistancer::distance; - distance_matrix[BV_AABB][GEOM_CONVEX] = &BVHShapeDistancer::distance; - distance_matrix[BV_AABB][GEOM_PLANE] = &BVHShapeDistancer::distance; - distance_matrix[BV_AABB][GEOM_HALFSPACE] = &BVHShapeDistancer::distance; - - distance_matrix[BV_OBB][GEOM_BOX] = &BVHShapeDistancer::distance; - distance_matrix[BV_OBB][GEOM_SPHERE] = &BVHShapeDistancer::distance; - distance_matrix[BV_OBB][GEOM_ELLIPSOID] = &BVHShapeDistancer::distance; - distance_matrix[BV_OBB][GEOM_CAPSULE] = &BVHShapeDistancer::distance; - distance_matrix[BV_OBB][GEOM_CONE] = &BVHShapeDistancer::distance; - distance_matrix[BV_OBB][GEOM_CYLINDER] = &BVHShapeDistancer::distance; - distance_matrix[BV_OBB][GEOM_CONVEX] = &BVHShapeDistancer::distance; - distance_matrix[BV_OBB][GEOM_PLANE] = &BVHShapeDistancer::distance; - distance_matrix[BV_OBB][GEOM_HALFSPACE] = &BVHShapeDistancer::distance; - */ - - distance_matrix[BV_RSS][GEOM_BOX] = &BVHShapeDistancer::distance; - distance_matrix[BV_RSS][GEOM_SPHERE] = &BVHShapeDistancer::distance; - distance_matrix[BV_RSS][GEOM_ELLIPSOID] = &BVHShapeDistancer::distance; - distance_matrix[BV_RSS][GEOM_CAPSULE] = &BVHShapeDistancer::distance; - distance_matrix[BV_RSS][GEOM_CONE] = &BVHShapeDistancer::distance; - distance_matrix[BV_RSS][GEOM_CYLINDER] = &BVHShapeDistancer::distance; - distance_matrix[BV_RSS][GEOM_CONVEX] = &BVHShapeDistancer::distance; - distance_matrix[BV_RSS][GEOM_PLANE] = &BVHShapeDistancer::distance; - distance_matrix[BV_RSS][GEOM_HALFSPACE] = &BVHShapeDistancer::distance; - - /* KDOP distance not implemented */ - /* - distance_matrix[BV_KDOP16][GEOM_BOX] = &BVHShapeDistancer, Box, NarrowPhaseSolver>::distance; - distance_matrix[BV_KDOP16][GEOM_SPHERE] = &BVHShapeDistancer, Sphere, NarrowPhaseSolver>::distance; - distance_matrix[BV_KDOP16][GEOM_ELLIPSOID] = &BVHShapeDistancer, Ellipsoid, NarrowPhaseSolver>::distance; - distance_matrix[BV_KDOP16][GEOM_CAPSULE] = &BVHShapeDistancer, Capsule, NarrowPhaseSolver>::distance; - distance_matrix[BV_KDOP16][GEOM_CONE] = &BVHShapeDistancer, Cone, NarrowPhaseSolver>::distance; - distance_matrix[BV_KDOP16][GEOM_CYLINDER] = &BVHShapeDistancer, Cylinder, NarrowPhaseSolver>::distance; - distance_matrix[BV_KDOP16][GEOM_CONVEX] = &BVHShapeDistancer, Convex, NarrowPhaseSolver>::distance; - distance_matrix[BV_KDOP16][GEOM_PLANE] = &BVHShapeDistancer, Plane, NarrowPhaseSolver>::distance; - distance_matrix[BV_KDOP16][GEOM_HALFSPACE] = &BVHShapeDistancer, Halfspace, NarrowPhaseSolver>::distance; - - distance_matrix[BV_KDOP18][GEOM_BOX] = &BVHShapeDistancer, Box, NarrowPhaseSolver>::distance; - distance_matrix[BV_KDOP18][GEOM_SPHERE] = &BVHShapeDistancer, Sphere, NarrowPhaseSolver>::distance; - distance_matrix[BV_KDOP18][GEOM_ELLIPSOID] = &BVHShapeDistancer, Ellipsoid, NarrowPhaseSolver>::distance; - distance_matrix[BV_KDOP18][GEOM_CAPSULE] = &BVHShapeDistancer, Capsule, NarrowPhaseSolver>::distance; - distance_matrix[BV_KDOP18][GEOM_CONE] = &BVHShapeDistancer, Cone, NarrowPhaseSolver>::distance; - distance_matrix[BV_KDOP18][GEOM_CYLINDER] = &BVHShapeDistancer, Cylinder, NarrowPhaseSolver>::distance; - distance_matrix[BV_KDOP18][GEOM_CONVEX] = &BVHShapeDistancer, Convex, NarrowPhaseSolver>::distance; - distance_matrix[BV_KDOP18][GEOM_PLANE] = &BVHShapeDistancer, Plane, NarrowPhaseSolver>::distance; - distance_matrix[BV_KDOP18][GEOM_HALFSPACE] = &BVHShapeDistancer, Halfspace, NarrowPhaseSolver>::distance; - - distance_matrix[BV_KDOP24][GEOM_BOX] = &BVHShapeDistancer, Box, NarrowPhaseSolver>::distance; - distance_matrix[BV_KDOP24][GEOM_SPHERE] = &BVHShapeDistancer, Sphere, NarrowPhaseSolver>::distance; - distance_matrix[BV_KDOP24][GEOM_ELLIPSOID] = &BVHShapeDistancer, Ellipsoid, NarrowPhaseSolver>::distance; - distance_matrix[BV_KDOP24][GEOM_CAPSULE] = &BVHShapeDistancer, Capsule, NarrowPhaseSolver>::distance; - distance_matrix[BV_KDOP24][GEOM_CONE] = &BVHShapeDistancer, Cone, NarrowPhaseSolver>::distance; - distance_matrix[BV_KDOP24][GEOM_CYLINDER] = &BVHShapeDistancer, Cylinder, NarrowPhaseSolver>::distance; - distance_matrix[BV_KDOP24][GEOM_CONVEX] = &BVHShapeDistancer, Convex, NarrowPhaseSolver>::distance; - distance_matrix[BV_KDOP24][GEOM_PLANE] = &BVHShapeDistancer, Plane, NarrowPhaseSolver>::distance; - distance_matrix[BV_KDOP24][GEOM_HALFSPACE] = &BVHShapeDistancer, Halfspace, NarrowPhaseSolver>::distance; - */ - - distance_matrix[BV_kIOS][GEOM_BOX] = &BVHShapeDistancer::distance; - distance_matrix[BV_kIOS][GEOM_SPHERE] = &BVHShapeDistancer::distance; - distance_matrix[BV_kIOS][GEOM_ELLIPSOID] = &BVHShapeDistancer::distance; - distance_matrix[BV_kIOS][GEOM_CAPSULE] = &BVHShapeDistancer::distance; - distance_matrix[BV_kIOS][GEOM_CONE] = &BVHShapeDistancer::distance; - distance_matrix[BV_kIOS][GEOM_CYLINDER] = &BVHShapeDistancer::distance; - distance_matrix[BV_kIOS][GEOM_CONVEX] = &BVHShapeDistancer::distance; - distance_matrix[BV_kIOS][GEOM_PLANE] = &BVHShapeDistancer::distance; - distance_matrix[BV_kIOS][GEOM_HALFSPACE] = &BVHShapeDistancer::distance; - - distance_matrix[BV_OBBRSS][GEOM_BOX] = &BVHShapeDistancer::distance; - distance_matrix[BV_OBBRSS][GEOM_SPHERE] = &BVHShapeDistancer::distance; - distance_matrix[BV_OBBRSS][GEOM_ELLIPSOID] = &BVHShapeDistancer::distance; - distance_matrix[BV_OBBRSS][GEOM_CAPSULE] = &BVHShapeDistancer::distance; - distance_matrix[BV_OBBRSS][GEOM_CONE] = &BVHShapeDistancer::distance; - distance_matrix[BV_OBBRSS][GEOM_CYLINDER] = &BVHShapeDistancer::distance; - distance_matrix[BV_OBBRSS][GEOM_CONVEX] = &BVHShapeDistancer::distance; - distance_matrix[BV_OBBRSS][GEOM_PLANE] = &BVHShapeDistancer::distance; - distance_matrix[BV_OBBRSS][GEOM_HALFSPACE] = &BVHShapeDistancer::distance; - - distance_matrix[BV_AABB][BV_AABB] = &BVHDistance; - distance_matrix[BV_RSS][BV_RSS] = &BVHDistance; - distance_matrix[BV_kIOS][BV_kIOS] = &BVHDistance; - distance_matrix[BV_OBBRSS][BV_OBBRSS] = &BVHDistance; - -#if FCL_HAVE_OCTOMAP - distance_matrix[GEOM_OCTREE][GEOM_BOX] = &OcTreeShapeDistance; - distance_matrix[GEOM_OCTREE][GEOM_SPHERE] = &OcTreeShapeDistance; - distance_matrix[GEOM_OCTREE][GEOM_ELLIPSOID] = &OcTreeShapeDistance; - distance_matrix[GEOM_OCTREE][GEOM_CAPSULE] = &OcTreeShapeDistance; - distance_matrix[GEOM_OCTREE][GEOM_CONE] = &OcTreeShapeDistance; - distance_matrix[GEOM_OCTREE][GEOM_CYLINDER] = &OcTreeShapeDistance; - distance_matrix[GEOM_OCTREE][GEOM_CONVEX] = &OcTreeShapeDistance; - distance_matrix[GEOM_OCTREE][GEOM_PLANE] = &OcTreeShapeDistance; - distance_matrix[GEOM_OCTREE][GEOM_HALFSPACE] = &OcTreeShapeDistance; - - distance_matrix[GEOM_BOX][GEOM_OCTREE] = &ShapeOcTreeDistance; - distance_matrix[GEOM_SPHERE][GEOM_OCTREE] = &ShapeOcTreeDistance; - distance_matrix[GEOM_ELLIPSOID][GEOM_OCTREE] = &ShapeOcTreeDistance; - distance_matrix[GEOM_CAPSULE][GEOM_OCTREE] = &ShapeOcTreeDistance; - distance_matrix[GEOM_CONE][GEOM_OCTREE] = &ShapeOcTreeDistance; - distance_matrix[GEOM_CYLINDER][GEOM_OCTREE] = &ShapeOcTreeDistance; - distance_matrix[GEOM_CONVEX][GEOM_OCTREE] = &ShapeOcTreeDistance; - distance_matrix[GEOM_PLANE][GEOM_OCTREE] = &ShapeOcTreeDistance; - distance_matrix[GEOM_HALFSPACE][GEOM_OCTREE] = &ShapeOcTreeDistance; - - distance_matrix[GEOM_OCTREE][GEOM_OCTREE] = &OcTreeDistance; - - distance_matrix[GEOM_OCTREE][BV_AABB] = &OcTreeBVHDistance; - distance_matrix[GEOM_OCTREE][BV_OBB] = &OcTreeBVHDistance; - distance_matrix[GEOM_OCTREE][BV_RSS] = &OcTreeBVHDistance; - distance_matrix[GEOM_OCTREE][BV_OBBRSS] = &OcTreeBVHDistance; - distance_matrix[GEOM_OCTREE][BV_kIOS] = &OcTreeBVHDistance; - distance_matrix[GEOM_OCTREE][BV_KDOP16] = &OcTreeBVHDistance, NarrowPhaseSolver>; - distance_matrix[GEOM_OCTREE][BV_KDOP18] = &OcTreeBVHDistance, NarrowPhaseSolver>; - distance_matrix[GEOM_OCTREE][BV_KDOP24] = &OcTreeBVHDistance, NarrowPhaseSolver>; - - distance_matrix[BV_AABB][GEOM_OCTREE] = &BVHOcTreeDistance; - distance_matrix[BV_OBB][GEOM_OCTREE] = &BVHOcTreeDistance; - distance_matrix[BV_RSS][GEOM_OCTREE] = &BVHOcTreeDistance; - distance_matrix[BV_OBBRSS][GEOM_OCTREE] = &BVHOcTreeDistance; - distance_matrix[BV_kIOS][GEOM_OCTREE] = &BVHOcTreeDistance; - distance_matrix[BV_KDOP16][GEOM_OCTREE] = &BVHOcTreeDistance, NarrowPhaseSolver>; - distance_matrix[BV_KDOP18][GEOM_OCTREE] = &BVHOcTreeDistance, NarrowPhaseSolver>; - distance_matrix[BV_KDOP24][GEOM_OCTREE] = &BVHOcTreeDistance, NarrowPhaseSolver>; -#endif - - -} - -template struct DistanceFunctionMatrix; -template struct DistanceFunctionMatrix; - - -} diff --git a/src/intersect.cpp b/src/intersect.cpp deleted file mode 100644 index f9eac58b6..000000000 --- a/src/intersect.cpp +++ /dev/null @@ -1,1827 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#include "fcl/intersect.h" - -#include -#include -#include - -#include "fcl/math/geometry.h" - -namespace fcl -{ -const FCL_REAL PolySolver::NEAR_ZERO_THRESHOLD = 1e-9; - - -bool PolySolver::isZero(FCL_REAL v) -{ - return (v < NEAR_ZERO_THRESHOLD) && (v > -NEAR_ZERO_THRESHOLD); -} - -bool PolySolver::cbrt(FCL_REAL v) -{ - return powf(v, 1.0 / 3.0); -} - -int PolySolver::solveLinear(FCL_REAL c[2], FCL_REAL s[1]) -{ - if(isZero(c[1])) - return 0; - s[0] = - c[0] / c[1]; - return 1; -} - -int PolySolver::solveQuadric(FCL_REAL c[3], FCL_REAL s[2]) -{ - FCL_REAL p, q, D; - - // make sure we have a d2 equation - - if(isZero(c[2])) - return solveLinear(c, s); - - // normal for: x^2 + px + q - p = c[1] / (2.0 * c[2]); - q = c[0] / c[2]; - D = p * p - q; - - if(isZero(D)) - { - // one FCL_REAL root - s[0] = s[1] = -p; - return 1; - } - - if(D < 0.0) - // no real root - return 0; - else - { - // two real roots - FCL_REAL sqrt_D = sqrt(D); - s[0] = sqrt_D - p; - s[1] = -sqrt_D - p; - return 2; - } -} - -int PolySolver::solveCubic(FCL_REAL c[4], FCL_REAL s[3]) -{ - int i, num; - FCL_REAL sub, A, B, C, sq_A, p, q, cb_p, D; - const FCL_REAL ONE_OVER_THREE = 1 / 3.0; - const FCL_REAL PI = 3.14159265358979323846; - - // make sure we have a d2 equation - if(isZero(c[3])) - return solveQuadric(c, s); - - // normalize the equation:x ^ 3 + Ax ^ 2 + Bx + C = 0 - A = c[2] / c[3]; - B = c[1] / c[3]; - C = c[0] / c[3]; - - // substitute x = y - A / 3 to eliminate the quadratic term: x^3 + px + q = 0 - sq_A = A * A; - p = (-ONE_OVER_THREE * sq_A + B) * ONE_OVER_THREE; - q = 0.5 * (2.0 / 27.0 * A * sq_A - ONE_OVER_THREE * A * B + C); - - // use Cardano's formula - cb_p = p * p * p; - D = q * q + cb_p; - - if(isZero(D)) - { - if(isZero(q)) - { - // one triple solution - s[0] = 0.0; - num = 1; - } - else - { - // one single and one FCL_REAL solution - FCL_REAL u = cbrt(-q); - s[0] = 2.0 * u; - s[1] = -u; - num = 2; - } - } - else - { - if(D < 0.0) - { - // three real solutions - FCL_REAL phi = ONE_OVER_THREE * acos(-q / sqrt(-cb_p)); - FCL_REAL t = 2.0 * sqrt(-p); - s[0] = t * cos(phi); - s[1] = -t * cos(phi + PI / 3.0); - s[2] = -t * cos(phi - PI / 3.0); - num = 3; - } - else - { - // one real solution - FCL_REAL sqrt_D = sqrt(D); - FCL_REAL u = cbrt(sqrt_D + fabs(q)); - if(q > 0.0) - s[0] = - u + p / u ; - else - s[0] = u - p / u; - num = 1; - } - } - - // re-substitute - sub = ONE_OVER_THREE * A; - for(i = 0; i < num; i++) - s[i] -= sub; - return num; -} - - - -const FCL_REAL Intersect::EPSILON = 1e-5; -const FCL_REAL Intersect::NEAR_ZERO_THRESHOLD = 1e-7; -const FCL_REAL Intersect::CCD_RESOLUTION = 1e-7; - - -bool Intersect::isZero(FCL_REAL v) -{ - return (v < NEAR_ZERO_THRESHOLD) && (v > -NEAR_ZERO_THRESHOLD); -} - -/// @brief data: only used for EE, return the intersect point -bool Intersect::solveCubicWithIntervalNewton(const Vector3d& a0, const Vector3d& b0, const Vector3d& c0, const Vector3d& d0, - const Vector3d& va, const Vector3d& vb, const Vector3d& vc, const Vector3d& vd, - FCL_REAL& l, FCL_REAL& r, bool bVF, FCL_REAL coeffs[], Vector3d* data) -{ - FCL_REAL v2[2]= {l*l,r*r}; - FCL_REAL v[2]= {l,r}; - FCL_REAL r_backup; - - unsigned char min3, min2, min1, max3, max2, max1; - - min3= *((unsigned char*)&coeffs[3]+7)>>7; max3=min3^1; - min2= *((unsigned char*)&coeffs[2]+7)>>7; max2=min2^1; - min1= *((unsigned char*)&coeffs[1]+7)>>7; max1=min1^1; - - // bound the cubic - - FCL_REAL minor = coeffs[3]*v2[min3]*v[min3]+coeffs[2]*v2[min2]+coeffs[1]*v[min1]+coeffs[0]; - FCL_REAL major = coeffs[3]*v2[max3]*v[max3]+coeffs[2]*v2[max2]+coeffs[1]*v[max1]+coeffs[0]; - - if(major<0) return false; - if(minor>0) return false; - - // starting here, the bounds have opposite values - FCL_REAL m = 0.5 * (r + l); - - // bound the derivative - FCL_REAL dminor = 3.0*coeffs[3]*v2[min3]+2.0*coeffs[2]*v[min2]+coeffs[1]; - FCL_REAL dmajor = 3.0*coeffs[3]*v2[max3]+2.0*coeffs[2]*v[max2]+coeffs[1]; - - if((dminor > 0)||(dmajor < 0)) // we can use Newton - { - FCL_REAL m2 = m*m; - FCL_REAL fm = coeffs[3]*m2*m+coeffs[2]*m2+coeffs[1]*m+coeffs[0]; - FCL_REAL nl = m; - FCL_REAL nu = m; - if(fm>0) - { - nl-=(fm/dminor); - nu-=(fm/dmajor); - } - else - { - nu-=(fm/dminor); - nl-=(fm/dmajor); - } - - //intersect with [l,r] - - if(nl>r) return false; - if(nul) - { - if(nu 1) - return false; - - *mub = (d3134 + d3412 * (*mua)) / d3434; - if(*mub < 0 || *mub > 1) - return false; - - *pa = p1 + p12 * (*mua); - *pb = p3 + p34 * (*mub); - return true; -} - -bool Intersect::checkRootValidity_VF(const Vector3d& a0, const Vector3d& b0, const Vector3d& c0, const Vector3d& p0, - const Vector3d& va, const Vector3d& vb, const Vector3d& vc, const Vector3d& vp, - FCL_REAL t) -{ - return insideTriangle(a0 + va * t, b0 + vb * t, c0 + vc * t, p0 + vp * t); -} - -bool Intersect::checkRootValidity_EE(const Vector3d& a0, const Vector3d& b0, const Vector3d& c0, const Vector3d& d0, - const Vector3d& va, const Vector3d& vb, const Vector3d& vc, const Vector3d& vd, - FCL_REAL t, Vector3d* q_i) -{ - Vector3d a = a0 + va * t; - Vector3d b = b0 + vb * t; - Vector3d c = c0 + vc * t; - Vector3d d = d0 + vd * t; - Vector3d p1, p2; - FCL_REAL t_ab, t_cd; - if(linelineIntersect(a, b, c, d, &p1, &p2, &t_ab, &t_cd)) - { - if(q_i) *q_i = p1; - return true; - } - - return false; -} - -bool Intersect::checkRootValidity_VE(const Vector3d& a0, const Vector3d& b0, const Vector3d& p0, - const Vector3d& va, const Vector3d& vb, const Vector3d& vp, - FCL_REAL t) -{ - return insideLineSegment(a0 + va * t, b0 + vb * t, p0 + vp * t); -} - -bool Intersect::solveSquare(FCL_REAL a, FCL_REAL b, FCL_REAL c, - const Vector3d& a0, const Vector3d& b0, const Vector3d& c0, const Vector3d& d0, - const Vector3d& va, const Vector3d& vb, const Vector3d& vc, const Vector3d& vd, - bool bVF, - FCL_REAL* ret) -{ - FCL_REAL discriminant = b * b - 4 * a * c; - if(discriminant < 0) - return false; - - FCL_REAL sqrt_dis = sqrt(discriminant); - FCL_REAL r1 = (-b + sqrt_dis) / (2 * a); - bool v1 = (r1 >= 0.0 && r1 <= 1.0) ? ((bVF) ? checkRootValidity_VF(a0, b0, c0, d0, va, vb, vc, vd, r1) : checkRootValidity_EE(a0, b0, c0, d0, va, vb, vc, vd, r1)) : false; - - FCL_REAL r2 = (-b - sqrt_dis) / (2 * a); - bool v2 = (r2 >= 0.0 && r2 <= 1.0) ? ((bVF) ? checkRootValidity_VF(a0, b0, c0, d0, va, vb, vc, vd, r2) : checkRootValidity_EE(a0, b0, c0, d0, va, vb, vc, vd, r2)) : false; - - if(v1 && v2) - { - *ret = (r1 > r2) ? r2 : r1; - return true; - } - if(v1) - { - *ret = r1; - return true; - } - if(v2) - { - *ret = r2; - return true; - } - - return false; -} - -bool Intersect::solveSquare(FCL_REAL a, FCL_REAL b, FCL_REAL c, - const Vector3d& a0, const Vector3d& b0, const Vector3d& p0, - const Vector3d& va, const Vector3d& vb, const Vector3d& vp) -{ - if(isZero(a)) - { - FCL_REAL t = -c/b; - return (t >= 0 && t <= 1) ? checkRootValidity_VE(a0, b0, p0, va, vb, vp, t) : false; - } - - FCL_REAL discriminant = b*b-4*a*c; - if(discriminant < 0) - return false; - - FCL_REAL sqrt_dis = sqrt(discriminant); - - FCL_REAL r1 = (-b+sqrt_dis) / (2 * a); - bool v1 = (r1 >= 0.0 && r1 <= 1.0) ? checkRootValidity_VE(a0, b0, p0, va, vb, vp, r1) : false; - if(v1) return true; - - FCL_REAL r2 = (-b-sqrt_dis) / (2 * a); - bool v2 = (r2 >= 0.0 && r2 <= 1.0) ? checkRootValidity_VE(a0, b0, p0, va, vb, vp, r2) : false; - return v2; -} - - - -/// @brief Compute the cubic coefficients for VF case -/// See Paper "Interactive Continuous Collision Detection between Deformable Models using Connectivity-Based Culling", Equation 1. -void Intersect::computeCubicCoeff_VF(const Vector3d& a0, const Vector3d& b0, const Vector3d& c0, const Vector3d& p0, - const Vector3d& va, const Vector3d& vb, const Vector3d& vc, const Vector3d& vp, - FCL_REAL* a, FCL_REAL* b, FCL_REAL* c, FCL_REAL* d) -{ - Vector3d vavb = vb - va; - Vector3d vavc = vc - va; - Vector3d vavp = vp - va; - Vector3d a0b0 = b0 - a0; - Vector3d a0c0 = c0 - a0; - Vector3d a0p0 = p0 - a0; - - Vector3d vavb_cross_vavc = vavb.cross(vavc); - Vector3d vavb_cross_a0c0 = vavb.cross(a0c0); - Vector3d a0b0_cross_vavc = a0b0.cross(vavc); - Vector3d a0b0_cross_a0c0 = a0b0.cross(a0c0); - - *a = vavp.dot(vavb_cross_vavc); - *b = a0p0.dot(vavb_cross_vavc) + vavp.dot(vavb_cross_a0c0 + a0b0_cross_vavc); - *c = vavp.dot(a0b0_cross_a0c0) + a0p0.dot(vavb_cross_a0c0 + a0b0_cross_vavc); - *d = a0p0.dot(a0b0_cross_a0c0); -} - -void Intersect::computeCubicCoeff_EE(const Vector3d& a0, const Vector3d& b0, const Vector3d& c0, const Vector3d& d0, - const Vector3d& va, const Vector3d& vb, const Vector3d& vc, const Vector3d& vd, - FCL_REAL* a, FCL_REAL* b, FCL_REAL* c, FCL_REAL* d) -{ - Vector3d vavb = vb - va; - Vector3d vcvd = vd - vc; - Vector3d vavc = vc - va; - Vector3d c0d0 = d0 - c0; - Vector3d a0b0 = b0 - a0; - Vector3d a0c0 = c0 - a0; - Vector3d vavb_cross_vcvd = vavb.cross(vcvd); - Vector3d vavb_cross_c0d0 = vavb.cross(c0d0); - Vector3d a0b0_cross_vcvd = a0b0.cross(vcvd); - Vector3d a0b0_cross_c0d0 = a0b0.cross(c0d0); - - *a = vavc.dot(vavb_cross_vcvd); - *b = a0c0.dot(vavb_cross_vcvd) + vavc.dot(vavb_cross_c0d0 + a0b0_cross_vcvd); - *c = vavc.dot(a0b0_cross_c0d0) + a0c0.dot(vavb_cross_c0d0 + a0b0_cross_vcvd); - *d = a0c0.dot(a0b0_cross_c0d0); -} - -void Intersect::computeCubicCoeff_VE(const Vector3d& a0, const Vector3d& b0, const Vector3d& p0, - const Vector3d& va, const Vector3d& vb, const Vector3d& vp, - const Vector3d& L, - FCL_REAL* a, FCL_REAL* b, FCL_REAL* c) -{ - Vector3d vbva = va - vb; - Vector3d vbvp = vp - vb; - Vector3d b0a0 = a0 - b0; - Vector3d b0p0 = p0 - b0; - - Vector3d L_cross_vbvp = L.cross(vbvp); - Vector3d L_cross_b0p0 = L.cross(b0p0); - - *a = L_cross_vbvp.dot(vbva); - *b = L_cross_vbvp.dot(b0a0) + L_cross_b0p0.dot(vbva); - *c = L_cross_b0p0.dot(b0a0); -} - - -bool Intersect::intersect_VF(const Vector3d& a0, const Vector3d& b0, const Vector3d& c0, const Vector3d& p0, - const Vector3d& a1, const Vector3d& b1, const Vector3d& c1, const Vector3d& p1, - FCL_REAL* collision_time, Vector3d* p_i, bool useNewton) -{ - *collision_time = 2.0; - - Vector3d vp, va, vb, vc; - vp = p1 - p0; - va = a1 - a0; - vb = b1 - b0; - vc = c1 - c0; - - FCL_REAL a, b, c, d; - computeCubicCoeff_VF(a0, b0, c0, p0, va, vb, vc, vp, &a, &b, &c, &d); - - if(isZero(a) && isZero(b) && isZero(c) && isZero(d)) - { - return false; - } - - - /// if(isZero(a)) - /// { - /// return solveSquare(b, c, d, a0, b0, c0, p0, va, vb, vc, vp, true, collision_time); - /// } - - FCL_REAL coeffs[4]; - coeffs[3] = a, coeffs[2] = b, coeffs[1] = c, coeffs[0] = d; - - if(useNewton) - { - FCL_REAL l = 0; - FCL_REAL r = 1; - - if(solveCubicWithIntervalNewton(a0, b0, c0, p0, va, vb, vc, vp, l, r, true, coeffs)) - { - *collision_time = 0.5 * (l + r); - } - } - else - { - FCL_REAL roots[3]; - int num = PolySolver::solveCubic(coeffs, roots); - for(int i = 0; i < num; ++i) - { - FCL_REAL r = roots[i]; - if(r < 0 || r > 1) continue; - if(checkRootValidity_VF(a0, b0, c0, p0, va, vb, vc, vp, r)) - { - *collision_time = r; - break; - } - } - } - - if(*collision_time > 1) - { - return false; - } - - *p_i = vp * (*collision_time) + p0; - return true; -} - -bool Intersect::intersect_EE(const Vector3d& a0, const Vector3d& b0, const Vector3d& c0, const Vector3d& d0, - const Vector3d& a1, const Vector3d& b1, const Vector3d& c1, const Vector3d& d1, - FCL_REAL* collision_time, Vector3d* p_i, bool useNewton) -{ - *collision_time = 2.0; - - Vector3d va, vb, vc, vd; - va = a1 - a0; - vb = b1 - b0; - vc = c1 - c0; - vd = d1 - d0; - - FCL_REAL a, b, c, d; - computeCubicCoeff_EE(a0, b0, c0, d0, va, vb, vc, vd, &a, &b, &c, &d); - - if(isZero(a) && isZero(b) && isZero(c) && isZero(d)) - { - return false; - } - - /// if(isZero(a)) - /// { - /// return solveSquare(b, c, d, a0, b0, c0, d0, va, vb, vc, vd, collision_time, false); - /// } - - - FCL_REAL coeffs[4]; - coeffs[3] = a, coeffs[2] = b, coeffs[1] = c, coeffs[0] = d; - - if(useNewton) - { - FCL_REAL l = 0; - FCL_REAL r = 1; - - if(solveCubicWithIntervalNewton(a0, b0, c0, d0, va, vb, vc, vd, l, r, false, coeffs, p_i)) - { - *collision_time = (l + r) * 0.5; - } - } - else - { - FCL_REAL roots[3]; - int num = PolySolver::solveCubic(coeffs, roots); - for(int i = 0; i < num; ++i) - { - FCL_REAL r = roots[i]; - if(r < 0 || r > 1) continue; - - if(checkRootValidity_EE(a0, b0, c0, d0, va, vb, vc, vd, r, p_i)) - { - *collision_time = r; - break; - } - } - } - - if(*collision_time > 1) - { - return false; - } - - return true; -} - - -bool Intersect::intersect_VE(const Vector3d& a0, const Vector3d& b0, const Vector3d& p0, - const Vector3d& a1, const Vector3d& b1, const Vector3d& p1, - const Vector3d& L) -{ - Vector3d va, vb, vp; - va = a1 - a0; - vb = b1 - b0; - vp = p1 - p0; - - FCL_REAL a, b, c; - computeCubicCoeff_VE(a0, b0, p0, va, vb, vp, L, &a, &b, &c); - - if(isZero(a) && isZero(b) && isZero(c)) - return true; - - return solveSquare(a, b, c, a0, b0, p0, va, vb, vp); - -} - - -/// @brief Prefilter for intersection, works for both VF and EE -bool Intersect::intersectPreFiltering(const Vector3d& a0, const Vector3d& b0, const Vector3d& c0, const Vector3d& d0, - const Vector3d& a1, const Vector3d& b1, const Vector3d& c1, const Vector3d& d1) -{ - Vector3d n0 = (b0 - a0).cross(c0 - a0); - Vector3d n1 = (b1 - a1).cross(c1 - a1); - Vector3d a0a1 = a1 - a0; - Vector3d b0b1 = b1 - b0; - Vector3d c0c1 = c1 - c0; - Vector3d delta = (b0b1 - a0a1).cross(c0c1 - a0a1); - Vector3d nx = (n0 + n1 - delta) * 0.5; - - Vector3d a0d0 = d0 - a0; - Vector3d a1d1 = d1 - a1; - - FCL_REAL A = n0.dot(a0d0); - FCL_REAL B = n1.dot(a1d1); - FCL_REAL C = nx.dot(a0d0); - FCL_REAL D = nx.dot(a1d1); - FCL_REAL E = n1.dot(a0d0); - FCL_REAL F = n0.dot(a1d1); - - if(A > 0 && B > 0 && (2*C +F) > 0 && (2*D+E) > 0) - return false; - if(A < 0 && B < 0 && (2*C +F) < 0 && (2*D+E) < 0) - return false; - - return true; -} - -bool Intersect::intersect_VF_filtered(const Vector3d& a0, const Vector3d& b0, const Vector3d& c0, const Vector3d& p0, - const Vector3d& a1, const Vector3d& b1, const Vector3d& c1, const Vector3d& p1, - FCL_REAL* collision_time, Vector3d* p_i, bool useNewton) -{ - if(intersectPreFiltering(a0, b0, c0, p0, a1, b1, c1, p1)) - { - return intersect_VF(a0, b0, c0, p0, a1, b1, c1, p1, collision_time, p_i, useNewton); - } - else - return false; -} - -bool Intersect::intersect_EE_filtered(const Vector3d& a0, const Vector3d& b0, const Vector3d& c0, const Vector3d& d0, - const Vector3d& a1, const Vector3d& b1, const Vector3d& c1, const Vector3d& d1, - FCL_REAL* collision_time, Vector3d* p_i, bool useNewton) -{ - if(intersectPreFiltering(a0, b0, c0, d0, a1, b1, c1, d1)) - { - return intersect_EE(a0, b0, c0, d0, a1, b1, c1, d1, collision_time, p_i, useNewton); - } - else - return false; -} - -bool Intersect::intersect_Triangle(const Vector3d& P1, const Vector3d& P2, const Vector3d& P3, - const Vector3d& Q1, const Vector3d& Q2, const Vector3d& Q3, - const Matrix3d& R, const Vector3d& T, - Vector3d* contact_points, - unsigned int* num_contact_points, - FCL_REAL* penetration_depth, - Vector3d* normal) -{ - Vector3d Q1_ = R * Q1 + T; - Vector3d Q2_ = R * Q2 + T; - Vector3d Q3_ = R * Q3 + T; - - return intersect_Triangle(P1, P2, P3, Q1_, Q2_, Q3_, contact_points, num_contact_points, penetration_depth, normal); -} - -bool Intersect::intersect_Triangle(const Vector3d& P1, const Vector3d& P2, const Vector3d& P3, - const Vector3d& Q1, const Vector3d& Q2, const Vector3d& Q3, - const Transform3d& tf, - Vector3d* contact_points, - unsigned int* num_contact_points, - FCL_REAL* penetration_depth, - Vector3d* normal) -{ - Vector3d Q1_ = tf * Q1; - Vector3d Q2_ = tf * Q2; - Vector3d Q3_ = tf * Q3; - - return intersect_Triangle(P1, P2, P3, Q1_, Q2_, Q3_, contact_points, num_contact_points, penetration_depth, normal); -} - - -#if ODE_STYLE -bool Intersect::intersect_Triangle(const Vector3d& P1, const Vector3d& P2, const Vector3d& P3, - const Vector3d& Q1, const Vector3d& Q2, const Vector3d& Q3, - Vector3d* contact_points, - unsigned int* num_contact_points, - FCL_REAL* penetration_depth, - Vector3d* normal) -{ - - - Vector3d n1; - FCL_REAL t1; - bool b1 = buildTrianglePlane(P1, P2, P3, &n1, &t1); - if(!b1) return false; - - Vector3d n2; - FCL_REAL t2; - bool b2 = buildTrianglePlane(Q1, Q2, Q3, &n2, &t2); - if(!b2) return false; - - if(sameSideOfPlane(P1, P2, P3, n2, t2)) - return false; - - if(sameSideOfPlane(Q1, Q2, Q3, n1, t1)) - return false; - - Vector3d clipped_points1[MAX_TRIANGLE_CLIPS]; - unsigned int num_clipped_points1 = 0; - Vector3d clipped_points2[MAX_TRIANGLE_CLIPS]; - unsigned int num_clipped_points2 = 0; - - Vector3d deepest_points1[MAX_TRIANGLE_CLIPS]; - unsigned int num_deepest_points1 = 0; - Vector3d deepest_points2[MAX_TRIANGLE_CLIPS]; - unsigned int num_deepest_points2 = 0; - FCL_REAL penetration_depth1 = -1, penetration_depth2 = -1; - - clipTriangleByTriangleAndEdgePlanes(Q1, Q2, Q3, P1, P2, P3, n1, t1, clipped_points2, &num_clipped_points2); - - if(num_clipped_points2 == 0) - return false; - - computeDeepestPoints(clipped_points2, num_clipped_points2, n1, t1, &penetration_depth2, deepest_points2, &num_deepest_points2); - if(num_deepest_points2 == 0) - return false; - - clipTriangleByTriangleAndEdgePlanes(P1, P2, P3, Q1, Q2, Q3, n2, t2, clipped_points1, &num_clipped_points1); - if(num_clipped_points1 == 0) - return false; - - computeDeepestPoints(clipped_points1, num_clipped_points1, n2, t2, &penetration_depth1, deepest_points1, &num_deepest_points1); - if(num_deepest_points1 == 0) - return false; - - - /// Return contact information - if(contact_points && num_contact_points && penetration_depth && normal) - { - if(penetration_depth1 > penetration_depth2) - { - *num_contact_points = num_deepest_points2; - for(unsigned int i = 0; i < num_deepest_points2; ++i) - { - contact_points[i] = deepest_points2[i]; - } - - *normal = n1; - *penetration_depth = penetration_depth2; - } - else - { - *num_contact_points = num_deepest_points1; - for(unsigned int i = 0; i < num_deepest_points1; ++i) - { - contact_points[i] = deepest_points1[i]; - } - - *normal = -n2; - *penetration_depth = penetration_depth1; - } - } - - return true; -} -#else -bool Intersect::intersect_Triangle(const Vector3d& P1, const Vector3d& P2, const Vector3d& P3, - const Vector3d& Q1, const Vector3d& Q2, const Vector3d& Q3, - Vector3d* contact_points, - unsigned int* num_contact_points, - FCL_REAL* penetration_depth, - Vector3d* normal) -{ - Vector3d p1 = P1 - P1; - Vector3d p2 = P2 - P1; - Vector3d p3 = P3 - P1; - Vector3d q1 = Q1 - P1; - Vector3d q2 = Q2 - P1; - Vector3d q3 = Q3 - P1; - - Vector3d e1 = p2 - p1; - Vector3d e2 = p3 - p2; - Vector3d n1 = e1.cross(e2); - if (!project6(n1, p1, p2, p3, q1, q2, q3)) return false; - - Vector3d f1 = q2 - q1; - Vector3d f2 = q3 - q2; - Vector3d m1 = f1.cross(f2); - if (!project6(m1, p1, p2, p3, q1, q2, q3)) return false; - - Vector3d ef11 = e1.cross(f1); - if (!project6(ef11, p1, p2, p3, q1, q2, q3)) return false; - - Vector3d ef12 = e1.cross(f2); - if (!project6(ef12, p1, p2, p3, q1, q2, q3)) return false; - - Vector3d f3 = q1 - q3; - Vector3d ef13 = e1.cross(f3); - if (!project6(ef13, p1, p2, p3, q1, q2, q3)) return false; - - Vector3d ef21 = e2.cross(f1); - if (!project6(ef21, p1, p2, p3, q1, q2, q3)) return false; - - Vector3d ef22 = e2.cross(f2); - if (!project6(ef22, p1, p2, p3, q1, q2, q3)) return false; - - Vector3d ef23 = e2.cross(f3); - if (!project6(ef23, p1, p2, p3, q1, q2, q3)) return false; - - Vector3d e3 = p1 - p3; - Vector3d ef31 = e3.cross(f1); - if (!project6(ef31, p1, p2, p3, q1, q2, q3)) return false; - - Vector3d ef32 = e3.cross(f2); - if (!project6(ef32, p1, p2, p3, q1, q2, q3)) return false; - - Vector3d ef33 = e3.cross(f3); - if (!project6(ef33, p1, p2, p3, q1, q2, q3)) return false; - - Vector3d g1 = e1.cross(n1); - if (!project6(g1, p1, p2, p3, q1, q2, q3)) return false; - - Vector3d g2 = e2.cross(n1); - if (!project6(g2, p1, p2, p3, q1, q2, q3)) return false; - - Vector3d g3 = e3.cross(n1); - if (!project6(g3, p1, p2, p3, q1, q2, q3)) return false; - - Vector3d h1 = f1.cross(m1); - if (!project6(h1, p1, p2, p3, q1, q2, q3)) return false; - - Vector3d h2 = f2.cross(m1); - if (!project6(h2, p1, p2, p3, q1, q2, q3)) return false; - - Vector3d h3 = f3.cross(m1); - if (!project6(h3, p1, p2, p3, q1, q2, q3)) return false; - - if(contact_points && num_contact_points && penetration_depth && normal) - { - Vector3d n1, n2; - FCL_REAL t1, t2; - buildTrianglePlane(P1, P2, P3, &n1, &t1); - buildTrianglePlane(Q1, Q2, Q3, &n2, &t2); - - Vector3d deepest_points1[3]; - unsigned int num_deepest_points1 = 0; - Vector3d deepest_points2[3]; - unsigned int num_deepest_points2 = 0; - FCL_REAL penetration_depth1, penetration_depth2; - - Vector3d P[3] = {P1, P2, P3}; - Vector3d Q[3] = {Q1, Q2, Q3}; - - computeDeepestPoints(Q, 3, n1, t1, &penetration_depth2, deepest_points2, &num_deepest_points2); - computeDeepestPoints(P, 3, n2, t2, &penetration_depth1, deepest_points1, &num_deepest_points1); - - - if(penetration_depth1 > penetration_depth2) - { - *num_contact_points = std::min(num_deepest_points2, (unsigned int)2); - for(unsigned int i = 0; i < *num_contact_points; ++i) - { - contact_points[i] = deepest_points2[i]; - } - - *normal = n1; - *penetration_depth = penetration_depth2; - } - else - { - *num_contact_points = std::min(num_deepest_points1, (unsigned int)2); - for(unsigned int i = 0; i < *num_contact_points; ++i) - { - contact_points[i] = deepest_points1[i]; - } - - *normal = -n2; - *penetration_depth = penetration_depth1; - } - } - - return true; -} -#endif - - -void Intersect::computeDeepestPoints(Vector3d* clipped_points, unsigned int num_clipped_points, const Vector3d& n, FCL_REAL t, FCL_REAL* penetration_depth, Vector3d* deepest_points, unsigned int* num_deepest_points) -{ - *num_deepest_points = 0; - FCL_REAL max_depth = -std::numeric_limits::max(); - unsigned int num_deepest_points_ = 0; - unsigned int num_neg = 0; - unsigned int num_pos = 0; - unsigned int num_zero = 0; - - for(unsigned int i = 0; i < num_clipped_points; ++i) - { - FCL_REAL dist = -distanceToPlane(n, t, clipped_points[i]); - if(dist > EPSILON) num_pos++; - else if(dist < -EPSILON) num_neg++; - else num_zero++; - if(dist > max_depth) - { - max_depth = dist; - num_deepest_points_ = 1; - deepest_points[num_deepest_points_ - 1] = clipped_points[i]; - } - else if(dist + 1e-6 >= max_depth) - { - num_deepest_points_++; - deepest_points[num_deepest_points_ - 1] = clipped_points[i]; - } - } - - if(max_depth < -EPSILON) - num_deepest_points_ = 0; - - if(num_zero == 0 && ((num_neg == 0) || (num_pos == 0))) - num_deepest_points_ = 0; - - *penetration_depth = max_depth; - *num_deepest_points = num_deepest_points_; -} - -void Intersect::clipTriangleByTriangleAndEdgePlanes(const Vector3d& v1, const Vector3d& v2, const Vector3d& v3, - const Vector3d& t1, const Vector3d& t2, const Vector3d& t3, - const Vector3d& tn, FCL_REAL to, - Vector3d clipped_points[], unsigned int* num_clipped_points, - bool clip_triangle) -{ - *num_clipped_points = 0; - Vector3d temp_clip[MAX_TRIANGLE_CLIPS]; - Vector3d temp_clip2[MAX_TRIANGLE_CLIPS]; - unsigned int num_temp_clip = 0; - unsigned int num_temp_clip2 = 0; - Vector3d v[3] = {v1, v2, v3}; - - Vector3d plane_n; - FCL_REAL plane_dist; - - if(buildEdgePlane(t1, t2, tn, &plane_n, &plane_dist)) - { - clipPolygonByPlane(v, 3, plane_n, plane_dist, temp_clip, &num_temp_clip); - if(num_temp_clip > 0) - { - if(buildEdgePlane(t2, t3, tn, &plane_n, &plane_dist)) - { - clipPolygonByPlane(temp_clip, num_temp_clip, plane_n, plane_dist, temp_clip2, &num_temp_clip2); - if(num_temp_clip2 > 0) - { - if(buildEdgePlane(t3, t1, tn, &plane_n, &plane_dist)) - { - if(clip_triangle) - { - num_temp_clip = 0; - clipPolygonByPlane(temp_clip2, num_temp_clip2, plane_n, plane_dist, temp_clip, &num_temp_clip); - if(num_temp_clip > 0) - { - clipPolygonByPlane(temp_clip, num_temp_clip, tn, to, clipped_points, num_clipped_points); - } - } - else - { - clipPolygonByPlane(temp_clip2, num_temp_clip2, plane_n, plane_dist, clipped_points, num_clipped_points); - } - } - } - } - } - } -} - -void Intersect::clipPolygonByPlane(Vector3d* polygon_points, unsigned int num_polygon_points, const Vector3d& n, FCL_REAL t, Vector3d clipped_points[], unsigned int* num_clipped_points) -{ - *num_clipped_points = 0; - - unsigned int num_clipped_points_ = 0; - unsigned int vi; - unsigned int prev_classify = 2; - unsigned int classify; - for(unsigned int i = 0; i <= num_polygon_points; ++i) - { - vi = (i % num_polygon_points); - FCL_REAL d = distanceToPlane(n, t, polygon_points[i]); - classify = ((d > EPSILON) ? 1 : 0); - if(classify == 0) - { - if(prev_classify == 1) - { - if(num_clipped_points_ < MAX_TRIANGLE_CLIPS) - { - Vector3d tmp; - clipSegmentByPlane(polygon_points[i - 1], polygon_points[vi], n, t, &tmp); - if(num_clipped_points_ > 0) - { - if((tmp - clipped_points[num_clipped_points_ - 1]).squaredNorm() > EPSILON) - { - clipped_points[num_clipped_points_] = tmp; - num_clipped_points_++; - } - } - else - { - clipped_points[num_clipped_points_] = tmp; - num_clipped_points_++; - } - } - } - - if(num_clipped_points_ < MAX_TRIANGLE_CLIPS && i < num_polygon_points) - { - clipped_points[num_clipped_points_] = polygon_points[vi]; - num_clipped_points_++; - } - } - else - { - if(prev_classify == 0) - { - if(num_clipped_points_ < MAX_TRIANGLE_CLIPS) - { - Vector3d tmp; - clipSegmentByPlane(polygon_points[i - 1], polygon_points[vi], n, t, &tmp); - if(num_clipped_points_ > 0) - { - if((tmp - clipped_points[num_clipped_points_ - 1]).squaredNorm() > EPSILON) - { - clipped_points[num_clipped_points_] = tmp; - num_clipped_points_++; - } - } - else - { - clipped_points[num_clipped_points_] = tmp; - num_clipped_points_++; - } - } - } - } - - prev_classify = classify; - } - - if(num_clipped_points_ > 2) - { - if((clipped_points[0] - clipped_points[num_clipped_points_ - 1]).squaredNorm() < EPSILON) - { - num_clipped_points_--; - } - } - - *num_clipped_points = num_clipped_points_; -} - -void Intersect::clipSegmentByPlane(const Vector3d& v1, const Vector3d& v2, const Vector3d& n, FCL_REAL t, Vector3d* clipped_point) -{ - FCL_REAL dist1 = distanceToPlane(n, t, v1); - Vector3d tmp = v2 - v1; - FCL_REAL dist2 = tmp.dot(n); - *clipped_point = tmp * (-dist1 / dist2) + v1; -} - -FCL_REAL Intersect::distanceToPlane(const Vector3d& n, FCL_REAL t, const Vector3d& v) -{ - return n.dot(v) - t; -} - -bool Intersect::buildTrianglePlane(const Vector3d& v1, const Vector3d& v2, const Vector3d& v3, Vector3d* n, FCL_REAL* t) -{ - Vector3d n_ = (v2 - v1).cross(v3 - v1); - bool can_normalize = false; - normalize(n_, &can_normalize); - if(can_normalize) - { - *n = n_; - *t = n_.dot(v1); - return true; - } - - return false; -} - -bool Intersect::buildEdgePlane(const Vector3d& v1, const Vector3d& v2, const Vector3d& tn, Vector3d* n, FCL_REAL* t) -{ - Vector3d n_ = (v2 - v1).cross(tn); - bool can_normalize = false; - normalize(n_, &can_normalize); - if(can_normalize) - { - *n = n_; - *t = n_.dot(v1); - return true; - } - - return false; -} - -bool Intersect::sameSideOfPlane(const Vector3d& v1, const Vector3d& v2, const Vector3d& v3, const Vector3d& n, FCL_REAL t) -{ - FCL_REAL dist1 = distanceToPlane(n, t, v1); - FCL_REAL dist2 = dist1 * distanceToPlane(n, t, v2); - FCL_REAL dist3 = dist1 * distanceToPlane(n, t, v3); - if((dist2 > 0) && (dist3 > 0)) - return true; - return false; -} - -int Intersect::project6(const Vector3d& ax, - const Vector3d& p1, const Vector3d& p2, const Vector3d& p3, - const Vector3d& q1, const Vector3d& q2, const Vector3d& q3) -{ - FCL_REAL P1 = ax.dot(p1); - FCL_REAL P2 = ax.dot(p2); - FCL_REAL P3 = ax.dot(p3); - FCL_REAL Q1 = ax.dot(q1); - FCL_REAL Q2 = ax.dot(q2); - FCL_REAL Q3 = ax.dot(q3); - - FCL_REAL mn1 = std::min(P1, std::min(P2, P3)); - FCL_REAL mx2 = std::max(Q1, std::max(Q2, Q3)); - if(mn1 > mx2) return 0; - - FCL_REAL mx1 = std::max(P1, std::max(P2, P3)); - FCL_REAL mn2 = std::min(Q1, std::min(Q2, Q3)); - - if(mn2 > mx1) return 0; - return 1; -} - - - -void TriangleDistance::segPoints(const Vector3d& P, const Vector3d& A, const Vector3d& Q, const Vector3d& B, - Vector3d& VEC, Vector3d& X, Vector3d& Y) -{ - Vector3d T; - FCL_REAL A_dot_A, B_dot_B, A_dot_B, A_dot_T, B_dot_T; - Vector3d TMP; - - T = Q - P; - A_dot_A = A.dot(A); - B_dot_B = B.dot(B); - A_dot_B = A.dot(B); - A_dot_T = A.dot(T); - B_dot_T = B.dot(T); - - // t parameterizes ray P,A - // u parameterizes ray Q,B - - FCL_REAL t, u; - - // compute t for the closest point on ray P,A to - // ray Q,B - - FCL_REAL denom = A_dot_A*B_dot_B - A_dot_B*A_dot_B; - - t = (A_dot_T*B_dot_B - B_dot_T*A_dot_B) / denom; - - // clamp result so t is on the segment P,A - - if((t < 0) || std::isnan(t)) t = 0; else if(t > 1) t = 1; - - // find u for point on ray Q,B closest to point at t - - u = (t*A_dot_B - B_dot_T) / B_dot_B; - - // if u is on segment Q,B, t and u correspond to - // closest points, otherwise, clamp u, recompute and - // clamp t - - if((u <= 0) || std::isnan(u)) - { - Y = Q; - - t = A_dot_T / A_dot_A; - - if((t <= 0) || std::isnan(t)) - { - X = P; - VEC = Q - P; - } - else if(t >= 1) - { - X = P + A; - VEC = Q - X; - } - else - { - X = P + A * t; - TMP = T.cross(A); - VEC = A.cross(TMP); - } - } - else if (u >= 1) - { - Y = Q + B; - - t = (A_dot_B + A_dot_T) / A_dot_A; - - if((t <= 0) || std::isnan(t)) - { - X = P; - VEC = Y - P; - } - else if(t >= 1) - { - X = P + A; - VEC = Y - X; - } - else - { - X = P + A * t; - T = Y - P; - TMP = T.cross(A); - VEC= A.cross(TMP); - } - } - else - { - Y = Q + B * u; - - if((t <= 0) || std::isnan(t)) - { - X = P; - TMP = T.cross(B); - VEC = B.cross(TMP); - } - else if(t >= 1) - { - X = P + A; - T = Q - X; - TMP = T.cross(B); - VEC = B.cross(TMP); - } - else - { - X = P + A * t; - VEC = A.cross(B); - if(VEC.dot(T) < 0) - { - VEC = VEC * (-1); - } - } - } -} - - -FCL_REAL TriangleDistance::triDistance(const Vector3d S[3], const Vector3d T[3], Vector3d& P, Vector3d& Q) -{ - // Compute vectors along the 6 sides - - Vector3d Sv[3]; - Vector3d Tv[3]; - Vector3d VEC; - - Sv[0] = S[1] - S[0]; - Sv[1] = S[2] - S[1]; - Sv[2] = S[0] - S[2]; - - Tv[0] = T[1] - T[0]; - Tv[1] = T[2] - T[1]; - Tv[2] = T[0] - T[2]; - - // For each edge pair, the vector connecting the closest points - // of the edges defines a slab (parallel planes at head and tail - // enclose the slab). If we can show that the off-edge vertex of - // each triangle is outside of the slab, then the closest points - // of the edges are the closest points for the triangles. - // Even if these tests fail, it may be helpful to know the closest - // points found, and whether the triangles were shown disjoint - - Vector3d V; - Vector3d Z; - Vector3d minP = Vector3d::Zero(); - Vector3d minQ = Vector3d::Zero(); - FCL_REAL mindd; - int shown_disjoint = 0; - - mindd = (S[0] - T[0]).squaredNorm() + 1; // Set first minimum safely high - - for(int i = 0; i < 3; ++i) - { - for(int j = 0; j < 3; ++j) - { - // Find closest points on edges i & j, plus the - // vector (and distance squared) between these points - segPoints(S[i], Sv[i], T[j], Tv[j], VEC, P, Q); - - V = Q - P; - FCL_REAL dd = V.dot(V); - - // Verify this closest point pair only if the distance - // squared is less than the minimum found thus far. - - if(dd <= mindd) - { - minP = P; - minQ = Q; - mindd = dd; - - Z = S[(i+2)%3] - P; - FCL_REAL a = Z.dot(VEC); - Z = T[(j+2)%3] - Q; - FCL_REAL b = Z.dot(VEC); - - if((a <= 0) && (b >= 0)) return sqrt(dd); - - FCL_REAL p = V.dot(VEC); - - if(a < 0) a = 0; - if(b > 0) b = 0; - if((p - a + b) > 0) shown_disjoint = 1; - } - } - } - - // No edge pairs contained the closest points. - // either: - // 1. one of the closest points is a vertex, and the - // other point is interior to a face. - // 2. the triangles are overlapping. - // 3. an edge of one triangle is parallel to the other's face. If - // cases 1 and 2 are not true, then the closest points from the 9 - // edge pairs checks above can be taken as closest points for the - // triangles. - // 4. possibly, the triangles were degenerate. When the - // triangle points are nearly colinear or coincident, one - // of above tests might fail even though the edges tested - // contain the closest points. - - // First check for case 1 - - Vector3d Sn; - FCL_REAL Snl; - - Sn = Sv[0].cross(Sv[1]); // Compute normal to S triangle - Snl = Sn.dot(Sn); // Compute square of length of normal - - // If cross product is long enough, - - if(Snl > 1e-15) - { - // Get projection lengths of T points - - Vector3d Tp; - - V = S[0] - T[0]; - Tp[0] = V.dot(Sn); - - V = S[0] - T[1]; - Tp[1] = V.dot(Sn); - - V = S[0] - T[2]; - Tp[2] = V.dot(Sn); - - // If Sn is a separating direction, - // find point with smallest projection - - int point = -1; - if((Tp[0] > 0) && (Tp[1] > 0) && (Tp[2] > 0)) - { - if(Tp[0] < Tp[1]) point = 0; else point = 1; - if(Tp[2] < Tp[point]) point = 2; - } - else if((Tp[0] < 0) && (Tp[1] < 0) && (Tp[2] < 0)) - { - if(Tp[0] > Tp[1]) point = 0; else point = 1; - if(Tp[2] > Tp[point]) point = 2; - } - - // If Sn is a separating direction, - - if(point >= 0) - { - shown_disjoint = 1; - - // Test whether the point found, when projected onto the - // other triangle, lies within the face. - - V = T[point] - S[0]; - Z = Sn.cross(Sv[0]); - if(V.dot(Z) > 0) - { - V = T[point] - S[1]; - Z = Sn.cross(Sv[1]); - if(V.dot(Z) > 0) - { - V = T[point] - S[2]; - Z = Sn.cross(Sv[2]); - if(V.dot(Z) > 0) - { - // T[point] passed the test - it's a closest point for - // the T triangle; the other point is on the face of S - P = T[point] + Sn * (Tp[point] / Snl); - Q = T[point]; - return (P - Q).norm(); - } - } - } - } - } - - Vector3d Tn; - FCL_REAL Tnl; - - Tn = Tv[0].cross(Tv[1]); - Tnl = Tn.dot(Tn); - - if(Tnl > 1e-15) - { - Vector3d Sp; - - V = T[0] - S[0]; - Sp[0] = V.dot(Tn); - - V = T[0] - S[1]; - Sp[1] = V.dot(Tn); - - V = T[0] - S[2]; - Sp[2] = V.dot(Tn); - - int point = -1; - if((Sp[0] > 0) && (Sp[1] > 0) && (Sp[2] > 0)) - { - if(Sp[0] < Sp[1]) point = 0; else point = 1; - if(Sp[2] < Sp[point]) point = 2; - } - else if((Sp[0] < 0) && (Sp[1] < 0) && (Sp[2] < 0)) - { - if(Sp[0] > Sp[1]) point = 0; else point = 1; - if(Sp[2] > Sp[point]) point = 2; - } - - if(point >= 0) - { - shown_disjoint = 1; - - V = S[point] - T[0]; - Z = Tn.cross(Tv[0]); - if(V.dot(Z) > 0) - { - V = S[point] - T[1]; - Z = Tn.cross(Tv[1]); - if(V.dot(Z) > 0) - { - V = S[point] - T[2]; - Z = Tn.cross(Tv[2]); - if(V.dot(Z) > 0) - { - P = S[point]; - Q = S[point] + Tn * (Sp[point] / Tnl); - return (P - Q).norm(); - } - } - } - } - } - - // Case 1 can't be shown. - // If one of these tests showed the triangles disjoint, - // we assume case 3 or 4, otherwise we conclude case 2, - // that the triangles overlap. - - if(shown_disjoint) - { - P = minP; - Q = minQ; - return sqrt(mindd); - } - else return 0; -} - - -FCL_REAL TriangleDistance::triDistance(const Vector3d& S1, const Vector3d& S2, const Vector3d& S3, - const Vector3d& T1, const Vector3d& T2, const Vector3d& T3, - Vector3d& P, Vector3d& Q) -{ - Vector3d S[3]; - Vector3d T[3]; - S[0] = S1; S[1] = S2; S[2] = S3; - T[0] = T1; T[1] = T2; T[2] = T3; - - return triDistance(S, T, P, Q); -} - -FCL_REAL TriangleDistance::triDistance(const Vector3d S[3], const Vector3d T[3], - const Matrix3d& R, const Vector3d& Tl, - Vector3d& P, Vector3d& Q) -{ - Vector3d T_transformed[3]; - T_transformed[0] = R * T[0] + Tl; - T_transformed[1] = R * T[1] + Tl; - T_transformed[2] = R * T[2] + Tl; - - return triDistance(S, T_transformed, P, Q); -} - - -FCL_REAL TriangleDistance::triDistance(const Vector3d S[3], const Vector3d T[3], - const Transform3d& tf, - Vector3d& P, Vector3d& Q) -{ - Vector3d T_transformed[3]; - T_transformed[0] = tf * T[0]; - T_transformed[1] = tf * T[1]; - T_transformed[2] = tf * T[2]; - - return triDistance(S, T_transformed, P, Q); -} - - -FCL_REAL TriangleDistance::triDistance(const Vector3d& S1, const Vector3d& S2, const Vector3d& S3, - const Vector3d& T1, const Vector3d& T2, const Vector3d& T3, - const Matrix3d& R, const Vector3d& Tl, - Vector3d& P, Vector3d& Q) -{ - Vector3d T1_transformed = R * T1 + Tl; - Vector3d T2_transformed = R * T2 + Tl; - Vector3d T3_transformed = R * T3 + Tl; - return triDistance(S1, S2, S3, T1_transformed, T2_transformed, T3_transformed, P, Q); -} - -FCL_REAL TriangleDistance::triDistance(const Vector3d& S1, const Vector3d& S2, const Vector3d& S3, - const Vector3d& T1, const Vector3d& T2, const Vector3d& T3, - const Transform3d& tf, - Vector3d& P, Vector3d& Q) -{ - Vector3d T1_transformed = tf * T1; - Vector3d T2_transformed = tf * T2; - Vector3d T3_transformed = tf * T3; - return triDistance(S1, S2, S3, T1_transformed, T2_transformed, T3_transformed, P, Q); -} - - - - - -Project::ProjectResult Project::projectLine(const Vector3d& a, const Vector3d& b, const Vector3d& p) -{ - ProjectResult res; - - const Vector3d d = b - a; - const FCL_REAL l = d.squaredNorm(); - - if(l > 0) - { - const FCL_REAL t = (p - a).dot(d); - res.parameterization[1] = (t >= l) ? 1 : ((t <= 0) ? 0 : (t / l)); - res.parameterization[0] = 1 - res.parameterization[1]; - if(t >= l) { res.sqr_distance = (p - b).squaredNorm(); res.encode = 2; /* 0x10 */ } - else if(t <= 0) { res.sqr_distance = (p - a).squaredNorm(); res.encode = 1; /* 0x01 */ } - else { res.sqr_distance = (a + d * res.parameterization[1] - p).squaredNorm(); res.encode = 3; /* 0x00 */ } - } - - return res; -} - -Project::ProjectResult Project::projectTriangle(const Vector3d& a, const Vector3d& b, const Vector3d& c, const Vector3d& p) -{ - ProjectResult res; - - static const size_t nexti[3] = {1, 2, 0}; - const Vector3d* vt[] = {&a, &b, &c}; - const Vector3d dl[] = {a - b, b - c, c - a}; - const Vector3d& n = dl[0].cross(dl[1]); - const FCL_REAL l = n.squaredNorm(); - - if(l > 0) - { - FCL_REAL mindist = -1; - for(size_t i = 0; i < 3; ++i) - { - if((*vt[i] - p).dot(dl[i].cross(n)) > 0) // origin is to the outside part of the triangle edge, then the optimal can only be on the edge - { - size_t j = nexti[i]; - ProjectResult res_line = projectLine(*vt[i], *vt[j], p); - - if(mindist < 0 || res_line.sqr_distance < mindist) - { - mindist = res_line.sqr_distance; - res.encode = static_cast(((res_line.encode&1)?1< 0) // abs(vl) == 0, the tetrahedron is degenerated; if ng is false, then the last vertex in the tetrahedron does not grow toward the origin (in fact origin is on the other side of the abc face) - { - FCL_REAL mindist = -1; - - for(size_t i = 0; i < 3; ++i) - { - size_t j = nexti[i]; - FCL_REAL s = vl * (d-p).dot(dl[i].cross(dl[j])); - if(s > 0) // the origin is to the outside part of a triangle face, then the optimal can only be on the triangle face - { - ProjectResult res_triangle = projectTriangle(*vt[i], *vt[j], d, p); - if(mindist < 0 || res_triangle.sqr_distance < mindist) - { - mindist = res_triangle.sqr_distance; - res.encode = static_cast( (res_triangle.encode&1?1< 0) - { - const FCL_REAL t = - a.dot(d); - res.parameterization[1] = (t >= l) ? 1 : ((t <= 0) ? 0 : (t / l)); - res.parameterization[0] = 1 - res.parameterization[1]; - if(t >= l) { res.sqr_distance = b.squaredNorm(); res.encode = 2; /* 0x10 */ } - else if(t <= 0) { res.sqr_distance = a.squaredNorm(); res.encode = 1; /* 0x01 */ } - else { res.sqr_distance = (a + d * res.parameterization[1]).squaredNorm(); res.encode = 3; /* 0x00 */ } - } - - return res; -} - -Project::ProjectResult Project::projectTriangleOrigin(const Vector3d& a, const Vector3d& b, const Vector3d& c) -{ - ProjectResult res; - - static const size_t nexti[3] = {1, 2, 0}; - const Vector3d* vt[] = {&a, &b, &c}; - const Vector3d dl[] = {a - b, b - c, c - a}; - const Vector3d& n = dl[0].cross(dl[1]); - const FCL_REAL l = n.squaredNorm(); - - if(l > 0) - { - FCL_REAL mindist = -1; - for(size_t i = 0; i < 3; ++i) - { - if(vt[i]->dot(dl[i].cross(n)) > 0) // origin is to the outside part of the triangle edge, then the optimal can only be on the edge - { - size_t j = nexti[i]; - ProjectResult res_line = projectLineOrigin(*vt[i], *vt[j]); - - if(mindist < 0 || res_line.sqr_distance < mindist) - { - mindist = res_line.sqr_distance; - res.encode = static_cast(((res_line.encode&1)?1< 0) // abs(vl) == 0, the tetrahedron is degenerated; if ng is false, then the last vertex in the tetrahedron does not grow toward the origin (in fact origin is on the other side of the abc face) - { - FCL_REAL mindist = -1; - - for(size_t i = 0; i < 3; ++i) - { - size_t j = nexti[i]; - FCL_REAL s = vl * d.dot(dl[i].cross(dl[j])); - if(s > 0) // the origin is to the outside part of a triangle face, then the optimal can only be on the triangle face - { - ProjectResult res_triangle = projectTriangleOrigin(*vt[i], *vt[j], d); - if(mindist < 0 || res_triangle.sqr_distance < mindist) - { - mindist = res_triangle.sqr_distance; - res.encode = static_cast( (res_triangle.encode&1?1< -#include -#include - -namespace fcl -{ - -/// The seed the user asked for (cannot be 0) -static std::uint_fast32_t userSetSeed = 0; - -/// Flag indicating whether the first seed has already been generated or not -static bool firstSeedGenerated = false; - -/// The value of the first seed -static std::uint_fast32_t firstSeedValue = 0; - -/// Compute the first seed to be used; this function should be called only once -static std::uint_fast32_t firstSeed() -{ - static std::mutex fsLock; - std::unique_lock slock(fsLock); - - if(firstSeedGenerated) - return firstSeedValue; - - if(userSetSeed != 0) - firstSeedValue = userSetSeed; - else firstSeedValue = (std::uint_fast32_t)std::chrono::duration_cast( - std::chrono::system_clock::now() - std::chrono::system_clock::time_point()).count(); - firstSeedGenerated = true; - - return firstSeedValue; -} - -/// We use a different random number generator for the seeds of the -/// Other random generators. The root seed is from the number of -/// nano-seconds in the current time. -static std::uint_fast32_t nextSeed() -{ - static std::mutex rngMutex; - std::unique_lock slock(rngMutex); - static std::ranlux24_base sGen; - static std::uniform_int_distribution<> sDist(1, 1000000000); - return sDist(sGen); -} - -std::uint_fast32_t RNG::getSeed() -{ - return firstSeed(); -} - -void RNG::setSeed(std::uint_fast32_t seed) -{ - if(firstSeedGenerated) - { - std::cerr << "Random number generation already started. Changing seed now will not lead to deterministic sampling." << std::endl; - } - if(seed == 0) - { - std::cerr << "Random generator seed cannot be 0. Using 1 instead." << std::endl; - userSetSeed = 1; - } - else - userSetSeed = seed; -} - -RNG::RNG() : generator_(nextSeed()), - uniDist_(0, 1), - normalDist_(0, 1) -{ -} - -double RNG::halfNormalReal(double r_min, double r_max, double focus) -{ - assert(r_min <= r_max); - - const double mean = r_max - r_min; - double v = gaussian(mean, mean / focus); - - if(v > mean) v = 2.0 * mean - v; - double r = v >= 0.0 ? v + r_min : r_min; - return r > r_max ? r_max : r; -} - -int RNG::halfNormalInt(int r_min, int r_max, double focus) -{ - int r = (int)floor(halfNormalReal((double)r_min, (double)(r_max) + 1.0, focus)); - return (r > r_max) ? r_max : r; -} - -// From: "Uniform Random Rotations", Ken Shoemake, Graphics Gems III, -// pg. 124-132 -void RNG::quaternion(double value[4]) -{ - double x0 = uniDist_(generator_); - double r1 = sqrt(1.0 - x0), r2 = sqrt(x0); - double t1 = 2.0 * constants::pi * uniDist_(generator_), t2 = 2.0 * constants::pi * uniDist_(generator_); - double c1 = cos(t1), s1 = sin(t1); - double c2 = cos(t2), s2 = sin(t2); - value[0] = s1 * r1; - value[1] = c1 * r1; - value[2] = s2 * r2; - value[3] = c2 * r2; -} - -// From Effective Sampling and Distance Metrics for 3D Rigid Body Path Planning, by James Kuffner, ICRA 2004 -void RNG::eulerRPY(double value[3]) -{ - value[0] = constants::pi * (2.0 * uniDist_(generator_) - 1.0); - value[1] = acos(1.0 - 2.0 * uniDist_(generator_)) - constants::pi / 2.0; - value[2] = constants::pi * (2.0 * uniDist_(generator_) - 1.0); -} - -void RNG::disk(double r_min, double r_max, double& x, double& y) -{ - double a = uniform01(); - double b = uniform01(); - double r = std::sqrt(a * r_max * r_max + (1 - a) * r_min * r_min); - double theta = 2 * constants::pi * b; - x = r * std::cos(theta); - y = r * std::sin(theta); -} - -void RNG::ball(double r_min, double r_max, double& x, double& y, double& z) -{ - double a = uniform01(); - double b = uniform01(); - double c = uniform01(); - double r = std::pow(a * r_max * r_max * r_max + (1 - a) * r_min * r_min * r_min, 1 / 3.0); - double theta = std::acos(1 - 2 * b); - double phi = 2 * constants::pi * c; - - double costheta = std::cos(theta); - double sintheta = std::sin(theta); - double cosphi = std::cos(phi); - double sinphi = std::sin(phi); - x = r * costheta; - y = r * sintheta * cosphi; - z = r * sintheta * sinphi; -} - -} diff --git a/src/narrowphase/gjk.cpp b/src/narrowphase/gjk.cpp deleted file mode 100644 index 0f63d1670..000000000 --- a/src/narrowphase/gjk.cpp +++ /dev/null @@ -1,679 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#include "fcl/math/geometry.h" -#include "fcl/narrowphase/gjk.h" -#include "fcl/intersect.h" - -namespace fcl -{ - -namespace details -{ - -Vector3d getSupport(const ShapeBase* shape, const Vector3d& dir) -{ - switch(shape->getNodeType()) - { - case GEOM_TRIANGLE: - { - const TriangleP* triangle = static_cast(shape); - FCL_REAL dota = dir.dot(triangle->a); - FCL_REAL dotb = dir.dot(triangle->b); - FCL_REAL dotc = dir.dot(triangle->c); - if(dota > dotb) - { - if(dotc > dota) - return triangle->c; - else - return triangle->a; - } - else - { - if(dotc > dotb) - return triangle->c; - else - return triangle->b; - } - } - break; - case GEOM_BOX: - { - const Box* box = static_cast(shape); - return Vector3d((dir[0]>0)?(box->side[0]/2):(-box->side[0]/2), - (dir[1]>0)?(box->side[1]/2):(-box->side[1]/2), - (dir[2]>0)?(box->side[2]/2):(-box->side[2]/2)); - } - break; - case GEOM_SPHERE: - { - const Sphere* sphere = static_cast(shape); - return dir * sphere->radius; - } - break; - case GEOM_ELLIPSOID: - { - const Ellipsoid* ellipsoid = static_cast(shape); - - const FCL_REAL a2 = ellipsoid->radii[0] * ellipsoid->radii[0]; - const FCL_REAL b2 = ellipsoid->radii[1] * ellipsoid->radii[1]; - const FCL_REAL c2 = ellipsoid->radii[2] * ellipsoid->radii[2]; - - const Vector3d v(a2 * dir[0], b2 * dir[1], c2 * dir[2]); - const FCL_REAL d = std::sqrt(v.dot(dir)); - - return v / d; - } - break; - case GEOM_CAPSULE: - { - const Capsule* capsule = static_cast(shape); - FCL_REAL half_h = capsule->lz * 0.5; - Vector3d pos1(0, 0, half_h); - Vector3d pos2(0, 0, -half_h); - Vector3d v = dir * capsule->radius; - pos1 += v; - pos2 += v; - if(dir.dot(pos1) > dir.dot(pos2)) - return pos1; - else return pos2; - } - break; - case GEOM_CONE: - { - const Cone* cone = static_cast(shape); - FCL_REAL zdist = dir[0] * dir[0] + dir[1] * dir[1]; - FCL_REAL len = zdist + dir[2] * dir[2]; - zdist = std::sqrt(zdist); - len = std::sqrt(len); - FCL_REAL half_h = cone->lz * 0.5; - FCL_REAL radius = cone->radius; - - FCL_REAL sin_a = radius / std::sqrt(radius * radius + 4 * half_h * half_h); - - if(dir[2] > len * sin_a) - return Vector3d(0, 0, half_h); - else if(zdist > 0) - { - FCL_REAL rad = radius / zdist; - return Vector3d(rad * dir[0], rad * dir[1], -half_h); - } - else - return Vector3d(0, 0, -half_h); - } - break; - case GEOM_CYLINDER: - { - const Cylinder* cylinder = static_cast(shape); - FCL_REAL zdist = std::sqrt(dir[0] * dir[0] + dir[1] * dir[1]); - FCL_REAL half_h = cylinder->lz * 0.5; - if(zdist == 0.0) - { - return Vector3d(0, 0, (dir[2]>0)? half_h:-half_h); - } - else - { - FCL_REAL d = cylinder->radius / zdist; - return Vector3d(d * dir[0], d * dir[1], (dir[2]>0)?half_h:-half_h); - } - } - break; - case GEOM_CONVEX: - { - const Convex* convex = static_cast(shape); - FCL_REAL maxdot = - std::numeric_limits::max(); - Vector3d* curp = convex->points; - Vector3d bestv = Vector3d::Zero(); - for(int i = 0; i < convex->num_points; ++i, curp+=1) - { - FCL_REAL dot = dir.dot(*curp); - if(dot > maxdot) - { - bestv = *curp; - maxdot = dot; - } - } - return bestv; - } - break; - case GEOM_PLANE: - break; - default: - ; // nothing - } - - return Vector3d::Zero(); -} - -void GJK::initialize() -{ - ray.setZero(); - nfree = 0; - status = Failed; - current = 0; - distance = 0.0; - simplex = NULL; -} - - -Vector3d GJK::getGuessFromSimplex() const -{ - return ray; -} - - -GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vector3d& guess) -{ - size_t iterations = 0; - FCL_REAL alpha = 0; - Vector3d lastw[4]; - size_t clastw = 0; - - free_v[0] = &store_v[0]; - free_v[1] = &store_v[1]; - free_v[2] = &store_v[2]; - free_v[3] = &store_v[3]; - - nfree = 4; - current = 0; - status = Valid; - shape = shape_; - distance = 0.0; - simplices[0].rank = 0; - ray = guess; - - appendVertex(simplices[0], (ray.squaredNorm() > 0) ? (-ray).eval() : Vector3d::UnitX()); - simplices[0].p[0] = 1; - ray = simplices[0].c[0]->w; - lastw[0] = lastw[1] = lastw[2] = lastw[3] = ray; // cache previous support points, the new support point will compare with it to avoid too close support points - - do - { - size_t next = 1 - current; - Simplex& curr_simplex = simplices[current]; - Simplex& next_simplex = simplices[next]; - - // check A: when origin is near the existing simplex, stop - FCL_REAL rl = ray.norm(); - if(rl < tolerance) // mean origin is near the face of original simplex, return touch - { - status = Inside; - break; - } - - appendVertex(curr_simplex, -ray); // see below, ray points away from origin - - // check B: when the new support point is close to previous support points, stop (as the new simplex is degenerated) - Vector3d& w = curr_simplex.c[curr_simplex.rank - 1]->w; - bool found = false; - for(size_t i = 0; i < 4; ++i) - { - if((w - lastw[i]).squaredNorm() < tolerance) - { - found = true; break; - } - } - - if(found) - { - removeVertex(simplices[current]); - break; - } - else - { - lastw[clastw = (clastw+1)&3] = w; - } - - // check C: when the new support point is close to the sub-simplex where the ray point lies, stop (as the new simplex again is degenerated) - FCL_REAL omega = ray.dot(w) / rl; - alpha = std::max(alpha, omega); - if((rl - alpha) - tolerance * rl <= 0) - { - removeVertex(simplices[current]); - break; - } - - Project::ProjectResult project_res; - switch(curr_simplex.rank) - { - case 2: - project_res = Project::projectLineOrigin(curr_simplex.c[0]->w, curr_simplex.c[1]->w); break; - case 3: - project_res = Project::projectTriangleOrigin(curr_simplex.c[0]->w, curr_simplex.c[1]->w, curr_simplex.c[2]->w); break; - case 4: - project_res = Project::projectTetrahedraOrigin(curr_simplex.c[0]->w, curr_simplex.c[1]->w, curr_simplex.c[2]->w, curr_simplex.c[3]->w); break; - } - - if(project_res.sqr_distance >= 0) - { - next_simplex.rank = 0; - ray.setZero(); - current = next; - for(size_t i = 0; i < curr_simplex.rank; ++i) - { - if(project_res.encode & (1 << i)) - { - next_simplex.c[next_simplex.rank] = curr_simplex.c[i]; - next_simplex.p[next_simplex.rank++] = project_res.parameterization[i]; // weights[i]; - ray += curr_simplex.c[i]->w * project_res.parameterization[i]; // weights[i]; - } - else - free_v[nfree++] = curr_simplex.c[i]; - } - if(project_res.encode == 15) status = Inside; // the origin is within the 4-simplex, collision - } - else - { - removeVertex(simplices[current]); - break; - } - - status = ((++iterations) < max_iterations) ? status : Failed; - - } while(status == Valid); - - simplex = &simplices[current]; - switch(status) - { - case Valid: distance = ray.norm(); break; - case Inside: distance = 0; break; - default: break; - } - return status; -} - -void GJK::getSupport(const Vector3d& d, SimplexV& sv) const -{ - sv.d = d.normalized(); - sv.w = shape.support(sv.d); -} - -void GJK::getSupport(const Vector3d& d, const Vector3d& v, SimplexV& sv) const -{ - sv.d = d.normalized(); - sv.w = shape.support(sv.d, v); -} - -void GJK::removeVertex(Simplex& simplex) -{ - free_v[nfree++] = simplex.c[--simplex.rank]; -} - -void GJK::appendVertex(Simplex& simplex, const Vector3d& v) -{ - simplex.p[simplex.rank] = 0; // initial weight 0 - simplex.c[simplex.rank] = free_v[--nfree]; // set the memory - getSupport(v, *simplex.c[simplex.rank++]); -} - -bool GJK::encloseOrigin() -{ - switch(simplex->rank) - { - case 1: - { - for(size_t i = 0; i < 3; ++i) - { - Vector3d axis; - axis[i] = 1; - appendVertex(*simplex, axis); - if(encloseOrigin()) return true; - removeVertex(*simplex); - appendVertex(*simplex, -axis); - if(encloseOrigin()) return true; - removeVertex(*simplex); - } - } - break; - case 2: - { - Vector3d d = simplex->c[1]->w - simplex->c[0]->w; - for(size_t i = 0; i < 3; ++i) - { - Vector3d axis; - axis[i] = 1; - Vector3d p = d.cross(axis); - if(p.squaredNorm() > 0) - { - appendVertex(*simplex, p); - if(encloseOrigin()) return true; - removeVertex(*simplex); - appendVertex(*simplex, -p); - if(encloseOrigin()) return true; - removeVertex(*simplex); - } - } - } - break; - case 3: - { - Vector3d n = (simplex->c[1]->w - simplex->c[0]->w).cross(simplex->c[2]->w - simplex->c[0]->w); - if(n.squaredNorm() > 0) - { - appendVertex(*simplex, n); - if(encloseOrigin()) return true; - removeVertex(*simplex); - appendVertex(*simplex, -n); - if(encloseOrigin()) return true; - removeVertex(*simplex); - } - } - break; - case 4: - { - if(std::abs(triple(simplex->c[0]->w - simplex->c[3]->w, simplex->c[1]->w - simplex->c[3]->w, simplex->c[2]->w - simplex->c[3]->w)) > 0) - return true; - } - break; - } - - return false; -} - - -void EPA::initialize() -{ - sv_store = new SimplexV[max_vertex_num]; - fc_store = new SimplexF[max_face_num]; - status = Failed; - normal = Vector3d(0, 0, 0); - depth = 0; - nextsv = 0; - for(size_t i = 0; i < max_face_num; ++i) - stock.append(&fc_store[max_face_num-i-1]); -} - -bool EPA::getEdgeDist(SimplexF* face, SimplexV* a, SimplexV* b, FCL_REAL& dist) -{ - Vector3d ba = b->w - a->w; - Vector3d n_ab = ba.cross(face->n); - FCL_REAL a_dot_nab = a->w.dot(n_ab); - - if(a_dot_nab < 0) // the origin is on the outside part of ab - { - // following is similar to projectOrigin for two points - // however, as we dont need to compute the parameterization, dont need to compute 0 or 1 - FCL_REAL a_dot_ba = a->w.dot(ba); - FCL_REAL b_dot_ba = b->w.dot(ba); - - if(a_dot_ba > 0) - dist = a->w.norm(); - else if(b_dot_ba < 0) - dist = b->w.norm(); - else - { - FCL_REAL a_dot_b = a->w.dot(b->w); - dist = std::sqrt(std::max(a->w.squaredNorm() * b->w.squaredNorm() - a_dot_b * a_dot_b, (FCL_REAL)0)); - } - - return true; - } - - return false; -} - -EPA::SimplexF* EPA::newFace(SimplexV* a, SimplexV* b, SimplexV* c, bool forced) -{ - if(stock.root) - { - SimplexF* face = stock.root; - stock.remove(face); - hull.append(face); - face->pass = 0; - face->c[0] = a; - face->c[1] = b; - face->c[2] = c; - face->n = (b->w - a->w).cross(c->w - a->w); - FCL_REAL l = face->n.norm(); - - if(l > tolerance) - { - if(!(getEdgeDist(face, a, b, face->d) || - getEdgeDist(face, b, c, face->d) || - getEdgeDist(face, c, a, face->d))) - { - face->d = a->w.dot(face->n) / l; - } - - face->n /= l; - if(forced || face->d >= -tolerance) - return face; - else - status = NonConvex; - } - else - status = Degenerated; - - hull.remove(face); - stock.append(face); - return NULL; - } - - status = stock.root ? OutOfVertices : OutOfFaces; - return NULL; -} - -/** \brief Find the best polytope face to split */ -EPA::SimplexF* EPA::findBest() -{ - SimplexF* minf = hull.root; - FCL_REAL mind = minf->d * minf->d; - for(SimplexF* f = minf->l[1]; f; f = f->l[1]) - { - FCL_REAL sqd = f->d * f->d; - if(sqd < mind) - { - minf = f; - mind = sqd; - } - } - return minf; -} - -EPA::Status EPA::evaluate(GJK& gjk, const Vector3d& guess) -{ - GJK::Simplex& simplex = *gjk.getSimplex(); - if((simplex.rank > 1) && gjk.encloseOrigin()) - { - while(hull.root) - { - SimplexF* f = hull.root; - hull.remove(f); - stock.append(f); - } - - status = Valid; - nextsv = 0; - - if((simplex.c[0]->w - simplex.c[3]->w).dot((simplex.c[1]->w - simplex.c[3]->w).cross(simplex.c[2]->w - simplex.c[3]->w)) < 0) - { - SimplexV* tmp = simplex.c[0]; - simplex.c[0] = simplex.c[1]; - simplex.c[1] = tmp; - - FCL_REAL tmpv = simplex.p[0]; - simplex.p[0] = simplex.p[1]; - simplex.p[1] = tmpv; - } - - SimplexF* tetrahedron[] = {newFace(simplex.c[0], simplex.c[1], simplex.c[2], true), - newFace(simplex.c[1], simplex.c[0], simplex.c[3], true), - newFace(simplex.c[2], simplex.c[1], simplex.c[3], true), - newFace(simplex.c[0], simplex.c[2], simplex.c[3], true) }; - - if(hull.count == 4) - { - SimplexF* best = findBest(); // find the best face (the face with the minimum distance to origin) to split - SimplexF outer = *best; - size_t pass = 0; - size_t iterations = 0; - - // set the face connectivity - bind(tetrahedron[0], 0, tetrahedron[1], 0); - bind(tetrahedron[0], 1, tetrahedron[2], 0); - bind(tetrahedron[0], 2, tetrahedron[3], 0); - bind(tetrahedron[1], 1, tetrahedron[3], 2); - bind(tetrahedron[1], 2, tetrahedron[2], 1); - bind(tetrahedron[2], 2, tetrahedron[3], 1); - - status = Valid; - for(; iterations < max_iterations; ++iterations) - { - if(nextsv < max_vertex_num) - { - SimplexHorizon horizon; - SimplexV* w = &sv_store[nextsv++]; - bool valid = true; - best->pass = ++pass; - gjk.getSupport(best->n, *w); - FCL_REAL wdist = best->n.dot(w->w) - best->d; - if(wdist > tolerance) - { - for(size_t j = 0; (j < 3) && valid; ++j) - { - valid &= expand(pass, w, best->f[j], best->e[j], horizon); - } - - - if(valid && horizon.nf >= 3) - { - // need to add the edge connectivity between first and last faces - bind(horizon.ff, 2, horizon.cf, 1); - hull.remove(best); - stock.append(best); - best = findBest(); - outer = *best; - } - else - { - status = InvalidHull; break; - } - } - else - { - status = AccuracyReached; break; - } - } - else - { - status = OutOfVertices; break; - } - } - - Vector3d projection = outer.n * outer.d; - normal = outer.n; - depth = outer.d; - result.rank = 3; - result.c[0] = outer.c[0]; - result.c[1] = outer.c[1]; - result.c[2] = outer.c[2]; - result.p[0] = ((outer.c[1]->w - projection).cross(outer.c[2]->w - projection)).norm(); - result.p[1] = ((outer.c[2]->w - projection).cross(outer.c[0]->w - projection)).norm(); - result.p[2] = ((outer.c[0]->w - projection).cross(outer.c[1]->w - projection)).norm(); - - FCL_REAL sum = result.p[0] + result.p[1] + result.p[2]; - result.p[0] /= sum; - result.p[1] /= sum; - result.p[2] /= sum; - return status; - } - } - - status = FallBack; - normal = -guess; - FCL_REAL nl = normal.norm(); - if(nl > 0) normal /= nl; - else normal = Vector3d(1, 0, 0); - depth = 0; - result.rank = 1; - result.c[0] = simplex.c[0]; - result.p[0] = 1; - return status; -} - - -/** \brief the goal is to add a face connecting vertex w and face edge f[e] */ -bool EPA::expand(size_t pass, SimplexV* w, SimplexF* f, size_t e, SimplexHorizon& horizon) -{ - static const size_t nexti[] = {1, 2, 0}; - static const size_t previ[] = {2, 0, 1}; - - if(f->pass != pass) - { - const size_t e1 = nexti[e]; - - // case 1: the new face is not degenerated, i.e., the new face is not coplanar with the old face f. - if(f->n.dot(w->w) - f->d < -tolerance) - { - SimplexF* nf = newFace(f->c[e1], f->c[e], w, false); - if(nf) - { - // add face-face connectivity - bind(nf, 0, f, e); - - // if there is last face in the horizon, then need to add another connectivity, i.e. the edge connecting the current new add edge and the last new add edge. - // This does not finish all the connectivities because the final face need to connect with the first face, this will be handled in the evaluate function. - // Notice the face is anti-clockwise, so the edges are 0 (bottom), 1 (right), 2 (left) - if(horizon.cf) - bind(nf, 2, horizon.cf, 1); - else - horizon.ff = nf; - - horizon.cf = nf; - ++horizon.nf; - return true; - } - } - else // case 2: the new face is coplanar with the old face f. We need to add two faces and delete the old face - { - const size_t e2 = previ[e]; - f->pass = pass; - if(expand(pass, w, f->f[e1], f->e[e1], horizon) && expand(pass, w, f->f[e2], f->e[e2], horizon)) - { - hull.remove(f); - stock.append(f); - return true; - } - } - } - - return false; -} - -} // details - -} // fcl diff --git a/src/narrowphase/narrowphase.cpp b/src/narrowphase/narrowphase.cpp deleted file mode 100755 index 6fbfcbfde..000000000 --- a/src/narrowphase/narrowphase.cpp +++ /dev/null @@ -1,3568 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#include "fcl/narrowphase/narrowphase.h" - -#include -#include - -#include "fcl/shape/geometric_shapes_utility.h" -#include "fcl/intersect.h" - -namespace fcl -{ - -namespace details -{ - - // Clamp n to lie within the range [min, max] - float clamp(float n, float min, float max) { - if (n < min) return min; - if (n > max) return max; - return n; - } - - // Computes closest points C1 and C2 of S1(s)=P1+s*(Q1-P1) and - // S2(t)=P2+t*(Q2-P2), returning s and t. Function result is squared - // distance between between S1(s) and S2(t) - float closestPtSegmentSegment(Vector3d p1, Vector3d q1, Vector3d p2, Vector3d q2, - float &s, float &t, Vector3d &c1, Vector3d &c2) - { - const float EPSILON = 0.001; - Vector3d d1 = q1 - p1; // Direction vector of segment S1 - Vector3d d2 = q2 - p2; // Direction vector of segment S2 - Vector3d r = p1 - p2; - float a = d1.dot(d1); // Squared length of segment S1, always nonnegative - - float e = d2.dot(d2); // Squared length of segment S2, always nonnegative - float f = d2.dot(r); - // Check if either or both segments degenerate into points - if (a <= EPSILON && e <= EPSILON) { - // Both segments degenerate into points - s = t = 0.0f; - c1 = p1; - c2 = p2; - Vector3d diff = c1-c2; - float res = diff.dot(diff); - return res; - } - if (a <= EPSILON) { - // First segment degenerates into a point - s = 0.0f; - t = f / e; // s = 0 => t = (b*s + f) / e = f / e - t = clamp(t, 0.0f, 1.0f); - } else { - float c = d1.dot(r); - if (e <= EPSILON) { - // Second segment degenerates into a point - t = 0.0f; - s = clamp(-c / a, 0.0f, 1.0f); // t = 0 => s = (b*t - c) / a = -c / a - } else { - // The general nondegenerate case starts here - float b = d1.dot(d2); - float denom = a*e-b*b; // Always nonnegative - // If segments not parallel, compute closest point on L1 to L2 and - // clamp to segment S1. Else pick arbitrary s (here 0) - if (denom != 0.0f) { - std::cerr << "denominator equals zero, using 0 as reference" << std::endl; - s = clamp((b*f - c*e) / denom, 0.0f, 1.0f); - } else s = 0.0f; - // Compute point on L2 closest to S1(s) using - // t = Dot((P1 + D1*s) - P2,D2) / Dot(D2,D2) = (b*s + f) / e - t = (b*s + f) / e; - - // - //If t in [0,1] done. Else clamp t, recompute s for the new value - //of t using s = Dot((P2 + D2*t) - P1,D1) / Dot(D1,D1)= (t*b - c) / a - //and clamp s to [0, 1] - if(t < 0.0f) { - t = 0.0f; - s = clamp(-c / a, 0.0f, 1.0f); - } else if (t > 1.0f) { - t = 1.0f; - s = clamp((b - c) / a, 0.0f, 1.0f); - } - } - } - c1 = p1 + d1 * s; - c2 = p2 + d2 * t; - Vector3d diff = c1-c2; - float res = diff.dot(diff); - return res; - } - - - // Computes closest points C1 and C2 of S1(s)=P1+s*(Q1-P1) and - // S2(t)=P2+t*(Q2-P2), returning s and t. Function result is squared - // distance between between S1(s) and S2(t) - - bool capsuleCapsuleDistance(const Capsule& s1, const Transform3d& tf1, - const Capsule& s2, const Transform3d& tf2, - FCL_REAL* dist, Vector3d* p1_res, Vector3d* p2_res) - { - - Vector3d p1(tf1.translation()); - Vector3d p2(tf2.translation()); - - // line segment composes two points. First point is given by the origin, second point is computed by the origin transformed along z. - // extension along z-axis means transformation with identity matrix and translation vector z pos - Transform3d transformQ1 = tf1 * Eigen::Translation3d(Vector3d(0,0,s1.lz)); - Vector3d q1 = transformQ1.translation(); - - Transform3d transformQ2 = tf2 * Eigen::Translation3d(Vector3d(0,0,s2.lz)); - Vector3d q2 = transformQ2.translation(); - - // s and t correspont to the length of the line segment - float s, t; - Vector3d c1, c2; - - float result = closestPtSegmentSegment(p1, q1, p2, q2, s, t, c1, c2); - *dist = sqrt(result)-s1.radius-s2.radius; - - // getting directional unit vector - Vector3d distVec = c2 -c1; - distVec.normalize(); - - // extend the point to be border of the capsule. - // Done by following the directional unit vector for the length of the capsule radius - *p1_res = c1 + distVec*s1.radius; - - distVec = c1-c2; - distVec.normalize(); - - *p2_res = c2 + distVec*s2.radius; - - return true; - } - - - - -// Compute the point on a line segment that is the closest point on the -// segment to to another point. The code is inspired by the explanation -// given by Dan Sunday's page: -// http://geomalgorithms.com/a02-_lines.html -static inline void lineSegmentPointClosestToPoint (const Vector3d &p, const Vector3d &s1, const Vector3d &s2, Vector3d &sp) { - Vector3d v = s2 - s1; - Vector3d w = p - s1; - - FCL_REAL c1 = w.dot(v); - FCL_REAL c2 = v.dot(v); - - if (c1 <= 0) { - sp = s1; - } else if (c2 <= c1) { - sp = s2; - } else { - FCL_REAL b = c1/c2; - Vector3d Pb = s1 + v * b; - sp = Pb; - } -} - -bool sphereCapsuleIntersect(const Sphere& s1, const Transform3d& tf1, - const Capsule& s2, const Transform3d& tf2, - std::vector* contacts) -{ - const Vector3d pos1(0., 0., 0.5 * s2.lz); - const Vector3d pos2(0., 0., -0.5 * s2.lz); - const Vector3d s_c = tf2.inverse() * tf1.translation(); - - Vector3d segment_point; - - lineSegmentPointClosestToPoint (s_c, pos1, pos2, segment_point); - Vector3d diff = s_c - segment_point; - - const FCL_REAL distance = diff.norm() - s1.radius - s2.radius; - - if (distance > 0) - return false; - - const Vector3d local_normal = -diff.normalized(); - - if (contacts) - { - const Vector3d normal = tf2.linear() * local_normal; - const Vector3d point = tf2 * (segment_point + local_normal * distance); - const FCL_REAL penetration_depth = -distance; - - contacts->push_back(ContactPoint(normal, point, penetration_depth)); - } - - return true; -} - -bool sphereCapsuleDistance(const Sphere& s1, const Transform3d& tf1, - const Capsule& s2, const Transform3d& tf2, - FCL_REAL* dist, Vector3d* p1, Vector3d* p2) -{ - Vector3d pos1(0., 0., 0.5 * s2.lz); - Vector3d pos2(0., 0., -0.5 * s2.lz); - Vector3d s_c = tf2.inverse() * tf1.translation(); - - Vector3d segment_point; - - lineSegmentPointClosestToPoint (s_c, pos1, pos2, segment_point); - Vector3d diff = s_c - segment_point; - - FCL_REAL distance = diff.norm() - s1.radius - s2.radius; - - if(distance <= 0) - return false; - - if(dist) *dist = distance; - - if(p1 || p2) diff.normalize(); - if(p1) - { - *p1 = s_c - diff * s1.radius; - *p1 = tf1.inverse() * tf2 * (*p1); - } - - if(p2) *p2 = segment_point + diff * s1.radius; - - return true; -} - -bool sphereSphereIntersect(const Sphere& s1, const Transform3d& tf1, - const Sphere& s2, const Transform3d& tf2, - std::vector* contacts) -{ - Vector3d diff = tf2.translation() - tf1.translation(); - FCL_REAL len = diff.norm(); - if(len > s1.radius + s2.radius) - return false; - - if(contacts) - { - // If the centers of two sphere are at the same position, the normal is (0, 0, 0). - // Otherwise, normal is pointing from center of object 1 to center of object 2 - const Vector3d normal = len > 0 ? (diff / len).eval() : diff; - const Vector3d point = tf1.translation() + diff * s1.radius / (s1.radius + s2.radius); - const FCL_REAL penetration_depth = s1.radius + s2.radius - len; - contacts->push_back(ContactPoint(normal, point, penetration_depth)); - } - - return true; -} - -bool sphereSphereDistance(const Sphere& s1, const Transform3d& tf1, - const Sphere& s2, const Transform3d& tf2, - FCL_REAL* dist, Vector3d* p1, Vector3d* p2) -{ - Vector3d o1 = tf1.translation(); - Vector3d o2 = tf2.translation(); - Vector3d diff = o1 - o2; - FCL_REAL len = diff.norm(); - if(len > s1.radius + s2.radius) - { - if(dist) *dist = len - (s1.radius + s2.radius); - if(p1) *p1 = tf1.inverse() * (o1 - diff * (s1.radius / len)); - if(p2) *p2 = tf2.inverse() * (o2 + diff * (s2.radius / len)); - return true; - } - - if(dist) *dist = -1; - return false; -} - -/** \brief the minimum distance from a point to a line */ -FCL_REAL segmentSqrDistance(const Vector3d& from, const Vector3d& to,const Vector3d& p, Vector3d& nearest) -{ - Vector3d diff = p - from; - Vector3d v = to - from; - FCL_REAL t = v.dot(diff); - - if(t > 0) - { - FCL_REAL dotVV = v.dot(v); - if(t < dotVV) - { - t /= dotVV; - diff -= v * t; - } - else - { - t = 1; - diff -= v; - } - } - else - t = 0; - - nearest = from + v * t; - return diff.dot(diff); -} - -/// @brief Whether a point's projection is in a triangle -bool projectInTriangle(const Vector3d& p1, const Vector3d& p2, const Vector3d& p3, const Vector3d& normal, const Vector3d& p) -{ - Vector3d edge1(p2 - p1); - Vector3d edge2(p3 - p2); - Vector3d edge3(p1 - p3); - - Vector3d p1_to_p(p - p1); - Vector3d p2_to_p(p - p2); - Vector3d p3_to_p(p - p3); - - Vector3d edge1_normal(edge1.cross(normal)); - Vector3d edge2_normal(edge2.cross(normal)); - Vector3d edge3_normal(edge3.cross(normal)); - - FCL_REAL r1, r2, r3; - r1 = edge1_normal.dot(p1_to_p); - r2 = edge2_normal.dot(p2_to_p); - r3 = edge3_normal.dot(p3_to_p); - if ( ( r1 > 0 && r2 > 0 && r3 > 0 ) || - ( r1 <= 0 && r2 <= 0 && r3 <= 0 ) ) - return true; - return false; -} - - -bool sphereTriangleIntersect(const Sphere& s, const Transform3d& tf, - const Vector3d& P1, const Vector3d& P2, const Vector3d& P3, Vector3d* contact_points, FCL_REAL* penetration_depth, Vector3d* normal_) -{ - Vector3d normal = (P2 - P1).cross(P3 - P1); - normal.normalize(); - const Vector3d& center = tf.translation(); - const FCL_REAL& radius = s.radius; - FCL_REAL radius_with_threshold = radius + std::numeric_limits::epsilon(); - Vector3d p1_to_center = center - P1; - FCL_REAL distance_from_plane = p1_to_center.dot(normal); - - if(distance_from_plane < 0) - { - distance_from_plane *= -1; - normal *= -1; - } - - bool is_inside_contact_plane = (distance_from_plane < radius_with_threshold); - - bool has_contact = false; - Vector3d contact_point; - if(is_inside_contact_plane) - { - if(projectInTriangle(P1, P2, P3, normal, center)) - { - has_contact = true; - contact_point = center - normal * distance_from_plane; - } - else - { - FCL_REAL contact_capsule_radius_sqr = radius_with_threshold * radius_with_threshold; - Vector3d nearest_on_edge; - FCL_REAL distance_sqr; - distance_sqr = segmentSqrDistance(P1, P2, center, nearest_on_edge); - if(distance_sqr < contact_capsule_radius_sqr) - { - has_contact = true; - contact_point = nearest_on_edge; - } - - distance_sqr = segmentSqrDistance(P2, P3, center, nearest_on_edge); - if(distance_sqr < contact_capsule_radius_sqr) - { - has_contact = true; - contact_point = nearest_on_edge; - } - - distance_sqr = segmentSqrDistance(P3, P1, center, nearest_on_edge); - if(distance_sqr < contact_capsule_radius_sqr) - { - has_contact = true; - contact_point = nearest_on_edge; - } - } - } - - if(has_contact) - { - Vector3d contact_to_center = contact_point - center; - FCL_REAL distance_sqr = contact_to_center.squaredNorm(); - - if(distance_sqr < radius_with_threshold * radius_with_threshold) - { - if(distance_sqr > 0) - { - FCL_REAL distance = std::sqrt(distance_sqr); - if(normal_) *normal_ = contact_to_center.normalized(); - if(contact_points) *contact_points = contact_point; - if(penetration_depth) *penetration_depth = -(radius - distance); - } - else - { - if(normal_) *normal_ = -normal; - if(contact_points) *contact_points = contact_point; - if(penetration_depth) *penetration_depth = -radius; - } - - return true; - } - } - - return false; -} - - -bool sphereTriangleDistance(const Sphere& sp, const Transform3d& tf, - const Vector3d& P1, const Vector3d& P2, const Vector3d& P3, - FCL_REAL* dist) -{ - // from geometric tools, very different from the collision code. - - const Vector3d& center = tf.translation(); - FCL_REAL radius = sp.radius; - Vector3d diff = P1 - center; - Vector3d edge0 = P2 - P1; - Vector3d edge1 = P3 - P1; - FCL_REAL a00 = edge0.squaredNorm(); - FCL_REAL a01 = edge0.dot(edge1); - FCL_REAL a11 = edge1.squaredNorm(); - FCL_REAL b0 = diff.dot(edge0); - FCL_REAL b1 = diff.dot(edge1); - FCL_REAL c = diff.squaredNorm(); - FCL_REAL det = fabs(a00*a11 - a01*a01); - FCL_REAL s = a01*b1 - a11*b0; - FCL_REAL t = a01*b0 - a00*b1; - - FCL_REAL sqr_dist; - - if(s + t <= det) - { - if(s < 0) - { - if(t < 0) // region 4 - { - if(b0 < 0) - { - t = 0; - if(-b0 >= a00) - { - s = 1; - sqr_dist = a00 + 2*b0 + c; - } - else - { - s = -b0/a00; - sqr_dist = b0*s + c; - } - } - else - { - s = 0; - if(b1 >= 0) - { - t = 0; - sqr_dist = c; - } - else if(-b1 >= a11) - { - t = 1; - sqr_dist = a11 + 2*b1 + c; - } - else - { - t = -b1/a11; - sqr_dist = b1*t + c; - } - } - } - else // region 3 - { - s = 0; - if(b1 >= 0) - { - t = 0; - sqr_dist = c; - } - else if(-b1 >= a11) - { - t = 1; - sqr_dist = a11 + 2*b1 + c; - } - else - { - t = -b1/a11; - sqr_dist = b1*t + c; - } - } - } - else if(t < 0) // region 5 - { - t = 0; - if(b0 >= 0) - { - s = 0; - sqr_dist = c; - } - else if(-b0 >= a00) - { - s = 1; - sqr_dist = a00 + 2*b0 + c; - } - else - { - s = -b0/a00; - sqr_dist = b0*s + c; - } - } - else // region 0 - { - // minimum at interior point - FCL_REAL inv_det = (1)/det; - s *= inv_det; - t *= inv_det; - sqr_dist = s*(a00*s + a01*t + 2*b0) + t*(a01*s + a11*t + 2*b1) + c; - } - } - else - { - FCL_REAL tmp0, tmp1, numer, denom; - - if(s < 0) // region 2 - { - tmp0 = a01 + b0; - tmp1 = a11 + b1; - if(tmp1 > tmp0) - { - numer = tmp1 - tmp0; - denom = a00 - 2*a01 + a11; - if(numer >= denom) - { - s = 1; - t = 0; - sqr_dist = a00 + 2*b0 + c; - } - else - { - s = numer/denom; - t = 1 - s; - sqr_dist = s*(a00*s + a01*t + 2*b0) + t*(a01*s + a11*t + 2*b1) + c; - } - } - else - { - s = 0; - if(tmp1 <= 0) - { - t = 1; - sqr_dist = a11 + 2*b1 + c; - } - else if(b1 >= 0) - { - t = 0; - sqr_dist = c; - } - else - { - t = -b1/a11; - sqr_dist = b1*t + c; - } - } - } - else if(t < 0) // region 6 - { - tmp0 = a01 + b1; - tmp1 = a00 + b0; - if(tmp1 > tmp0) - { - numer = tmp1 - tmp0; - denom = a00 - 2*a01 + a11; - if(numer >= denom) - { - t = 1; - s = 0; - sqr_dist = a11 + 2*b1 + c; - } - else - { - t = numer/denom; - s = 1 - t; - sqr_dist = s*(a00*s + a01*t + 2*b0) + t*(a01*s + a11*t + 2*b1) + c; - } - } - else - { - t = 0; - if(tmp1 <= 0) - { - s = 1; - sqr_dist = a00 + 2*b0 + c; - } - else if(b0 >= 0) - { - s = 0; - sqr_dist = c; - } - else - { - s = -b0/a00; - sqr_dist = b0*s + c; - } - } - } - else // region 1 - { - numer = a11 + b1 - a01 - b0; - if(numer <= 0) - { - s = 0; - t = 1; - sqr_dist = a11 + 2*b1 + c; - } - else - { - denom = a00 - 2*a01 + a11; - if(numer >= denom) - { - s = 1; - t = 0; - sqr_dist = a00 + 2*b0 + c; - } - else - { - s = numer/denom; - t = 1 - s; - sqr_dist = s*(a00*s + a01*t + 2*b0) + t*(a01*s + a11*t + 2*b1) + c; - } - } - } - } - - // Account for numerical round-off error. - if(sqr_dist < 0) - sqr_dist = 0; - - if(sqr_dist > radius * radius) - { - if(dist) *dist = std::sqrt(sqr_dist) - radius; - return true; - } - else - { - if(dist) *dist = -1; - return false; - } -} - - -bool sphereTriangleDistance(const Sphere& sp, const Transform3d& tf, - const Vector3d& P1, const Vector3d& P2, const Vector3d& P3, - FCL_REAL* dist, Vector3d* p1, Vector3d* p2) -{ - if(p1 || p2) - { - Vector3d o = tf.translation(); - Project::ProjectResult result; - result = Project::projectTriangle(P1, P2, P3, o); - if(result.sqr_distance > sp.radius * sp.radius) - { - if(dist) *dist = std::sqrt(result.sqr_distance) - sp.radius; - Vector3d project_p = P1 * result.parameterization[0] + P2 * result.parameterization[1] + P3 * result.parameterization[2]; - Vector3d dir = o - project_p; - dir.normalize(); - if(p1) { *p1 = o - dir * sp.radius; *p1 = tf.inverse() * (*p1); } - if(p2) *p2 = project_p; - return true; - } - else - return false; - } - else - { - return sphereTriangleDistance(sp, tf, P1, P2, P3, dist); - } -} - - -bool sphereTriangleDistance(const Sphere& sp, const Transform3d& tf1, - const Vector3d& P1, const Vector3d& P2, const Vector3d& P3, const Transform3d& tf2, - FCL_REAL* dist, Vector3d* p1, Vector3d* p2) -{ - bool res = details::sphereTriangleDistance(sp, tf1, tf2 * P1, tf2 * P2, tf2 * P3, dist, p1, p2); - if(p2) *p2 = tf2.inverse() * (*p2); - - return res; -} - -static inline void lineClosestApproach(const Vector3d& pa, const Vector3d& ua, - const Vector3d& pb, const Vector3d& ub, - FCL_REAL* alpha, FCL_REAL* beta) -{ - Vector3d p = pb - pa; - FCL_REAL uaub = ua.dot(ub); - FCL_REAL q1 = ua.dot(p); - FCL_REAL q2 = -ub.dot(p); - FCL_REAL d = 1 - uaub * uaub; - if(d <= (FCL_REAL)(0.0001f)) - { - *alpha = 0; - *beta = 0; - } - else - { - d = 1 / d; - *alpha = (q1 + uaub * q2) * d; - *beta = (uaub * q1 + q2) * d; - } -} - -// find all the intersection points between the 2D rectangle with vertices -// at (+/-h[0],+/-h[1]) and the 2D quadrilateral with vertices (p[0],p[1]), -// (p[2],p[3]),(p[4],p[5]),(p[6],p[7]). -// -// the intersection points are returned as x,y pairs in the 'ret' array. -// the number of intersection points is returned by the function (this will -// be in the range 0 to 8). -static int intersectRectQuad2(FCL_REAL h[2], FCL_REAL p[8], FCL_REAL ret[16]) -{ - // q (and r) contain nq (and nr) coordinate points for the current (and - // chopped) polygons - int nq = 4, nr = 0; - FCL_REAL buffer[16]; - FCL_REAL* q = p; - FCL_REAL* r = ret; - for(int dir = 0; dir <= 1; ++dir) - { - // direction notation: xy[0] = x axis, xy[1] = y axis - for(int sign = -1; sign <= 1; sign += 2) - { - // chop q along the line xy[dir] = sign*h[dir] - FCL_REAL* pq = q; - FCL_REAL* pr = r; - nr = 0; - for(int i = nq; i > 0; --i) - { - // go through all points in q and all lines between adjacent points - if(sign * pq[dir] < h[dir]) - { - // this point is inside the chopping line - pr[0] = pq[0]; - pr[1] = pq[1]; - pr += 2; - nr++; - if(nr & 8) - { - q = r; - goto done; - } - } - FCL_REAL* nextq = (i > 1) ? pq+2 : q; - if((sign*pq[dir] < h[dir]) ^ (sign*nextq[dir] < h[dir])) - { - // this line crosses the chopping line - pr[1-dir] = pq[1-dir] + (nextq[1-dir]-pq[1-dir]) / - (nextq[dir]-pq[dir]) * (sign*h[dir]-pq[dir]); - pr[dir] = sign*h[dir]; - pr += 2; - nr++; - if(nr & 8) - { - q = r; - goto done; - } - } - pq += 2; - } - q = r; - r = (q == ret) ? buffer : ret; - nq = nr; - } - } - - done: - if(q != ret) memcpy(ret, q, nr*2*sizeof(FCL_REAL)); - return nr; -} - -// given n points in the plane (array p, of size 2*n), generate m points that -// best represent the whole set. the definition of 'best' here is not -// predetermined - the idea is to select points that give good box-box -// collision detection behavior. the chosen point indexes are returned in the -// array iret (of size m). 'i0' is always the first entry in the array. -// n must be in the range [1..8]. m must be in the range [1..n]. i0 must be -// in the range [0..n-1]. -static inline void cullPoints2(int n, FCL_REAL p[], int m, int i0, int iret[]) -{ - // compute the centroid of the polygon in cx,cy - FCL_REAL a, cx, cy, q; - switch(n) - { - case 1: - cx = p[0]; - cy = p[1]; - break; - case 2: - cx = 0.5 * (p[0] + p[2]); - cy = 0.5 * (p[1] + p[3]); - break; - default: - a = 0; - cx = 0; - cy = 0; - for(int i = 0; i < n-1; ++i) - { - q = p[i*2]*p[i*2+3] - p[i*2+2]*p[i*2+1]; - a += q; - cx += q*(p[i*2]+p[i*2+2]); - cy += q*(p[i*2+1]+p[i*2+3]); - } - q = p[n*2-2]*p[1] - p[0]*p[n*2-1]; - if(std::abs(a+q) > std::numeric_limits::epsilon()) - a = 1/(3*(a+q)); - else - a= 1e18f; - - cx = a*(cx + q*(p[n*2-2]+p[0])); - cy = a*(cy + q*(p[n*2-1]+p[1])); - } - - - // compute the angle of each point w.r.t. the centroid - FCL_REAL A[8]; - for(int i = 0; i < n; ++i) - A[i] = atan2(p[i*2+1]-cy,p[i*2]-cx); - - // search for points that have angles closest to A[i0] + i*(2*pi/m). - int avail[8]; - for(int i = 0; i < n; ++i) avail[i] = 1; - avail[i0] = 0; - iret[0] = i0; - iret++; - const double pi = constants::pi; - for(int j = 1; j < m; ++j) - { - a = j*(2*pi/m) + A[i0]; - if (a > pi) a -= 2*pi; - FCL_REAL maxdiff= 1e9, diff; - - *iret = i0; // iret is not allowed to keep this value, but it sometimes does, when diff=#QNAN0 - for(int i = 0; i < n; ++i) - { - if(avail[i]) - { - diff = std::abs(A[i]-a); - if(diff > pi) diff = 2*pi - diff; - if(diff < maxdiff) - { - maxdiff = diff; - *iret = i; - } - } - } - avail[*iret] = 0; - iret++; - } -} - - - -int boxBox2(const Vector3d& side1, const Matrix3d& R1, const Vector3d& T1, - const Vector3d& side2, const Matrix3d& R2, const Vector3d& T2, - Vector3d& normal, FCL_REAL* depth, int* return_code, - int maxc, std::vector& contacts) -{ - const FCL_REAL fudge_factor = FCL_REAL(1.05); - Vector3d normalC; - FCL_REAL s, s2, l; - int invert_normal, code; - - Vector3d p = T2 - T1; // get vector from centers of box 1 to box 2, relative to box 1 - Vector3d pp = R1.transpose() * p; // get pp = p relative to body 1 - - // get side lengths / 2 - Vector3d A = side1 * 0.5; - Vector3d B = side2 * 0.5; - - // Rij is R1'*R2, i.e. the relative rotation between R1 and R2 - Matrix3d R = R1.transpose() * R2; - Matrix3d Q = R.cwiseAbs(); - - - // for all 15 possible separating axes: - // * see if the axis separates the boxes. if so, return 0. - // * find the depth of the penetration along the separating axis (s2) - // * if this is the largest depth so far, record it. - // the normal vector will be set to the separating axis with the smallest - // depth. note: normalR is set to point to a column of R1 or R2 if that is - // the smallest depth normal so far. otherwise normalR is 0 and normalC is - // set to a vector relative to body 1. invert_normal is 1 if the sign of - // the normal should be flipped. - - int best_col_id = -1; - const Matrix3d* normalR = 0; - FCL_REAL tmp = 0; - - s = - std::numeric_limits::max(); - invert_normal = 0; - code = 0; - - // separating axis = u1, u2, u3 - tmp = pp[0]; - s2 = std::abs(tmp) - (Q.row(0).dot(B) + A[0]); - if(s2 > 0) { *return_code = 0; return 0; } - if(s2 > s) - { - s = s2; - best_col_id = 0; - normalR = &R1; - invert_normal = (tmp < 0); - code = 1; - } - - tmp = pp[1]; - s2 = std::abs(tmp) - (Q.row(1).dot(B) + A[1]); - if(s2 > 0) { *return_code = 0; return 0; } - if(s2 > s) - { - s = s2; - best_col_id = 1; - normalR = &R1; - invert_normal = (tmp < 0); - code = 2; - } - - tmp = pp[2]; - s2 = std::abs(tmp) - (Q.row(2).dot(B) + A[2]); - if(s2 > 0) { *return_code = 0; return 0; } - if(s2 > s) - { - s = s2; - best_col_id = 2; - normalR = &R1; - invert_normal = (tmp < 0); - code = 3; - } - - // separating axis = v1, v2, v3 - tmp = R2.col(0).dot(p); - s2 = std::abs(tmp) - (Q.col(0).dot(A) + B[0]); - if(s2 > 0) { *return_code = 0; return 0; } - if(s2 > s) - { - s = s2; - best_col_id = 0; - normalR = &R2; - invert_normal = (tmp < 0); - code = 4; - } - - tmp = R2.col(1).dot(p); - s2 = std::abs(tmp) - (Q.col(1).dot(A) + B[1]); - if(s2 > 0) { *return_code = 0; return 0; } - if(s2 > s) - { - s = s2; - best_col_id = 1; - normalR = &R2; - invert_normal = (tmp < 0); - code = 5; - } - - tmp = R2.col(2).dot(p); - s2 = std::abs(tmp) - (Q.col(2).dot(A) + B[2]); - if(s2 > 0) { *return_code = 0; return 0; } - if(s2 > s) - { - s = s2; - best_col_id = 2; - normalR = &R2; - invert_normal = (tmp < 0); - code = 6; - } - - - FCL_REAL fudge2(1.0e-6); - Q.array() += fudge2; - - Vector3d n; - FCL_REAL eps = std::numeric_limits::epsilon(); - - // separating axis = u1 x (v1,v2,v3) - tmp = pp[2] * R(1, 0) - pp[1] * R(2, 0); - s2 = std::abs(tmp) - (A[1] * Q(2, 0) + A[2] * Q(1, 0) + B[1] * Q(0, 2) + B[2] * Q(0, 1)); - if(s2 > 0) { *return_code = 0; return 0; } - n = Vector3d(0, -R(2, 0), R(1, 0)); - l = n.norm(); - if(l > eps) - { - s2 /= l; - if(s2 * fudge_factor > s) - { - s = s2; - best_col_id = -1; - normalC = n / l; - invert_normal = (tmp < 0); - code = 7; - } - } - - tmp = pp[2] * R(1, 1) - pp[1] * R(2, 1); - s2 = std::abs(tmp) - (A[1] * Q(2, 1) + A[2] * Q(1, 1) + B[0] * Q(0, 2) + B[2] * Q(0, 0)); - if(s2 > 0) { *return_code = 0; return 0; } - n = Vector3d(0, -R(2, 1), R(1, 1)); - l = n.norm(); - if(l > eps) - { - s2 /= l; - if(s2 * fudge_factor > s) - { - s = s2; - best_col_id = -1; - normalC = n / l; - invert_normal = (tmp < 0); - code = 8; - } - } - - tmp = pp[2] * R(1, 2) - pp[1] * R(2, 2); - s2 = std::abs(tmp) - (A[1] * Q(2, 2) + A[2] * Q(1, 2) + B[0] * Q(0, 1) + B[1] * Q(0, 0)); - if(s2 > 0) { *return_code = 0; return 0; } - n = Vector3d(0, -R(2, 2), R(1, 2)); - l = n.norm(); - if(l > eps) - { - s2 /= l; - if(s2 * fudge_factor > s) - { - s = s2; - best_col_id = -1; - normalC = n / l; - invert_normal = (tmp < 0); - code = 9; - } - } - - // separating axis = u2 x (v1,v2,v3) - tmp = pp[0] * R(2, 0) - pp[2] * R(0, 0); - s2 = std::abs(tmp) - (A[0] * Q(2, 0) + A[2] * Q(0, 0) + B[1] * Q(1, 2) + B[2] * Q(1, 1)); - if(s2 > 0) { *return_code = 0; return 0; } - n = Vector3d(R(2, 0), 0, -R(0, 0)); - l = n.norm(); - if(l > eps) - { - s2 /= l; - if(s2 * fudge_factor > s) - { - s = s2; - best_col_id = -1; - normalC = n / l; - invert_normal = (tmp < 0); - code = 10; - } - } - - tmp = pp[0] * R(2, 1) - pp[2] * R(0, 1); - s2 = std::abs(tmp) - (A[0] * Q(2, 1) + A[2] * Q(0, 1) + B[0] * Q(1, 2) + B[2] * Q(1, 0)); - if(s2 > 0) { *return_code = 0; return 0; } - n = Vector3d(R(2, 1), 0, -R(0, 1)); - l = n.norm(); - if(l > eps) - { - s2 /= l; - if(s2 * fudge_factor > s) - { - s = s2; - best_col_id = -1; - normalC = n / l; - invert_normal = (tmp < 0); - code = 11; - } - } - - tmp = pp[0] * R(2, 2) - pp[2] * R(0, 2); - s2 = std::abs(tmp) - (A[0] * Q(2, 2) + A[2] * Q(0, 2) + B[0] * Q(1, 1) + B[1] * Q(1, 0)); - if(s2 > 0) { *return_code = 0; return 0; } - n = Vector3d(R(2, 2), 0, -R(0, 2)); - l = n.norm(); - if(l > eps) - { - s2 /= l; - if(s2 * fudge_factor > s) - { - s = s2; - best_col_id = -1; - normalC = n / l; - invert_normal = (tmp < 0); - code = 12; - } - } - - // separating axis = u3 x (v1,v2,v3) - tmp = pp[1] * R(0, 0) - pp[0] * R(1, 0); - s2 = std::abs(tmp) - (A[0] * Q(1, 0) + A[1] * Q(0, 0) + B[1] * Q(2, 2) + B[2] * Q(2, 1)); - if(s2 > 0) { *return_code = 0; return 0; } - n = Vector3d(-R(1, 0), R(0, 0), 0); - l = n.norm(); - if(l > eps) - { - s2 /= l; - if(s2 * fudge_factor > s) - { - s = s2; - best_col_id = -1; - normalC = n / l; - invert_normal = (tmp < 0); - code = 13; - } - } - - tmp = pp[1] * R(0, 1) - pp[0] * R(1, 1); - s2 = std::abs(tmp) - (A[0] * Q(1, 1) + A[1] * Q(0, 1) + B[0] * Q(2, 2) + B[2] * Q(2, 0)); - if(s2 > 0) { *return_code = 0; return 0; } - n = Vector3d(-R(1, 1), R(0, 1), 0); - l = n.norm(); - if(l > eps) - { - s2 /= l; - if(s2 * fudge_factor > s) - { - s = s2; - best_col_id = -1; - normalC = n / l; - invert_normal = (tmp < 0); - code = 14; - } - } - - tmp = pp[1] * R(0, 2) - pp[0] * R(1, 2); - s2 = std::abs(tmp) - (A[0] * Q(1, 2) + A[1] * Q(0, 2) + B[0] * Q(2, 1) + B[1] * Q(2, 0)); - if(s2 > 0) { *return_code = 0; return 0; } - n = Vector3d(-R(1, 2), R(0, 2), 0); - l = n.norm(); - if(l > eps) - { - s2 /= l; - if(s2 * fudge_factor > s) - { - s = s2; - best_col_id = -1; - normalC = n / l; - invert_normal = (tmp < 0); - code = 15; - } - } - - - - if (!code) { *return_code = code; return 0; } - - // if we get to this point, the boxes interpenetrate. compute the normal - // in global coordinates. - if(best_col_id != -1) - normal = normalR->col(best_col_id); - else - normal = R1 * normalC; - - if(invert_normal) - normal = -normal; - - *depth = -s; // s is negative when the boxes are in collision - - // compute contact point(s) - - if(code > 6) - { - // an edge from box 1 touches an edge from box 2. - // find a point pa on the intersecting edge of box 1 - Vector3d pa(T1); - FCL_REAL sign; - - for(int j = 0; j < 3; ++j) - { - sign = (R1.col(j).dot(normal) > 0) ? 1 : -1; - pa += R1.col(j) * (A[j] * sign); - } - - // find a point pb on the intersecting edge of box 2 - Vector3d pb(T2); - - for(int j = 0; j < 3; ++j) - { - sign = (R2.col(j).dot(normal) > 0) ? -1 : 1; - pb += R2.col(j) * (B[j] * sign); - } - - FCL_REAL alpha, beta; - Vector3d ua(R1.col((code-7)/3)); - Vector3d ub(R2.col((code-7)%3)); - - lineClosestApproach(pa, ua, pb, ub, &alpha, &beta); - pa += ua * alpha; - pb += ub * beta; - - - // Vector3d pointInWorld((pa + pb) * 0.5); - // contacts.push_back(ContactPoint(-normal, pointInWorld, -*depth)); - contacts.push_back(ContactPoint(normal,pb,-*depth)); - *return_code = code; - - return 1; - } - - // okay, we have a face-something intersection (because the separating - // axis is perpendicular to a face). define face 'a' to be the reference - // face (i.e. the normal vector is perpendicular to this) and face 'b' to be - // the incident face (the closest face of the other box). - - const Matrix3d *Ra, *Rb; - const Vector3d *pa, *pb, *Sa, *Sb; - - if(code <= 3) - { - Ra = &R1; - Rb = &R2; - pa = &T1; - pb = &T2; - Sa = &A; - Sb = &B; - } - else - { - Ra = &R2; - Rb = &R1; - pa = &T2; - pb = &T1; - Sa = &B; - Sb = &A; - } - - // nr = normal vector of reference face dotted with axes of incident box. - // anr = absolute values of nr. - Vector3d normal2, nr, anr; - if(code <= 3) - normal2 = normal; - else - normal2 = -normal; - - nr = Rb->transpose() * normal2; - anr = nr.cwiseAbs(); - - // find the largest compontent of anr: this corresponds to the normal - // for the indident face. the other axis numbers of the indicent face - // are stored in a1,a2. - int lanr, a1, a2; - if(anr[1] > anr[0]) - { - if(anr[1] > anr[2]) - { - a1 = 0; - lanr = 1; - a2 = 2; - } - else - { - a1 = 0; - a2 = 1; - lanr = 2; - } - } - else - { - if(anr[0] > anr[2]) - { - lanr = 0; - a1 = 1; - a2 = 2; - } - else - { - a1 = 0; - a2 = 1; - lanr = 2; - } - } - - // compute center point of incident face, in reference-face coordinates - Vector3d center; - if(nr[lanr] < 0) - center = (*pb) - (*pa) + Rb->col(lanr) * ((*Sb)[lanr]); - else - center = (*pb) - (*pa) - Rb->col(lanr) * ((*Sb)[lanr]); - - // find the normal and non-normal axis numbers of the reference box - int codeN, code1, code2; - if(code <= 3) - codeN = code-1; - else codeN = code-4; - - if(codeN == 0) - { - code1 = 1; - code2 = 2; - } - else if(codeN == 1) - { - code1 = 0; - code2 = 2; - } - else - { - code1 = 0; - code2 = 1; - } - - // find the four corners of the incident face, in reference-face coordinates - FCL_REAL quad[8]; // 2D coordinate of incident face (x,y pairs) - FCL_REAL c1, c2, m11, m12, m21, m22; - c1 = Ra->col(code1).dot(center); - c2 = Ra->col(code2).dot(center); - // optimize this? - we have already computed this data above, but it is not - // stored in an easy-to-index format. for now it's quicker just to recompute - // the four dot products. - Vector3d tempRac = Ra->col(code1); - m11 = Rb->col(a1).dot(tempRac); - m12 = Rb->col(a2).dot(tempRac); - tempRac = Ra->col(code2); - m21 = Rb->col(a1).dot(tempRac); - m22 = Rb->col(a2).dot(tempRac); - - FCL_REAL k1 = m11 * (*Sb)[a1]; - FCL_REAL k2 = m21 * (*Sb)[a1]; - FCL_REAL k3 = m12 * (*Sb)[a2]; - FCL_REAL k4 = m22 * (*Sb)[a2]; - quad[0] = c1 - k1 - k3; - quad[1] = c2 - k2 - k4; - quad[2] = c1 - k1 + k3; - quad[3] = c2 - k2 + k4; - quad[4] = c1 + k1 + k3; - quad[5] = c2 + k2 + k4; - quad[6] = c1 + k1 - k3; - quad[7] = c2 + k2 - k4; - - // find the size of the reference face - FCL_REAL rect[2]; - rect[0] = (*Sa)[code1]; - rect[1] = (*Sa)[code2]; - - // intersect the incident and reference faces - FCL_REAL ret[16]; - int n_intersect = intersectRectQuad2(rect, quad, ret); - if(n_intersect < 1) { *return_code = code; return 0; } // this should never happen - - // convert the intersection points into reference-face coordinates, - // and compute the contact position and depth for each point. only keep - // those points that have a positive (penetrating) depth. delete points in - // the 'ret' array as necessary so that 'point' and 'ret' correspond. - Vector3d points[8]; // penetrating contact points - FCL_REAL dep[8]; // depths for those points - FCL_REAL det1 = 1.f/(m11*m22 - m12*m21); - m11 *= det1; - m12 *= det1; - m21 *= det1; - m22 *= det1; - int cnum = 0; // number of penetrating contact points found - for(int j = 0; j < n_intersect; ++j) - { - FCL_REAL k1 = m22*(ret[j*2]-c1) - m12*(ret[j*2+1]-c2); - FCL_REAL k2 = -m21*(ret[j*2]-c1) + m11*(ret[j*2+1]-c2); - points[cnum] = center + Rb->col(a1) * k1 + Rb->col(a2) * k2; - dep[cnum] = (*Sa)[codeN] - normal2.dot(points[cnum]); - if(dep[cnum] >= 0) - { - ret[cnum*2] = ret[j*2]; - ret[cnum*2+1] = ret[j*2+1]; - cnum++; - } - } - if(cnum < 1) { *return_code = code; return 0; } // this should never happen - - // we can't generate more contacts than we actually have - if(maxc > cnum) maxc = cnum; - if(maxc < 1) maxc = 1; - - if(cnum <= maxc) - { - if(code<4) - { - // we have less contacts than we need, so we use them all - for(int j = 0; j < cnum; ++j) - { - Vector3d pointInWorld = points[j] + (*pa); - contacts.push_back(ContactPoint(normal, pointInWorld, -dep[j])); - } - } - else - { - // we have less contacts than we need, so we use them all - for(int j = 0; j < cnum; ++j) - { - Vector3d pointInWorld = points[j] + (*pa) - normal * dep[j]; - contacts.push_back(ContactPoint(normal, pointInWorld, -dep[j])); - } - } - } - else - { - // we have more contacts than are wanted, some of them must be culled. - // find the deepest point, it is always the first contact. - int i1 = 0; - FCL_REAL maxdepth = dep[0]; - for(int i = 1; i < cnum; ++i) - { - if(dep[i] > maxdepth) - { - maxdepth = dep[i]; - i1 = i; - } - } - - int iret[8]; - cullPoints2(cnum, ret, maxc, i1, iret); - - for(int j = 0; j < maxc; ++j) - { - Vector3d posInWorld = points[iret[j]] + (*pa); - if(code < 4) - contacts.push_back(ContactPoint(normal, posInWorld, -dep[iret[j]])); - else - contacts.push_back(ContactPoint(normal, posInWorld - normal * dep[iret[j]], -dep[iret[j]])); - } - cnum = maxc; - } - - *return_code = code; - return cnum; -} - -bool boxBoxIntersect(const Box& s1, const Transform3d& tf1, - const Box& s2, const Transform3d& tf2, - std::vector* contacts_) -{ - std::vector contacts; - int return_code; - Vector3d normal; - FCL_REAL depth; - /* int cnum = */ boxBox2(s1.side, tf1.linear(), tf1.translation(), - s2.side, tf2.linear(), tf2.translation(), - normal, &depth, &return_code, - 4, contacts); - - if(contacts_) - *contacts_ = contacts; - - return return_code != 0; -} - -template -T halfspaceIntersectTolerance() -{ - return 0; -} - -template<> -float halfspaceIntersectTolerance() -{ - return 0.0001f; -} - -template<> -double halfspaceIntersectTolerance() -{ - return 0.0000001; -} - -bool sphereHalfspaceIntersect(const Sphere& s1, const Transform3d& tf1, - const Halfspace& s2, const Transform3d& tf2, - std::vector* contacts) -{ - const Halfspace new_s2 = transform(s2, tf2); - const Vector3d& center = tf1.translation(); - const FCL_REAL depth = s1.radius - new_s2.signedDistance(center); - - if (depth >= 0) - { - if (contacts) - { - const Vector3d normal = -new_s2.n; // pointing from s1 to s2 - const Vector3d point = center - new_s2.n * s1.radius + new_s2.n * (depth * 0.5); - const FCL_REAL penetration_depth = depth; - - contacts->push_back(ContactPoint(normal, point, penetration_depth)); - } - - return true; - } - else - { - return false; - } -} - -bool ellipsoidHalfspaceIntersect(const Ellipsoid& s1, const Transform3d& tf1, - const Halfspace& s2, const Transform3d& tf2, - std::vector* contacts) -{ - // We first compute a single contact in the ellipsoid coordinates, tf1, then - // will transform it to the world frame. So we use a new halfspace that is - // expressed in the ellipsoid coordinates. - const Halfspace& new_s2 = transform(s2, tf1.inverse() * tf2); - - // Compute distance between the ellipsoid's center and a contact plane, whose - // normal is equal to the halfspace's normal. - const Vector3d normal2(std::pow(new_s2.n[0], 2), std::pow(new_s2.n[1], 2), std::pow(new_s2.n[2], 2)); - const Vector3d radii2(std::pow(s1.radii[0], 2), std::pow(s1.radii[1], 2), std::pow(s1.radii[2], 2)); - const FCL_REAL center_to_contact_plane = std::sqrt(normal2.dot(radii2)); - - // Depth is the distance between the contact plane and the halfspace. - const FCL_REAL depth = center_to_contact_plane + new_s2.d; - - if (depth >= 0) - { - if (contacts) - { - // Transform the results to the world coordinates. - const Vector3d normal = tf1.linear() * -new_s2.n; // pointing from s1 to s2 - const Vector3d support_vector = (1.0/center_to_contact_plane) * Vector3d(radii2[0]*new_s2.n[0], radii2[1]*new_s2.n[1], radii2[2]*new_s2.n[2]); - const Vector3d point_in_halfspace_coords = support_vector * (0.5 * depth / new_s2.n.dot(support_vector) - 1.0); - const Vector3d point = tf1 * point_in_halfspace_coords; // roughly speaking, a middle point of the intersecting volume - const FCL_REAL penetration_depth = depth; - - contacts->push_back(ContactPoint(normal, point, penetration_depth)); - } - - return true; - } - else - { - return false; - } -} - -/// @brief box half space, a, b, c = +/- edge size -/// n^T * (R(o + a v1 + b v2 + c v3) + T) <= d -/// so (R^T n) (a v1 + b v2 + c v3) + n * T <= d -/// check whether d - n * T - (R^T n) (a v1 + b v2 + c v3) >= 0 for some a, b, c -/// the max value of left side is d - n * T + |(R^T n) (a v1 + b v2 + c v3)|, check that is enough -bool boxHalfspaceIntersect(const Box& s1, const Transform3d& tf1, - const Halfspace& s2, const Transform3d& tf2) -{ - Halfspace new_s2 = transform(s2, tf2); - - const Matrix3d& R = tf1.linear(); - const Vector3d& T = tf1.translation(); - - Vector3d Q = R.transpose() * new_s2.n; - Vector3d A(Q[0] * s1.side[0], Q[1] * s1.side[1], Q[2] * s1.side[2]); - Vector3d B = A.cwiseAbs(); - - FCL_REAL depth = 0.5 * (B[0] + B[1] + B[2]) - new_s2.signedDistance(T); - return (depth >= 0); -} - - -bool boxHalfspaceIntersect(const Box& s1, const Transform3d& tf1, - const Halfspace& s2, const Transform3d& tf2, - std::vector* contacts) -{ - if(!contacts) - { - return boxHalfspaceIntersect(s1, tf1, s2, tf2); - } - else - { - const Halfspace new_s2 = transform(s2, tf2); - - const Matrix3d& R = tf1.linear(); - const Vector3d& T = tf1.translation(); - - Vector3d Q = R.transpose() * new_s2.n; - Vector3d A(Q[0] * s1.side[0], Q[1] * s1.side[1], Q[2] * s1.side[2]); - Vector3d B = A.cwiseAbs(); - - FCL_REAL depth = 0.5 * (B[0] + B[1] + B[2]) - new_s2.signedDistance(T); - if(depth < 0) return false; - - Vector3d axis[3]; - axis[0] = R.col(0); - axis[1] = R.col(1); - axis[2] = R.col(2); - - /// find deepest point - Vector3d p(T); - int sign = 0; - - if(std::abs(Q[0] - 1) < halfspaceIntersectTolerance() || std::abs(Q[0] + 1) < halfspaceIntersectTolerance()) - { - sign = (A[0] > 0) ? -1 : 1; - p += axis[0] * (0.5 * s1.side[0] * sign); - } - else if(std::abs(Q[1] - 1) < halfspaceIntersectTolerance() || std::abs(Q[1] + 1) < halfspaceIntersectTolerance()) - { - sign = (A[1] > 0) ? -1 : 1; - p += axis[1] * (0.5 * s1.side[1] * sign); - } - else if(std::abs(Q[2] - 1) < halfspaceIntersectTolerance() || std::abs(Q[2] + 1) < halfspaceIntersectTolerance()) - { - sign = (A[2] > 0) ? -1 : 1; - p += axis[2] * (0.5 * s1.side[2] * sign); - } - else - { - for(std::size_t i = 0; i < 3; ++i) - { - sign = (A[i] > 0) ? -1 : 1; - p += axis[i] * (0.5 * s1.side[i] * sign); - } - } - - /// compute the contact point from the deepest point - if (contacts) - { - const Vector3d normal = -new_s2.n; - const Vector3d point = p + new_s2.n * (depth * 0.5); - const FCL_REAL penetration_depth = depth; - - contacts->push_back(ContactPoint(normal, point, penetration_depth)); - } - - return true; - } -} - -bool capsuleHalfspaceIntersect(const Capsule& s1, const Transform3d& tf1, - const Halfspace& s2, const Transform3d& tf2, - std::vector* contacts) -{ - Halfspace new_s2 = transform(s2, tf2); - - const Matrix3d& R = tf1.linear(); - const Vector3d& T = tf1.translation(); - - Vector3d dir_z = R.col(2); - - FCL_REAL cosa = dir_z.dot(new_s2.n); - if(std::abs(cosa) < halfspaceIntersectTolerance()) - { - FCL_REAL signed_dist = new_s2.signedDistance(T); - FCL_REAL depth = s1.radius - signed_dist; - if(depth < 0) return false; - - if (contacts) - { - const Vector3d normal = -new_s2.n; - const Vector3d point = T + new_s2.n * (0.5 * depth - s1.radius); - const FCL_REAL penetration_depth = depth; - - contacts->push_back(ContactPoint(normal, point, penetration_depth)); - } - - return true; - } - else - { - int sign = (cosa > 0) ? -1 : 1; - Vector3d p = T + dir_z * (s1.lz * 0.5 * sign); - - FCL_REAL signed_dist = new_s2.signedDistance(p); - FCL_REAL depth = s1.radius - signed_dist; - if(depth < 0) return false; - - if (contacts) - { - const Vector3d normal = -new_s2.n; - const Vector3d point = p - new_s2.n * s1.radius + new_s2.n * (0.5 * depth); // deepest point - const FCL_REAL penetration_depth = depth; - - contacts->push_back(ContactPoint(normal, point, penetration_depth)); - } - - return true; - } -} - - -bool cylinderHalfspaceIntersect(const Cylinder& s1, const Transform3d& tf1, - const Halfspace& s2, const Transform3d& tf2, - std::vector* contacts) -{ - Halfspace new_s2 = transform(s2, tf2); - - const Matrix3d& R = tf1.linear(); - const Vector3d& T = tf1.translation(); - - Vector3d dir_z = R.col(2); - FCL_REAL cosa = dir_z.dot(new_s2.n); - - if(cosa < halfspaceIntersectTolerance()) - { - FCL_REAL signed_dist = new_s2.signedDistance(T); - FCL_REAL depth = s1.radius - signed_dist; - if(depth < 0) return false; - - if (contacts) - { - const Vector3d normal = -new_s2.n; - const Vector3d point = T + new_s2.n * (0.5 * depth - s1.radius); - const FCL_REAL penetration_depth = depth; - - contacts->push_back(ContactPoint(normal, point, penetration_depth)); - } - - return true; - } - else - { - Vector3d C = dir_z * cosa - new_s2.n; - if(std::abs(cosa + 1) < halfspaceIntersectTolerance() || std::abs(cosa - 1) < halfspaceIntersectTolerance()) - C = Vector3d(0, 0, 0); - else - { - FCL_REAL s = C.norm(); - s = s1.radius / s; - C *= s; - } - - int sign = (cosa > 0) ? -1 : 1; - // deepest point - Vector3d p = T + dir_z * (s1.lz * 0.5 * sign) + C; - FCL_REAL depth = -new_s2.signedDistance(p); - if(depth < 0) return false; - else - { - if (contacts) - { - const Vector3d normal = -new_s2.n; - const Vector3d point = p + new_s2.n * (0.5 * depth); - const FCL_REAL penetration_depth = depth; - - contacts->push_back(ContactPoint(normal, point, penetration_depth)); - } - - return true; - } - } -} - - -bool coneHalfspaceIntersect(const Cone& s1, const Transform3d& tf1, - const Halfspace& s2, const Transform3d& tf2, - std::vector* contacts) -{ - Halfspace new_s2 = transform(s2, tf2); - - const Matrix3d& R = tf1.linear(); - const Vector3d& T = tf1.translation(); - - Vector3d dir_z = R.col(2); - FCL_REAL cosa = dir_z.dot(new_s2.n); - - if(cosa < halfspaceIntersectTolerance()) - { - FCL_REAL signed_dist = new_s2.signedDistance(T); - FCL_REAL depth = s1.radius - signed_dist; - if(depth < 0) return false; - else - { - if (contacts) - { - const Vector3d normal = -new_s2.n; - const Vector3d point = T - dir_z * (s1.lz * 0.5) + new_s2.n * (0.5 * depth - s1.radius); - const FCL_REAL penetration_depth = depth; - - contacts->push_back(ContactPoint(normal, point, penetration_depth)); - } - - return true; - } - } - else - { - Vector3d C = dir_z * cosa - new_s2.n; - if(std::abs(cosa + 1) < halfspaceIntersectTolerance() || std::abs(cosa - 1) < halfspaceIntersectTolerance()) - C = Vector3d(0, 0, 0); - else - { - FCL_REAL s = C.norm(); - s = s1.radius / s; - C *= s; - } - - Vector3d p1 = T + dir_z * (0.5 * s1.lz); - Vector3d p2 = T - dir_z * (0.5 * s1.lz) + C; - - FCL_REAL d1 = new_s2.signedDistance(p1); - FCL_REAL d2 = new_s2.signedDistance(p2); - - if(d1 > 0 && d2 > 0) return false; - else - { - if (contacts) - { - const FCL_REAL penetration_depth = -std::min(d1, d2); - const Vector3d normal = -new_s2.n; - const Vector3d point = ((d1 < d2) ? p1 : p2) + new_s2.n * (0.5 * penetration_depth); - - contacts->push_back(ContactPoint(normal, point, penetration_depth)); - } - - return true; - } - } -} - -bool convexHalfspaceIntersect(const Convex& s1, const Transform3d& tf1, - const Halfspace& s2, const Transform3d& tf2, - Vector3d* contact_points, FCL_REAL* penetration_depth, Vector3d* normal) -{ - Halfspace new_s2 = transform(s2, tf2); - - Vector3d v; - FCL_REAL depth = std::numeric_limits::max(); - - for(int i = 0; i < s1.num_points; ++i) - { - Vector3d p = tf1 * s1.points[i]; - - FCL_REAL d = new_s2.signedDistance(p); - if(d < depth) - { - depth = d; - v = p; - } - } - - if(depth <= 0) - { - if(contact_points) *contact_points = v - new_s2.n * (0.5 * depth); - if(penetration_depth) *penetration_depth = depth; - if(normal) *normal = -new_s2.n; - return true; - } - else - return false; -} - -bool halfspaceTriangleIntersect(const Halfspace& s1, const Transform3d& tf1, - const Vector3d& P1, const Vector3d& P2, const Vector3d& P3, const Transform3d& tf2, - Vector3d* contact_points, FCL_REAL* penetration_depth, Vector3d* normal) -{ - Halfspace new_s1 = transform(s1, tf1); - - Vector3d v = tf2 * P1; - FCL_REAL depth = new_s1.signedDistance(v); - - Vector3d p = tf2 * P2; - FCL_REAL d = new_s1.signedDistance(p); - if(d < depth) - { - depth = d; - v = p; - } - - p = tf2 * P3; - d = new_s1.signedDistance(p); - if(d < depth) - { - depth = d; - v = p; - } - - if(depth <= 0) - { - if(penetration_depth) *penetration_depth = -depth; - if(normal) *normal = new_s1.n; - if(contact_points) *contact_points = v - new_s1.n * (0.5 * depth); - return true; - } - else - return false; -} - - -/// @brief return whether plane collides with halfspace -/// if the separation plane of the halfspace is parallel with the plane -/// return code 1, if the plane's normal is the same with halfspace's normal and plane is inside halfspace, also return plane in pl -/// return code 2, if the plane's normal is oppositie to the halfspace's normal and plane is inside halfspace, also return plane in pl -/// plane is outside halfspace, collision-free -/// if not parallel -/// return the intersection ray, return code 3. ray origin is p and direction is d -bool planeHalfspaceIntersect(const Plane& s1, const Transform3d& tf1, - const Halfspace& s2, const Transform3d& tf2, - Plane& pl, - Vector3d& p, Vector3d& d, - FCL_REAL& penetration_depth, - int& ret) -{ - Plane new_s1 = transform(s1, tf1); - Halfspace new_s2 = transform(s2, tf2); - - ret = 0; - - Vector3d dir = (new_s1.n).cross(new_s2.n); - FCL_REAL dir_norm = dir.squaredNorm(); - if(dir_norm < std::numeric_limits::epsilon()) // parallel - { - if((new_s1.n).dot(new_s2.n) > 0) - { - if(new_s1.d < new_s2.d) - { - penetration_depth = new_s2.d - new_s1.d; - ret = 1; - pl = new_s1; - return true; - } - else - return false; - } - else - { - if(new_s1.d + new_s2.d > 0) - return false; - else - { - penetration_depth = -(new_s1.d + new_s2.d); - ret = 2; - pl = new_s1; - return true; - } - } - } - - Vector3d n = new_s2.n * new_s1.d - new_s1.n * new_s2.d; - Vector3d origin = n.cross(dir); - origin *= (1.0 / dir_norm); - - p = origin; - d = dir; - ret = 3; - penetration_depth = std::numeric_limits::max(); - - return true; -} - -///@ brief return whether two halfspace intersect -/// if the separation planes of the two halfspaces are parallel -/// return code 1, if two halfspaces' normal are same and s1 is in s2, also return s1 in s; -/// return code 2, if two halfspaces' normal are same and s2 is in s1, also return s2 in s; -/// return code 3, if two halfspaces' normal are opposite and s1 and s2 are into each other; -/// collision free, if two halfspaces' are separate; -/// if the separation planes of the two halfspaces are not parallel, return intersection ray, return code 4. ray origin is p and direction is d -/// collision free return code 0 -bool halfspaceIntersect(const Halfspace& s1, const Transform3d& tf1, - const Halfspace& s2, const Transform3d& tf2, - Vector3d& p, Vector3d& d, - Halfspace& s, - FCL_REAL& penetration_depth, - int& ret) -{ - Halfspace new_s1 = transform(s1, tf1); - Halfspace new_s2 = transform(s2, tf2); - - ret = 0; - - Vector3d dir = (new_s1.n).cross(new_s2.n); - FCL_REAL dir_norm = dir.squaredNorm(); - if(dir_norm < std::numeric_limits::epsilon()) // parallel - { - if((new_s1.n).dot(new_s2.n) > 0) - { - if(new_s1.d < new_s2.d) // s1 is inside s2 - { - ret = 1; - penetration_depth = std::numeric_limits::max(); - s = new_s1; - } - else // s2 is inside s1 - { - ret = 2; - penetration_depth = std::numeric_limits::max(); - s = new_s2; - } - return true; - } - else - { - if(new_s1.d + new_s2.d > 0) // not collision - return false; - else // in each other - { - ret = 3; - penetration_depth = -(new_s1.d + new_s2.d); - return true; - } - } - } - - Vector3d n = new_s2.n * new_s1.d - new_s1.n * new_s2.d; - Vector3d origin = n.cross(dir); - origin *= (1.0 / dir_norm); - - p = origin; - d = dir; - ret = 4; - penetration_depth = std::numeric_limits::max(); - - return true; -} - -template -T planeIntersectTolerance() -{ - return 0; -} - -template<> -double planeIntersectTolerance() -{ - return 0.0000001; -} - -template<> -float planeIntersectTolerance() -{ - return 0.0001; -} - -bool spherePlaneIntersect(const Sphere& s1, const Transform3d& tf1, - const Plane& s2, const Transform3d& tf2, - std::vector* contacts) -{ - const Plane new_s2 = transform(s2, tf2); - - const Vector3d& center = tf1.translation(); - const FCL_REAL signed_dist = new_s2.signedDistance(center); - const FCL_REAL depth = - std::abs(signed_dist) + s1.radius; - - if(depth >= 0) - { - if (contacts) - { - const Vector3d normal = (signed_dist > 0) ? (-new_s2.n).eval() : new_s2.n; - const Vector3d point = center - new_s2.n * signed_dist; - const FCL_REAL penetration_depth = depth; - - contacts->push_back(ContactPoint(normal, point, penetration_depth)); - } - - return true; - } - else - { - return false; - } -} - -bool ellipsoidPlaneIntersect(const Ellipsoid& s1, const Transform3d& tf1, - const Plane& s2, const Transform3d& tf2, - std::vector* contacts) -{ - // We first compute a single contact in the ellipsoid coordinates, tf1, then - // will transform it to the world frame. So we use a new plane that is - // expressed in the ellipsoid coordinates. - const Plane& new_s2 = transform(s2, tf1.inverse() * tf2); - - // Compute distance between the ellipsoid's center and a contact plane, whose - // normal is equal to the plane's normal. - const Vector3d normal2(std::pow(new_s2.n[0], 2), std::pow(new_s2.n[1], 2), std::pow(new_s2.n[2], 2)); - const Vector3d radii2(std::pow(s1.radii[0], 2), std::pow(s1.radii[1], 2), std::pow(s1.radii[2], 2)); - const FCL_REAL center_to_contact_plane = std::sqrt(normal2.dot(radii2)); - - const FCL_REAL signed_dist = -new_s2.d; - - // Depth is the distance between the contact plane and the given plane. - const FCL_REAL depth = center_to_contact_plane - std::abs(signed_dist); - - if (depth >= 0) - { - if (contacts) - { - // Transform the results to the world coordinates. - const Vector3d normal = (signed_dist > 0) ? (tf1.linear() * -new_s2.n).eval() : (tf1.linear() * new_s2.n).eval(); // pointing from the ellipsoid's center to the plane - const Vector3d support_vector = (1.0/center_to_contact_plane) * Vector3d(radii2[0]*new_s2.n[0], radii2[1]*new_s2.n[1], radii2[2]*new_s2.n[2]); - const Vector3d point_in_plane_coords = support_vector * (depth / new_s2.n.dot(support_vector) - 1.0); - const Vector3d point = (signed_dist > 0) ? tf1 * point_in_plane_coords : tf1 * -point_in_plane_coords; // a middle point of the intersecting volume - const FCL_REAL penetration_depth = depth; - - contacts->push_back(ContactPoint(normal, point, penetration_depth)); - } - - return true; - } - else - { - return false; - } -} - -/// @brief box half space, a, b, c = +/- edge size -/// n^T * (R(o + a v1 + b v2 + c v3) + T) ~ d -/// so (R^T n) (a v1 + b v2 + c v3) + n * T ~ d -/// check whether d - n * T - (R^T n) (a v1 + b v2 + c v3) >= 0 for some a, b, c and <=0 for some a, b, c -/// so need to check whether |d - n * T| <= |(R^T n)(a v1 + b v2 + c v3)|, the reason is as follows: -/// (R^T n) (a v1 + b v2 + c v3) can get |(R^T n) (a v1 + b v2 + c v3)| for one a, b, c. -/// if |d - n * T| <= |(R^T n)(a v1 + b v2 + c v3)| then can get both positive and negative value on the right side. -bool boxPlaneIntersect(const Box& s1, const Transform3d& tf1, - const Plane& s2, const Transform3d& tf2, - std::vector* contacts) -{ - Plane new_s2 = transform(s2, tf2); - - const Matrix3d& R = tf1.linear(); - const Vector3d& T = tf1.translation(); - - Vector3d Q = R.transpose() * new_s2.n; - Vector3d A(Q[0] * s1.side[0], Q[1] * s1.side[1], Q[2] * s1.side[2]); - Vector3d B = A.cwiseAbs(); - - FCL_REAL signed_dist = new_s2.signedDistance(T); - FCL_REAL depth = 0.5 * (B[0] + B[1] + B[2]) - std::abs(signed_dist); - if(depth < 0) return false; - - Vector3d axis[3]; - axis[0] = R.col(0); - axis[1] = R.col(1); - axis[2] = R.col(2); - - // find the deepest point - Vector3d p = T; - - // when center is on the positive side of the plane, use a, b, c make (R^T n) (a v1 + b v2 + c v3) the minimum - // otherwise, use a, b, c make (R^T n) (a v1 + b v2 + c v3) the maximum - int sign = (signed_dist > 0) ? 1 : -1; - - if(std::abs(Q[0] - 1) < planeIntersectTolerance() || std::abs(Q[0] + 1) < planeIntersectTolerance()) - { - int sign2 = (A[0] > 0) ? -1 : 1; - sign2 *= sign; - p += axis[0] * (0.5 * s1.side[0] * sign2); - } - else if(std::abs(Q[1] - 1) < planeIntersectTolerance() || std::abs(Q[1] + 1) < planeIntersectTolerance()) - { - int sign2 = (A[1] > 0) ? -1 : 1; - sign2 *= sign; - p += axis[1] * (0.5 * s1.side[1] * sign2); - } - else if(std::abs(Q[2] - 1) < planeIntersectTolerance() || std::abs(Q[2] + 1) < planeIntersectTolerance()) - { - int sign2 = (A[2] > 0) ? -1 : 1; - sign2 *= sign; - p += axis[2] * (0.5 * s1.side[2] * sign2); - } - else - { - for(std::size_t i = 0; i < 3; ++i) - { - int sign2 = (A[i] > 0) ? -1 : 1; - sign2 *= sign; - p += axis[i] * (0.5 * s1.side[i] * sign2); - } - } - - // compute the contact point by project the deepest point onto the plane - if (contacts) - { - const Vector3d normal = (signed_dist > 0) ? (-new_s2.n).eval() : new_s2.n; - const Vector3d point = p - new_s2.n * new_s2.signedDistance(p); - const FCL_REAL penetration_depth = depth; - - contacts->push_back(ContactPoint(normal, point, penetration_depth)); - } - - return true; -} - - -bool capsulePlaneIntersect(const Capsule& s1, const Transform3d& tf1, - const Plane& s2, const Transform3d& tf2) -{ - Plane new_s2 = transform(s2, tf2); - - const Matrix3d& R = tf1.linear(); - const Vector3d& T = tf1.translation(); - - Vector3d dir_z = R.col(2); - Vector3d p1 = T + dir_z * (0.5 * s1.lz); - Vector3d p2 = T - dir_z * (0.5 * s1.lz); - - FCL_REAL d1 = new_s2.signedDistance(p1); - FCL_REAL d2 = new_s2.signedDistance(p2); - - // two end points on different side of the plane - if(d1 * d2 <= 0) - return true; - - // two end points on the same side of the plane, but the end point spheres might intersect the plane - return (std::abs(d1) <= s1.radius) || (std::abs(d2) <= s1.radius); -} - -bool capsulePlaneIntersect(const Capsule& s1, const Transform3d& tf1, - const Plane& s2, const Transform3d& tf2, - std::vector* contacts) -{ - if(!contacts) - { - return capsulePlaneIntersect(s1, tf1, s2, tf2); - } - else - { - Plane new_s2 = transform(s2, tf2); - - const Matrix3d& R = tf1.linear(); - const Vector3d& T = tf1.translation(); - - Vector3d dir_z = R.col(2); - - - Vector3d p1 = T + dir_z * (0.5 * s1.lz); - Vector3d p2 = T - dir_z * (0.5 * s1.lz); - - FCL_REAL d1 = new_s2.signedDistance(p1); - FCL_REAL d2 = new_s2.signedDistance(p2); - - FCL_REAL abs_d1 = std::abs(d1); - FCL_REAL abs_d2 = std::abs(d2); - - // two end points on different side of the plane - // the contact point is the intersect of axis with the plane - // the normal is the direction to avoid intersection - // the depth is the minimum distance to resolve the collision - if(d1 * d2 < -planeIntersectTolerance()) - { - if(abs_d1 < abs_d2) - { - if (contacts) - { - const Vector3d normal = (d1 < 0) ? (-new_s2.n).eval() : new_s2.n; - const Vector3d point = p1 * (abs_d2 / (abs_d1 + abs_d2)) + p2 * (abs_d1 / (abs_d1 + abs_d2)); - const FCL_REAL penetration_depth = abs_d1 + s1.radius; - - contacts->push_back(ContactPoint(normal, point, penetration_depth)); - } - } - else - { - if (contacts) - { - const Vector3d normal = (d2 < 0) ? (-new_s2.n).eval() : new_s2.n; - const Vector3d point = p1 * (abs_d2 / (abs_d1 + abs_d2)) + p2 * (abs_d1 / (abs_d1 + abs_d2)); - const FCL_REAL penetration_depth = abs_d2 + s1.radius; - - contacts->push_back(ContactPoint(normal, point, penetration_depth)); - } - } - return true; - } - - if(abs_d1 > s1.radius && abs_d2 > s1.radius) - { - return false; - } - else - { - if (contacts) - { - const Vector3d normal = (d1 < 0) ? new_s2.n : (-new_s2.n).eval(); - const FCL_REAL penetration_depth = s1.radius - std::min(abs_d1, abs_d2); - Vector3d point; - if(abs_d1 <= s1.radius && abs_d2 <= s1.radius) - { - const Vector3d c1 = p1 - new_s2.n * d2; - const Vector3d c2 = p2 - new_s2.n * d1; - point = (c1 + c2) * 0.5; - } - else if(abs_d1 <= s1.radius) - { - const Vector3d c = p1 - new_s2.n * d1; - point = c; - } - else if(abs_d2 <= s1.radius) - { - const Vector3d c = p2 - new_s2.n * d2; - point = c; - } - - contacts->push_back(ContactPoint(normal, point, penetration_depth)); - } - - return true; - } - } -} - -/// @brief cylinder-plane intersect -/// n^T (R (r * cosa * v1 + r * sina * v2 + h * v3) + T) ~ d -/// need one point to be positive and one to be negative -/// (n^T * v3) * h + n * T -d + r * (cosa * (n^T * R * v1) + sina * (n^T * R * v2)) ~ 0 -/// (n^T * v3) * h + r * (cosa * (n^T * R * v1) + sina * (n^T * R * v2)) + n * T - d ~ 0 -bool cylinderPlaneIntersect(const Cylinder& s1, const Transform3d& tf1, - const Plane& s2, const Transform3d& tf2) -{ - Plane new_s2 = transform(s2, tf2); - - const Matrix3d& R = tf1.linear(); - const Vector3d& T = tf1.translation(); - - Vector3d Q = R.transpose() * new_s2.n; - - FCL_REAL term = std::abs(Q[2]) * s1.lz + s1.radius * std::sqrt(Q[0] * Q[0] + Q[1] * Q[1]); - FCL_REAL dist = new_s2.distance(T); - FCL_REAL depth = term - dist; - - if(depth < 0) - return false; - else - return true; -} - -bool cylinderPlaneIntersect(const Cylinder& s1, const Transform3d& tf1, - const Plane& s2, const Transform3d& tf2, - std::vector* contacts) -{ - if(!contacts) - { - return cylinderPlaneIntersect(s1, tf1, s2, tf2); - } - else - { - Plane new_s2 = transform(s2, tf2); - - const Matrix3d& R = tf1.linear(); - const Vector3d& T = tf1.translation(); - - Vector3d dir_z = R.col(2); - FCL_REAL cosa = dir_z.dot(new_s2.n); - - if(std::abs(cosa) < planeIntersectTolerance()) - { - FCL_REAL d = new_s2.signedDistance(T); - FCL_REAL depth = s1.radius - std::abs(d); - if(depth < 0) return false; - else - { - if (contacts) - { - const Vector3d normal = (d < 0) ? new_s2.n : (-new_s2.n).eval(); - const Vector3d point = T - new_s2.n * d; - const FCL_REAL penetration_depth = depth; - - contacts->push_back(ContactPoint(normal, point, penetration_depth)); - } - return true; - } - } - else - { - Vector3d C = dir_z * cosa - new_s2.n; - if(std::abs(cosa + 1) < planeIntersectTolerance() || std::abs(cosa - 1) < planeIntersectTolerance()) - C = Vector3d(0, 0, 0); - else - { - FCL_REAL s = C.norm(); - s = s1.radius / s; - C *= s; - } - - Vector3d p1 = T + dir_z * (0.5 * s1.lz); - Vector3d p2 = T - dir_z * (0.5 * s1.lz); - - Vector3d c1, c2; - if(cosa > 0) - { - c1 = p1 - C; - c2 = p2 + C; - } - else - { - c1 = p1 + C; - c2 = p2 - C; - } - - FCL_REAL d1 = new_s2.signedDistance(c1); - FCL_REAL d2 = new_s2.signedDistance(c2); - - if(d1 * d2 <= 0) - { - FCL_REAL abs_d1 = std::abs(d1); - FCL_REAL abs_d2 = std::abs(d2); - - if(abs_d1 > abs_d2) - { - if (contacts) - { - const Vector3d normal = (d2 < 0) ? (-new_s2.n).eval() : new_s2.n; - const Vector3d point = c2 - new_s2.n * d2; - const FCL_REAL penetration_depth = abs_d2; - - contacts->push_back(ContactPoint(normal, point, penetration_depth)); - } - } - else - { - if (contacts) - { - const Vector3d normal = (d1 < 0) ? (-new_s2.n).eval() : new_s2.n; - const Vector3d point = c1 - new_s2.n * d1; - const FCL_REAL penetration_depth = abs_d1; - - contacts->push_back(ContactPoint(normal, point, penetration_depth)); - } - } - return true; - } - else - { - return false; - } - } - } -} - -bool conePlaneIntersect(const Cone& s1, const Transform3d& tf1, - const Plane& s2, const Transform3d& tf2, - std::vector* contacts) -{ - Plane new_s2 = transform(s2, tf2); - - const Matrix3d& R = tf1.linear(); - const Vector3d& T = tf1.translation(); - - Vector3d dir_z = R.col(2); - FCL_REAL cosa = dir_z.dot(new_s2.n); - - if(std::abs(cosa) < planeIntersectTolerance()) - { - FCL_REAL d = new_s2.signedDistance(T); - FCL_REAL depth = s1.radius - std::abs(d); - if(depth < 0) return false; - else - { - if (contacts) - { - const Vector3d normal = (d < 0) ? new_s2.n : (-new_s2.n).eval(); - const Vector3d point = T - dir_z * (0.5 * s1.lz) + dir_z * (0.5 * depth / s1.radius * s1.lz) - new_s2.n * d; - const FCL_REAL penetration_depth = depth; - - contacts->push_back(ContactPoint(normal, point, penetration_depth)); - } - - return true; - } - } - else - { - Vector3d C = dir_z * cosa - new_s2.n; - if(std::abs(cosa + 1) < planeIntersectTolerance() || std::abs(cosa - 1) < planeIntersectTolerance()) - C = Vector3d(0, 0, 0); - else - { - FCL_REAL s = C.norm(); - s = s1.radius / s; - C *= s; - } - - Vector3d c[3]; - c[0] = T + dir_z * (0.5 * s1.lz); - c[1] = T - dir_z * (0.5 * s1.lz) + C; - c[2] = T - dir_z * (0.5 * s1.lz) - C; - - FCL_REAL d[3]; - d[0] = new_s2.signedDistance(c[0]); - d[1] = new_s2.signedDistance(c[1]); - d[2] = new_s2.signedDistance(c[2]); - - if((d[0] >= 0 && d[1] >= 0 && d[2] >= 0) || (d[0] <= 0 && d[1] <= 0 && d[2] <= 0)) - return false; - else - { - bool positive[3]; - for(std::size_t i = 0; i < 3; ++i) - positive[i] = (d[i] >= 0); - - int n_positive = 0; - FCL_REAL d_positive = 0, d_negative = 0; - for(std::size_t i = 0; i < 3; ++i) - { - if(positive[i]) - { - n_positive++; - if(d_positive <= d[i]) d_positive = d[i]; - } - else - { - if(d_negative <= -d[i]) d_negative = -d[i]; - } - } - - if (contacts) - { - const Vector3d normal = (d_positive > d_negative) ? (-new_s2.n).eval() : new_s2.n; - const FCL_REAL penetration_depth = std::min(d_positive, d_negative); - - Vector3d point; - Vector3d p[2]; - Vector3d q; - - FCL_REAL p_d[2]; - FCL_REAL q_d(0); - - if(n_positive == 2) - { - for(std::size_t i = 0, j = 0; i < 3; ++i) - { - if(positive[i]) { p[j] = c[i]; p_d[j] = d[i]; j++; } - else { q = c[i]; q_d = d[i]; } - } - - const Vector3d t1 = (-p[0] * q_d + q * p_d[0]) / (-q_d + p_d[0]); - const Vector3d t2 = (-p[1] * q_d + q * p_d[1]) / (-q_d + p_d[1]); - point = (t1 + t2) * 0.5; - } - else - { - for(std::size_t i = 0, j = 0; i < 3; ++i) - { - if(!positive[i]) { p[j] = c[i]; p_d[j] = d[i]; j++; } - else { q = c[i]; q_d = d[i]; } - } - - const Vector3d t1 = (p[0] * q_d - q * p_d[0]) / (q_d - p_d[0]); - const Vector3d t2 = (p[1] * q_d - q * p_d[1]) / (q_d - p_d[1]); - point = (t1 + t2) * 0.5; - } - - contacts->push_back(ContactPoint(normal, point, penetration_depth)); - } - - return true; - } - } -} - -bool convexPlaneIntersect(const Convex& s1, const Transform3d& tf1, - const Plane& s2, const Transform3d& tf2, - Vector3d* contact_points, FCL_REAL* penetration_depth, Vector3d* normal) -{ - Plane new_s2 = transform(s2, tf2); - - Vector3d v_min, v_max; - FCL_REAL d_min = std::numeric_limits::max(), d_max = -std::numeric_limits::max(); - - for(int i = 0; i < s1.num_points; ++i) - { - Vector3d p = tf1 * s1.points[i]; - - FCL_REAL d = new_s2.signedDistance(p); - - if(d < d_min) { d_min = d; v_min = p; } - if(d > d_max) { d_max = d; v_max = p; } - } - - if(d_min * d_max > 0) return false; - else - { - if(d_min + d_max > 0) - { - if(penetration_depth) *penetration_depth = -d_min; - if(normal) *normal = -new_s2.n; - if(contact_points) *contact_points = v_min - new_s2.n * d_min; - } - else - { - if(penetration_depth) *penetration_depth = d_max; - if(normal) *normal = new_s2.n; - if(contact_points) *contact_points = v_max - new_s2.n * d_max; - } - return true; - } -} - - - -bool planeTriangleIntersect(const Plane& s1, const Transform3d& tf1, - const Vector3d& P1, const Vector3d& P2, const Vector3d& P3, const Transform3d& tf2, - Vector3d* contact_points, FCL_REAL* penetration_depth, Vector3d* normal) -{ - Plane new_s1 = transform(s1, tf1); - - Vector3d c[3]; - c[0] = tf2 * P1; - c[1] = tf2 * P2; - c[2] = tf2 * P3; - - FCL_REAL d[3]; - d[0] = new_s1.signedDistance(c[0]); - d[1] = new_s1.signedDistance(c[1]); - d[2] = new_s1.signedDistance(c[2]); - - if((d[0] >= 0 && d[1] >= 0 && d[2] >= 0) || (d[0] <= 0 && d[1] <= 0 && d[2] <= 0)) - return false; - else - { - bool positive[3]; - for(std::size_t i = 0; i < 3; ++i) - positive[i] = (d[i] > 0); - - int n_positive = 0; - FCL_REAL d_positive = 0, d_negative = 0; - for(std::size_t i = 0; i < 3; ++i) - { - if(positive[i]) - { - n_positive++; - if(d_positive <= d[i]) d_positive = d[i]; - } - else - { - if(d_negative <= -d[i]) d_negative = -d[i]; - } - } - - if(penetration_depth) *penetration_depth = std::min(d_positive, d_negative); - if(normal) *normal = (d_positive > d_negative) ? new_s1.n : (-new_s1.n).eval(); - if(contact_points) - { - Vector3d p[2] = {Vector3d::Zero(), Vector3d::Zero()}; - Vector3d q = Vector3d::Zero(); - - FCL_REAL p_d[2]; - FCL_REAL q_d(0); - - if(n_positive == 2) - { - for(std::size_t i = 0, j = 0; i < 3; ++i) - { - if(positive[i]) { p[j] = c[i]; p_d[j] = d[i]; j++; } - else { q = c[i]; q_d = d[i]; } - } - - Vector3d t1 = (-p[0] * q_d + q * p_d[0]) / (-q_d + p_d[0]); - Vector3d t2 = (-p[1] * q_d + q * p_d[1]) / (-q_d + p_d[1]); - *contact_points = (t1 + t2) * 0.5; - } - else - { - for(std::size_t i = 0, j = 0; i < 3; ++i) - { - if(!positive[i]) { p[j] = c[i]; p_d[j] = d[i]; j++; } - else { q = c[i]; q_d = d[i]; } - } - - Vector3d t1 = (p[0] * q_d - q * p_d[0]) / (q_d - p_d[0]); - Vector3d t2 = (p[1] * q_d - q * p_d[1]) / (q_d - p_d[1]); - *contact_points = (t1 + t2) * 0.5; - } - } - return true; - } -} - -bool halfspacePlaneIntersect(const Halfspace& s1, const Transform3d& tf1, - const Plane& s2, const Transform3d& tf2, - Plane& pl, Vector3d& p, Vector3d& d, - FCL_REAL& penetration_depth, - int& ret) -{ - return planeHalfspaceIntersect(s2, tf2, s1, tf1, pl, p, d, penetration_depth, ret); -} - -bool planeIntersect(const Plane& s1, const Transform3d& tf1, - const Plane& s2, const Transform3d& tf2, - std::vector* /*contacts*/) -{ - Plane new_s1 = transform(s1, tf1); - Plane new_s2 = transform(s2, tf2); - - FCL_REAL a = (new_s1.n).dot(new_s2.n); - if(a == 1 && new_s1.d != new_s2.d) - return false; - if(a == -1 && new_s1.d != -new_s2.d) - return false; - - return true; -} - - - -} // details - -// Shape intersect algorithms not using libccd -// -// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -// | | box | sphere | ellipsoid | capsule | cone | cylinder | plane | half-space | triangle | -// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -// | box | O | | | | | | O | O | | -// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -// | sphere |/////| O | | O | | | O | O | O | -// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -// | ellipsoid |/////|////////| | | | | O | O | TODO | -// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -// | capsule |/////|////////|///////////| | | | O | O | | -// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -// | cone |/////|////////|///////////|/////////| | | O | O | | -// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -// | cylinder |/////|////////|///////////|/////////|//////| | O | O | | -// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -// | plane |/////|////////|///////////|/////////|//////|//////////| O | O | O | -// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -// | half-space |/////|////////|///////////|/////////|//////|//////////|///////| O | O | -// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -// | triangle |/////|////////|///////////|/////////|//////|//////////|///////|////////////| | -// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ - -void flipNormal(std::vector& contacts) -{ - for (std::vector::iterator it = contacts.begin(); it != contacts.end(); ++it) - (*it).normal *= -1.0; -} - -template<> -bool GJKSolver_libccd::shapeIntersect(const Sphere &s1, const Transform3d& tf1, - const Capsule &s2, const Transform3d& tf2, - std::vector* contacts) const -{ - return details::sphereCapsuleIntersect(s1, tf1, s2, tf2, contacts); -} - -template<> -bool GJKSolver_libccd::shapeIntersect(const Capsule &s1, const Transform3d& tf1, - const Sphere &s2, const Transform3d& tf2, - std::vector* contacts) const -{ - const bool res = details::sphereCapsuleIntersect(s2, tf2, s1, tf1, contacts); - if (contacts) flipNormal(*contacts); - return res; -} - -template<> -bool GJKSolver_libccd::shapeIntersect(const Sphere& s1, const Transform3d& tf1, - const Sphere& s2, const Transform3d& tf2, - std::vector* contacts) const -{ - return details::sphereSphereIntersect(s1, tf1, s2, tf2, contacts); -} - -template<> -bool GJKSolver_libccd::shapeIntersect(const Box& s1, const Transform3d& tf1, - const Box& s2, const Transform3d& tf2, - std::vector* contacts) const -{ - return details::boxBoxIntersect(s1, tf1, s2, tf2, contacts); -} - -template<> -bool GJKSolver_libccd::shapeIntersect(const Sphere& s1, const Transform3d& tf1, - const Halfspace& s2, const Transform3d& tf2, - std::vector* contacts) const -{ - return details::sphereHalfspaceIntersect(s1, tf1, s2, tf2, contacts); -} - -template<> -bool GJKSolver_libccd::shapeIntersect(const Halfspace& s1, const Transform3d& tf1, - const Sphere& s2, const Transform3d& tf2, - std::vector* contacts) const -{ - const bool res = details::sphereHalfspaceIntersect(s2, tf2, s1, tf1, contacts); - if (contacts) flipNormal(*contacts); - return res; -} - -template<> -bool GJKSolver_libccd::shapeIntersect(const Ellipsoid& s1, const Transform3d& tf1, - const Halfspace& s2, const Transform3d& tf2, - std::vector* contacts) const -{ - return details::ellipsoidHalfspaceIntersect(s1, tf1, s2, tf2, contacts); -} - -template<> -bool GJKSolver_libccd::shapeIntersect(const Halfspace& s1, const Transform3d& tf1, - const Ellipsoid& s2, const Transform3d& tf2, - std::vector* contacts) const -{ - const bool res = details::ellipsoidHalfspaceIntersect(s2, tf2, s1, tf1, contacts); - if (contacts) flipNormal(*contacts); - return res; -} - -template<> -bool GJKSolver_libccd::shapeIntersect(const Box& s1, const Transform3d& tf1, - const Halfspace& s2, const Transform3d& tf2, - std::vector* contacts) const -{ - return details::boxHalfspaceIntersect(s1, tf1, s2, tf2, contacts); -} - -template<> -bool GJKSolver_libccd::shapeIntersect(const Halfspace& s1, const Transform3d& tf1, - const Box& s2, const Transform3d& tf2, - std::vector* contacts) const -{ - const bool res = details::boxHalfspaceIntersect(s2, tf2, s1, tf1, contacts); - if (contacts) flipNormal(*contacts); - return res; -} - -template<> -bool GJKSolver_libccd::shapeIntersect(const Capsule& s1, const Transform3d& tf1, - const Halfspace& s2, const Transform3d& tf2, - std::vector* contacts) const -{ - return details::capsuleHalfspaceIntersect(s1, tf1, s2, tf2, contacts); -} - -template<> -bool GJKSolver_libccd::shapeIntersect(const Halfspace& s1, const Transform3d& tf1, - const Capsule& s2, const Transform3d& tf2, - std::vector* contacts) const -{ - const bool res = details::capsuleHalfspaceIntersect(s2, tf2, s1, tf1, contacts); - if (contacts) flipNormal(*contacts); - return res; -} - -template<> -bool GJKSolver_libccd::shapeIntersect(const Cylinder& s1, const Transform3d& tf1, - const Halfspace& s2, const Transform3d& tf2, - std::vector* contacts) const -{ - return details::cylinderHalfspaceIntersect(s1, tf1, s2, tf2, contacts); -} - -template<> -bool GJKSolver_libccd::shapeIntersect(const Halfspace& s1, const Transform3d& tf1, - const Cylinder& s2, const Transform3d& tf2, - std::vector* contacts) const -{ - const bool res = details::cylinderHalfspaceIntersect(s2, tf2, s1, tf1, contacts); - if (contacts) flipNormal(*contacts); - return res; -} - -template<> -bool GJKSolver_libccd::shapeIntersect(const Cone& s1, const Transform3d& tf1, - const Halfspace& s2, const Transform3d& tf2, - std::vector* contacts) const -{ - return details::coneHalfspaceIntersect(s1, tf1, s2, tf2, contacts); -} - -template<> -bool GJKSolver_libccd::shapeIntersect(const Halfspace& s1, const Transform3d& tf1, - const Cone& s2, const Transform3d& tf2, - std::vector* contacts) const -{ - const bool res = details::coneHalfspaceIntersect(s2, tf2, s1, tf1, contacts); - if (contacts) flipNormal(*contacts); - return res; -} - -template<> -bool GJKSolver_libccd::shapeIntersect(const Halfspace& s1, const Transform3d& tf1, - const Halfspace& s2, const Transform3d& tf2, - std::vector* contacts) const -{ - Halfspace s; - Vector3d p, d; - FCL_REAL depth; - int ret; - return details::halfspaceIntersect(s1, tf1, s2, tf2, p, d, s, depth, ret); -} - -template<> -bool GJKSolver_libccd::shapeIntersect(const Plane& s1, const Transform3d& tf1, - const Halfspace& s2, const Transform3d& tf2, - std::vector* contacts) const -{ - Plane pl; - Vector3d p, d; - FCL_REAL depth; - int ret; - return details::planeHalfspaceIntersect(s1, tf1, s2, tf2, pl, p, d, depth, ret); -} - -template<> -bool GJKSolver_libccd::shapeIntersect(const Halfspace& s1, const Transform3d& tf1, - const Plane& s2, const Transform3d& tf2, - std::vector* contacts) const -{ - Plane pl; - Vector3d p, d; - FCL_REAL depth; - int ret; - return details::halfspacePlaneIntersect(s1, tf1, s2, tf2, pl, p, d, depth, ret); -} - -template<> -bool GJKSolver_libccd::shapeIntersect(const Sphere& s1, const Transform3d& tf1, - const Plane& s2, const Transform3d& tf2, - std::vector* contacts) const -{ - return details::spherePlaneIntersect(s1, tf1, s2, tf2, contacts); -} - -template<> -bool GJKSolver_libccd::shapeIntersect(const Plane& s1, const Transform3d& tf1, - const Sphere& s2, const Transform3d& tf2, - std::vector* contacts) const -{ - const bool res = details::spherePlaneIntersect(s2, tf2, s1, tf1, contacts); - if (contacts) flipNormal(*contacts); - return res; -} - -template<> -bool GJKSolver_libccd::shapeIntersect(const Ellipsoid& s1, const Transform3d& tf1, - const Plane& s2, const Transform3d& tf2, - std::vector* contacts) const -{ - return details::ellipsoidPlaneIntersect(s1, tf1, s2, tf2, contacts); -} - -template<> -bool GJKSolver_libccd::shapeIntersect(const Plane& s1, const Transform3d& tf1, - const Ellipsoid& s2, const Transform3d& tf2, - std::vector* contacts) const -{ - const bool res = details::ellipsoidPlaneIntersect(s2, tf2, s1, tf1, contacts); - if (contacts) flipNormal(*contacts); - return res; -} - -template<> -bool GJKSolver_libccd::shapeIntersect(const Box& s1, const Transform3d& tf1, - const Plane& s2, const Transform3d& tf2, - std::vector* contacts) const -{ - return details::boxPlaneIntersect(s1, tf1, s2, tf2, contacts); -} - -template<> -bool GJKSolver_libccd::shapeIntersect(const Plane& s1, const Transform3d& tf1, - const Box& s2, const Transform3d& tf2, - std::vector* contacts) const -{ - const bool res = details::boxPlaneIntersect(s2, tf2, s1, tf1, contacts); - if (contacts) flipNormal(*contacts); - return res; -} - -template<> -bool GJKSolver_libccd::shapeIntersect(const Capsule& s1, const Transform3d& tf1, - const Plane& s2, const Transform3d& tf2, - std::vector* contacts) const -{ - return details::capsulePlaneIntersect(s1, tf1, s2, tf2, contacts); -} - -template<> -bool GJKSolver_libccd::shapeIntersect(const Plane& s1, const Transform3d& tf1, - const Capsule& s2, const Transform3d& tf2, - std::vector* contacts) const -{ - const bool res = details::capsulePlaneIntersect(s2, tf2, s1, tf1, contacts); - if (contacts) flipNormal(*contacts); - return res; -} - -template<> -bool GJKSolver_libccd::shapeIntersect(const Cylinder& s1, const Transform3d& tf1, - const Plane& s2, const Transform3d& tf2, - std::vector* contacts) const -{ - return details::cylinderPlaneIntersect(s1, tf1, s2, tf2, contacts); -} - -template<> -bool GJKSolver_libccd::shapeIntersect(const Plane& s1, const Transform3d& tf1, - const Cylinder& s2, const Transform3d& tf2, - std::vector* contacts) const -{ - const bool res = details::cylinderPlaneIntersect(s2, tf2, s1, tf1, contacts); - if (contacts) flipNormal(*contacts); - return res; -} - -template<> -bool GJKSolver_libccd::shapeIntersect(const Cone& s1, const Transform3d& tf1, - const Plane& s2, const Transform3d& tf2, - std::vector* contacts) const -{ - return details::conePlaneIntersect(s1, tf1, s2, tf2, contacts); -} - -template<> -bool GJKSolver_libccd::shapeIntersect(const Plane& s1, const Transform3d& tf1, - const Cone& s2, const Transform3d& tf2, - std::vector* contacts) const -{ - const bool res = details::conePlaneIntersect(s2, tf2, s1, tf1, contacts); - if (contacts) flipNormal(*contacts); - return res; -} - -template<> -bool GJKSolver_libccd::shapeIntersect(const Plane& s1, const Transform3d& tf1, - const Plane& s2, const Transform3d& tf2, - std::vector* contacts) const -{ - return details::planeIntersect(s1, tf1, s2, tf2, contacts); -} - - - - -template<> -bool GJKSolver_libccd::shapeTriangleIntersect(const Sphere& s, const Transform3d& tf, - const Vector3d& P1, const Vector3d& P2, const Vector3d& P3, Vector3d* contact_points, FCL_REAL* penetration_depth, Vector3d* normal) const -{ - return details::sphereTriangleIntersect(s, tf, P1, P2, P3, contact_points, penetration_depth, normal); -} - -template<> -bool GJKSolver_libccd::shapeTriangleIntersect(const Sphere& s, const Transform3d& tf1, - const Vector3d& P1, const Vector3d& P2, const Vector3d& P3, const Transform3d& tf2, Vector3d* contact_points, FCL_REAL* penetration_depth, Vector3d* normal) const -{ - return details::sphereTriangleIntersect(s, tf1, tf2 * P1, tf2 * P2, tf2 * P3, contact_points, penetration_depth, normal); -} - -template<> -bool GJKSolver_libccd::shapeTriangleIntersect(const Halfspace& s, const Transform3d& tf1, - const Vector3d& P1, const Vector3d& P2, const Vector3d& P3, const Transform3d& tf2, Vector3d* contact_points, FCL_REAL* penetration_depth, Vector3d* normal) const -{ - return details::halfspaceTriangleIntersect(s, tf1, P1, P2, P3, tf2, contact_points, penetration_depth, normal); -} - -template<> -bool GJKSolver_libccd::shapeTriangleIntersect(const Plane& s, const Transform3d& tf1, - const Vector3d& P1, const Vector3d& P2, const Vector3d& P3, const Transform3d& tf2, Vector3d* contact_points, FCL_REAL* penetration_depth, Vector3d* normal) const -{ - return details::planeTriangleIntersect(s, tf1, P1, P2, P3, tf2, contact_points, penetration_depth, normal); -} - -// Shape distance algorithms not using libccd -// -// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -// | | box | sphere | ellipsoid | capsule | cone | cylinder | plane | half-space | triangle | -// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -// | box | | | | | | | | | | -// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -// | sphere |/////| O | | O | | | | | O | -// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -// | ellipsoid |/////|////////| | | | | | | | -// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -// | capsule |/////|////////|///////////| O | | | | | | -// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -// | cone |/////|////////|///////////|/////////| | | | | | -// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -// | cylinder |/////|////////|///////////|/////////|//////| | | | | -// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -// | plane |/////|////////|///////////|/////////|//////|//////////| | | | -// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -// | half-space |/////|////////|///////////|/////////|//////|//////////|///////| | | -// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -// | triangle |/////|////////|///////////|/////////|//////|//////////|///////|////////////| | -// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ - -template<> -bool GJKSolver_libccd::shapeDistance(const Sphere& s1, const Transform3d& tf1, - const Capsule& s2, const Transform3d& tf2, - FCL_REAL* dist, Vector3d* p1, Vector3d* p2) const -{ - return details::sphereCapsuleDistance(s1, tf1, s2, tf2, dist, p1, p2); -} - -template<> -bool GJKSolver_libccd::shapeDistance(const Capsule& s1, const Transform3d& tf1, - const Sphere& s2, const Transform3d& tf2, - FCL_REAL* dist, Vector3d* p1, Vector3d* p2) const -{ - return details::sphereCapsuleDistance(s2, tf2, s1, tf1, dist, p2, p1); -} - -template<> -bool GJKSolver_libccd::shapeDistance(const Sphere& s1, const Transform3d& tf1, - const Sphere& s2, const Transform3d& tf2, - FCL_REAL* dist, Vector3d* p1, Vector3d* p2) const -{ - return details::sphereSphereDistance(s1, tf1, s2, tf2, dist, p1, p2); -} - -template<> -bool GJKSolver_libccd::shapeDistance(const Capsule& s1, const Transform3d& tf1, - const Capsule& s2, const Transform3d& tf2, - FCL_REAL* dist, Vector3d* p1, Vector3d* p2) const -{ - return details::capsuleCapsuleDistance(s1, tf1, s2, tf2, dist, p1, p2); -} - - - - -template<> -bool GJKSolver_libccd::shapeTriangleDistance(const Sphere& s, const Transform3d& tf, - const Vector3d& P1, const Vector3d& P2, const Vector3d& P3, - FCL_REAL* dist, Vector3d* p1, Vector3d* p2) const -{ - return details::sphereTriangleDistance(s, tf, P1, P2, P3, dist, p1, p2); -} - -template<> -bool GJKSolver_libccd::shapeTriangleDistance(const Sphere& s, const Transform3d& tf1, - const Vector3d& P1, const Vector3d& P2, const Vector3d& P3, const Transform3d& tf2, - FCL_REAL* dist, Vector3d* p1, Vector3d* p2) const -{ - return details::sphereTriangleDistance(s, tf1, P1, P2, P3, tf2, dist, p1, p2); -} - -// Shape intersect algorithms not using built-in GJK algorithm -// -// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -// | | box | sphere | ellipsoid | capsule | cone | cylinder | plane | half-space | triangle | -// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -// | box | O | | | | | | O | O | | -// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -// | sphere |/////| O | | O | | | O | O | O | -// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -// | ellipsoid |/////|////////| | | | | O | O | | -// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -// | capsule |/////|////////|///////////| | | | O | O | | -// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -// | cone |/////|////////|///////////|/////////| | | O | O | | -// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -// | cylinder |/////|////////|///////////|/////////|//////| | O | O | | -// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -// | plane |/////|////////|///////////|/////////|//////|//////////| O | O | O | -// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -// | half-space |/////|////////|///////////|/////////|//////|//////////|///////| O | O | -// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -// | triangle |/////|////////|///////////|/////////|//////|//////////|///////|////////////| | -// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ - -template<> -bool GJKSolver_indep::shapeIntersect(const Sphere &s1, const Transform3d& tf1, - const Capsule &s2, const Transform3d& tf2, - std::vector* contacts) const -{ - return details::sphereCapsuleIntersect(s1, tf1, s2, tf2, contacts); -} - -template<> -bool GJKSolver_indep::shapeIntersect(const Capsule &s1, const Transform3d& tf1, - const Sphere &s2, const Transform3d& tf2, - std::vector* contacts) const -{ - const bool res = details::sphereCapsuleIntersect(s2, tf2, s1, tf1, contacts); - if (contacts) flipNormal(*contacts); - return res; -} - -template<> -bool GJKSolver_indep::shapeIntersect(const Sphere& s1, const Transform3d& tf1, - const Sphere& s2, const Transform3d& tf2, - std::vector* contacts) const -{ - return details::sphereSphereIntersect(s1, tf1, s2, tf2, contacts); -} - -template<> -bool GJKSolver_indep::shapeIntersect(const Box& s1, const Transform3d& tf1, - const Box& s2, const Transform3d& tf2, - std::vector* contacts) const -{ - return details::boxBoxIntersect(s1, tf1, s2, tf2, contacts); -} - -template<> -bool GJKSolver_indep::shapeIntersect(const Sphere& s1, const Transform3d& tf1, - const Halfspace& s2, const Transform3d& tf2, - std::vector* contacts) const -{ - return details::sphereHalfspaceIntersect(s1, tf1, s2, tf2, contacts); -} - -template<> -bool GJKSolver_indep::shapeIntersect(const Halfspace& s1, const Transform3d& tf1, - const Sphere& s2, const Transform3d& tf2, - std::vector* contacts) const -{ - const bool res = details::sphereHalfspaceIntersect(s2, tf2, s1, tf1, contacts); - if (contacts) flipNormal(*contacts); - return res; -} - -template<> -bool GJKSolver_indep::shapeIntersect(const Ellipsoid& s1, const Transform3d& tf1, - const Halfspace& s2, const Transform3d& tf2, - std::vector* contacts) const -{ - return details::ellipsoidHalfspaceIntersect(s1, tf1, s2, tf2, contacts); -} - -template<> -bool GJKSolver_indep::shapeIntersect(const Halfspace& s1, const Transform3d& tf1, - const Ellipsoid& s2, const Transform3d& tf2, - std::vector* contacts) const -{ - const bool res = details::ellipsoidHalfspaceIntersect(s2, tf2, s1, tf1, contacts); - if (contacts) flipNormal(*contacts); - return res; -} - -template<> -bool GJKSolver_indep::shapeIntersect(const Box& s1, const Transform3d& tf1, - const Halfspace& s2, const Transform3d& tf2, - std::vector* contacts) const -{ - return details::boxHalfspaceIntersect(s1, tf1, s2, tf2, contacts); -} - -template<> -bool GJKSolver_indep::shapeIntersect(const Halfspace& s1, const Transform3d& tf1, - const Box& s2, const Transform3d& tf2, - std::vector* contacts) const -{ - const bool res = details::boxHalfspaceIntersect(s2, tf2, s1, tf1, contacts); - if (contacts) flipNormal(*contacts); - return res; -} - -template<> -bool GJKSolver_indep::shapeIntersect(const Capsule& s1, const Transform3d& tf1, - const Halfspace& s2, const Transform3d& tf2, - std::vector* contacts) const -{ - return details::capsuleHalfspaceIntersect(s1, tf1, s2, tf2, contacts); -} - -template<> -bool GJKSolver_indep::shapeIntersect(const Halfspace& s1, const Transform3d& tf1, - const Capsule& s2, const Transform3d& tf2, - std::vector* contacts) const -{ - const bool res = details::capsuleHalfspaceIntersect(s2, tf2, s1, tf1, contacts); - if (contacts) flipNormal(*contacts); - return res; -} - -template<> -bool GJKSolver_indep::shapeIntersect(const Cylinder& s1, const Transform3d& tf1, - const Halfspace& s2, const Transform3d& tf2, - std::vector* contacts) const -{ - return details::cylinderHalfspaceIntersect(s1, tf1, s2, tf2, contacts); -} - -template<> -bool GJKSolver_indep::shapeIntersect(const Halfspace& s1, const Transform3d& tf1, - const Cylinder& s2, const Transform3d& tf2, - std::vector* contacts) const -{ - const bool res = details::cylinderHalfspaceIntersect(s2, tf2, s1, tf1, contacts); - if (contacts) flipNormal(*contacts); - return res; -} - -template<> -bool GJKSolver_indep::shapeIntersect(const Cone& s1, const Transform3d& tf1, - const Halfspace& s2, const Transform3d& tf2, - std::vector* contacts) const -{ - return details::coneHalfspaceIntersect(s1, tf1, s2, tf2, contacts); -} - -template<> -bool GJKSolver_indep::shapeIntersect(const Halfspace& s1, const Transform3d& tf1, - const Cone& s2, const Transform3d& tf2, - std::vector* contacts) const -{ - const bool res = details::coneHalfspaceIntersect(s2, tf2, s1, tf1, contacts); - if (contacts) flipNormal(*contacts); - return res; -} - -template<> -bool GJKSolver_indep::shapeIntersect(const Halfspace& s1, const Transform3d& tf1, - const Halfspace& s2, const Transform3d& tf2, - std::vector* contacts) const -{ - Halfspace s; - Vector3d p, d; - FCL_REAL depth; - int ret; - return details::halfspaceIntersect(s1, tf1, s2, tf2, p, d, s, depth, ret); -} - -template<> -bool GJKSolver_indep::shapeIntersect(const Plane& s1, const Transform3d& tf1, - const Halfspace& s2, const Transform3d& tf2, - std::vector* contacts) const -{ - Plane pl; - Vector3d p, d; - FCL_REAL depth; - int ret; - return details::planeHalfspaceIntersect(s1, tf1, s2, tf2, pl, p, d, depth, ret); -} - -template<> -bool GJKSolver_indep::shapeIntersect(const Halfspace& s1, const Transform3d& tf1, - const Plane& s2, const Transform3d& tf2, - std::vector* contacts) const -{ - Plane pl; - Vector3d p, d; - FCL_REAL depth; - int ret; - return details::halfspacePlaneIntersect(s1, tf1, s2, tf2, pl, p, d, depth, ret); -} - -template<> -bool GJKSolver_indep::shapeIntersect(const Sphere& s1, const Transform3d& tf1, - const Plane& s2, const Transform3d& tf2, - std::vector* contacts) const -{ - return details::spherePlaneIntersect(s1, tf1, s2, tf2, contacts); -} - -template<> -bool GJKSolver_indep::shapeIntersect(const Plane& s1, const Transform3d& tf1, - const Sphere& s2, const Transform3d& tf2, - std::vector* contacts) const -{ - const bool res = details::spherePlaneIntersect(s2, tf2, s1, tf1, contacts); - if (contacts) flipNormal(*contacts); - return res; -} - -template<> -bool GJKSolver_indep::shapeIntersect(const Ellipsoid& s1, const Transform3d& tf1, - const Plane& s2, const Transform3d& tf2, - std::vector* contacts) const -{ - return details::ellipsoidPlaneIntersect(s1, tf1, s2, tf2, contacts); -} - -template<> -bool GJKSolver_indep::shapeIntersect(const Plane& s1, const Transform3d& tf1, - const Ellipsoid& s2, const Transform3d& tf2, - std::vector* contacts) const -{ - const bool res = details::ellipsoidPlaneIntersect(s2, tf2, s1, tf1, contacts); - if (contacts) flipNormal(*contacts); - return res; -} - -template<> -bool GJKSolver_indep::shapeIntersect(const Box& s1, const Transform3d& tf1, - const Plane& s2, const Transform3d& tf2, - std::vector* contacts) const -{ - return details::boxPlaneIntersect(s1, tf1, s2, tf2, contacts); -} - -template<> -bool GJKSolver_indep::shapeIntersect(const Plane& s1, const Transform3d& tf1, - const Box& s2, const Transform3d& tf2, - std::vector* contacts) const -{ - const bool res = details::boxPlaneIntersect(s2, tf2, s1, tf1, contacts); - if (contacts) flipNormal(*contacts); - return res; -} - -template<> -bool GJKSolver_indep::shapeIntersect(const Capsule& s1, const Transform3d& tf1, - const Plane& s2, const Transform3d& tf2, - std::vector* contacts) const -{ - return details::capsulePlaneIntersect(s1, tf1, s2, tf2, contacts); -} - -template<> -bool GJKSolver_indep::shapeIntersect(const Plane& s1, const Transform3d& tf1, - const Capsule& s2, const Transform3d& tf2, - std::vector* contacts) const -{ - const bool res = details::capsulePlaneIntersect(s2, tf2, s1, tf1, contacts); - if (contacts) flipNormal(*contacts); - return res; -} - -template<> -bool GJKSolver_indep::shapeIntersect(const Cylinder& s1, const Transform3d& tf1, - const Plane& s2, const Transform3d& tf2, - std::vector* contacts) const -{ - return details::cylinderPlaneIntersect(s1, tf1, s2, tf2, contacts); -} - -template<> -bool GJKSolver_indep::shapeIntersect(const Plane& s1, const Transform3d& tf1, - const Cylinder& s2, const Transform3d& tf2, - std::vector* contacts) const -{ - const bool res = details::cylinderPlaneIntersect(s2, tf2, s1, tf1, contacts); - if (contacts) flipNormal(*contacts); - return res; -} - -template<> -bool GJKSolver_indep::shapeIntersect(const Cone& s1, const Transform3d& tf1, - const Plane& s2, const Transform3d& tf2, - std::vector* contacts) const -{ - return details::conePlaneIntersect(s1, tf1, s2, tf2, contacts); -} - -template<> -bool GJKSolver_indep::shapeIntersect(const Plane& s1, const Transform3d& tf1, - const Cone& s2, const Transform3d& tf2, - std::vector* contacts) const -{ - const bool res = details::conePlaneIntersect(s2, tf2, s1, tf1, contacts); - if (contacts) flipNormal(*contacts); - return res; -} - -template<> -bool GJKSolver_indep::shapeIntersect(const Plane& s1, const Transform3d& tf1, - const Plane& s2, const Transform3d& tf2, - std::vector* contacts) const -{ - return details::planeIntersect(s1, tf1, s2, tf2, contacts); -} - - - - -template<> -bool GJKSolver_indep::shapeTriangleIntersect(const Sphere& s, const Transform3d& tf, - const Vector3d& P1, const Vector3d& P2, const Vector3d& P3, Vector3d* contact_points, FCL_REAL* penetration_depth, Vector3d* normal) const -{ - return details::sphereTriangleIntersect(s, tf, P1, P2, P3, contact_points, penetration_depth, normal); -} - -template<> -bool GJKSolver_indep::shapeTriangleIntersect(const Sphere& s, const Transform3d& tf1, - const Vector3d& P1, const Vector3d& P2, const Vector3d& P3, const Transform3d& tf2, Vector3d* contact_points, FCL_REAL* penetration_depth, Vector3d* normal) const -{ - return details::sphereTriangleIntersect(s, tf1, tf2 * P1, tf2 * P2, tf2 * P3, contact_points, penetration_depth, normal); -} - -template<> -bool GJKSolver_indep::shapeTriangleIntersect(const Halfspace& s, const Transform3d& tf1, - const Vector3d& P1, const Vector3d& P2, const Vector3d& P3, const Transform3d& tf2, Vector3d* contact_points, FCL_REAL* penetration_depth, Vector3d* normal) const -{ - return details::halfspaceTriangleIntersect(s, tf1, P1, P2, P3, tf2, contact_points, penetration_depth, normal); -} - -template<> -bool GJKSolver_indep::shapeTriangleIntersect(const Plane& s, const Transform3d& tf1, - const Vector3d& P1, const Vector3d& P2, const Vector3d& P3, const Transform3d& tf2, Vector3d* contact_points, FCL_REAL* penetration_depth, Vector3d* normal) const -{ - return details::planeTriangleIntersect(s, tf1, P1, P2, P3, tf2, contact_points, penetration_depth, normal); -} - -// Shape distance algorithms not using built-in GJK algorithm -// -// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -// | | box | sphere | ellipsoid | capsule | cone | cylinder | plane | half-space | triangle | -// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -// | box | | | | | | | | | | -// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -// | sphere |/////| O | | O | | | | | O | -// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -// | ellipsoid |/////|////////| | | | | | | | -// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -// | capsule |/////|////////|///////////| O | | | | | | -// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -// | cone |/////|////////|///////////|/////////| | | | | | -// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -// | cylinder |/////|////////|///////////|/////////|//////| | | | | -// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -// | plane |/////|////////|///////////|/////////|//////|//////////| | | | -// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -// | half-space |/////|////////|///////////|/////////|//////|//////////|///////| | | -// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -// | triangle |/////|////////|///////////|/////////|//////|//////////|///////|////////////| | -// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ - -template<> -bool GJKSolver_indep::shapeDistance(const Sphere& s1, const Transform3d& tf1, - const Capsule& s2, const Transform3d& tf2, - FCL_REAL* dist, Vector3d* p1, Vector3d* p2) const -{ - return details::sphereCapsuleDistance(s1, tf1, s2, tf2, dist, p1, p2); -} - -template<> -bool GJKSolver_indep::shapeDistance(const Capsule& s1, const Transform3d& tf1, - const Sphere& s2, const Transform3d& tf2, - FCL_REAL* dist, Vector3d* p1, Vector3d* p2) const -{ - return details::sphereCapsuleDistance(s2, tf2, s1, tf1, dist, p2, p1); -} - -template<> -bool GJKSolver_indep::shapeDistance(const Sphere& s1, const Transform3d& tf1, - const Sphere& s2, const Transform3d& tf2, - FCL_REAL* dist, Vector3d* p1, Vector3d* p2) const -{ - return details::sphereSphereDistance(s1, tf1, s2, tf2, dist, p1, p2); -} - -template<> -bool GJKSolver_indep::shapeDistance(const Capsule& s1, const Transform3d& tf1, - const Capsule& s2, const Transform3d& tf2, - FCL_REAL* dist, Vector3d* p1, Vector3d* p2) const -{ - return details::capsuleCapsuleDistance(s1, tf1, s2, tf2, dist, p1, p2); -} - - - - -template<> -bool GJKSolver_indep::shapeTriangleDistance(const Sphere& s, const Transform3d& tf, - const Vector3d& P1, const Vector3d& P2, const Vector3d& P3, - FCL_REAL* dist, Vector3d* p1, Vector3d* p2) const -{ - return details::sphereTriangleDistance(s, tf, P1, P2, P3, dist, p1, p2); -} - -template<> -bool GJKSolver_indep::shapeTriangleDistance(const Sphere& s, const Transform3d& tf1, - const Vector3d& P1, const Vector3d& P2, const Vector3d& P3, const Transform3d& tf2, - FCL_REAL* dist, Vector3d* p1, Vector3d* p2) const -{ - return details::sphereTriangleDistance(s, tf1, P1, P2, P3, tf2, dist, p1, p2); -} - -} // fcl diff --git a/src/profile.cpp b/src/profile.cpp deleted file mode 100644 index f5d9b8c4a..000000000 --- a/src/profile.cpp +++ /dev/null @@ -1,262 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2008-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - - -/** \author Ioan Sucan */ - -#include "fcl/profile.h" -#include - -fcl::tools::Profiler& fcl::tools::Profiler::Instance(void) -{ - static Profiler p(true, false); - return p; -} - -#if ENABLE_PROFILING - -#include -#include -#include - -void fcl::tools::Profiler::start(void) -{ - lock_.lock(); - if (!running_) - { - tinfo_.set(); - running_ = true; - } - lock_.unlock(); -} - -void fcl::tools::Profiler::stop(void) -{ - lock_.lock(); - if (running_) - { - tinfo_.update(); - running_ = false; - } - lock_.unlock(); -} - -void fcl::tools::Profiler::clear(void) -{ - lock_.lock(); - data_.clear(); - tinfo_ = TimeInfo(); - if (running_) - tinfo_.set(); - lock_.unlock(); -} - -void fcl::tools::Profiler::event(const std::string &name, const unsigned int times) -{ - lock_.lock(); - data_[std::this_thread::get_id()].events[name] += times; - lock_.unlock(); -} - -void fcl::tools::Profiler::average(const std::string &name, const double value) -{ - lock_.lock(); - AvgInfo &a = data_[std::this_thread::get_id()].avg[name]; - a.total += value; - a.totalSqr += value*value; - a.parts++; - lock_.unlock(); -} - -void fcl::tools::Profiler::begin(const std::string &name) -{ - lock_.lock(); - data_[std::this_thread::get_id()].time[name].set(); - lock_.unlock(); -} - -void fcl::tools::Profiler::end(const std::string &name) -{ - lock_.lock(); - data_[std::this_thread::get_id()].time[name].update(); - lock_.unlock(); -} - -void fcl::tools::Profiler::status(std::ostream &out, bool merge) -{ - stop(); - lock_.lock(); - printOnDestroy_ = false; - - out << std::endl; - out << " *** Profiling statistics. Total counted time : " << time::seconds(tinfo_.total) << " seconds" << std::endl; - - if (merge) - { - PerThread combined; - for (std::map::const_iterator it = data_.begin() ; it != data_.end() ; ++it) - { - for (std::map::const_iterator iev = it->second.events.begin() ; iev != it->second.events.end(); ++iev) - combined.events[iev->first] += iev->second; - for (std::map::const_iterator iavg = it->second.avg.begin() ; iavg != it->second.avg.end(); ++iavg) - { - combined.avg[iavg->first].total += iavg->second.total; - combined.avg[iavg->first].totalSqr += iavg->second.totalSqr; - combined.avg[iavg->first].parts += iavg->second.parts; - } - for (std::map::const_iterator itm = it->second.time.begin() ; itm != it->second.time.end(); ++itm) - { - TimeInfo &tc = combined.time[itm->first]; - tc.total = tc.total + itm->second.total; - tc.parts = tc.parts + itm->second.parts; - if (tc.shortest > itm->second.shortest) - tc.shortest = itm->second.shortest; - if (tc.longest < itm->second.longest) - tc.longest = itm->second.longest; - } - } - printThreadInfo(out, combined); - } - else - for (std::map::const_iterator it = data_.begin() ; it != data_.end() ; ++it) - { - out << "Thread " << it->first << ":" << std::endl; - printThreadInfo(out, it->second); - } - lock_.unlock(); -} - - -/// @cond IGNORE -namespace fcl -{ - -struct dataIntVal -{ - std::string name; - unsigned long int value; -}; - -struct SortIntByValue -{ - bool operator()(const dataIntVal &a, const dataIntVal &b) const - { - return a.value > b.value; - } -}; - -struct dataDoubleVal -{ - std::string name; - double value; -}; - -struct SortDoubleByValue -{ - bool operator()(const dataDoubleVal &a, const dataDoubleVal &b) const - { - return a.value > b.value; - } -}; -} -/// @endcond - -void fcl::tools::Profiler::printThreadInfo(std::ostream &out, const PerThread &data) -{ - double total = time::seconds(tinfo_.total); - - std::vector events; - for (std::map::const_iterator iev = data.events.begin() ; iev != data.events.end() ; ++iev) - { - dataIntVal next = {iev->first, iev->second}; - events.push_back(next); - } - std::sort(events.begin(), events.end(), SortIntByValue()); - if (!events.empty()) - out << "Events:" << std::endl; - for (unsigned int i = 0 ; i < events.size() ; ++i) - out << events[i].name << ": " << events[i].value << std::endl; - - std::vector avg; - for (std::map::const_iterator ia = data.avg.begin() ; ia != data.avg.end() ; ++ia) - { - dataDoubleVal next = {ia->first, ia->second.total / (double)ia->second.parts}; - avg.push_back(next); - } - std::sort(avg.begin(), avg.end(), SortDoubleByValue()); - if (!avg.empty()) - out << "Averages:" << std::endl; - for (unsigned int i = 0 ; i < avg.size() ; ++i) - { - const AvgInfo &a = data.avg.find(avg[i].name)->second; - out << avg[i].name << ": " << avg[i].value << " (stddev = " << - std::sqrt(std::abs(a.totalSqr - (double)a.parts * avg[i].value * avg[i].value) / ((double)a.parts - 1.)) << ")" << std::endl; - } - - std::vector time; - - for (std::map::const_iterator itm = data.time.begin() ; itm != data.time.end() ; ++itm) - { - dataDoubleVal next = {itm->first, time::seconds(itm->second.total)}; - time.push_back(next); - } - - std::sort(time.begin(), time.end(), SortDoubleByValue()); - if (!time.empty()) - out << "Blocks of time:" << std::endl; - - double unaccounted = total; - for (unsigned int i = 0 ; i < time.size() ; ++i) - { - const TimeInfo &d = data.time.find(time[i].name)->second; - - double tS = time::seconds(d.shortest); - double tL = time::seconds(d.longest); - out << time[i].name << ": " << time[i].value << "s (" << (100.0 * time[i].value/total) << "%), [" - << tS << "s --> " << tL << " s], " << d.parts << " parts"; - if (d.parts > 0) - out << ", " << (time::seconds(d.total) / (double)d.parts) << " s on average"; - out << std::endl; - unaccounted -= time[i].value; - } - out << "Unaccounted time : " << unaccounted; - if (total > 0.0) - out << " (" << (100.0 * unaccounted / total) << " %)"; - out << std::endl; - - out << std::endl; -} - -#endif diff --git a/src/shape/geometric_shapes.cpp b/src/shape/geometric_shapes.cpp deleted file mode 100644 index d551f5357..000000000 --- a/src/shape/geometric_shapes.cpp +++ /dev/null @@ -1,205 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - - -#include "fcl/shape/geometric_shapes.h" -#include "fcl/shape/geometric_shapes_utility.h" - -namespace fcl -{ - -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; - } -} - -void Halfspace::unitNormalTest() -{ - FCL_REAL l = n.norm(); - if(l > 0) - { - FCL_REAL inv_l = 1.0 / l; - n *= inv_l; - d *= inv_l; - } - else - { - n << 1, 0, 0; - d = 0; - } -} - -void Plane::unitNormalTest() -{ - FCL_REAL l = n.norm(); - if(l > 0) - { - FCL_REAL inv_l = 1.0 / l; - n *= inv_l; - d *= inv_l; - } - else - { - n << 1, 0, 0; - d = 0; - } -} - - -void Box::computeLocalAABB() -{ - computeBV(*this, Transform3d::Identity(), aabb_local); - aabb_center = aabb_local.center(); - aabb_radius = (aabb_local.min_ - aabb_center).norm(); -} - -void Sphere::computeLocalAABB() -{ - computeBV(*this, Transform3d::Identity(), aabb_local); - aabb_center = aabb_local.center(); - aabb_radius = radius; -} - -void Ellipsoid::computeLocalAABB() -{ - computeBV(*this, Transform3d::Identity(), aabb_local); - aabb_center = aabb_local.center(); - aabb_radius = (aabb_local.min_ - aabb_center).norm(); -} - -void Capsule::computeLocalAABB() -{ - computeBV(*this, Transform3d::Identity(), aabb_local); - aabb_center = aabb_local.center(); - aabb_radius = (aabb_local.min_ - aabb_center).norm(); -} - -void Cone::computeLocalAABB() -{ - computeBV(*this, Transform3d::Identity(), aabb_local); - aabb_center = aabb_local.center(); - aabb_radius = (aabb_local.min_ - aabb_center).norm(); -} - -void Cylinder::computeLocalAABB() -{ - computeBV(*this, Transform3d::Identity(), aabb_local); - aabb_center = aabb_local.center(); - aabb_radius = (aabb_local.min_ - aabb_center).norm(); -} - -void Convex::computeLocalAABB() -{ - computeBV(*this, Transform3d::Identity(), aabb_local); - aabb_center = aabb_local.center(); - aabb_radius = (aabb_local.min_ - aabb_center).norm(); -} - -void Halfspace::computeLocalAABB() -{ - computeBV(*this, Transform3d::Identity(), aabb_local); - aabb_center = aabb_local.center(); - aabb_radius = (aabb_local.min_ - aabb_center).norm(); -} - -void Plane::computeLocalAABB() -{ - computeBV(*this, Transform3d::Identity(), aabb_local); - aabb_center = aabb_local.center(); - aabb_radius = (aabb_local.min_ - aabb_center).norm(); -} - -void TriangleP::computeLocalAABB() -{ - computeBV(*this, Transform3d::Identity(), aabb_local); - aabb_center = aabb_local.center(); - aabb_radius = (aabb_local.min_ - aabb_center).norm(); -} - - -} diff --git a/src/shape/geometric_shapes_utility.cpp b/src/shape/geometric_shapes_utility.cpp deleted file mode 100644 index 6a37c6598..000000000 --- a/src/shape/geometric_shapes_utility.cpp +++ /dev/null @@ -1,1081 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#include "fcl/math/geometry.h" -#include "fcl/shape/geometric_shapes_utility.h" -#include "fcl/BVH/BV_fitter.h" - -namespace fcl -{ - -namespace details -{ - -std::vector getBoundVertices(const Box& box, const Transform3d& tf) -{ - std::vector result(8); - FCL_REAL a = box.side[0] / 2; - FCL_REAL b = box.side[1] / 2; - FCL_REAL c = box.side[2] / 2; - result[0] = tf * Vector3d(a, b, c); - result[1] = tf * Vector3d(a, b, -c); - result[2] = tf * Vector3d(a, -b, c); - result[3] = tf * Vector3d(a, -b, -c); - result[4] = tf * Vector3d(-a, b, c); - result[5] = tf * Vector3d(-a, b, -c); - result[6] = tf * Vector3d(-a, -b, c); - result[7] = tf * Vector3d(-a, -b, -c); - - return result; -} - -// we use icosahedron to bound the sphere -std::vector getBoundVertices(const Sphere& sphere, const Transform3d& tf) -{ - std::vector result(12); - const FCL_REAL m = (1 + sqrt(5.0)) / 2.0; - FCL_REAL edge_size = sphere.radius * 6 / (sqrt(27.0) + sqrt(15.0)); - - FCL_REAL a = edge_size; - FCL_REAL b = m * edge_size; - result[0] = tf * Vector3d(0, a, b); - result[1] = tf * Vector3d(0, -a, b); - result[2] = tf * Vector3d(0, a, -b); - result[3] = tf * Vector3d(0, -a, -b); - result[4] = tf * Vector3d(a, b, 0); - result[5] = tf * Vector3d(-a, b, 0); - result[6] = tf * Vector3d(a, -b, 0); - result[7] = tf * Vector3d(-a, -b, 0); - result[8] = tf * Vector3d(b, 0, a); - result[9] = tf * Vector3d(b, 0, -a); - result[10] = tf * Vector3d(-b, 0, a); - result[11] = tf * Vector3d(-b, 0, -a); - - return result; -} - -std::vector getBoundVertices(const Ellipsoid& ellipsoid, const Transform3d& tf) -{ - // we use scaled icosahedron to bound the ellipsoid - - std::vector result(12); - - const FCL_REAL phi = (1.0 + std::sqrt(5.0)) / 2.0; // golden ratio - - const FCL_REAL a = std::sqrt(3.0) / (phi * phi); - const FCL_REAL b = phi * a; - - const FCL_REAL& A = ellipsoid.radii[0]; - const FCL_REAL& B = ellipsoid.radii[1]; - const FCL_REAL& C = ellipsoid.radii[2]; - - const FCL_REAL Aa = A * a; - const FCL_REAL Ab = A * b; - const FCL_REAL Ba = B * a; - const FCL_REAL Bb = B * b; - const FCL_REAL Ca = C * a; - const FCL_REAL Cb = C * b; - - result[0] = tf * Vector3d(0, Ba, Cb); - result[1] = tf * Vector3d(0, -Ba, Cb); - result[2] = tf * Vector3d(0, Ba, -Cb); - result[3] = tf * Vector3d(0, -Ba, -Cb); - result[4] = tf * Vector3d(Aa, Bb, 0); - result[5] = tf * Vector3d(-Aa, Bb, 0); - result[6] = tf * Vector3d(Aa, -Bb, 0); - result[7] = tf * Vector3d(-Aa, -Bb, 0); - result[8] = tf * Vector3d(Ab, 0, Ca); - result[9] = tf * Vector3d(Ab, 0, -Ca); - result[10] = tf * Vector3d(-Ab, 0, Ca); - result[11] = tf * Vector3d(-Ab, 0, -Ca); - - return result; -} - -std::vector getBoundVertices(const Capsule& capsule, const Transform3d& tf) -{ - std::vector result(36); - const FCL_REAL m = (1 + sqrt(5.0)) / 2.0; - - FCL_REAL hl = capsule.lz * 0.5; - FCL_REAL edge_size = capsule.radius * 6 / (sqrt(27.0) + sqrt(15.0)); - FCL_REAL a = edge_size; - FCL_REAL b = m * edge_size; - FCL_REAL r2 = capsule.radius * 2 / sqrt(3.0); - - - result[0] = tf * Vector3d(0, a, b + hl); - result[1] = tf * Vector3d(0, -a, b + hl); - result[2] = tf * Vector3d(0, a, -b + hl); - result[3] = tf * Vector3d(0, -a, -b + hl); - result[4] = tf * Vector3d(a, b, hl); - result[5] = tf * Vector3d(-a, b, hl); - result[6] = tf * Vector3d(a, -b, hl); - result[7] = tf * Vector3d(-a, -b, hl); - result[8] = tf * Vector3d(b, 0, a + hl); - result[9] = tf * Vector3d(b, 0, -a + hl); - result[10] = tf * Vector3d(-b, 0, a + hl); - result[11] = tf * Vector3d(-b, 0, -a + hl); - - result[12] = tf * Vector3d(0, a, b - hl); - result[13] = tf * Vector3d(0, -a, b - hl); - result[14] = tf * Vector3d(0, a, -b - hl); - result[15] = tf * Vector3d(0, -a, -b - hl); - result[16] = tf * Vector3d(a, b, -hl); - result[17] = tf * Vector3d(-a, b, -hl); - result[18] = tf * Vector3d(a, -b, -hl); - result[19] = tf * Vector3d(-a, -b, -hl); - result[20] = tf * Vector3d(b, 0, a - hl); - result[21] = tf * Vector3d(b, 0, -a - hl); - result[22] = tf * Vector3d(-b, 0, a - hl); - result[23] = tf * Vector3d(-b, 0, -a - hl); - - FCL_REAL c = 0.5 * r2; - FCL_REAL d = capsule.radius; - result[24] = tf * Vector3d(r2, 0, hl); - result[25] = tf * Vector3d(c, d, hl); - result[26] = tf * Vector3d(-c, d, hl); - result[27] = tf * Vector3d(-r2, 0, hl); - result[28] = tf * Vector3d(-c, -d, hl); - result[29] = tf * Vector3d(c, -d, hl); - - result[30] = tf * Vector3d(r2, 0, -hl); - result[31] = tf * Vector3d(c, d, -hl); - result[32] = tf * Vector3d(-c, d, -hl); - result[33] = tf * Vector3d(-r2, 0, -hl); - result[34] = tf * Vector3d(-c, -d, -hl); - result[35] = tf * Vector3d(c, -d, -hl); - - return result; -} - - -std::vector getBoundVertices(const Cone& cone, const Transform3d& tf) -{ - std::vector result(7); - - FCL_REAL hl = cone.lz * 0.5; - FCL_REAL r2 = cone.radius * 2 / sqrt(3.0); - FCL_REAL a = 0.5 * r2; - FCL_REAL b = cone.radius; - - result[0] = tf * Vector3d(r2, 0, -hl); - result[1] = tf * Vector3d(a, b, -hl); - result[2] = tf * Vector3d(-a, b, -hl); - result[3] = tf * Vector3d(-r2, 0, -hl); - result[4] = tf * Vector3d(-a, -b, -hl); - result[5] = tf * Vector3d(a, -b, -hl); - - result[6] = tf * Vector3d(0, 0, hl); - - return result; -} - -std::vector getBoundVertices(const Cylinder& cylinder, const Transform3d& tf) -{ - std::vector result(12); - - FCL_REAL hl = cylinder.lz * 0.5; - FCL_REAL r2 = cylinder.radius * 2 / sqrt(3.0); - FCL_REAL a = 0.5 * r2; - FCL_REAL b = cylinder.radius; - - result[0] = tf * Vector3d(r2, 0, -hl); - result[1] = tf * Vector3d(a, b, -hl); - result[2] = tf * Vector3d(-a, b, -hl); - result[3] = tf * Vector3d(-r2, 0, -hl); - result[4] = tf * Vector3d(-a, -b, -hl); - result[5] = tf * Vector3d(a, -b, -hl); - - result[6] = tf * Vector3d(r2, 0, hl); - result[7] = tf * Vector3d(a, b, hl); - result[8] = tf * Vector3d(-a, b, hl); - result[9] = tf * Vector3d(-r2, 0, hl); - result[10] = tf * Vector3d(-a, -b, hl); - result[11] = tf * Vector3d(a, -b, hl); - - return result; -} - -std::vector getBoundVertices(const Convex& convex, const Transform3d& tf) -{ - std::vector result(convex.num_points); - for(int i = 0; i < convex.num_points; ++i) - { - result[i] = tf * convex.points[i]; - } - - return result; -} - -std::vector getBoundVertices(const TriangleP& triangle, const Transform3d& tf) -{ - std::vector result(3); - result[0] = tf * triangle.a; - result[1] = tf * triangle.b; - result[2] = tf * triangle.c; - - return result; -} - -} // end detail - -Halfspace transform(const Halfspace& a, const Transform3d& tf) -{ - /// suppose the initial halfspace is n * x <= d - /// after transform (R, T), x --> x' = R x + T - /// and the new half space becomes n' * x' <= d' - /// where n' = R * n - /// and d' = d + n' * T - - Vector3d n = tf.linear() * a.n; - FCL_REAL d = a.d + n.dot(tf.translation()); - - return Halfspace(n, d); -} - - -Plane transform(const Plane& a, const Transform3d& tf) -{ - /// suppose the initial halfspace is n * x <= d - /// after transform (R, T), x --> x' = R x + T - /// and the new half space becomes n' * x' <= d' - /// where n' = R * n - /// and d' = d + n' * T - - Vector3d n = tf.linear() * a.n; - FCL_REAL d = a.d + n.dot(tf.translation()); - - return Plane(n, d); -} - - - -template<> -void computeBV(const Box& s, const Transform3d& tf, AABB& bv) -{ - const Matrix3d& R = tf.linear(); - const Vector3d& T = tf.translation(); - - FCL_REAL x_range = 0.5 * (fabs(R(0, 0) * s.side[0]) + fabs(R(0, 1) * s.side[1]) + fabs(R(0, 2) * s.side[2])); - FCL_REAL y_range = 0.5 * (fabs(R(1, 0) * s.side[0]) + fabs(R(1, 1) * s.side[1]) + fabs(R(1, 2) * s.side[2])); - FCL_REAL z_range = 0.5 * (fabs(R(2, 0) * s.side[0]) + fabs(R(2, 1) * s.side[1]) + fabs(R(2, 2) * s.side[2])); - - Vector3d v_delta(x_range, y_range, z_range); - bv.max_ = T + v_delta; - bv.min_ = T - v_delta; -} - -template<> -void computeBV(const Sphere& s, const Transform3d& tf, AABB& bv) -{ - const Vector3d& T = tf.translation(); - - Vector3d v_delta = Vector3d::Constant(s.radius); - bv.max_ = T + v_delta; - bv.min_ = T - v_delta; -} - -template<> -void computeBV(const Ellipsoid& s, const Transform3d& tf, AABB& bv) -{ - const Matrix3d& R = tf.linear(); - const Vector3d& T = tf.translation(); - - FCL_REAL x_range = (fabs(R(0, 0) * s.radii[0]) + fabs(R(0, 1) * s.radii[1]) + fabs(R(0, 2) * s.radii[2])); - FCL_REAL y_range = (fabs(R(1, 0) * s.radii[0]) + fabs(R(1, 1) * s.radii[1]) + fabs(R(1, 2) * s.radii[2])); - FCL_REAL z_range = (fabs(R(2, 0) * s.radii[0]) + fabs(R(2, 1) * s.radii[1]) + fabs(R(2, 2) * s.radii[2])); - - Vector3d v_delta(x_range, y_range, z_range); - bv.max_ = T + v_delta; - bv.min_ = T - v_delta; -} - -template<> -void computeBV(const Capsule& s, const Transform3d& tf, AABB& bv) -{ - const Matrix3d& R = tf.linear(); - const Vector3d& T = tf.translation(); - - FCL_REAL x_range = 0.5 * fabs(R(0, 2) * s.lz) + s.radius; - FCL_REAL y_range = 0.5 * fabs(R(1, 2) * s.lz) + s.radius; - FCL_REAL z_range = 0.5 * fabs(R(2, 2) * s.lz) + s.radius; - - Vector3d v_delta(x_range, y_range, z_range); - bv.max_ = T + v_delta; - bv.min_ = T - v_delta; -} - -template<> -void computeBV(const Cone& s, const Transform3d& tf, AABB& bv) -{ - const Matrix3d& R = tf.linear(); - const Vector3d& T = tf.translation(); - - FCL_REAL x_range = fabs(R(0, 0) * s.radius) + fabs(R(0, 1) * s.radius) + 0.5 * fabs(R(0, 2) * s.lz); - FCL_REAL y_range = fabs(R(1, 0) * s.radius) + fabs(R(1, 1) * s.radius) + 0.5 * fabs(R(1, 2) * s.lz); - FCL_REAL z_range = fabs(R(2, 0) * s.radius) + fabs(R(2, 1) * s.radius) + 0.5 * fabs(R(2, 2) * s.lz); - - Vector3d v_delta(x_range, y_range, z_range); - bv.max_ = T + v_delta; - bv.min_ = T - v_delta; -} - -template<> -void computeBV(const Cylinder& s, const Transform3d& tf, AABB& bv) -{ - const Matrix3d& R = tf.linear(); - const Vector3d& T = tf.translation(); - - FCL_REAL x_range = fabs(R(0, 0) * s.radius) + fabs(R(0, 1) * s.radius) + 0.5 * fabs(R(0, 2) * s.lz); - FCL_REAL y_range = fabs(R(1, 0) * s.radius) + fabs(R(1, 1) * s.radius) + 0.5 * fabs(R(1, 2) * s.lz); - FCL_REAL z_range = fabs(R(2, 0) * s.radius) + fabs(R(2, 1) * s.radius) + 0.5 * fabs(R(2, 2) * s.lz); - - Vector3d v_delta(x_range, y_range, z_range); - bv.max_ = T + v_delta; - bv.min_ = T - v_delta; -} - -template<> -void computeBV(const Convex& s, const Transform3d& tf, AABB& bv) -{ - const Matrix3d& R = tf.linear(); - const Vector3d& T = tf.translation(); - - AABB bv_; - for(int i = 0; i < s.num_points; ++i) - { - Vector3d new_p = R * s.points[i] + T; - bv_ += new_p; - } - - bv = bv_; -} - -template<> -void computeBV(const TriangleP& s, const Transform3d& tf, AABB& bv) -{ - bv = AABB(tf * s.a, tf * s.b, tf * s.c); -} - - -template<> -void computeBV(const Halfspace& s, const Transform3d& tf, AABB& bv) -{ - Halfspace new_s = transform(s, tf); - const Vector3d& n = new_s.n; - const FCL_REAL& d = new_s.d; - - AABB bv_; - bv_.min_ = Vector3d::Constant(-std::numeric_limits::max()); - bv_.max_ = Vector3d::Constant(std::numeric_limits::max()); - if(n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) - { - // normal aligned with x axis - if(n[0] < 0) bv_.min_[0] = -d; - else if(n[0] > 0) bv_.max_[0] = d; - } - else if(n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) - { - // normal aligned with y axis - if(n[1] < 0) bv_.min_[1] = -d; - else if(n[1] > 0) bv_.max_[1] = d; - } - else if(n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) - { - // normal aligned with z axis - if(n[2] < 0) bv_.min_[2] = -d; - else if(n[2] > 0) bv_.max_[2] = d; - } - - bv = bv_; -} - -template<> -void computeBV(const Plane& s, const Transform3d& tf, AABB& bv) -{ - Plane new_s = transform(s, tf); - const Vector3d& n = new_s.n; - const FCL_REAL& d = new_s.d; - - AABB bv_; - bv_.min_ = Vector3d::Constant(-std::numeric_limits::max()); - bv_.max_ = Vector3d::Constant(std::numeric_limits::max()); - if(n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) - { - // normal aligned with x axis - if(n[0] < 0) { bv_.min_[0] = bv_.max_[0] = -d; } - else if(n[0] > 0) { bv_.min_[0] = bv_.max_[0] = d; } - } - else if(n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) - { - // normal aligned with y axis - if(n[1] < 0) { bv_.min_[1] = bv_.max_[1] = -d; } - else if(n[1] > 0) { bv_.min_[1] = bv_.max_[1] = d; } - } - else if(n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) - { - // normal aligned with z axis - if(n[2] < 0) { bv_.min_[2] = bv_.max_[2] = -d; } - else if(n[2] > 0) { bv_.min_[2] = bv_.max_[2] = d; } - } - - bv = bv_; -} - - -template<> -void computeBV(const Box& s, const Transform3d& tf, OBB& bv) -{ - bv.To = tf.translation(); - bv.axis = tf.linear(); - bv.extent = s.side * (FCL_REAL)0.5; -} - -template<> -void computeBV(const Sphere& s, const Transform3d& tf, OBB& bv) -{ - bv.To = tf.translation(); - bv.axis.setIdentity(); - bv.extent.setConstant(s.radius); -} - -template<> -void computeBV(const Ellipsoid& s, const Transform3d& tf, OBB& bv) -{ - bv.To = tf.translation(); - bv.axis = tf.linear(); - bv.extent = s.radii; -} - -template<> -void computeBV(const Capsule& s, const Transform3d& tf, OBB& bv) -{ - bv.To = tf.translation(); - bv.axis = tf.linear(); - bv.extent << s.radius, s.radius, s.lz / 2 + s.radius; -} - -template<> -void computeBV(const Cone& s, const Transform3d& tf, OBB& bv) -{ - bv.To = tf.translation(); - bv.axis = tf.linear(); - bv.extent << s.radius, s.radius, s.lz / 2; -} - -template<> -void computeBV(const Cylinder& s, const Transform3d& tf, OBB& bv) -{ - bv.To = tf.translation(); - bv.axis = tf.linear(); - bv.extent << s.radius, s.radius, s.lz / 2; -} - -template<> -void computeBV(const Convex& s, const Transform3d& tf, OBB& bv) -{ - fit(s.points, s.num_points, bv); - - bv.axis = tf.linear(); - bv.To = tf * bv.To; -} - -template<> -void computeBV(const Halfspace& s, const Transform3d& tf, OBB& bv) -{ - /// Half space can only have very rough OBB - bv.axis.setIdentity(); - bv.To.setZero(); - bv.extent.setConstant(std::numeric_limits::max()); -} - -template<> -void computeBV(const Halfspace& s, const Transform3d& tf, RSS& bv) -{ - /// Half space can only have very rough RSS - bv.axis.setIdentity(); - bv.Tr.setZero(); - bv.l[0] = bv.l[1] = bv.r = std::numeric_limits::max(); -} - -template<> -void computeBV(const Halfspace& s, const Transform3d& tf, OBBRSS& bv) -{ - computeBV(s, tf, bv.obb); - computeBV(s, tf, bv.rss); -} - -template<> -void computeBV(const Halfspace& s, const Transform3d& tf, kIOS& bv) -{ - bv.num_spheres = 1; - computeBV(s, tf, bv.obb); - bv.spheres[0].o.setZero(); - bv.spheres[0].r = std::numeric_limits::max(); -} - -template<> -void computeBV, Halfspace>(const Halfspace& s, const Transform3d& tf, KDOP<16>& bv) -{ - Halfspace new_s = transform(s, tf); - const Vector3d& n = new_s.n; - const FCL_REAL& d = new_s.d; - - const std::size_t D = 8; - for(std::size_t i = 0; i < D; ++i) - bv.dist(i) = -std::numeric_limits::max(); - for(std::size_t i = D; i < 2 * D; ++i) - bv.dist(i) = std::numeric_limits::max(); - - if(n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) - { - if(n[0] > 0) bv.dist(D) = d; - else bv.dist(0) = -d; - } - else if(n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) - { - if(n[1] > 0) bv.dist(D + 1) = d; - else bv.dist(1) = -d; - } - else if(n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) - { - if(n[2] > 0) bv.dist(D + 2) = d; - else bv.dist(2) = -d; - } - else if(n[2] == (FCL_REAL)0.0 && n[0] == n[1]) - { - if(n[0] > 0) bv.dist(D + 3) = n[0] * d * 2; - else bv.dist(3) = n[0] * d * 2; - } - else if(n[1] == (FCL_REAL)0.0 && n[0] == n[2]) - { - if(n[1] > 0) bv.dist(D + 4) = n[0] * d * 2; - else bv.dist(4) = n[0] * d * 2; - } - else if(n[0] == (FCL_REAL)0.0 && n[1] == n[2]) - { - if(n[1] > 0) bv.dist(D + 5) = n[1] * d * 2; - else bv.dist(5) = n[1] * d * 2; - } - else if(n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) - { - if(n[0] > 0) bv.dist(D + 6) = n[0] * d * 2; - else bv.dist(6) = n[0] * d * 2; - } - else if(n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) - { - if(n[0] > 0) bv.dist(D + 7) = n[0] * d * 2; - else bv.dist(7) = n[0] * d * 2; - } -} - -template<> -void computeBV, Halfspace>(const Halfspace& s, const Transform3d& tf, KDOP<18>& bv) -{ - Halfspace new_s = transform(s, tf); - const Vector3d& n = new_s.n; - const FCL_REAL& d = new_s.d; - - const std::size_t D = 9; - - for(std::size_t i = 0; i < D; ++i) - bv.dist(i) = -std::numeric_limits::max(); - for(std::size_t i = D; i < 2 * D; ++i) - bv.dist(i) = std::numeric_limits::max(); - - if(n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) - { - if(n[0] > 0) bv.dist(D) = d; - else bv.dist(0) = -d; - } - else if(n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) - { - if(n[1] > 0) bv.dist(D + 1) = d; - else bv.dist(1) = -d; - } - else if(n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) - { - if(n[2] > 0) bv.dist(D + 2) = d; - else bv.dist(2) = -d; - } - else if(n[2] == (FCL_REAL)0.0 && n[0] == n[1]) - { - if(n[0] > 0) bv.dist(D + 3) = n[0] * d * 2; - else bv.dist(3) = n[0] * d * 2; - } - else if(n[1] == (FCL_REAL)0.0 && n[0] == n[2]) - { - if(n[1] > 0) bv.dist(D + 4) = n[0] * d * 2; - else bv.dist(4) = n[0] * d * 2; - } - else if(n[0] == (FCL_REAL)0.0 && n[1] == n[2]) - { - if(n[1] > 0) bv.dist(D + 5) = n[1] * d * 2; - else bv.dist(5) = n[1] * d * 2; - } - else if(n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) - { - if(n[0] > 0) bv.dist(D + 6) = n[0] * d * 2; - else bv.dist(6) = n[0] * d * 2; - } - else if(n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) - { - if(n[0] > 0) bv.dist(D + 7) = n[0] * d * 2; - else bv.dist(7) = n[0] * d * 2; - } - else if(n[0] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0) - { - if(n[1] > 0) bv.dist(D + 8) = n[1] * d * 2; - else bv.dist(8) = n[1] * d * 2; - } -} - -template<> -void computeBV, Halfspace>(const Halfspace& s, const Transform3d& tf, KDOP<24>& bv) -{ - Halfspace new_s = transform(s, tf); - const Vector3d& n = new_s.n; - const FCL_REAL& d = new_s.d; - - const std::size_t D = 12; - - for(std::size_t i = 0; i < D; ++i) - bv.dist(i) = -std::numeric_limits::max(); - for(std::size_t i = D; i < 2 * D; ++i) - bv.dist(i) = std::numeric_limits::max(); - - if(n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) - { - if(n[0] > 0) bv.dist(D) = d; - else bv.dist(0) = -d; - } - else if(n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) - { - if(n[1] > 0) bv.dist(D + 1) = d; - else bv.dist(1) = -d; - } - else if(n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) - { - if(n[2] > 0) bv.dist(D + 2) = d; - else bv.dist(2) = -d; - } - else if(n[2] == (FCL_REAL)0.0 && n[0] == n[1]) - { - if(n[0] > 0) bv.dist(D + 3) = n[0] * d * 2; - else bv.dist(3) = n[0] * d * 2; - } - else if(n[1] == (FCL_REAL)0.0 && n[0] == n[2]) - { - if(n[1] > 0) bv.dist(D + 4) = n[0] * d * 2; - else bv.dist(4) = n[0] * d * 2; - } - else if(n[0] == (FCL_REAL)0.0 && n[1] == n[2]) - { - if(n[1] > 0) bv.dist(D + 5) = n[1] * d * 2; - else bv.dist(5) = n[1] * d * 2; - } - else if(n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) - { - if(n[0] > 0) bv.dist(D + 6) = n[0] * d * 2; - else bv.dist(6) = n[0] * d * 2; - } - else if(n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) - { - if(n[0] > 0) bv.dist(D + 7) = n[0] * d * 2; - else bv.dist(7) = n[0] * d * 2; - } - else if(n[0] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0) - { - if(n[1] > 0) bv.dist(D + 8) = n[1] * d * 2; - else bv.dist(8) = n[1] * d * 2; - } - else if(n[0] + n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) - { - if(n[0] > 0) bv.dist(D + 9) = n[0] * d * 3; - else bv.dist(9) = n[0] * d * 3; - } - else if(n[0] + n[1] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0) - { - if(n[0] > 0) bv.dist(D + 10) = n[0] * d * 3; - else bv.dist(10) = n[0] * d * 3; - } - else if(n[0] + n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) - { - if(n[1] > 0) bv.dist(D + 11) = n[1] * d * 3; - else bv.dist(11) = n[1] * d * 3; - } -} - - - -template<> -void computeBV(const Plane& s, const Transform3d& tf, OBB& bv) -{ - Vector3d n = tf.linear() * s.n; - bv.axis.col(0) = n; - generateCoordinateSystem(bv.axis); - - bv.extent << 0, std::numeric_limits::max(), std::numeric_limits::max(); - - Vector3d p = s.n * s.d; - bv.To = tf * p; /// n'd' = R * n * (d + (R * n) * T) = R * (n * d) + T -} - -template<> -void computeBV(const Plane& s, const Transform3d& tf, RSS& bv) -{ - Vector3d n = tf.linear() * s.n; - - bv.axis.col(0) = n; - generateCoordinateSystem(bv.axis); - - bv.l[0] = std::numeric_limits::max(); - bv.l[1] = std::numeric_limits::max(); - - bv.r = 0; - - Vector3d p = s.n * s.d; - bv.Tr = tf * p; -} - -template<> -void computeBV(const Plane& s, const Transform3d& tf, OBBRSS& bv) -{ - computeBV(s, tf, bv.obb); - computeBV(s, tf, bv.rss); -} - -template<> -void computeBV(const Plane& s, const Transform3d& tf, kIOS& bv) -{ - bv.num_spheres = 1; - computeBV(s, tf, bv.obb); - bv.spheres[0].o.setZero(); - bv.spheres[0].r = std::numeric_limits::max(); -} - -template<> -void computeBV, Plane>(const Plane& s, const Transform3d& tf, KDOP<16>& bv) -{ - Plane new_s = transform(s, tf); - const Vector3d& n = new_s.n; - const FCL_REAL& d = new_s.d; - - const std::size_t D = 8; - - for(std::size_t i = 0; i < D; ++i) - bv.dist(i) = -std::numeric_limits::max(); - for(std::size_t i = D; i < 2 * D; ++i) - bv.dist(i) = std::numeric_limits::max(); - - if(n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) - { - if(n[0] > 0) bv.dist(0) = bv.dist(D) = d; - else bv.dist(0) = bv.dist(D) = -d; - } - else if(n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) - { - if(n[1] > 0) bv.dist(1) = bv.dist(D + 1) = d; - else bv.dist(1) = bv.dist(D + 1) = -d; - } - else if(n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) - { - if(n[2] > 0) bv.dist(2) = bv.dist(D + 2) = d; - else bv.dist(2) = bv.dist(D + 2) = -d; - } - else if(n[2] == (FCL_REAL)0.0 && n[0] == n[1]) - { - bv.dist(3) = bv.dist(D + 3) = n[0] * d * 2; - } - else if(n[1] == (FCL_REAL)0.0 && n[0] == n[2]) - { - bv.dist(4) = bv.dist(D + 4) = n[0] * d * 2; - } - else if(n[0] == (FCL_REAL)0.0 && n[1] == n[2]) - { - bv.dist(6) = bv.dist(D + 5) = n[1] * d * 2; - } - else if(n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) - { - bv.dist(6) = bv.dist(D + 6) = n[0] * d * 2; - } - else if(n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) - { - bv.dist(7) = bv.dist(D + 7) = n[0] * d * 2; - } -} - -template<> -void computeBV, Plane>(const Plane& s, const Transform3d& tf, KDOP<18>& bv) -{ - Plane new_s = transform(s, tf); - const Vector3d& n = new_s.n; - const FCL_REAL& d = new_s.d; - - const std::size_t D = 9; - - for(std::size_t i = 0; i < D; ++i) - bv.dist(i) = -std::numeric_limits::max(); - for(std::size_t i = D; i < 2 * D; ++i) - bv.dist(i) = std::numeric_limits::max(); - - if(n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) - { - if(n[0] > 0) bv.dist(0) = bv.dist(D) = d; - else bv.dist(0) = bv.dist(D) = -d; - } - else if(n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) - { - if(n[1] > 0) bv.dist(1) = bv.dist(D + 1) = d; - else bv.dist(1) = bv.dist(D + 1) = -d; - } - else if(n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) - { - if(n[2] > 0) bv.dist(2) = bv.dist(D + 2) = d; - else bv.dist(2) = bv.dist(D + 2) = -d; - } - else if(n[2] == (FCL_REAL)0.0 && n[0] == n[1]) - { - bv.dist(3) = bv.dist(D + 3) = n[0] * d * 2; - } - else if(n[1] == (FCL_REAL)0.0 && n[0] == n[2]) - { - bv.dist(4) = bv.dist(D + 4) = n[0] * d * 2; - } - else if(n[0] == (FCL_REAL)0.0 && n[1] == n[2]) - { - bv.dist(5) = bv.dist(D + 5) = n[1] * d * 2; - } - else if(n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) - { - bv.dist(6) = bv.dist(D + 6) = n[0] * d * 2; - } - else if(n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) - { - bv.dist(7) = bv.dist(D + 7) = n[0] * d * 2; - } - else if(n[0] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0) - { - bv.dist(8) = bv.dist(D + 8) = n[1] * d * 2; - } -} - -template<> -void computeBV, Plane>(const Plane& s, const Transform3d& tf, KDOP<24>& bv) -{ - Plane new_s = transform(s, tf); - const Vector3d& n = new_s.n; - const FCL_REAL& d = new_s.d; - - const std::size_t D = 12; - - for(std::size_t i = 0; i < D; ++i) - bv.dist(i) = -std::numeric_limits::max(); - for(std::size_t i = D; i < 2 * D; ++i) - bv.dist(i) = std::numeric_limits::max(); - - if(n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) - { - if(n[0] > 0) bv.dist(0) = bv.dist(D) = d; - else bv.dist(0) = bv.dist(D) = -d; - } - else if(n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) - { - if(n[1] > 0) bv.dist(1) = bv.dist(D + 1) = d; - else bv.dist(1) = bv.dist(D + 1) = -d; - } - else if(n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) - { - if(n[2] > 0) bv.dist(2) = bv.dist(D + 2) = d; - else bv.dist(2) = bv.dist(D + 2) = -d; - } - else if(n[2] == (FCL_REAL)0.0 && n[0] == n[1]) - { - bv.dist(3) = bv.dist(D + 3) = n[0] * d * 2; - } - else if(n[1] == (FCL_REAL)0.0 && n[0] == n[2]) - { - bv.dist(4) = bv.dist(D + 4) = n[0] * d * 2; - } - else if(n[0] == (FCL_REAL)0.0 && n[1] == n[2]) - { - bv.dist(5) = bv.dist(D + 5) = n[1] * d * 2; - } - else if(n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) - { - bv.dist(6) = bv.dist(D + 6) = n[0] * d * 2; - } - else if(n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) - { - bv.dist(7) = bv.dist(D + 7) = n[0] * d * 2; - } - else if(n[0] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0) - { - bv.dist(8) = bv.dist(D + 8) = n[1] * d * 2; - } - else if(n[0] + n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) - { - bv.dist(9) = bv.dist(D + 9) = n[0] * d * 3; - } - else if(n[0] + n[1] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0) - { - bv.dist(10) = bv.dist(D + 10) = n[0] * d * 3; - } - else if(n[0] + n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) - { - bv.dist(11) = bv.dist(D + 11) = n[1] * d * 3; - } -} - - -void constructBox(const AABB& bv, Box& box, Transform3d& tf) -{ - box = Box(bv.max_ - bv.min_); - tf.linear().setIdentity(); - tf.translation() = bv.center(); -} - -void constructBox(const OBB& bv, Box& box, Transform3d& tf) -{ - box = Box(bv.extent * 2); - tf.linear() = bv.axis; - tf.translation() = bv.To; -} - -void constructBox(const OBBRSS& bv, Box& box, Transform3d& tf) -{ - box = Box(bv.obb.extent * 2); - tf.linear() = bv.obb.axis; - tf.translation() = bv.obb.To; -} - -void constructBox(const kIOS& bv, Box& box, Transform3d& tf) -{ - box = Box(bv.obb.extent * 2); - tf.linear() = bv.obb.axis; - tf.translation() = bv.obb.To; -} - -void constructBox(const RSS& bv, Box& box, Transform3d& tf) -{ - box = Box(bv.width(), bv.height(), bv.depth()); - tf.linear() = bv.axis; - tf.translation() = bv.Tr; -} - -void constructBox(const KDOP<16>& bv, Box& box, Transform3d& tf) -{ - box = Box(bv.width(), bv.height(), bv.depth()); - tf.linear().setIdentity(); - tf.translation() = bv.center(); -} - -void constructBox(const KDOP<18>& bv, Box& box, Transform3d& tf) -{ - box = Box(bv.width(), bv.height(), bv.depth()); - tf.linear().setIdentity(); - tf.translation() = bv.center(); -} - -void constructBox(const KDOP<24>& bv, Box& box, Transform3d& tf) -{ - box = Box(bv.width(), bv.height(), bv.depth()); - tf.linear().setIdentity(); - tf.translation() = bv.center(); -} - - - -void constructBox(const AABB& bv, const Transform3d& tf_bv, Box& box, Transform3d& tf) -{ - box = Box(bv.max_ - bv.min_); - tf = tf_bv * Eigen::Translation3d(bv.center()); -} - -void constructBox(const OBB& bv, const Transform3d& tf_bv, Box& box, Transform3d& tf) -{ - box = Box(bv.extent * 2); - tf.linear() = bv.axis; - tf.translation() = bv.To; -} - -void constructBox(const OBBRSS& bv, const Transform3d& tf_bv, Box& box, Transform3d& tf) -{ - box = Box(bv.obb.extent * 2); - tf.linear() = bv.obb.axis; - tf.translation() = bv.obb.To; - tf = tf_bv * tf; -} - -void constructBox(const kIOS& bv, const Transform3d& tf_bv, Box& box, Transform3d& tf) -{ - box = Box(bv.obb.extent * 2); - tf.linear() = bv.obb.axis; - tf.translation() = bv.obb.To; - tf = tf_bv * tf; -} - -void constructBox(const RSS& bv, const Transform3d& tf_bv, Box& box, Transform3d& tf) -{ - box = Box(bv.width(), bv.height(), bv.depth()); - tf.linear() = bv.axis; - tf.translation() = bv.Tr; - tf = tf_bv * tf; -} - -void constructBox(const KDOP<16>& bv, const Transform3d& tf_bv, Box& box, Transform3d& tf) -{ - box = Box(bv.width(), bv.height(), bv.depth()); - tf = tf_bv * Eigen::Translation3d(bv.center()); -} - -void constructBox(const KDOP<18>& bv, const Transform3d& tf_bv, Box& box, Transform3d& tf) -{ - box = Box(bv.width(), bv.height(), bv.depth()); - tf = tf_bv * Eigen::Translation3d(bv.center()); -} - -void constructBox(const KDOP<24>& bv, const Transform3d& tf_bv, Box& box, Transform3d& tf) -{ - box = Box(bv.width(), bv.height(), bv.depth()); - tf = tf_bv * Eigen::Translation3d(bv.center()); -} - - - -} diff --git a/src/traversal/traversal_node_bvhs.cpp b/src/traversal/traversal_node_bvhs.cpp deleted file mode 100644 index a88b92641..000000000 --- a/src/traversal/traversal_node_bvhs.cpp +++ /dev/null @@ -1,687 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - - -#include "fcl/traversal/traversal_node_bvhs.h" - -namespace fcl -{ - -namespace details -{ -template -static inline void meshCollisionOrientedNodeLeafTesting(int b1, int b2, - const BVHModel* model1, const BVHModel* model2, - Vector3d* vertices1, Vector3d* vertices2, - Triangle* tri_indices1, Triangle* tri_indices2, - const Matrix3d& R, const Vector3d& T, - const Transform3d& tf1, const Transform3d& tf2, - bool enable_statistics, - FCL_REAL cost_density, - int& num_leaf_tests, - const CollisionRequest& request, - CollisionResult& result) -{ - if(enable_statistics) num_leaf_tests++; - - const BVNode& node1 = model1->getBV(b1); - const BVNode& node2 = model2->getBV(b2); - - int primitive_id1 = node1.primitiveId(); - int primitive_id2 = node2.primitiveId(); - - const Triangle& tri_id1 = tri_indices1[primitive_id1]; - const Triangle& tri_id2 = tri_indices2[primitive_id2]; - - const Vector3d& p1 = vertices1[tri_id1[0]]; - const Vector3d& p2 = vertices1[tri_id1[1]]; - const Vector3d& p3 = vertices1[tri_id1[2]]; - const Vector3d& q1 = vertices2[tri_id2[0]]; - const Vector3d& q2 = vertices2[tri_id2[1]]; - const Vector3d& q3 = vertices2[tri_id2[2]]; - - if(model1->isOccupied() && model2->isOccupied()) - { - bool is_intersect = false; - - if(!request.enable_contact) // only interested in collision or not - { - if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3, R, T)) - { - is_intersect = true; - if(result.numContacts() < request.num_max_contacts) - result.addContact(Contact(model1, model2, primitive_id1, primitive_id2)); - } - } - else // need compute the contact information - { - FCL_REAL penetration; - Vector3d normal; - unsigned int n_contacts; - Vector3d contacts[2]; - - if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3, - R, T, - contacts, - &n_contacts, - &penetration, - &normal)) - { - is_intersect = true; - - if(request.num_max_contacts < result.numContacts() + n_contacts) - n_contacts = (request.num_max_contacts > result.numContacts()) ? (request.num_max_contacts - result.numContacts()) : 0; - - for(unsigned int i = 0; i < n_contacts; ++i) - { - result.addContact(Contact(model1, model2, primitive_id1, primitive_id2, tf1 * contacts[i], tf1.linear() * normal, penetration)); - } - } - } - - if(is_intersect && request.enable_cost) - { - AABB overlap_part; - AABB(tf1 * p1, tf1 * p2, tf1 * p3).overlap(AABB(tf2 * q1, tf2 * q2, tf2 * q3), overlap_part); - result.addCostSource(CostSource(overlap_part, cost_density), request.num_max_cost_sources); - } - } - else if((!model1->isFree() && !model2->isFree()) && request.enable_cost) - { - if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3, R, T)) - { - AABB overlap_part; - AABB(tf1 * p1, tf1 * p2, tf1 * p3).overlap(AABB(tf2 * q1, tf2 * q2, tf2 * q3), overlap_part); - result.addCostSource(CostSource(overlap_part, cost_density), request.num_max_cost_sources); - } - } -} - - - - -template -static inline void meshDistanceOrientedNodeLeafTesting(int b1, int b2, - const BVHModel* model1, const BVHModel* model2, - Vector3d* vertices1, Vector3d* vertices2, - Triangle* tri_indices1, Triangle* tri_indices2, - const Matrix3d& R, const Vector3d& T, - bool enable_statistics, - int& num_leaf_tests, - const DistanceRequest& request, - DistanceResult& result) -{ - if(enable_statistics) num_leaf_tests++; - - const BVNode& node1 = model1->getBV(b1); - const BVNode& node2 = model2->getBV(b2); - - int primitive_id1 = node1.primitiveId(); - int primitive_id2 = node2.primitiveId(); - - const Triangle& tri_id1 = tri_indices1[primitive_id1]; - const Triangle& tri_id2 = tri_indices2[primitive_id2]; - - const Vector3d& t11 = vertices1[tri_id1[0]]; - const Vector3d& t12 = vertices1[tri_id1[1]]; - const Vector3d& t13 = vertices1[tri_id1[2]]; - - const Vector3d& t21 = vertices2[tri_id2[0]]; - const Vector3d& t22 = vertices2[tri_id2[1]]; - const Vector3d& t23 = vertices2[tri_id2[2]]; - - // nearest point pair - Vector3d P1, P2; - - FCL_REAL d = TriangleDistance::triDistance(t11, t12, t13, t21, t22, t23, - R, T, - P1, P2); - - if(request.enable_nearest_points) - result.update(d, model1, model2, primitive_id1, primitive_id2, P1, P2); - else - result.update(d, model1, model2, primitive_id1, primitive_id2); -} - -} - -MeshCollisionTraversalNodeOBB::MeshCollisionTraversalNodeOBB() : MeshCollisionTraversalNode() -{ - R.setIdentity(); -} - -bool MeshCollisionTraversalNodeOBB::BVTesting(int b1, int b2) const -{ - if(enable_statistics) num_bv_tests++; - return !overlap(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv); -} - -void MeshCollisionTraversalNodeOBB::leafTesting(int b1, int b2) const -{ - details::meshCollisionOrientedNodeLeafTesting(b1, b2, model1, model2, vertices1, vertices2, - tri_indices1, tri_indices2, - R, T, - tf1, tf2, - enable_statistics, cost_density, - num_leaf_tests, - request, *result); -} - - -bool MeshCollisionTraversalNodeOBB::BVTesting(int b1, int b2, const Matrix3d& Rc, const Vector3d& Tc) const -{ - if(enable_statistics) num_bv_tests++; - return obbDisjoint(Rc, Tc, model1->getBV(b1).bv.extent, model2->getBV(b2).bv.extent); -} - -void MeshCollisionTraversalNodeOBB::leafTesting(int b1, int b2, const Matrix3d& Rc, const Vector3d& Tc) const -{ - details::meshCollisionOrientedNodeLeafTesting(b1, b2, model1, model2, vertices1, vertices2, - tri_indices1, tri_indices2, - R, T, - tf1, tf2, - enable_statistics, cost_density, - num_leaf_tests, - request, *result); -} - - - -MeshCollisionTraversalNodeRSS::MeshCollisionTraversalNodeRSS() : MeshCollisionTraversalNode() -{ - R.setIdentity(); -} - -bool MeshCollisionTraversalNodeRSS::BVTesting(int b1, int b2) const -{ - if(enable_statistics) num_bv_tests++; - return !overlap(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv); -} - -void MeshCollisionTraversalNodeRSS::leafTesting(int b1, int b2) const -{ - details::meshCollisionOrientedNodeLeafTesting(b1, b2, model1, model2, vertices1, vertices2, - tri_indices1, tri_indices2, - R, T, - tf1, tf2, - enable_statistics, cost_density, - num_leaf_tests, - request, *result); -} - - - - -MeshCollisionTraversalNodekIOS::MeshCollisionTraversalNodekIOS() : MeshCollisionTraversalNode() -{ - R.setIdentity(); -} - -bool MeshCollisionTraversalNodekIOS::BVTesting(int b1, int b2) const -{ - if(enable_statistics) num_bv_tests++; - return !overlap(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv); -} - -void MeshCollisionTraversalNodekIOS::leafTesting(int b1, int b2) const -{ - details::meshCollisionOrientedNodeLeafTesting(b1, b2, model1, model2, vertices1, vertices2, - tri_indices1, tri_indices2, - R, T, - tf1, tf2, - enable_statistics, cost_density, - num_leaf_tests, - request, *result); -} - - - -MeshCollisionTraversalNodeOBBRSS::MeshCollisionTraversalNodeOBBRSS() : MeshCollisionTraversalNode() -{ - R.setIdentity(); -} - -bool MeshCollisionTraversalNodeOBBRSS::BVTesting(int b1, int b2) const -{ - if(enable_statistics) num_bv_tests++; - return !overlap(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv); -} - -void MeshCollisionTraversalNodeOBBRSS::leafTesting(int b1, int b2) const -{ - details::meshCollisionOrientedNodeLeafTesting(b1, b2, model1, model2, vertices1, vertices2, - tri_indices1, tri_indices2, - R, T, - tf1, tf2, - enable_statistics, cost_density, - num_leaf_tests, - request,*result); -} - - -namespace details -{ - -template -static inline void distancePreprocessOrientedNode(const BVHModel* model1, const BVHModel* model2, - const Vector3d* vertices1, Vector3d* vertices2, - Triangle* tri_indices1, Triangle* tri_indices2, - int init_tri_id1, int init_tri_id2, - const Matrix3d& R, const Vector3d& T, - const DistanceRequest& request, - DistanceResult& result) -{ - const Triangle& init_tri1 = tri_indices1[init_tri_id1]; - const Triangle& init_tri2 = tri_indices2[init_tri_id2]; - - Vector3d init_tri1_points[3]; - Vector3d init_tri2_points[3]; - - init_tri1_points[0] = vertices1[init_tri1[0]]; - init_tri1_points[1] = vertices1[init_tri1[1]]; - init_tri1_points[2] = vertices1[init_tri1[2]]; - - init_tri2_points[0] = vertices2[init_tri2[0]]; - init_tri2_points[1] = vertices2[init_tri2[1]]; - init_tri2_points[2] = vertices2[init_tri2[2]]; - - Vector3d p1, p2; - FCL_REAL distance = TriangleDistance::triDistance(init_tri1_points[0], init_tri1_points[1], init_tri1_points[2], - init_tri2_points[0], init_tri2_points[1], init_tri2_points[2], - R, T, p1, p2); - - if(request.enable_nearest_points) - result.update(distance, model1, model2, init_tri_id1, init_tri_id2, p1, p2); - else - result.update(distance, model1, model2, init_tri_id1, init_tri_id2); -} - -template -static inline void distancePostprocessOrientedNode(const BVHModel* model1, const BVHModel* model2, - const Transform3d& tf1, const DistanceRequest& request, DistanceResult& result) -{ - /// the points obtained by triDistance are not in world space: both are in object1's local coordinate system, so we need to convert them into the world space. - if(request.enable_nearest_points && (result.o1 == model1) && (result.o2 == model2)) - { - result.nearest_points[0] = tf1 * result.nearest_points[0]; - result.nearest_points[1] = tf1 * result.nearest_points[1]; - } -} - -} - -MeshDistanceTraversalNodeRSS::MeshDistanceTraversalNodeRSS() : MeshDistanceTraversalNode(), R(Matrix3d::Identity()), T(Vector3d::Zero()) -{ -} - -void MeshDistanceTraversalNodeRSS::preprocess() -{ - details::distancePreprocessOrientedNode(model1, model2, vertices1, vertices2, tri_indices1, tri_indices2, 0, 0, R, T, request, *result); -} - -void MeshDistanceTraversalNodeRSS::postprocess() -{ - details::distancePostprocessOrientedNode(model1, model2, tf1, request, *result); -} - -FCL_REAL MeshDistanceTraversalNodeRSS::BVTesting(int b1, int b2) const -{ - if(enable_statistics) num_bv_tests++; - return distance(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv); -} - -void MeshDistanceTraversalNodeRSS::leafTesting(int b1, int b2) const -{ - details::meshDistanceOrientedNodeLeafTesting(b1, b2, model1, model2, vertices1, vertices2, tri_indices1, tri_indices2, - R, T, enable_statistics, num_leaf_tests, - request, *result); -} - -MeshDistanceTraversalNodekIOS::MeshDistanceTraversalNodekIOS() : MeshDistanceTraversalNode() -{ - R.setIdentity(); -} - -void MeshDistanceTraversalNodekIOS::preprocess() -{ - details::distancePreprocessOrientedNode(model1, model2, vertices1, vertices2, tri_indices1, tri_indices2, 0, 0, R, T, request, *result); -} - -void MeshDistanceTraversalNodekIOS::postprocess() -{ - details::distancePostprocessOrientedNode(model1, model2, tf1, request, *result); -} - -FCL_REAL MeshDistanceTraversalNodekIOS::BVTesting(int b1, int b2) const -{ - if(enable_statistics) num_bv_tests++; - return distance(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv); -} - -void MeshDistanceTraversalNodekIOS::leafTesting(int b1, int b2) const -{ - details::meshDistanceOrientedNodeLeafTesting(b1, b2, model1, model2, vertices1, vertices2, tri_indices1, tri_indices2, - R, T, enable_statistics, num_leaf_tests, - request, *result); -} - -MeshDistanceTraversalNodeOBBRSS::MeshDistanceTraversalNodeOBBRSS() : MeshDistanceTraversalNode() -{ - R.setIdentity(); -} - -void MeshDistanceTraversalNodeOBBRSS::preprocess() -{ - details::distancePreprocessOrientedNode(model1, model2, vertices1, vertices2, tri_indices1, tri_indices2, 0, 0, R, T, request, *result); -} - -void MeshDistanceTraversalNodeOBBRSS::postprocess() -{ - details::distancePostprocessOrientedNode(model1, model2, tf1, request, *result); -} - -FCL_REAL MeshDistanceTraversalNodeOBBRSS::BVTesting(int b1, int b2) const -{ - if(enable_statistics) num_bv_tests++; - return distance(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv); -} - -void MeshDistanceTraversalNodeOBBRSS::leafTesting(int b1, int b2) const -{ - details::meshDistanceOrientedNodeLeafTesting(b1, b2, model1, model2, vertices1, vertices2, tri_indices1, tri_indices2, - R, T, enable_statistics, num_leaf_tests, - request, *result); -} - - - - - - -namespace details -{ - -template -bool meshConservativeAdvancementOrientedNodeCanStop(FCL_REAL c, - FCL_REAL min_distance, - FCL_REAL abs_err, FCL_REAL rel_err, FCL_REAL w, - const BVHModel* model1, const BVHModel* model2, - const MotionBase* motion1, const MotionBase* motion2, - std::vector& stack, - FCL_REAL& delta_t) -{ - if((c >= w * (min_distance - abs_err)) && (c * (1 + rel_err) >= w * min_distance)) - { - const ConservativeAdvancementStackData& data = stack.back(); - FCL_REAL d = data.d; - Vector3d n; - int c1, c2; - - if(d > c) - { - const ConservativeAdvancementStackData& data2 = stack[stack.size() - 2]; - d = data2.d; - n = data2.P2 - data2.P1; n.normalize(); - c1 = data2.c1; - c2 = data2.c2; - stack[stack.size() - 2] = stack[stack.size() - 1]; - } - else - { - n = data.P2 - data.P1; n.normalize(); - c1 = data.c1; - c2 = data.c2; - } - - assert(c == d); - - // n is in local frame of c1, so we need to turn n into the global frame - Vector3d n_transformed = - getBVAxis(model1->getBV(c1).bv, 0) * n[0] + - getBVAxis(model1->getBV(c1).bv, 1) * n[2] + // TODO(JS): not n[1]? - getBVAxis(model1->getBV(c1).bv, 2) * n[2]; - Quaternion3d R0; - motion1->getCurrentRotation(R0); - n_transformed = R0 * n_transformed; - n_transformed.normalize(); - - TBVMotionBoundVisitor mb_visitor1(model1->getBV(c1).bv, n_transformed), mb_visitor2(model2->getBV(c2).bv, -n_transformed); - FCL_REAL bound1 = motion1->computeMotionBound(mb_visitor1); - FCL_REAL bound2 = motion2->computeMotionBound(mb_visitor2); - - FCL_REAL bound = bound1 + bound2; - - FCL_REAL cur_delta_t; - if(bound <= c) cur_delta_t = 1; - else cur_delta_t = c / bound; - - if(cur_delta_t < delta_t) - delta_t = cur_delta_t; - - stack.pop_back(); - - return true; - } - else - { - const ConservativeAdvancementStackData& data = stack.back(); - FCL_REAL d = data.d; - - if(d > c) - stack[stack.size() - 2] = stack[stack.size() - 1]; - - stack.pop_back(); - - return false; - } -} - -template -void meshConservativeAdvancementOrientedNodeLeafTesting(int b1, int b2, - const BVHModel* model1, const BVHModel* model2, - const Triangle* tri_indices1, const Triangle* tri_indices2, - const Vector3d* vertices1, const Vector3d* vertices2, - const Matrix3d& R, const Vector3d& T, - const MotionBase* motion1, const MotionBase* motion2, - bool enable_statistics, - FCL_REAL& min_distance, - Vector3d& p1, Vector3d& p2, - int& last_tri_id1, int& last_tri_id2, - FCL_REAL& delta_t, - int& num_leaf_tests) -{ - if(enable_statistics) num_leaf_tests++; - - const BVNode& node1 = model1->getBV(b1); - const BVNode& node2 = model2->getBV(b2); - - int primitive_id1 = node1.primitiveId(); - int primitive_id2 = node2.primitiveId(); - - const Triangle& tri_id1 = tri_indices1[primitive_id1]; - const Triangle& tri_id2 = tri_indices2[primitive_id2]; - - const Vector3d& t11 = vertices1[tri_id1[0]]; - const Vector3d& t12 = vertices1[tri_id1[1]]; - const Vector3d& t13 = vertices1[tri_id1[2]]; - - const Vector3d& t21 = vertices2[tri_id2[0]]; - const Vector3d& t22 = vertices2[tri_id2[1]]; - const Vector3d& t23 = vertices2[tri_id2[2]]; - - // nearest point pair - Vector3d P1, P2; - - FCL_REAL d = TriangleDistance::triDistance(t11, t12, t13, t21, t22, t23, - R, T, - P1, P2); - - if(d < min_distance) - { - min_distance = d; - - p1 = P1; - p2 = P2; - - last_tri_id1 = primitive_id1; - last_tri_id2 = primitive_id2; - } - - - /// n is the local frame of object 1, pointing from object 1 to object2 - Vector3d n = P2 - P1; - /// turn n into the global frame, pointing from object 1 to object 2 - Quaternion3d R0; - motion1->getCurrentRotation(R0); - Vector3d n_transformed = R0 * n; - n_transformed.normalize(); // normalized here - - TriangleMotionBoundVisitor mb_visitor1(t11, t12, t13, n_transformed), mb_visitor2(t21, t22, t23, -n_transformed); - FCL_REAL bound1 = motion1->computeMotionBound(mb_visitor1); - FCL_REAL bound2 = motion2->computeMotionBound(mb_visitor2); - - FCL_REAL bound = bound1 + bound2; - - FCL_REAL cur_delta_t; - if(bound <= d) cur_delta_t = 1; - else cur_delta_t = d / bound; - - if(cur_delta_t < delta_t) - delta_t = cur_delta_t; -} - -} - - -MeshConservativeAdvancementTraversalNodeRSS::MeshConservativeAdvancementTraversalNodeRSS(FCL_REAL w_) - : MeshConservativeAdvancementTraversalNode(w_) -{ - R.setIdentity(); -} - -FCL_REAL MeshConservativeAdvancementTraversalNodeRSS::BVTesting(int b1, int b2) const -{ - if(enable_statistics) num_bv_tests++; - Vector3d P1, P2; - FCL_REAL d = distance(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv, &P1, &P2); - - stack.push_back(ConservativeAdvancementStackData(P1, P2, b1, b2, d)); - - return d; -} - - -void MeshConservativeAdvancementTraversalNodeRSS::leafTesting(int b1, int b2) const -{ - details::meshConservativeAdvancementOrientedNodeLeafTesting(b1, b2, - model1, model2, - tri_indices1, tri_indices2, - vertices1, vertices2, - R, T, - motion1, motion2, - enable_statistics, - min_distance, - closest_p1, closest_p2, - last_tri_id1, last_tri_id2, - delta_t, - num_leaf_tests); -} - -bool MeshConservativeAdvancementTraversalNodeRSS::canStop(FCL_REAL c) const -{ - return details::meshConservativeAdvancementOrientedNodeCanStop(c, - min_distance, - abs_err, rel_err, w, - model1, model2, - motion1, motion2, - stack, - delta_t); -} - - - - -MeshConservativeAdvancementTraversalNodeOBBRSS::MeshConservativeAdvancementTraversalNodeOBBRSS(FCL_REAL w_) - : MeshConservativeAdvancementTraversalNode(w_) -{ - R.setIdentity(); -} - -FCL_REAL MeshConservativeAdvancementTraversalNodeOBBRSS::BVTesting(int b1, int b2) const -{ - if(enable_statistics) num_bv_tests++; - Vector3d P1, P2; - FCL_REAL d = distance(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv, &P1, &P2); - - stack.push_back(ConservativeAdvancementStackData(P1, P2, b1, b2, d)); - - return d; -} - - -void MeshConservativeAdvancementTraversalNodeOBBRSS::leafTesting(int b1, int b2) const -{ - details::meshConservativeAdvancementOrientedNodeLeafTesting(b1, b2, - model1, model2, - tri_indices1, tri_indices2, - vertices1, vertices2, - R, T, - motion1, motion2, - enable_statistics, - min_distance, - closest_p1, closest_p2, - last_tri_id1, last_tri_id2, - delta_t, - num_leaf_tests); -} - -bool MeshConservativeAdvancementTraversalNodeOBBRSS::canStop(FCL_REAL c) const -{ - return details::meshConservativeAdvancementOrientedNodeCanStop(c, - min_distance, - abs_err, rel_err, w, - model1, model2, - motion1, motion2, - stack, - delta_t); -} - - - - - -} diff --git a/src/traversal/traversal_node_setup.cpp b/src/traversal/traversal_node_setup.cpp deleted file mode 100644 index 40e6c9b62..000000000 --- a/src/traversal/traversal_node_setup.cpp +++ /dev/null @@ -1,234 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - - -#include "fcl/traversal/traversal_node_setup.h" - -namespace fcl -{ - - -namespace details -{ -template -static inline bool setupMeshCollisionOrientedNode(OrientedNode& node, - const BVHModel& model1, const Transform3d& tf1, - const BVHModel& model2, const Transform3d& tf2, - const CollisionRequest& request, - CollisionResult& result) -{ - if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES) - return false; - - node.vertices1 = model1.vertices; - node.vertices2 = model2.vertices; - - node.tri_indices1 = model1.tri_indices; - node.tri_indices2 = model2.tri_indices; - - node.model1 = &model1; - node.tf1 = tf1; - node.model2 = &model2; - node.tf2 = tf2; - - node.request = request; - node.result = &result; - - node.cost_density = model1.cost_density * model2.cost_density; - - relativeTransform(tf1, tf2, node.R, node.T); - - return true; -} - -} - - -bool initialize(MeshCollisionTraversalNodeOBB& node, - const BVHModel& model1, const Transform3d& tf1, - const BVHModel& model2, const Transform3d& tf2, - const CollisionRequest& request, - CollisionResult& result) -{ - return details::setupMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, request, result); -} - - -bool initialize(MeshCollisionTraversalNodeRSS& node, - const BVHModel& model1, const Transform3d& tf1, - const BVHModel& model2, const Transform3d& tf2, - const CollisionRequest& request, - CollisionResult& result) -{ - return details::setupMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, request, result); -} - - -bool initialize(MeshCollisionTraversalNodekIOS& node, - const BVHModel& model1, const Transform3d& tf1, - const BVHModel& model2, const Transform3d& tf2, - const CollisionRequest& request, - CollisionResult& result) -{ - return details::setupMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, request, result); -} - -bool initialize(MeshCollisionTraversalNodeOBBRSS& node, - const BVHModel& model1, const Transform3d& tf1, - const BVHModel& model2, const Transform3d& tf2, - const CollisionRequest& request, - CollisionResult& result) -{ - return details::setupMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, request, result); -} - - -namespace details -{ -template -static inline bool setupMeshDistanceOrientedNode(OrientedNode& node, - const BVHModel& model1, const Transform3d& tf1, - const BVHModel& model2, const Transform3d& tf2, - const DistanceRequest& request, - DistanceResult& result) -{ - if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES) - return false; - - node.request = request; - node.result = &result; - - node.model1 = &model1; - node.tf1 = tf1; - node.model2 = &model2; - node.tf2 = tf2; - - node.vertices1 = model1.vertices; - node.vertices2 = model2.vertices; - - node.tri_indices1 = model1.tri_indices; - node.tri_indices2 = model2.tri_indices; - - relativeTransform(tf1, tf2, node.R, node.T); - - return true; -} - - -} - -bool initialize(MeshDistanceTraversalNodeRSS& node, - const BVHModel& model1, const Transform3d& tf1, - const BVHModel& model2, const Transform3d& tf2, - const DistanceRequest& request, - DistanceResult& result) -{ - return details::setupMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, request, result); -} - - -bool initialize(MeshDistanceTraversalNodekIOS& node, - const BVHModel& model1, const Transform3d& tf1, - const BVHModel& model2, const Transform3d& tf2, - const DistanceRequest& request, - DistanceResult& result) -{ - return details::setupMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, request, result); -} - -bool initialize(MeshDistanceTraversalNodeOBBRSS& node, - const BVHModel& model1, const Transform3d& tf1, - const BVHModel& model2, const Transform3d& tf2, - const DistanceRequest& request, - DistanceResult& result) -{ - return details::setupMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, request, result); -} - -namespace details -{ - - -template -static inline bool setupMeshConservativeAdvancementOrientedDistanceNode(OrientedDistanceNode& node, - const BVHModel& model1, const Transform3d& tf1, - const BVHModel& model2, const Transform3d& tf2, - FCL_REAL w) -{ - if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES) - return false; - - node.model1 = &model1; - node.model2 = &model2; - - node.vertices1 = model1.vertices; - node.vertices2 = model2.vertices; - - node.tri_indices1 = model1.tri_indices; - node.tri_indices2 = model2.tri_indices; - - node.w = w; - - relativeTransform(tf1, tf2, node.R, node.T); - - return true; -} - -} - - -bool initialize(MeshConservativeAdvancementTraversalNodeRSS& node, - const BVHModel& model1, const Transform3d& tf1, - const BVHModel& model2, const Transform3d& tf2, - FCL_REAL w) -{ - return details::setupMeshConservativeAdvancementOrientedDistanceNode(node, model1, tf1, model2, tf2, w); -} - - -bool initialize(MeshConservativeAdvancementTraversalNodeOBBRSS& node, - const BVHModel& model1, const Transform3d& tf1, - const BVHModel& model2, const Transform3d& tf2, - FCL_REAL w) -{ - return details::setupMeshConservativeAdvancementOrientedDistanceNode(node, model1, tf1, model2, tf2, w); -} - - - - -} diff --git a/src/traversal/traversal_recurse.cpp b/src/traversal/traversal_recurse.cpp deleted file mode 100644 index 77a446890..000000000 --- a/src/traversal/traversal_recurse.cpp +++ /dev/null @@ -1,451 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - - -#include "fcl/traversal/traversal_recurse.h" - -namespace fcl -{ -void collisionRecurse(CollisionTraversalNodeBase* node, int b1, int b2, BVHFrontList* front_list) -{ - bool l1 = node->isFirstNodeLeaf(b1); - bool l2 = node->isSecondNodeLeaf(b2); - - if(l1 && l2) - { - updateFrontList(front_list, b1, b2); - - if(node->BVTesting(b1, b2)) return; - - node->leafTesting(b1, b2); - return; - } - - if(node->BVTesting(b1, b2)) - { - updateFrontList(front_list, b1, b2); - return; - } - - if(node->firstOverSecond(b1, b2)) - { - int c1 = node->getFirstLeftChild(b1); - int c2 = node->getFirstRightChild(b1); - - collisionRecurse(node, c1, b2, front_list); - - // early stop is disabled is front_list is used - if(node->canStop() && !front_list) return; - - collisionRecurse(node, c2, b2, front_list); - } - else - { - int c1 = node->getSecondLeftChild(b2); - int c2 = node->getSecondRightChild(b2); - - collisionRecurse(node, b1, c1, front_list); - - // early stop is disabled is front_list is used - if(node->canStop() && !front_list) return; - - collisionRecurse(node, b1, c2, front_list); - } -} - -void collisionRecurse(MeshCollisionTraversalNodeOBB* node, int b1, int b2, const Matrix3d& R, const Vector3d& T, BVHFrontList* front_list) -{ - bool l1 = node->isFirstNodeLeaf(b1); - bool l2 = node->isSecondNodeLeaf(b2); - - if(l1 && l2) - { - updateFrontList(front_list, b1, b2); - - if(node->BVTesting(b1, b2, R, T)) return; - - node->leafTesting(b1, b2, R, T); - return; - } - - if(node->BVTesting(b1, b2, R, T)) - { - updateFrontList(front_list, b1, b2); - return; - } - - Vector3d temp; - - if(node->firstOverSecond(b1, b2)) - { - int c1 = node->getFirstLeftChild(b1); - int c2 = node->getFirstRightChild(b1); - - const OBB& bv1 = node->model1->getBV(c1).bv; - - Matrix3d Rc = R.transpose() * bv1.axis; - temp = T - bv1.To; - Vector3d Tc = temp.transpose() * bv1.axis; - - collisionRecurse(node, c1, b2, Rc, Tc, front_list); - - // early stop is disabled is front_list is used - if(node->canStop() && !front_list) return; - - const OBB& bv2 = node->model1->getBV(c2).bv; - - Rc = R.transpose() * bv2.axis; - temp = T - bv2.To; - Tc = temp.transpose() * bv2.axis; - - collisionRecurse(node, c2, b2, Rc, Tc, front_list); - } - else - { - int c1 = node->getSecondLeftChild(b2); - int c2 = node->getSecondRightChild(b2); - - const OBB& bv1 = node->model2->getBV(c1).bv; - Matrix3d Rc; - temp = R * bv1.axis.col(0); - Rc(0, 0) = temp[0]; Rc(1, 0) = temp[1]; Rc(2, 0) = temp[2]; - temp = R * bv1.axis.col(1); - Rc(0, 1) = temp[0]; Rc(1, 1) = temp[1]; Rc(2, 1) = temp[2]; - temp = R * bv1.axis.col(2); - Rc(0, 2) = temp[0]; Rc(1, 2) = temp[1]; Rc(2, 2) = temp[2]; - Vector3d Tc = R * bv1.To + T; - - collisionRecurse(node, b1, c1, Rc, Tc, front_list); - - // early stop is disabled is front_list is used - if(node->canStop() && !front_list) return; - - const OBB& bv2 = node->model2->getBV(c2).bv; - temp = R * bv2.axis.col(0); - Rc(0, 0) = temp[0]; Rc(1, 0) = temp[1]; Rc(2, 0) = temp[2]; - temp = R * bv2.axis.col(1); - Rc(0, 1) = temp[0]; Rc(1, 1) = temp[1]; Rc(2, 1) = temp[2]; - temp = R * bv2.axis.col(2); - Rc(0, 2) = temp[0]; Rc(1, 2) = temp[1]; Rc(2, 2) = temp[2]; - Tc = R * bv2.To + T; - - collisionRecurse(node, b1, c2, Rc, Tc, front_list); - } -} - -void collisionRecurse(MeshCollisionTraversalNodeRSS* node, int b1, int b2, const Matrix3d& R, const Vector3d& T, BVHFrontList* front_list) -{ - -} - -/** Recurse function for self collision - * Make sure node is set correctly so that the first and second tree are the same - */ -void selfCollisionRecurse(CollisionTraversalNodeBase* node, int b, BVHFrontList* front_list) -{ - bool l = node->isFirstNodeLeaf(b); - - if(l) return; - - int c1 = node->getFirstLeftChild(b); - int c2 = node->getFirstRightChild(b); - - selfCollisionRecurse(node, c1, front_list); - if(node->canStop() && !front_list) return; - - selfCollisionRecurse(node, c2, front_list); - if(node->canStop() && !front_list) return; - - collisionRecurse(node, c1, c2, front_list); -} - -void distanceRecurse(DistanceTraversalNodeBase* node, int b1, int b2, BVHFrontList* front_list) -{ - bool l1 = node->isFirstNodeLeaf(b1); - bool l2 = node->isSecondNodeLeaf(b2); - - if(l1 && l2) - { - updateFrontList(front_list, b1, b2); - - node->leafTesting(b1, b2); - return; - } - - int a1, a2, c1, c2; - - if(node->firstOverSecond(b1, b2)) - { - a1 = node->getFirstLeftChild(b1); - a2 = b2; - c1 = node->getFirstRightChild(b1); - c2 = b2; - } - else - { - a1 = b1; - a2 = node->getSecondLeftChild(b2); - c1 = b1; - c2 = node->getSecondRightChild(b2); - } - - FCL_REAL d1 = node->BVTesting(a1, a2); - FCL_REAL d2 = node->BVTesting(c1, c2); - - if(d2 < d1) - { - if(!node->canStop(d2)) - distanceRecurse(node, c1, c2, front_list); - else - updateFrontList(front_list, c1, c2); - - if(!node->canStop(d1)) - distanceRecurse(node, a1, a2, front_list); - else - updateFrontList(front_list, a1, a2); - } - else - { - if(!node->canStop(d1)) - distanceRecurse(node, a1, a2, front_list); - else - updateFrontList(front_list, a1, a2); - - if(!node->canStop(d2)) - distanceRecurse(node, c1, c2, front_list); - else - updateFrontList(front_list, c1, c2); - } -} - - -/** \brief Bounding volume test structure */ -struct BVT -{ - /** \brief distance between bvs */ - FCL_REAL d; - - /** \brief bv indices for a pair of bvs in two models */ - int b1, b2; -}; - -/** \brief Comparer between two BVT */ -struct BVT_Comparer -{ - bool operator() (const BVT& lhs, const BVT& rhs) const - { - return lhs.d > rhs.d; - } -}; - -struct BVTQ -{ - BVTQ() : qsize(2) {} - - bool empty() const - { - return pq.empty(); - } - - size_t size() const - { - return pq.size(); - } - - const BVT& top() const - { - return pq.top(); - } - - void push(const BVT& x) - { - pq.push(x); - } - - void pop() - { - pq.pop(); - } - - bool full() const - { - return (pq.size() + 1 >= qsize); - } - - std::priority_queue, BVT_Comparer> pq; - - /** \brief Queue size */ - unsigned int qsize; -}; - - -void distanceQueueRecurse(DistanceTraversalNodeBase* node, int b1, int b2, BVHFrontList* front_list, int qsize) -{ - BVTQ bvtq; - bvtq.qsize = qsize; - - BVT min_test; - min_test.b1 = b1; - min_test.b2 = b2; - - while(1) - { - bool l1 = node->isFirstNodeLeaf(min_test.b1); - bool l2 = node->isSecondNodeLeaf(min_test.b2); - - if(l1 && l2) - { - updateFrontList(front_list, min_test.b1, min_test.b2); - - node->leafTesting(min_test.b1, min_test.b2); - } - else if(bvtq.full()) - { - // queue should not get two more tests, recur - - distanceQueueRecurse(node, min_test.b1, min_test.b2, front_list, qsize); - } - else - { - // queue capacity is not full yet - BVT bvt1, bvt2; - - if(node->firstOverSecond(min_test.b1, min_test.b2)) - { - int c1 = node->getFirstLeftChild(min_test.b1); - int c2 = node->getFirstRightChild(min_test.b1); - bvt1.b1 = c1; - bvt1.b2 = min_test.b2; - bvt1.d = node->BVTesting(bvt1.b1, bvt1.b2); - - bvt2.b1 = c2; - bvt2.b2 = min_test.b2; - bvt2.d = node->BVTesting(bvt2.b1, bvt2.b2); - } - else - { - int c1 = node->getSecondLeftChild(min_test.b2); - int c2 = node->getSecondRightChild(min_test.b2); - bvt1.b1 = min_test.b1; - bvt1.b2 = c1; - bvt1.d = node->BVTesting(bvt1.b1, bvt1.b2); - - bvt2.b1 = min_test.b1; - bvt2.b2 = c2; - bvt2.d = node->BVTesting(bvt2.b1, bvt2.b2); - } - - bvtq.push(bvt1); - bvtq.push(bvt2); - } - - if(bvtq.empty()) - break; - else - { - min_test = bvtq.top(); - bvtq.pop(); - - if(node->canStop(min_test.d)) - { - updateFrontList(front_list, min_test.b1, min_test.b2); - break; - } - } - } -} - -void propagateBVHFrontListCollisionRecurse(CollisionTraversalNodeBase* node, BVHFrontList* front_list) -{ - BVHFrontList::iterator front_iter; - BVHFrontList append; - for(front_iter = front_list->begin(); front_iter != front_list->end(); ++front_iter) - { - int b1 = front_iter->left; - int b2 = front_iter->right; - bool l1 = node->isFirstNodeLeaf(b1); - bool l2 = node->isSecondNodeLeaf(b2); - - if(l1 & l2) - { - front_iter->valid = false; // the front node is no longer valid, in collideRecurse will add again. - collisionRecurse(node, b1, b2, &append); - } - else - { - if(!node->BVTesting(b1, b2)) - { - front_iter->valid = false; - - if(node->firstOverSecond(b1, b2)) - { - int c1 = node->getFirstLeftChild(b1); - int c2 = node->getFirstRightChild(b1); - - collisionRecurse(node, c1, b2, front_list); - collisionRecurse(node, c2, b2, front_list); - } - else - { - int c1 = node->getSecondLeftChild(b2); - int c2 = node->getSecondRightChild(b2); - - collisionRecurse(node, b1, c1, front_list); - collisionRecurse(node, b1, c2, front_list); - } - } - } - } - - - // clean the old front list (remove invalid node) - for(front_iter = front_list->begin(); front_iter != front_list->end();) - { - if(!front_iter->valid) - front_iter = front_list->erase(front_iter); - else - ++front_iter; - } - - for(front_iter = append.begin(); front_iter != append.end(); ++front_iter) - { - front_list->push_back(*front_iter); - } -} - - -} diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 01aabdc81..830526295 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -33,10 +33,14 @@ execute_process(COMMAND cmake -E make_directory ${CMAKE_BINARY_DIR}/test_results include_directories(${GTEST_INCLUDE_DIRS}) add_library(test_fcl_utility test_fcl_utility.cpp) +target_link_libraries(test_fcl_utility fcl) # test file list set(tests - test_fcl_broadphase.cpp + test_fcl_auto_diff.cpp + test_fcl_broadphase_collision_1.cpp + test_fcl_broadphase_collision_2.cpp + test_fcl_broadphase_distance.cpp test_fcl_bvh_models.cpp test_fcl_capsule_box_1.cpp test_fcl_capsule_box_2.cpp @@ -46,13 +50,16 @@ set(tests test_fcl_frontlist.cpp test_fcl_geometric_shapes.cpp test_fcl_math.cpp + test_fcl_profiler.cpp test_fcl_shape_mesh_consistency.cpp test_fcl_simple.cpp test_fcl_sphere_capsule.cpp ) if (FCL_HAVE_OCTOMAP) - list(APPEND tests test_fcl_octomap.cpp) + list(APPEND tests test_fcl_octomap_cost.cpp) + list(APPEND tests test_fcl_octomap_collision.cpp) + list(APPEND tests test_fcl_octomap_distance.cpp) endif() macro(add_fcl_test test_file_name) diff --git a/test/test_fcl_auto_diff.cpp b/test/test_fcl_auto_diff.cpp new file mode 100644 index 000000000..7bc846990 --- /dev/null +++ b/test/test_fcl_auto_diff.cpp @@ -0,0 +1,116 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * Copyright (c) 2016, Toyota Research Institute + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jeongseok Lee */ + +#include +#include +#include +#include "fcl/distance.h" + +using namespace fcl; + +//============================================================================== +template +S getDistance(const Vector3& p) +{ + GJKSolver_libccd solver; + + S dist; + + Sphere s1(20); + Sphere s2(10); + + Transform3 tf1 = Transform3::Identity(); + Transform3 tf2 = Transform3::Identity(); + + tf2.translation() = p; + + solver.shapeDistance(s1, tf1, s2, tf2, &dist); + + return dist; +} + +//============================================================================== +template +void test_basic() +{ + using derivative_t = Eigen::Matrix; + using scalar_t = Eigen::AutoDiffScalar; + using input_t = Eigen::Matrix; + + input_t pos(40, 0, 0); + pos(0).derivatives() = derivative_t::Unit(3, 0); + pos(1).derivatives() = derivative_t::Unit(3, 1); + pos(2).derivatives() = derivative_t::Unit(3, 2); + + auto dist = getDistance(pos); + EXPECT_EQ(dist, (S)10); + EXPECT_EQ(dist.value(), (S)10); + EXPECT_EQ(dist.derivatives(), Vector3(1, 0, 0)); + + pos << 0, 40, 0; + pos(0).derivatives() = derivative_t::Unit(3, 0); + pos(1).derivatives() = derivative_t::Unit(3, 1); + pos(2).derivatives() = derivative_t::Unit(3, 2); + dist = getDistance(pos); + EXPECT_EQ(dist, (S)10); + EXPECT_EQ(dist.value(), (S)10); + EXPECT_EQ(dist.derivatives(), Vector3(0, 1, 0)); + + pos << 0, 0, 40; + pos(0).derivatives() = derivative_t::Unit(3, 0); + pos(1).derivatives() = derivative_t::Unit(3, 1); + pos(2).derivatives() = derivative_t::Unit(3, 2); + dist = getDistance(pos); + EXPECT_EQ(dist, (S)10); + EXPECT_EQ(dist.value(), (S)10); + EXPECT_EQ(dist.derivatives(), Vector3(0, 0, 1)); +} + +//============================================================================== +GTEST_TEST(FCL_AUTO_DIFF, basic) +{ + test_basic(); + test_basic(); +} + +//============================================================================== +int main(int argc, char* argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test/test_fcl_broadphase.cpp b/test/test_fcl_broadphase.cpp deleted file mode 100644 index f375a3490..000000000 --- a/test/test_fcl_broadphase.cpp +++ /dev/null @@ -1,1265 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#include - -#include "fcl/config.h" -#include "fcl/broadphase/broadphase.h" -#include "fcl/shape/geometric_shape_to_BVH_model.h" -#include "test_fcl_utility.h" - -#if USE_GOOGLEHASH -#include -#include -#include -#endif - -#include -#include - -using namespace fcl; - -struct TStruct -{ - std::vector records; - double overall_time; - - TStruct() { overall_time = 0; } - - void push_back(double t) - { - records.push_back(t); - overall_time += t; - } -}; - -/// @brief Generate environment with 3 * n objects: n boxes, n spheres and n cylinders. -void generateEnvironments(std::vector& env, double env_scale, std::size_t n); - -/// @brief Generate environment with 3 * n objects for self distance, so we try to make sure none of them collide with each other. -void generateSelfDistanceEnvironments(std::vector& env, double env_scale, std::size_t n); - -/// @brief Generate environment with 3 * n objects, but all in meshes. -void generateEnvironmentsMesh(std::vector& env, double env_scale, std::size_t n); - -/// @brief Generate environment with 3 * n objects for self distance, but all in meshes. -void generateSelfDistanceEnvironmentsMesh(std::vector& env, double env_scale, std::size_t n); - -/// @brief test for broad phase collision and self collision -void broad_phase_collision_test(double env_scale, std::size_t env_size, std::size_t query_size, std::size_t num_max_contacts = 1, bool exhaustive = false, bool use_mesh = false); - -/// @brief test for broad phase distance -void broad_phase_distance_test(double env_scale, std::size_t env_size, std::size_t query_size, bool use_mesh = false); - -/// @brief test for broad phase self distance -void broad_phase_self_distance_test(double env_scale, std::size_t env_size, bool use_mesh = false); - -/// @brief test for broad phase update -void broad_phase_update_collision_test(double env_scale, std::size_t env_size, std::size_t query_size, std::size_t num_max_contacts = 1, bool exhaustive = false, bool use_mesh = false); - -FCL_REAL DELTA = 0.01; - - -#if USE_GOOGLEHASH -template -struct GoogleSparseHashTable : public google::sparse_hash_map, std::equal_to > {}; - -template -struct GoogleDenseHashTable : public google::dense_hash_map, std::equal_to > -{ - GoogleDenseHashTable() : google::dense_hash_map, std::equal_to >() - { - this->set_empty_key(NULL); - } -}; -#endif - -/// check the update, only return collision or not -GTEST_TEST(FCL_BROADPHASE, test_core_bf_broad_phase_update_collision_binary) -{ -#if FCL_BUILD_TYPE_DEBUG - broad_phase_update_collision_test(2000, 10, 100, 1, false); - broad_phase_update_collision_test(2000, 100, 100, 1, false); -#else - broad_phase_update_collision_test(2000, 100, 1000, 1, false); - broad_phase_update_collision_test(2000, 1000, 1000, 1, false); -#endif -} - -/// check the update, return 10 contacts -GTEST_TEST(FCL_BROADPHASE, test_core_bf_broad_phase_update_collision) -{ -#if FCL_BUILD_TYPE_DEBUG - broad_phase_update_collision_test(2000, 10, 100, 10, false); - broad_phase_update_collision_test(2000, 100, 100, 10, false); -#else - broad_phase_update_collision_test(2000, 100, 1000, 10, false); - broad_phase_update_collision_test(2000, 1000, 1000, 10, false); -#endif -} - -/// check the update, exhaustive -GTEST_TEST(FCL_BROADPHASE, test_core_bf_broad_phase_update_collision_exhaustive) -{ -#if FCL_BUILD_TYPE_DEBUG - broad_phase_update_collision_test(2000, 10, 100, 1, true); - broad_phase_update_collision_test(2000, 100, 100, 1, true); -#else - broad_phase_update_collision_test(2000, 100, 1000, 1, true); - broad_phase_update_collision_test(2000, 1000, 1000, 1, true); -#endif -} - -/// check broad phase distance -GTEST_TEST(FCL_BROADPHASE, test_core_bf_broad_phase_distance) -{ -#if FCL_BUILD_TYPE_DEBUG - broad_phase_distance_test(200, 10, 10); - broad_phase_distance_test(200, 100, 10); - broad_phase_distance_test(2000, 10, 10); - broad_phase_distance_test(2000, 100, 10); -#else - broad_phase_distance_test(200, 100, 100); - broad_phase_distance_test(200, 1000, 100); - broad_phase_distance_test(2000, 100, 100); - broad_phase_distance_test(2000, 1000, 100); -#endif -} - -/// check broad phase self distance -GTEST_TEST(FCL_BROADPHASE, test_core_bf_broad_phase_self_distance) -{ -#if FCL_BUILD_TYPE_DEBUG - broad_phase_self_distance_test(200, 256); - broad_phase_self_distance_test(200, 500); - broad_phase_self_distance_test(200, 2500); -#else - broad_phase_self_distance_test(200, 512); - broad_phase_self_distance_test(200, 1000); - broad_phase_self_distance_test(200, 5000); -#endif -} - -/// check broad phase collision for empty collision object set and queries -GTEST_TEST(FCL_BROADPHASE, test_core_bf_broad_phase_collision_empty) -{ -#if FCL_BUILD_TYPE_DEBUG - broad_phase_collision_test(2000, 0, 0, 10, false, false); - broad_phase_collision_test(2000, 0, 5, 10, false, false); - broad_phase_collision_test(2000, 2, 0, 10, false, false); - - broad_phase_collision_test(2000, 0, 0, 10, false, true); - broad_phase_collision_test(2000, 0, 5, 10, false, true); - broad_phase_collision_test(2000, 2, 0, 10, false, true); - - broad_phase_collision_test(2000, 0, 0, 10, true, false); - broad_phase_collision_test(2000, 0, 5, 10, true, false); - broad_phase_collision_test(2000, 2, 0, 10, true, false); - - broad_phase_collision_test(2000, 0, 0, 10, true, true); - broad_phase_collision_test(2000, 0, 5, 10, true, true); - broad_phase_collision_test(2000, 2, 0, 10, true, true); -#else - broad_phase_collision_test(2000, 0, 0, 10, false, false); - broad_phase_collision_test(2000, 0, 1000, 10, false, false); - broad_phase_collision_test(2000, 100, 0, 10, false, false); - - broad_phase_collision_test(2000, 0, 0, 10, false, true); - broad_phase_collision_test(2000, 0, 1000, 10, false, true); - broad_phase_collision_test(2000, 100, 0, 10, false, true); - - broad_phase_collision_test(2000, 0, 0, 10, true, false); - broad_phase_collision_test(2000, 0, 1000, 10, true, false); - broad_phase_collision_test(2000, 100, 0, 10, true, false); - - broad_phase_collision_test(2000, 0, 0, 10, true, true); - broad_phase_collision_test(2000, 0, 1000, 10, true, true); - broad_phase_collision_test(2000, 100, 0, 10, true, true); -#endif -} - -/// check broad phase collision and self collision, only return collision or not -GTEST_TEST(FCL_BROADPHASE, test_core_bf_broad_phase_collision_binary) -{ -#if FCL_BUILD_TYPE_DEBUG - broad_phase_collision_test(2000, 10, 100, 1, false); - broad_phase_collision_test(2000, 100, 100, 1, false); - broad_phase_collision_test(2000, 10, 100, 1, true); - broad_phase_collision_test(2000, 100, 100, 1, true); -#else - broad_phase_collision_test(2000, 100, 1000, 1, false); - broad_phase_collision_test(2000, 1000, 1000, 1, false); - broad_phase_collision_test(2000, 100, 1000, 1, true); - broad_phase_collision_test(2000, 1000, 1000, 1, true); -#endif -} - -/// check broad phase collision and self collision, return 10 contacts -GTEST_TEST(FCL_BROADPHASE, test_core_bf_broad_phase_collision) -{ -#if FCL_BUILD_TYPE_DEBUG - broad_phase_collision_test(2000, 10, 100, 10, false); - broad_phase_collision_test(2000, 100, 100, 10, false); -#else - broad_phase_collision_test(2000, 100, 1000, 10, false); - broad_phase_collision_test(2000, 1000, 1000, 10, false); -#endif -} - -/// check broad phase update, in mesh, only return collision or not -GTEST_TEST(FCL_BROADPHASE, test_core_mesh_bf_broad_phase_update_collision_mesh_binary) -{ -#if FCL_BUILD_TYPE_DEBUG - broad_phase_update_collision_test(2000, 2, 4, 1, false, true); - broad_phase_update_collision_test(2000, 4, 4, 1, false, true); -#else - broad_phase_update_collision_test(2000, 100, 1000, 1, false, true); - broad_phase_update_collision_test(2000, 1000, 1000, 1, false, true); -#endif -} - -/// check broad phase update, in mesh, return 10 contacts -GTEST_TEST(FCL_BROADPHASE, test_core_mesh_bf_broad_phase_update_collision_mesh) -{ -#if FCL_BUILD_TYPE_DEBUG - broad_phase_update_collision_test(200, 2, 4, 10, false, true); - broad_phase_update_collision_test(200, 4, 4, 10, false, true); -#else - broad_phase_update_collision_test(2000, 100, 1000, 10, false, true); - broad_phase_update_collision_test(2000, 1000, 1000, 10, false, true); -#endif -} - -/// check broad phase update, in mesh, exhaustive -GTEST_TEST(FCL_BROADPHASE, test_core_mesh_bf_broad_phase_update_collision_mesh_exhaustive) -{ -#if FCL_BUILD_TYPE_DEBUG - broad_phase_update_collision_test(2000, 2, 4, 1, true, true); - broad_phase_update_collision_test(2000, 4, 4, 1, true, true); -#else - broad_phase_update_collision_test(2000, 100, 1000, 1, true, true); - broad_phase_update_collision_test(2000, 1000, 1000, 1, true, true); -#endif -} - -/// check broad phase distance -GTEST_TEST(FCL_BROADPHASE, test_core_mesh_bf_broad_phase_distance_mesh) -{ -#if FCL_BUILD_TYPE_DEBUG - broad_phase_distance_test(200, 2, 2, true); - broad_phase_distance_test(200, 4, 2, true); - broad_phase_distance_test(2000, 2, 2, true); - broad_phase_distance_test(2000, 4, 2, true); -#else - broad_phase_distance_test(200, 100, 100, true); - broad_phase_distance_test(200, 1000, 100, true); - broad_phase_distance_test(2000, 100, 100, true); - broad_phase_distance_test(2000, 1000, 100, true); -#endif -} - -/// check broad phase self distance -GTEST_TEST(FCL_BROADPHASE, test_core_mesh_bf_broad_phase_self_distance_mesh) -{ -#if FCL_BUILD_TYPE_DEBUG - broad_phase_self_distance_test(200, 128, true); - broad_phase_self_distance_test(200, 250, true); - broad_phase_self_distance_test(200, 1250, true); -#else - broad_phase_self_distance_test(200, 512, true); - broad_phase_self_distance_test(200, 1000, true); - broad_phase_self_distance_test(200, 5000, true); -#endif -} - -/// check broad phase collision and self collision, return only collision or not, in mesh -GTEST_TEST(FCL_BROADPHASE, test_core_mesh_bf_broad_phase_collision_mesh_binary) -{ -#if FCL_BUILD_TYPE_DEBUG - broad_phase_collision_test(2000, 10, 100, 1, false, true); - broad_phase_collision_test(2000, 100, 100, 1, false, true); -#else - broad_phase_collision_test(2000, 100, 1000, 1, false, true); - broad_phase_collision_test(2000, 1000, 1000, 1, false, true); -#endif -} - -/// check broad phase collision and self collision, return 10 contacts, in mesh -GTEST_TEST(FCL_BROADPHASE, test_core_mesh_bf_broad_phase_collision_mesh) -{ -#if FCL_BUILD_TYPE_DEBUG - broad_phase_collision_test(2000, 2, 5, 10, false, true); - broad_phase_collision_test(2000, 5, 5, 10, false, true); -#else - broad_phase_collision_test(2000, 100, 1000, 10, false, true); - broad_phase_collision_test(2000, 1000, 1000, 10, false, true); -#endif -} - -/// check broad phase collision and self collision, exhaustive, in mesh -GTEST_TEST(FCL_BROADPHASE, test_core_mesh_bf_broad_phase_collision_mesh_exhaustive) -{ -#if FCL_BUILD_TYPE_DEBUG - broad_phase_collision_test(2000, 2, 5, 1, true, true); - broad_phase_collision_test(2000, 5, 5, 1, true, true); -#else - broad_phase_collision_test(2000, 100, 1000, 1, true, true); - broad_phase_collision_test(2000, 1000, 1000, 1, true, true); -#endif -} - -void generateEnvironments(std::vector& env, double env_scale, std::size_t n) -{ - FCL_REAL extents[] = {-env_scale, env_scale, -env_scale, env_scale, -env_scale, env_scale}; - std::vector transforms; - - generateRandomTransforms(extents, transforms, n); - for(std::size_t i = 0; i < n; ++i) - { - Box* box = new Box(5, 10, 20); - env.push_back(new CollisionObject(std::shared_ptr(box), transforms[i])); - } - - generateRandomTransforms(extents, transforms, n); - for(std::size_t i = 0; i < n; ++i) - { - Sphere* sphere = new Sphere(30); - env.push_back(new CollisionObject(std::shared_ptr(sphere), transforms[i])); - } - - generateRandomTransforms(extents, transforms, n); - for(std::size_t i = 0; i < n; ++i) - { - Cylinder* cylinder = new Cylinder(10, 40); - env.push_back(new CollisionObject(std::shared_ptr(cylinder), transforms[i])); - } -} - -void generateEnvironmentsMesh(std::vector& env, double env_scale, std::size_t n) -{ - FCL_REAL extents[] = {-env_scale, env_scale, -env_scale, env_scale, -env_scale, env_scale}; - std::vector transforms; - - generateRandomTransforms(extents, transforms, n); - Box box(5, 10, 20); - for(std::size_t i = 0; i < n; ++i) - { - BVHModel* model = new BVHModel(); - generateBVHModel(*model, box, Transform3d::Identity()); - env.push_back(new CollisionObject(std::shared_ptr(model), transforms[i])); - } - - generateRandomTransforms(extents, transforms, n); - Sphere sphere(30); - for(std::size_t i = 0; i < n; ++i) - { - BVHModel* model = new BVHModel(); - generateBVHModel(*model, sphere, Transform3d::Identity(), 16, 16); - env.push_back(new CollisionObject(std::shared_ptr(model), transforms[i])); - } - - generateRandomTransforms(extents, transforms, n); - Cylinder cylinder(10, 40); - for(std::size_t i = 0; i < n; ++i) - { - BVHModel* model = new BVHModel(); - generateBVHModel(*model, cylinder, Transform3d::Identity(), 16, 16); - env.push_back(new CollisionObject(std::shared_ptr(model), transforms[i])); - } -} - -void generateSelfDistanceEnvironments(std::vector& env, double env_scale, std::size_t n) -{ - unsigned int n_edge = std::floor(std::pow(n, 1/3.0)); - - FCL_REAL step_size = env_scale * 2 / n_edge; - FCL_REAL delta_size = step_size * 0.05; - FCL_REAL single_size = step_size - 2 * delta_size; - - unsigned int i = 0; - for(; i < n_edge * n_edge * n_edge / 4; ++i) - { - int x = i % (n_edge * n_edge); - int y = (i - n_edge * n_edge * x) % n_edge; - int z = i - n_edge * n_edge * x - n_edge * y; - - Box* box = new Box(single_size, single_size, single_size); - env.push_back(new CollisionObject(std::shared_ptr(box), - Transform3d(Eigen::Translation3d(Vector3d(x * step_size + delta_size + 0.5 * single_size - env_scale, - y * step_size + delta_size + 0.5 * single_size - env_scale, - z * step_size + delta_size + 0.5 * single_size - env_scale))))); - } - - for(; i < n_edge * n_edge * n_edge / 4; ++i) - { - int x = i % (n_edge * n_edge); - int y = (i - n_edge * n_edge * x) % n_edge; - int z = i - n_edge * n_edge * x - n_edge * y; - - Sphere* sphere = new Sphere(single_size / 2); - env.push_back(new CollisionObject(std::shared_ptr(sphere), - Transform3d(Eigen::Translation3d(Vector3d(x * step_size + delta_size + 0.5 * single_size - env_scale, - y * step_size + delta_size + 0.5 * single_size - env_scale, - z * step_size + delta_size + 0.5 * single_size - env_scale))))); - } - - for(; i < n_edge * n_edge * n_edge / 4; ++i) - { - int x = i % (n_edge * n_edge); - int y = (i - n_edge * n_edge * x) % n_edge; - int z = i - n_edge * n_edge * x - n_edge * y; - - Ellipsoid* ellipsoid = new Ellipsoid(single_size / 2, single_size / 2, single_size / 2); - env.push_back(new CollisionObject(std::shared_ptr(ellipsoid), - Transform3d(Eigen::Translation3d(Vector3d(x * step_size + delta_size + 0.5 * single_size - env_scale, - y * step_size + delta_size + 0.5 * single_size - env_scale, - z * step_size + delta_size + 0.5 * single_size - env_scale))))); - } - - for(; i < n_edge * n_edge * n_edge / 4; ++i) - { - int x = i % (n_edge * n_edge); - int y = (i - n_edge * n_edge * x) % n_edge; - int z = i - n_edge * n_edge * x - n_edge * y; - - Cylinder* cylinder = new Cylinder(single_size / 2, single_size); - env.push_back(new CollisionObject(std::shared_ptr(cylinder), - Transform3d(Eigen::Translation3d(Vector3d(x * step_size + delta_size + 0.5 * single_size - env_scale, - y * step_size + delta_size + 0.5 * single_size - env_scale, - z * step_size + delta_size + 0.5 * single_size - env_scale))))); - } - - for(; i < n_edge * n_edge * n_edge / 4; ++i) - { - int x = i % (n_edge * n_edge); - int y = (i - n_edge * n_edge * x) % n_edge; - int z = i - n_edge * n_edge * x - n_edge * y; - - Cone* cone = new Cone(single_size / 2, single_size); - env.push_back(new CollisionObject(std::shared_ptr(cone), - Transform3d(Eigen::Translation3d(Vector3d(x * step_size + delta_size + 0.5 * single_size - env_scale, - y * step_size + delta_size + 0.5 * single_size - env_scale, - z * step_size + delta_size + 0.5 * single_size - env_scale))))); - } -} - -void generateSelfDistanceEnvironmentsMesh(std::vector& env, double env_scale, std::size_t n) -{ - unsigned int n_edge = std::floor(std::pow(n, 1/3.0)); - - FCL_REAL step_size = env_scale * 2 / n_edge; - FCL_REAL delta_size = step_size * 0.05; - FCL_REAL single_size = step_size - 2 * delta_size; - - std::size_t i = 0; - for(; i < n_edge * n_edge * n_edge / 4; ++i) - { - int x = i % (n_edge * n_edge); - int y = (i - n_edge * n_edge * x) % n_edge; - int z = i - n_edge * n_edge * x - n_edge * y; - - Box box(single_size, single_size, single_size); - BVHModel* model = new BVHModel(); - generateBVHModel(*model, box, Transform3d::Identity()); - env.push_back(new CollisionObject(std::shared_ptr(model), - Transform3d(Eigen::Translation3d(Vector3d(x * step_size + delta_size + 0.5 * single_size - env_scale, - y * step_size + delta_size + 0.5 * single_size - env_scale, - z * step_size + delta_size + 0.5 * single_size - env_scale))))); - } - - for(; i < n_edge * n_edge * n_edge / 4; ++i) - { - int x = i % (n_edge * n_edge); - int y = (i - n_edge * n_edge * x) % n_edge; - int z = i - n_edge * n_edge * x - n_edge * y; - - Sphere sphere(single_size / 2); - BVHModel* model = new BVHModel(); - generateBVHModel(*model, sphere, Transform3d::Identity(), 16, 16); - env.push_back(new CollisionObject(std::shared_ptr(model), - Transform3d(Eigen::Translation3d(Vector3d(x * step_size + delta_size + 0.5 * single_size - env_scale, - y * step_size + delta_size + 0.5 * single_size - env_scale, - z * step_size + delta_size + 0.5 * single_size - env_scale))))); - } - - for(; i < n_edge * n_edge * n_edge / 4; ++i) - { - int x = i % (n_edge * n_edge); - int y = (i - n_edge * n_edge * x) % n_edge; - int z = i - n_edge * n_edge * x - n_edge * y; - - Ellipsoid ellipsoid(single_size / 2, single_size / 2, single_size / 2); - BVHModel* model = new BVHModel(); - generateBVHModel(*model, ellipsoid, Transform3d::Identity(), 16, 16); - env.push_back(new CollisionObject(std::shared_ptr(model), - Transform3d(Eigen::Translation3d(Vector3d(x * step_size + delta_size + 0.5 * single_size - env_scale, - y * step_size + delta_size + 0.5 * single_size - env_scale, - z * step_size + delta_size + 0.5 * single_size - env_scale))))); - } - - for(; i < n_edge * n_edge * n_edge / 4; ++i) - { - int x = i % (n_edge * n_edge); - int y = (i - n_edge * n_edge * x) % n_edge; - int z = i - n_edge * n_edge * x - n_edge * y; - - Cylinder cylinder(single_size / 2, single_size); - BVHModel* model = new BVHModel(); - generateBVHModel(*model, cylinder, Transform3d::Identity(), 16, 16); - env.push_back(new CollisionObject(std::shared_ptr(model), - Transform3d(Eigen::Translation3d(Vector3d(x * step_size + delta_size + 0.5 * single_size - env_scale, - y * step_size + delta_size + 0.5 * single_size - env_scale, - z * step_size + delta_size + 0.5 * single_size - env_scale))))); - } - - for(; i < n_edge * n_edge * n_edge / 4; ++i) - { - int x = i % (n_edge * n_edge); - int y = (i - n_edge * n_edge * x) % n_edge; - int z = i - n_edge * n_edge * x - n_edge * y; - - Cone cone(single_size / 2, single_size); - BVHModel* model = new BVHModel(); - generateBVHModel(*model, cone, Transform3d::Identity(), 16, 16); - env.push_back(new CollisionObject(std::shared_ptr(model), - Transform3d(Eigen::Translation3d(Vector3d(x * step_size + delta_size + 0.5 * single_size - env_scale, - y * step_size + delta_size + 0.5 * single_size - env_scale, - z * step_size + delta_size + 0.5 * single_size - env_scale))))); - } -} - - -void broad_phase_collision_test(double env_scale, std::size_t env_size, std::size_t query_size, std::size_t num_max_contacts, bool exhaustive, bool use_mesh) -{ - std::vector ts; - std::vector timers; - - std::vector env; - if(use_mesh) - generateEnvironmentsMesh(env, env_scale, env_size); - else - generateEnvironments(env, env_scale, env_size); - - std::vector query; - if(use_mesh) - generateEnvironmentsMesh(query, env_scale, query_size); - else - generateEnvironments(query, env_scale, query_size); - - std::vector managers; - - managers.push_back(new NaiveCollisionManager()); - managers.push_back(new SSaPCollisionManager()); - managers.push_back(new SaPCollisionManager()); - managers.push_back(new IntervalTreeCollisionManager()); - - Vector3d lower_limit, upper_limit; - SpatialHashingCollisionManager<>::computeBound(env, lower_limit, upper_limit); - // FCL_REAL ncell_per_axis = std::pow((FCL_REAL)env_size / 10, 1 / 3.0); - FCL_REAL ncell_per_axis = 20; - FCL_REAL cell_size = std::min(std::min((upper_limit[0] - lower_limit[0]) / ncell_per_axis, (upper_limit[1] - lower_limit[1]) / ncell_per_axis), (upper_limit[2] - lower_limit[2]) / ncell_per_axis); - // managers.push_back(new SpatialHashingCollisionManager<>(cell_size, lower_limit, upper_limit)); - managers.push_back(new SpatialHashingCollisionManager >(cell_size, lower_limit, upper_limit)); -#if USE_GOOGLEHASH - managers.push_back(new SpatialHashingCollisionManager >(cell_size, lower_limit, upper_limit)); - managers.push_back(new SpatialHashingCollisionManager >(cell_size, lower_limit, upper_limit)); -#endif - managers.push_back(new DynamicAABBTreeCollisionManager()); - - managers.push_back(new DynamicAABBTreeCollisionManager_Array()); - - { - DynamicAABBTreeCollisionManager* m = new DynamicAABBTreeCollisionManager(); - m->tree_init_level = 2; - managers.push_back(m); - } - - { - DynamicAABBTreeCollisionManager_Array* m = new DynamicAABBTreeCollisionManager_Array(); - m->tree_init_level = 2; - managers.push_back(m); - } - - ts.resize(managers.size()); - timers.resize(managers.size()); - - for(size_t i = 0; i < managers.size(); ++i) - { - timers[i].start(); - managers[i]->registerObjects(env); - timers[i].stop(); - ts[i].push_back(timers[i].getElapsedTime()); - } - - for(size_t i = 0; i < managers.size(); ++i) - { - timers[i].start(); - managers[i]->setup(); - timers[i].stop(); - ts[i].push_back(timers[i].getElapsedTime()); - } - - std::vector self_data(managers.size()); - for(size_t i = 0; i < managers.size(); ++i) - { - if(exhaustive) self_data[i].request.num_max_contacts = 100000; - else self_data[i].request.num_max_contacts = num_max_contacts; - } - - for(size_t i = 0; i < managers.size(); ++i) - { - timers[i].start(); - managers[i]->collide(&self_data[i], defaultCollisionFunction); - timers[i].stop(); - ts[i].push_back(timers[i].getElapsedTime()); - } - - for(size_t i = 0; i < managers.size(); ++i) - std::cout << self_data[i].result.numContacts() << " "; - std::cout << std::endl; - - if(exhaustive) - { - for(size_t i = 1; i < managers.size(); ++i) - EXPECT_TRUE(self_data[i].result.numContacts() == self_data[0].result.numContacts()); - } - else - { - std::vector self_res(managers.size()); - for(size_t i = 0; i < self_res.size(); ++i) - self_res[i] = (self_data[i].result.numContacts() > 0); - - for(size_t i = 1; i < self_res.size(); ++i) - EXPECT_TRUE(self_res[0] == self_res[i]); - - for(size_t i = 1; i < managers.size(); ++i) - EXPECT_TRUE(self_data[i].result.numContacts() == self_data[0].result.numContacts()); - } - - - for(size_t i = 0; i < query.size(); ++i) - { - std::vector query_data(managers.size()); - for(size_t j = 0; j < query_data.size(); ++j) - { - if(exhaustive) query_data[j].request.num_max_contacts = 100000; - else query_data[j].request.num_max_contacts = num_max_contacts; - } - - for(size_t j = 0; j < query_data.size(); ++j) - { - timers[j].start(); - managers[j]->collide(query[i], &query_data[j], defaultCollisionFunction); - timers[j].stop(); - ts[j].push_back(timers[j].getElapsedTime()); - } - - - // for(size_t j = 0; j < managers.size(); ++j) - // std::cout << query_data[j].result.numContacts() << " "; - // std::cout << std::endl; - - if(exhaustive) - { - for(size_t j = 1; j < managers.size(); ++j) - EXPECT_TRUE(query_data[j].result.numContacts() == query_data[0].result.numContacts()); - } - else - { - std::vector query_res(managers.size()); - for(size_t j = 0; j < query_res.size(); ++j) - query_res[j] = (query_data[j].result.numContacts() > 0); - for(size_t j = 1; j < query_res.size(); ++j) - EXPECT_TRUE(query_res[0] == query_res[j]); - - for(size_t j = 1; j < managers.size(); ++j) - EXPECT_TRUE(query_data[j].result.numContacts() == query_data[0].result.numContacts()); - } - } - - for(size_t i = 0; i < env.size(); ++i) - delete env[i]; - for(size_t i = 0; i < query.size(); ++i) - delete query[i]; - - for(size_t i = 0; i < managers.size(); ++i) - delete managers[i]; - - std::cout.setf(std::ios_base::left, std::ios_base::adjustfield); - size_t w = 7; - - std::cout << "collision timing summary" << std::endl; - std::cout << env_size << " objs, " << query_size << " queries" << std::endl; - std::cout << "register time" << std::endl; - for(size_t i = 0; i < ts.size(); ++i) - std::cout << std::setw(w) << ts[i].records[0] << " "; - std::cout << std::endl; - - std::cout << "setup time" << std::endl; - for(size_t i = 0; i < ts.size(); ++i) - std::cout << std::setw(w) << ts[i].records[1] << " "; - std::cout << std::endl; - - std::cout << "self collision time" << std::endl; - for(size_t i = 0; i < ts.size(); ++i) - std::cout << std::setw(w) << ts[i].records[2] << " "; - std::cout << std::endl; - - std::cout << "collision time" << std::endl; - for(size_t i = 0; i < ts.size(); ++i) - { - FCL_REAL tmp = 0; - for(size_t j = 3; j < ts[i].records.size(); ++j) - tmp += ts[i].records[j]; - std::cout << std::setw(w) << tmp << " "; - } - std::cout << std::endl; - - - std::cout << "overall time" << std::endl; - for(size_t i = 0; i < ts.size(); ++i) - std::cout << std::setw(w) << ts[i].overall_time << " "; - std::cout << std::endl; - std::cout << std::endl; - -} - -void broad_phase_self_distance_test(double env_scale, std::size_t env_size, bool use_mesh) -{ - std::vector ts; - std::vector timers; - - std::vector env; - if(use_mesh) - generateSelfDistanceEnvironmentsMesh(env, env_scale, env_size); - else - generateSelfDistanceEnvironments(env, env_scale, env_size); - - std::vector managers; - - managers.push_back(new NaiveCollisionManager()); - managers.push_back(new SSaPCollisionManager()); - managers.push_back(new SaPCollisionManager()); - managers.push_back(new IntervalTreeCollisionManager()); - - Vector3d lower_limit, upper_limit; - SpatialHashingCollisionManager<>::computeBound(env, lower_limit, upper_limit); - FCL_REAL cell_size = std::min(std::min((upper_limit[0] - lower_limit[0]) / 5, (upper_limit[1] - lower_limit[1]) / 5), (upper_limit[2] - lower_limit[2]) / 5); - // managers.push_back(new SpatialHashingCollisionManager<>(cell_size, lower_limit, upper_limit)); - managers.push_back(new SpatialHashingCollisionManager >(cell_size, lower_limit, upper_limit)); -#if USE_GOOGLEHASH - managers.push_back(new SpatialHashingCollisionManager >(cell_size, lower_limit, upper_limit)); - managers.push_back(new SpatialHashingCollisionManager >(cell_size, lower_limit, upper_limit)); -#endif - managers.push_back(new DynamicAABBTreeCollisionManager()); - managers.push_back(new DynamicAABBTreeCollisionManager_Array()); - - { - DynamicAABBTreeCollisionManager* m = new DynamicAABBTreeCollisionManager(); - m->tree_init_level = 2; - managers.push_back(m); - } - - { - DynamicAABBTreeCollisionManager_Array* m = new DynamicAABBTreeCollisionManager_Array(); - m->tree_init_level = 2; - managers.push_back(m); - } - - ts.resize(managers.size()); - timers.resize(managers.size()); - - for(size_t i = 0; i < managers.size(); ++i) - { - timers[i].start(); - managers[i]->registerObjects(env); - timers[i].stop(); - ts[i].push_back(timers[i].getElapsedTime()); - } - - for(size_t i = 0; i < managers.size(); ++i) - { - timers[i].start(); - managers[i]->setup(); - timers[i].stop(); - ts[i].push_back(timers[i].getElapsedTime()); - } - - - std::vector self_data(managers.size()); - - for(size_t i = 0; i < self_data.size(); ++i) - { - timers[i].start(); - managers[i]->distance(&self_data[i], defaultDistanceFunction); - timers[i].stop(); - ts[i].push_back(timers[i].getElapsedTime()); - // std::cout << self_data[i].result.min_distance << " "; - } - // std::cout << std::endl; - - for(size_t i = 1; i < managers.size(); ++i) - EXPECT_TRUE(fabs(self_data[0].result.min_distance - self_data[i].result.min_distance) < DELTA || - fabs(self_data[0].result.min_distance - self_data[i].result.min_distance) / fabs(self_data[0].result.min_distance) < DELTA); - - for(size_t i = 0; i < env.size(); ++i) - delete env[i]; - - for(size_t i = 0; i < managers.size(); ++i) - delete managers[i]; - - std::cout.setf(std::ios_base::left, std::ios_base::adjustfield); - size_t w = 7; - - std::cout << "self distance timing summary" << std::endl; - std::cout << env.size() << " objs" << std::endl; - std::cout << "register time" << std::endl; - for(size_t i = 0; i < ts.size(); ++i) - std::cout << std::setw(w) << ts[i].records[0] << " "; - std::cout << std::endl; - - std::cout << "setup time" << std::endl; - for(size_t i = 0; i < ts.size(); ++i) - std::cout << std::setw(w) << ts[i].records[1] << " "; - std::cout << std::endl; - - std::cout << "self distance time" << std::endl; - for(size_t i = 0; i < ts.size(); ++i) - std::cout << std::setw(w) << ts[i].records[2] << " "; - std::cout << std::endl; - - std::cout << "overall time" << std::endl; - for(size_t i = 0; i < ts.size(); ++i) - std::cout << std::setw(w) << ts[i].overall_time << " "; - std::cout << std::endl; - std::cout << std::endl; -} - - -void broad_phase_distance_test(double env_scale, std::size_t env_size, std::size_t query_size, bool use_mesh) -{ - std::vector ts; - std::vector timers; - - std::vector env; - if(use_mesh) - generateEnvironmentsMesh(env, env_scale, env_size); - else - generateEnvironments(env, env_scale, env_size); - - std::vector query; - - BroadPhaseCollisionManager* manager = new NaiveCollisionManager(); - for(std::size_t i = 0; i < env.size(); ++i) - manager->registerObject(env[i]); - manager->setup(); - - while(1) - { - std::vector candidates; - if(use_mesh) - generateEnvironmentsMesh(candidates, env_scale, query_size); - else - generateEnvironments(candidates, env_scale, query_size); - - for(std::size_t i = 0; i < candidates.size(); ++i) - { - CollisionData query_data; - manager->collide(candidates[i], &query_data, defaultCollisionFunction); - if(query_data.result.numContacts() == 0) - query.push_back(candidates[i]); - else - delete candidates[i]; - if(query.size() == query_size) break; - } - - if(query.size() == query_size) break; - } - - delete manager; - - std::vector managers; - - managers.push_back(new NaiveCollisionManager()); - managers.push_back(new SSaPCollisionManager()); - managers.push_back(new SaPCollisionManager()); - managers.push_back(new IntervalTreeCollisionManager()); - - Vector3d lower_limit, upper_limit; - SpatialHashingCollisionManager<>::computeBound(env, lower_limit, upper_limit); - FCL_REAL cell_size = std::min(std::min((upper_limit[0] - lower_limit[0]) / 20, (upper_limit[1] - lower_limit[1]) / 20), (upper_limit[2] - lower_limit[2])/20); - // managers.push_back(new SpatialHashingCollisionManager<>(cell_size, lower_limit, upper_limit)); - managers.push_back(new SpatialHashingCollisionManager >(cell_size, lower_limit, upper_limit)); -#if USE_GOOGLEHASH - managers.push_back(new SpatialHashingCollisionManager >(cell_size, lower_limit, upper_limit)); - managers.push_back(new SpatialHashingCollisionManager >(cell_size, lower_limit, upper_limit)); -#endif - managers.push_back(new DynamicAABBTreeCollisionManager()); - managers.push_back(new DynamicAABBTreeCollisionManager_Array()); - - { - DynamicAABBTreeCollisionManager* m = new DynamicAABBTreeCollisionManager(); - m->tree_init_level = 2; - managers.push_back(m); - } - - { - DynamicAABBTreeCollisionManager_Array* m = new DynamicAABBTreeCollisionManager_Array(); - m->tree_init_level = 2; - managers.push_back(m); - } - - ts.resize(managers.size()); - timers.resize(managers.size()); - - for(size_t i = 0; i < managers.size(); ++i) - { - timers[i].start(); - managers[i]->registerObjects(env); - timers[i].stop(); - ts[i].push_back(timers[i].getElapsedTime()); - } - - for(size_t i = 0; i < managers.size(); ++i) - { - timers[i].start(); - managers[i]->setup(); - timers[i].stop(); - ts[i].push_back(timers[i].getElapsedTime()); - } - - - for(size_t i = 0; i < query.size(); ++i) - { - std::vector query_data(managers.size()); - for(size_t j = 0; j < managers.size(); ++j) - { - timers[j].start(); - managers[j]->distance(query[i], &query_data[j], defaultDistanceFunction); - timers[j].stop(); - ts[j].push_back(timers[j].getElapsedTime()); - // std::cout << query_data[j].result.min_distance << " "; - } - // std::cout << std::endl; - - for(size_t j = 1; j < managers.size(); ++j) - EXPECT_TRUE(fabs(query_data[0].result.min_distance - query_data[j].result.min_distance) < DELTA || - fabs(query_data[0].result.min_distance - query_data[j].result.min_distance) / fabs(query_data[0].result.min_distance) < DELTA); - } - - - for(std::size_t i = 0; i < env.size(); ++i) - delete env[i]; - for(std::size_t i = 0; i < query.size(); ++i) - delete query[i]; - - for(size_t i = 0; i < managers.size(); ++i) - delete managers[i]; - - - std::cout.setf(std::ios_base::left, std::ios_base::adjustfield); - size_t w = 7; - - std::cout << "distance timing summary" << std::endl; - std::cout << env_size << " objs, " << query_size << " queries" << std::endl; - std::cout << "register time" << std::endl; - for(size_t i = 0; i < ts.size(); ++i) - std::cout << std::setw(w) << ts[i].records[0] << " "; - std::cout << std::endl; - - std::cout << "setup time" << std::endl; - for(size_t i = 0; i < ts.size(); ++i) - std::cout << std::setw(w) << ts[i].records[1] << " "; - std::cout << std::endl; - - std::cout << "distance time" << std::endl; - for(size_t i = 0; i < ts.size(); ++i) - { - FCL_REAL tmp = 0; - for(size_t j = 2; j < ts[i].records.size(); ++j) - tmp += ts[i].records[j]; - std::cout << std::setw(w) << tmp << " "; - } - std::cout << std::endl; - - std::cout << "overall time" << std::endl; - for(size_t i = 0; i < ts.size(); ++i) - std::cout << std::setw(w) << ts[i].overall_time << " "; - std::cout << std::endl; - std::cout << std::endl; -} - - -void broad_phase_update_collision_test(double env_scale, std::size_t env_size, std::size_t query_size, std::size_t num_max_contacts, bool exhaustive, bool use_mesh) -{ - std::vector ts; - std::vector timers; - - std::vector env; - if(use_mesh) - generateEnvironmentsMesh(env, env_scale, env_size); - else - generateEnvironments(env, env_scale, env_size); - - std::vector query; - if(use_mesh) - generateEnvironmentsMesh(query, env_scale, query_size); - else - generateEnvironments(query, env_scale, query_size); - - std::vector managers; - - managers.push_back(new NaiveCollisionManager()); - managers.push_back(new SSaPCollisionManager()); - - - managers.push_back(new SaPCollisionManager()); - managers.push_back(new IntervalTreeCollisionManager()); - - Vector3d lower_limit, upper_limit; - SpatialHashingCollisionManager<>::computeBound(env, lower_limit, upper_limit); - FCL_REAL cell_size = std::min(std::min((upper_limit[0] - lower_limit[0]) / 20, (upper_limit[1] - lower_limit[1]) / 20), (upper_limit[2] - lower_limit[2])/20); - // managers.push_back(new SpatialHashingCollisionManager<>(cell_size, lower_limit, upper_limit)); - managers.push_back(new SpatialHashingCollisionManager >(cell_size, lower_limit, upper_limit)); -#if USE_GOOGLEHASH - managers.push_back(new SpatialHashingCollisionManager >(cell_size, lower_limit, upper_limit)); - managers.push_back(new SpatialHashingCollisionManager >(cell_size, lower_limit, upper_limit)); -#endif - managers.push_back(new DynamicAABBTreeCollisionManager()); - managers.push_back(new DynamicAABBTreeCollisionManager_Array()); - - { - DynamicAABBTreeCollisionManager* m = new DynamicAABBTreeCollisionManager(); - m->tree_init_level = 2; - managers.push_back(m); - } - - { - DynamicAABBTreeCollisionManager_Array* m = new DynamicAABBTreeCollisionManager_Array(); - m->tree_init_level = 2; - managers.push_back(m); - } - - ts.resize(managers.size()); - timers.resize(managers.size()); - - for(size_t i = 0; i < managers.size(); ++i) - { - timers[i].start(); - managers[i]->registerObjects(env); - timers[i].stop(); - ts[i].push_back(timers[i].getElapsedTime()); - } - - for(size_t i = 0; i < managers.size(); ++i) - { - timers[i].start(); - managers[i]->setup(); - timers[i].stop(); - ts[i].push_back(timers[i].getElapsedTime()); - } - - // update the environment - FCL_REAL delta_angle_max = 10 / 360.0 * 2 * constants::pi; - FCL_REAL delta_trans_max = 0.01 * env_scale; - for(size_t i = 0; i < env.size(); ++i) - { - FCL_REAL rand_angle_x = 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_angle_max; - FCL_REAL rand_trans_x = 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_trans_max; - FCL_REAL rand_angle_y = 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_angle_max; - FCL_REAL rand_trans_y = 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_trans_max; - FCL_REAL rand_angle_z = 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_angle_max; - FCL_REAL rand_trans_z = 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_trans_max; - - Matrix3d dR( - Eigen::AngleAxisd(rand_angle_x, Vector3d::UnitX()) - * Eigen::AngleAxisd(rand_angle_y, Vector3d::UnitY()) - * Eigen::AngleAxisd(rand_angle_z, Vector3d::UnitZ())); - Vector3d dT(rand_trans_x, rand_trans_y, rand_trans_z); - - Matrix3d R = env[i]->getRotation(); - Vector3d T = env[i]->getTranslation(); - env[i]->setTransform(dR * R, dR * T + dT); - env[i]->computeAABB(); - } - - for(size_t i = 0; i < managers.size(); ++i) - { - timers[i].start(); - managers[i]->update(); - timers[i].stop(); - ts[i].push_back(timers[i].getElapsedTime()); - } - - std::vector self_data(managers.size()); - for(size_t i = 0; i < managers.size(); ++i) - { - if(exhaustive) self_data[i].request.num_max_contacts = 100000; - else self_data[i].request.num_max_contacts = num_max_contacts; - } - - for(size_t i = 0; i < managers.size(); ++i) - { - timers[i].start(); - managers[i]->collide(&self_data[i], defaultCollisionFunction); - timers[i].stop(); - ts[i].push_back(timers[i].getElapsedTime()); - } - - - for(size_t i = 0; i < managers.size(); ++i) - std::cout << self_data[i].result.numContacts() << " "; - std::cout << std::endl; - - if(exhaustive) - { - for(size_t i = 1; i < managers.size(); ++i) - EXPECT_TRUE(self_data[i].result.numContacts() == self_data[0].result.numContacts()); - } - else - { - std::vector self_res(managers.size()); - for(size_t i = 0; i < self_res.size(); ++i) - self_res[i] = (self_data[i].result.numContacts() > 0); - - for(size_t i = 1; i < self_res.size(); ++i) - EXPECT_TRUE(self_res[0] == self_res[i]); - - for(size_t i = 1; i < managers.size(); ++i) - EXPECT_TRUE(self_data[i].result.numContacts() == self_data[0].result.numContacts()); - } - - - for(size_t i = 0; i < query.size(); ++i) - { - std::vector query_data(managers.size()); - for(size_t j = 0; j < query_data.size(); ++j) - { - if(exhaustive) query_data[j].request.num_max_contacts = 100000; - else query_data[j].request.num_max_contacts = num_max_contacts; - } - - for(size_t j = 0; j < query_data.size(); ++j) - { - timers[j].start(); - managers[j]->collide(query[i], &query_data[j], defaultCollisionFunction); - timers[j].stop(); - ts[j].push_back(timers[j].getElapsedTime()); - } - - - // for(size_t j = 0; j < managers.size(); ++j) - // std::cout << query_data[j].result.numContacts() << " "; - // std::cout << std::endl; - - if(exhaustive) - { - for(size_t j = 1; j < managers.size(); ++j) - EXPECT_TRUE(query_data[j].result.numContacts() == query_data[0].result.numContacts()); - } - else - { - std::vector query_res(managers.size()); - for(size_t j = 0; j < query_res.size(); ++j) - query_res[j] = (query_data[j].result.numContacts() > 0); - for(size_t j = 1; j < query_res.size(); ++j) - EXPECT_TRUE(query_res[0] == query_res[j]); - - for(size_t j = 1; j < managers.size(); ++j) - EXPECT_TRUE(query_data[j].result.numContacts() == query_data[0].result.numContacts()); - } - } - - - for(size_t i = 0; i < env.size(); ++i) - delete env[i]; - for(size_t i = 0; i < query.size(); ++i) - delete query[i]; - - for(size_t i = 0; i < managers.size(); ++i) - delete managers[i]; - - - std::cout.setf(std::ios_base::left, std::ios_base::adjustfield); - size_t w = 7; - - std::cout << "collision timing summary" << std::endl; - std::cout << env_size << " objs, " << query_size << " queries" << std::endl; - std::cout << "register time" << std::endl; - for(size_t i = 0; i < ts.size(); ++i) - std::cout << std::setw(w) << ts[i].records[0] << " "; - std::cout << std::endl; - - std::cout << "setup time" << std::endl; - for(size_t i = 0; i < ts.size(); ++i) - std::cout << std::setw(w) << ts[i].records[1] << " "; - std::cout << std::endl; - - std::cout << "update time" << std::endl; - for(size_t i = 0; i < ts.size(); ++i) - std::cout << std::setw(w) << ts[i].records[2] << " "; - std::cout << std::endl; - - std::cout << "self collision time" << std::endl; - for(size_t i = 0; i < ts.size(); ++i) - std::cout << std::setw(w) << ts[i].records[3] << " "; - std::cout << std::endl; - - std::cout << "collision time" << std::endl; - for(size_t i = 0; i < ts.size(); ++i) - { - FCL_REAL tmp = 0; - for(size_t j = 4; j < ts[i].records.size(); ++j) - tmp += ts[i].records[j]; - std::cout << std::setw(w) << tmp << " "; - } - std::cout << std::endl; - - - std::cout << "overall time" << std::endl; - for(size_t i = 0; i < ts.size(); ++i) - std::cout << std::setw(w) << ts[i].overall_time << " "; - std::cout << std::endl; - std::cout << std::endl; -} - -//============================================================================== -int main(int argc, char* argv[]) -{ - ::testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/test/test_fcl_broadphase_collision_1.cpp b/test/test_fcl_broadphase_collision_1.cpp new file mode 100644 index 000000000..36b125217 --- /dev/null +++ b/test/test_fcl_broadphase_collision_1.cpp @@ -0,0 +1,584 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#include + +#include "fcl/config.h" +#include "fcl/broadphase/broadphase.h" +#include "fcl/broadphase/sparse_hash_table.h" +#include "fcl/broadphase/spatial_hash.h" +#include "fcl/shape/geometric_shape_to_BVH_model.h" +#include "test_fcl_utility.h" + +#if USE_GOOGLEHASH +#include +#include +#include +#endif + +#include +#include + +using namespace fcl; + +/// @brief make sure if broadphase algorithms doesn't check twice for the same +/// collision object pair +template +void broad_phase_duplicate_check_test(S env_scale, std::size_t env_size, bool verbose = false); + +/// @brief test for broad phase update +template +void broad_phase_update_collision_test(S env_scale, std::size_t env_size, std::size_t query_size, std::size_t num_max_contacts = 1, bool exhaustive = false, bool use_mesh = false); + +#if USE_GOOGLEHASH +template +struct GoogleSparseHashTable : public google::sparse_hash_map, std::equal_to > {}; + +template +struct GoogleDenseHashTable : public google::dense_hash_map, std::equal_to > +{ + GoogleDenseHashTable() : google::dense_hash_map, std::equal_to >() + { + this->set_empty_key(nullptr); + } +}; +#endif + +/// make sure if broadphase algorithms doesn't check twice for the same +/// collision object pair +GTEST_TEST(FCL_BROADPHASE, test_broad_phase_dont_duplicate_check) +{ +#ifdef NDEBUG + broad_phase_duplicate_check_test(2000, 1000); +#else + broad_phase_duplicate_check_test(2000, 100); +#endif +} + +/// check the update, only return collision or not +GTEST_TEST(FCL_BROADPHASE, test_core_bf_broad_phase_update_collision_binary) +{ +#ifdef NDEBUG + broad_phase_update_collision_test(2000, 100, 1000, 1, false); + broad_phase_update_collision_test(2000, 1000, 1000, 1, false); +#else + broad_phase_update_collision_test(2000, 10, 100, 1, false); + broad_phase_update_collision_test(2000, 100, 100, 1, false); +#endif +} + +/// check the update, return 10 contacts +GTEST_TEST(FCL_BROADPHASE, test_core_bf_broad_phase_update_collision) +{ +#ifdef NDEBUG + broad_phase_update_collision_test(2000, 100, 1000, 10, false); + broad_phase_update_collision_test(2000, 1000, 1000, 10, false); +#else + broad_phase_update_collision_test(2000, 10, 100, 10, false); + broad_phase_update_collision_test(2000, 100, 100, 10, false); +#endif +} + +/// check the update, exhaustive +GTEST_TEST(FCL_BROADPHASE, test_core_bf_broad_phase_update_collision_exhaustive) +{ +#ifdef NDEBUG + broad_phase_update_collision_test(2000, 100, 1000, 1, true); + broad_phase_update_collision_test(2000, 1000, 1000, 1, true); +#else + broad_phase_update_collision_test(2000, 10, 100, 1, true); + broad_phase_update_collision_test(2000, 100, 100, 1, true); +#endif +} + +/// check broad phase update, in mesh, only return collision or not +GTEST_TEST(FCL_BROADPHASE, test_core_mesh_bf_broad_phase_update_collision_mesh_binary) +{ +#ifdef NDEBUG + broad_phase_update_collision_test(2000, 100, 1000, 1, false, true); + broad_phase_update_collision_test(2000, 1000, 1000, 1, false, true); +#else + broad_phase_update_collision_test(2000, 2, 4, 1, false, true); + broad_phase_update_collision_test(2000, 4, 4, 1, false, true); +#endif +} + +/// check broad phase update, in mesh, return 10 contacts +GTEST_TEST(FCL_BROADPHASE, test_core_mesh_bf_broad_phase_update_collision_mesh) +{ +#ifdef NDEBUG + broad_phase_update_collision_test(2000, 100, 1000, 10, false, true); + broad_phase_update_collision_test(2000, 1000, 1000, 10, false, true); +#else + broad_phase_update_collision_test(200, 2, 4, 10, false, true); + broad_phase_update_collision_test(200, 4, 4, 10, false, true); +#endif +} + +/// check broad phase update, in mesh, exhaustive +GTEST_TEST(FCL_BROADPHASE, test_core_mesh_bf_broad_phase_update_collision_mesh_exhaustive) +{ +#ifdef NDEBUG + broad_phase_update_collision_test(2000, 100, 1000, 1, true, true); + broad_phase_update_collision_test(2000, 1000, 1000, 1, true, true); +#else + broad_phase_update_collision_test(2000, 2, 4, 1, true, true); + broad_phase_update_collision_test(2000, 4, 4, 1, true, true); +#endif +} + +//============================================================================== +template +struct CollisionDataForUniquenessChecking +{ + std::set*, CollisionObject*>> checkedPairs; + + bool checkUniquenessAndAddPair(CollisionObject* o1, CollisionObject* o2) + { + auto search = checkedPairs.find(std::make_pair(o1, o2)); + + if (search != checkedPairs.end()) + return false; + + checkedPairs.emplace(o1, o2); + + return true; + } +}; + +//============================================================================== +template +bool collisionFunctionForUniquenessChecking( + CollisionObject* o1, CollisionObject* o2, void* cdata_) +{ + auto* cdata = static_cast*>(cdata_); + + EXPECT_TRUE(cdata->checkUniquenessAndAddPair(o1, o2)); + + return false; +} + +//============================================================================== +template +void broad_phase_duplicate_check_test(S env_scale, std::size_t env_size, bool verbose) +{ + std::vector ts; + std::vector timers; + + std::vector*> env; + generateEnvironments(env, env_scale, env_size); + +// for (auto i = 0u; i < env_size; ++i) +// env.emplace_back(new CollisionObject(std::make_shared>(10))); + + std::vector*> managers; + managers.push_back(new NaiveCollisionManager()); + managers.push_back(new SSaPCollisionManager()); + managers.push_back(new SaPCollisionManager()); + managers.push_back(new IntervalTreeCollisionManager()); + Vector3 lower_limit, upper_limit; + SpatialHashingCollisionManager::computeBound(env, lower_limit, upper_limit); + S cell_size = std::min(std::min((upper_limit[0] - lower_limit[0]) / 20, (upper_limit[1] - lower_limit[1]) / 20), (upper_limit[2] - lower_limit[2])/20); + managers.push_back(new SpatialHashingCollisionManager, CollisionObject*, SpatialHash> >(cell_size, lower_limit, upper_limit)); +#if USE_GOOGLEHASH + managers.push_back(new SpatialHashingCollisionManager, CollisionObject*, SpatialHash, GoogleSparseHashTable> >(cell_size, lower_limit, upper_limit)); + managers.push_back(new SpatialHashingCollisionManager, CollisionObject*, SpatialHash, GoogleDenseHashTable> >(cell_size, lower_limit, upper_limit)); +#endif + managers.push_back(new DynamicAABBTreeCollisionManager()); + managers.push_back(new DynamicAABBTreeCollisionManager_Array()); + + { + DynamicAABBTreeCollisionManager* m = new DynamicAABBTreeCollisionManager(); + m->tree_init_level = 2; + managers.push_back(m); + } + + { + DynamicAABBTreeCollisionManager_Array* m = new DynamicAABBTreeCollisionManager_Array(); + m->tree_init_level = 2; + managers.push_back(m); + } + + ts.resize(managers.size()); + timers.resize(managers.size()); + + for(size_t i = 0; i < managers.size(); ++i) + { + timers[i].start(); + managers[i]->registerObjects(env); + timers[i].stop(); + ts[i].push_back(timers[i].getElapsedTime()); + } + + for(size_t i = 0; i < managers.size(); ++i) + { + timers[i].start(); + managers[i]->setup(); + timers[i].stop(); + ts[i].push_back(timers[i].getElapsedTime()); + } + + // update the environment + S delta_angle_max = 10 / 360.0 * 2 * constants::pi(); + S delta_trans_max = 0.01 * env_scale; + for(size_t i = 0; i < env.size(); ++i) + { + S rand_angle_x = 2 * (rand() / (S)RAND_MAX - 0.5) * delta_angle_max; + S rand_trans_x = 2 * (rand() / (S)RAND_MAX - 0.5) * delta_trans_max; + S rand_angle_y = 2 * (rand() / (S)RAND_MAX - 0.5) * delta_angle_max; + S rand_trans_y = 2 * (rand() / (S)RAND_MAX - 0.5) * delta_trans_max; + S rand_angle_z = 2 * (rand() / (S)RAND_MAX - 0.5) * delta_angle_max; + S rand_trans_z = 2 * (rand() / (S)RAND_MAX - 0.5) * delta_trans_max; + + Matrix3 dR( + AngleAxis(rand_angle_x, Vector3::UnitX()) + * AngleAxis(rand_angle_y, Vector3::UnitY()) + * AngleAxis(rand_angle_z, Vector3::UnitZ())); + Vector3 dT(rand_trans_x, rand_trans_y, rand_trans_z); + + Matrix3 R = env[i]->getRotation(); + Vector3 T = env[i]->getTranslation(); + env[i]->setTransform(dR * R, dR * T + dT); + env[i]->computeAABB(); + } + + for(size_t i = 0; i < managers.size(); ++i) + { + timers[i].start(); + managers[i]->update(); + timers[i].stop(); + ts[i].push_back(timers[i].getElapsedTime()); + } + + std::vector> self_data(managers.size()); + + for(size_t i = 0; i < managers.size(); ++i) + { + timers[i].start(); + managers[i]->collide(&self_data[i], collisionFunctionForUniquenessChecking); + timers[i].stop(); + ts[i].push_back(timers[i].getElapsedTime()); + } + + for (auto obj : env) + delete obj; + + if (!verbose) + return; + + std::cout.setf(std::ios_base::left, std::ios_base::adjustfield); + size_t w = 7; + + std::cout << "collision timing summary" << std::endl; + std::cout << env_size << " objs" << std::endl; + std::cout << "register time" << std::endl; + for(size_t i = 0; i < ts.size(); ++i) + std::cout << std::setw(w) << ts[i].records[0] << " "; + std::cout << std::endl; + + std::cout << "setup time" << std::endl; + for(size_t i = 0; i < ts.size(); ++i) + std::cout << std::setw(w) << ts[i].records[1] << " "; + std::cout << std::endl; + + std::cout << "update time" << std::endl; + for(size_t i = 0; i < ts.size(); ++i) + std::cout << std::setw(w) << ts[i].records[2] << " "; + std::cout << std::endl; + + std::cout << "self collision time" << std::endl; + for(size_t i = 0; i < ts.size(); ++i) + std::cout << std::setw(w) << ts[i].records[3] << " "; + std::cout << std::endl; + + std::cout << "collision time" << std::endl; + for(size_t i = 0; i < ts.size(); ++i) + { + S tmp = 0; + for(size_t j = 4; j < ts[i].records.size(); ++j) + tmp += ts[i].records[j]; + std::cout << std::setw(w) << tmp << " "; + } + std::cout << std::endl; + + std::cout << "overall time" << std::endl; + for(size_t i = 0; i < ts.size(); ++i) + std::cout << std::setw(w) << ts[i].overall_time << " "; + std::cout << std::endl; + std::cout << std::endl; +} + +template +void broad_phase_update_collision_test(S env_scale, std::size_t env_size, std::size_t query_size, std::size_t num_max_contacts, bool exhaustive, bool use_mesh) +{ + std::vector ts; + std::vector timers; + + std::vector*> env; + if(use_mesh) + generateEnvironmentsMesh(env, env_scale, env_size); + else + generateEnvironments(env, env_scale, env_size); + + std::vector*> query; + if(use_mesh) + generateEnvironmentsMesh(query, env_scale, query_size); + else + generateEnvironments(query, env_scale, query_size); + + std::vector*> managers; + + managers.push_back(new NaiveCollisionManager()); + managers.push_back(new SSaPCollisionManager()); + + + managers.push_back(new SaPCollisionManager()); + managers.push_back(new IntervalTreeCollisionManager()); + + Vector3 lower_limit, upper_limit; + SpatialHashingCollisionManager::computeBound(env, lower_limit, upper_limit); + S cell_size = std::min(std::min((upper_limit[0] - lower_limit[0]) / 20, (upper_limit[1] - lower_limit[1]) / 20), (upper_limit[2] - lower_limit[2])/20); + // managers.push_back(new SpatialHashingCollisionManager(cell_size, lower_limit, upper_limit)); + managers.push_back(new SpatialHashingCollisionManager, CollisionObject*, SpatialHash> >(cell_size, lower_limit, upper_limit)); +#if USE_GOOGLEHASH + managers.push_back(new SpatialHashingCollisionManager, CollisionObject*, SpatialHash, GoogleSparseHashTable> >(cell_size, lower_limit, upper_limit)); + managers.push_back(new SpatialHashingCollisionManager, CollisionObject*, SpatialHash, GoogleDenseHashTable> >(cell_size, lower_limit, upper_limit)); +#endif + managers.push_back(new DynamicAABBTreeCollisionManager()); + managers.push_back(new DynamicAABBTreeCollisionManager_Array()); + + { + DynamicAABBTreeCollisionManager* m = new DynamicAABBTreeCollisionManager(); + m->tree_init_level = 2; + managers.push_back(m); + } + + { + DynamicAABBTreeCollisionManager_Array* m = new DynamicAABBTreeCollisionManager_Array(); + m->tree_init_level = 2; + managers.push_back(m); + } + + ts.resize(managers.size()); + timers.resize(managers.size()); + + for(size_t i = 0; i < managers.size(); ++i) + { + timers[i].start(); + managers[i]->registerObjects(env); + timers[i].stop(); + ts[i].push_back(timers[i].getElapsedTime()); + } + + for(size_t i = 0; i < managers.size(); ++i) + { + timers[i].start(); + managers[i]->setup(); + timers[i].stop(); + ts[i].push_back(timers[i].getElapsedTime()); + } + + // update the environment + S delta_angle_max = 10 / 360.0 * 2 * constants::pi(); + S delta_trans_max = 0.01 * env_scale; + for(size_t i = 0; i < env.size(); ++i) + { + S rand_angle_x = 2 * (rand() / (S)RAND_MAX - 0.5) * delta_angle_max; + S rand_trans_x = 2 * (rand() / (S)RAND_MAX - 0.5) * delta_trans_max; + S rand_angle_y = 2 * (rand() / (S)RAND_MAX - 0.5) * delta_angle_max; + S rand_trans_y = 2 * (rand() / (S)RAND_MAX - 0.5) * delta_trans_max; + S rand_angle_z = 2 * (rand() / (S)RAND_MAX - 0.5) * delta_angle_max; + S rand_trans_z = 2 * (rand() / (S)RAND_MAX - 0.5) * delta_trans_max; + + Matrix3 dR( + AngleAxis(rand_angle_x, Vector3::UnitX()) + * AngleAxis(rand_angle_y, Vector3::UnitY()) + * AngleAxis(rand_angle_z, Vector3::UnitZ())); + Vector3 dT(rand_trans_x, rand_trans_y, rand_trans_z); + + Matrix3 R = env[i]->getRotation(); + Vector3 T = env[i]->getTranslation(); + env[i]->setTransform(dR * R, dR * T + dT); + env[i]->computeAABB(); + } + + for(size_t i = 0; i < managers.size(); ++i) + { + timers[i].start(); + managers[i]->update(); + timers[i].stop(); + ts[i].push_back(timers[i].getElapsedTime()); + } + + std::vector> self_data(managers.size()); + for(size_t i = 0; i < managers.size(); ++i) + { + if(exhaustive) self_data[i].request.num_max_contacts = 100000; + else self_data[i].request.num_max_contacts = num_max_contacts; + } + + for(size_t i = 0; i < managers.size(); ++i) + { + timers[i].start(); + managers[i]->collide(&self_data[i], defaultCollisionFunction); + timers[i].stop(); + ts[i].push_back(timers[i].getElapsedTime()); + } + + + for(size_t i = 0; i < managers.size(); ++i) + std::cout << self_data[i].result.numContacts() << " "; + std::cout << std::endl; + + if(exhaustive) + { + for(size_t i = 1; i < managers.size(); ++i) + EXPECT_TRUE(self_data[i].result.numContacts() == self_data[0].result.numContacts()); + } + else + { + std::vector self_res(managers.size()); + for(size_t i = 0; i < self_res.size(); ++i) + self_res[i] = (self_data[i].result.numContacts() > 0); + + for(size_t i = 1; i < self_res.size(); ++i) + EXPECT_TRUE(self_res[0] == self_res[i]); + + for(size_t i = 1; i < managers.size(); ++i) + EXPECT_TRUE(self_data[i].result.numContacts() == self_data[0].result.numContacts()); + } + + + for(size_t i = 0; i < query.size(); ++i) + { + std::vector> query_data(managers.size()); + for(size_t j = 0; j < query_data.size(); ++j) + { + if(exhaustive) query_data[j].request.num_max_contacts = 100000; + else query_data[j].request.num_max_contacts = num_max_contacts; + } + + for(size_t j = 0; j < query_data.size(); ++j) + { + timers[j].start(); + managers[j]->collide(query[i], &query_data[j], defaultCollisionFunction); + timers[j].stop(); + ts[j].push_back(timers[j].getElapsedTime()); + } + + + // for(size_t j = 0; j < managers.size(); ++j) + // std::cout << query_data[j].result.numContacts() << " "; + // std::cout << std::endl; + + if(exhaustive) + { + for(size_t j = 1; j < managers.size(); ++j) + EXPECT_TRUE(query_data[j].result.numContacts() == query_data[0].result.numContacts()); + } + else + { + std::vector query_res(managers.size()); + for(size_t j = 0; j < query_res.size(); ++j) + query_res[j] = (query_data[j].result.numContacts() > 0); + for(size_t j = 1; j < query_res.size(); ++j) + EXPECT_TRUE(query_res[0] == query_res[j]); + + for(size_t j = 1; j < managers.size(); ++j) + EXPECT_TRUE(query_data[j].result.numContacts() == query_data[0].result.numContacts()); + } + } + + + for(size_t i = 0; i < env.size(); ++i) + delete env[i]; + for(size_t i = 0; i < query.size(); ++i) + delete query[i]; + + for(size_t i = 0; i < managers.size(); ++i) + delete managers[i]; + + + std::cout.setf(std::ios_base::left, std::ios_base::adjustfield); + size_t w = 7; + + std::cout << "collision timing summary" << std::endl; + std::cout << env_size << " objs, " << query_size << " queries" << std::endl; + std::cout << "register time" << std::endl; + for(size_t i = 0; i < ts.size(); ++i) + std::cout << std::setw(w) << ts[i].records[0] << " "; + std::cout << std::endl; + + std::cout << "setup time" << std::endl; + for(size_t i = 0; i < ts.size(); ++i) + std::cout << std::setw(w) << ts[i].records[1] << " "; + std::cout << std::endl; + + std::cout << "update time" << std::endl; + for(size_t i = 0; i < ts.size(); ++i) + std::cout << std::setw(w) << ts[i].records[2] << " "; + std::cout << std::endl; + + std::cout << "self collision time" << std::endl; + for(size_t i = 0; i < ts.size(); ++i) + std::cout << std::setw(w) << ts[i].records[3] << " "; + std::cout << std::endl; + + std::cout << "collision time" << std::endl; + for(size_t i = 0; i < ts.size(); ++i) + { + S tmp = 0; + for(size_t j = 4; j < ts[i].records.size(); ++j) + tmp += ts[i].records[j]; + std::cout << std::setw(w) << tmp << " "; + } + std::cout << std::endl; + + + std::cout << "overall time" << std::endl; + for(size_t i = 0; i < ts.size(); ++i) + std::cout << std::setw(w) << ts[i].overall_time << " "; + std::cout << std::endl; + std::cout << std::endl; +} + +//============================================================================== +int main(int argc, char* argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test/test_fcl_broadphase_collision_2.cpp b/test/test_fcl_broadphase_collision_2.cpp new file mode 100644 index 000000000..e08764e6f --- /dev/null +++ b/test/test_fcl_broadphase_collision_2.cpp @@ -0,0 +1,379 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#include + +#include "fcl/config.h" +#include "fcl/broadphase/broadphase.h" +#include "fcl/broadphase/sparse_hash_table.h" +#include "fcl/broadphase/spatial_hash.h" +#include "fcl/shape/geometric_shape_to_BVH_model.h" +#include "test_fcl_utility.h" + +#if USE_GOOGLEHASH +#include +#include +#include +#endif + +#include +#include + +using namespace fcl; + +/// @brief test for broad phase collision and self collision +template +void broad_phase_collision_test(S env_scale, std::size_t env_size, std::size_t query_size, std::size_t num_max_contacts = 1, bool exhaustive = false, bool use_mesh = false); + +#if USE_GOOGLEHASH +template +struct GoogleSparseHashTable : public google::sparse_hash_map, std::equal_to > {}; + +template +struct GoogleDenseHashTable : public google::dense_hash_map, std::equal_to > +{ + GoogleDenseHashTable() : google::dense_hash_map, std::equal_to >() + { + this->set_empty_key(nullptr); + } +}; +#endif + +/// check broad phase collision for empty collision object set and queries +GTEST_TEST(FCL_BROADPHASE, test_core_bf_broad_phase_collision_empty) +{ +#ifdef NDEBUG + broad_phase_collision_test(2000, 0, 0, 10, false, false); + broad_phase_collision_test(2000, 0, 1000, 10, false, false); + broad_phase_collision_test(2000, 100, 0, 10, false, false); + + broad_phase_collision_test(2000, 0, 0, 10, false, true); + broad_phase_collision_test(2000, 0, 1000, 10, false, true); + broad_phase_collision_test(2000, 100, 0, 10, false, true); + + broad_phase_collision_test(2000, 0, 0, 10, true, false); + broad_phase_collision_test(2000, 0, 1000, 10, true, false); + broad_phase_collision_test(2000, 100, 0, 10, true, false); + + broad_phase_collision_test(2000, 0, 0, 10, true, true); + broad_phase_collision_test(2000, 0, 1000, 10, true, true); + broad_phase_collision_test(2000, 100, 0, 10, true, true); +#else + broad_phase_collision_test(2000, 0, 0, 10, false, false); + broad_phase_collision_test(2000, 0, 5, 10, false, false); + broad_phase_collision_test(2000, 2, 0, 10, false, false); + + broad_phase_collision_test(2000, 0, 0, 10, false, true); + broad_phase_collision_test(2000, 0, 5, 10, false, true); + broad_phase_collision_test(2000, 2, 0, 10, false, true); + + broad_phase_collision_test(2000, 0, 0, 10, true, false); + broad_phase_collision_test(2000, 0, 5, 10, true, false); + broad_phase_collision_test(2000, 2, 0, 10, true, false); + + broad_phase_collision_test(2000, 0, 0, 10, true, true); + broad_phase_collision_test(2000, 0, 5, 10, true, true); + broad_phase_collision_test(2000, 2, 0, 10, true, true); +#endif +} + +/// check broad phase collision and self collision, only return collision or not +GTEST_TEST(FCL_BROADPHASE, test_core_bf_broad_phase_collision_binary) +{ +#ifdef NDEBUG + broad_phase_collision_test(2000, 100, 1000, 1, false); + broad_phase_collision_test(2000, 1000, 1000, 1, false); + broad_phase_collision_test(2000, 100, 1000, 1, true); + broad_phase_collision_test(2000, 1000, 1000, 1, true); +#else + broad_phase_collision_test(2000, 10, 100, 1, false); + broad_phase_collision_test(2000, 100, 100, 1, false); + broad_phase_collision_test(2000, 10, 100, 1, true); + broad_phase_collision_test(2000, 100, 100, 1, true); +#endif +} + +/// check broad phase collision and self collision, return 10 contacts +GTEST_TEST(FCL_BROADPHASE, test_core_bf_broad_phase_collision) +{ +#ifdef NDEBUG + broad_phase_collision_test(2000, 100, 1000, 10, false); + broad_phase_collision_test(2000, 1000, 1000, 10, false); +#else + broad_phase_collision_test(2000, 10, 100, 10, false); + broad_phase_collision_test(2000, 100, 100, 10, false); +#endif +} + +/// check broad phase collision and self collision, return only collision or not, in mesh +GTEST_TEST(FCL_BROADPHASE, test_core_mesh_bf_broad_phase_collision_mesh_binary) +{ +#ifdef NDEBUG + broad_phase_collision_test(2000, 100, 1000, 1, false, true); + broad_phase_collision_test(2000, 1000, 1000, 1, false, true); +#else + broad_phase_collision_test(2000, 2, 5, 1, false, true); + broad_phase_collision_test(2000, 5, 5, 1, false, true); +#endif +} + +/// check broad phase collision and self collision, return 10 contacts, in mesh +GTEST_TEST(FCL_BROADPHASE, test_core_mesh_bf_broad_phase_collision_mesh) +{ +#ifdef NDEBUG + broad_phase_collision_test(2000, 100, 1000, 10, false, true); + broad_phase_collision_test(2000, 1000, 1000, 10, false, true); +#else + broad_phase_collision_test(2000, 2, 5, 10, false, true); + broad_phase_collision_test(2000, 5, 5, 10, false, true); +#endif +} + +/// check broad phase collision and self collision, exhaustive, in mesh +GTEST_TEST(FCL_BROADPHASE, test_core_mesh_bf_broad_phase_collision_mesh_exhaustive) +{ +#ifdef NDEBUG + broad_phase_collision_test(2000, 100, 1000, 1, true, true); + broad_phase_collision_test(2000, 1000, 1000, 1, true, true); +#else + broad_phase_collision_test(2000, 2, 5, 1, true, true); + broad_phase_collision_test(2000, 5, 5, 1, true, true); +#endif +} + +template +void broad_phase_collision_test(S env_scale, std::size_t env_size, std::size_t query_size, std::size_t num_max_contacts, bool exhaustive, bool use_mesh) +{ + std::vector ts; + std::vector timers; + + std::vector*> env; + if(use_mesh) + generateEnvironmentsMesh(env, env_scale, env_size); + else + generateEnvironments(env, env_scale, env_size); + + std::vector*> query; + if(use_mesh) + generateEnvironmentsMesh(query, env_scale, query_size); + else + generateEnvironments(query, env_scale, query_size); + + std::vector*> managers; + + managers.push_back(new NaiveCollisionManager()); + managers.push_back(new SSaPCollisionManager()); + managers.push_back(new SaPCollisionManager()); + managers.push_back(new IntervalTreeCollisionManager()); + + Vector3 lower_limit, upper_limit; + SpatialHashingCollisionManager::computeBound(env, lower_limit, upper_limit); + // S ncell_per_axis = std::pow((S)env_size / 10, 1 / 3.0); + S ncell_per_axis = 20; + S cell_size = std::min(std::min((upper_limit[0] - lower_limit[0]) / ncell_per_axis, (upper_limit[1] - lower_limit[1]) / ncell_per_axis), (upper_limit[2] - lower_limit[2]) / ncell_per_axis); + // managers.push_back(new SpatialHashingCollisionManager(cell_size, lower_limit, upper_limit)); + managers.push_back(new SpatialHashingCollisionManager, CollisionObject*, SpatialHash> >(cell_size, lower_limit, upper_limit)); +#if USE_GOOGLEHASH + managers.push_back(new SpatialHashingCollisionManager, CollisionObject*, SpatialHash, GoogleSparseHashTable> >(cell_size, lower_limit, upper_limit)); + managers.push_back(new SpatialHashingCollisionManager, CollisionObject*, SpatialHash, GoogleDenseHashTable> >(cell_size, lower_limit, upper_limit)); +#endif + managers.push_back(new DynamicAABBTreeCollisionManager()); + + managers.push_back(new DynamicAABBTreeCollisionManager_Array()); + + { + DynamicAABBTreeCollisionManager* m = new DynamicAABBTreeCollisionManager(); + m->tree_init_level = 2; + managers.push_back(m); + } + + { + DynamicAABBTreeCollisionManager_Array* m = new DynamicAABBTreeCollisionManager_Array(); + m->tree_init_level = 2; + managers.push_back(m); + } + + ts.resize(managers.size()); + timers.resize(managers.size()); + + for(size_t i = 0; i < managers.size(); ++i) + { + timers[i].start(); + managers[i]->registerObjects(env); + timers[i].stop(); + ts[i].push_back(timers[i].getElapsedTime()); + } + + for(size_t i = 0; i < managers.size(); ++i) + { + timers[i].start(); + managers[i]->setup(); + timers[i].stop(); + ts[i].push_back(timers[i].getElapsedTime()); + } + + std::vector> self_data(managers.size()); + for(size_t i = 0; i < managers.size(); ++i) + { + if(exhaustive) self_data[i].request.num_max_contacts = 100000; + else self_data[i].request.num_max_contacts = num_max_contacts; + } + + for(size_t i = 0; i < managers.size(); ++i) + { + timers[i].start(); + managers[i]->collide(&self_data[i], defaultCollisionFunction); + timers[i].stop(); + ts[i].push_back(timers[i].getElapsedTime()); + } + + for(size_t i = 0; i < managers.size(); ++i) + std::cout << self_data[i].result.numContacts() << " "; + std::cout << std::endl; + + if(exhaustive) + { + for(size_t i = 1; i < managers.size(); ++i) + EXPECT_TRUE(self_data[i].result.numContacts() == self_data[0].result.numContacts()); + } + else + { + std::vector self_res(managers.size()); + for(size_t i = 0; i < self_res.size(); ++i) + self_res[i] = (self_data[i].result.numContacts() > 0); + + for(size_t i = 1; i < self_res.size(); ++i) + EXPECT_TRUE(self_res[0] == self_res[i]); + + for(size_t i = 1; i < managers.size(); ++i) + EXPECT_TRUE(self_data[i].result.numContacts() == self_data[0].result.numContacts()); + } + + + for(size_t i = 0; i < query.size(); ++i) + { + std::vector> query_data(managers.size()); + for(size_t j = 0; j < query_data.size(); ++j) + { + if(exhaustive) query_data[j].request.num_max_contacts = 100000; + else query_data[j].request.num_max_contacts = num_max_contacts; + } + + for(size_t j = 0; j < query_data.size(); ++j) + { + timers[j].start(); + managers[j]->collide(query[i], &query_data[j], defaultCollisionFunction); + timers[j].stop(); + ts[j].push_back(timers[j].getElapsedTime()); + } + + + // for(size_t j = 0; j < managers.size(); ++j) + // std::cout << query_data[j].result.numContacts() << " "; + // std::cout << std::endl; + + if(exhaustive) + { + for(size_t j = 1; j < managers.size(); ++j) + EXPECT_TRUE(query_data[j].result.numContacts() == query_data[0].result.numContacts()); + } + else + { + std::vector query_res(managers.size()); + for(size_t j = 0; j < query_res.size(); ++j) + query_res[j] = (query_data[j].result.numContacts() > 0); + for(size_t j = 1; j < query_res.size(); ++j) + EXPECT_TRUE(query_res[0] == query_res[j]); + + for(size_t j = 1; j < managers.size(); ++j) + EXPECT_TRUE(query_data[j].result.numContacts() == query_data[0].result.numContacts()); + } + } + + for(size_t i = 0; i < env.size(); ++i) + delete env[i]; + for(size_t i = 0; i < query.size(); ++i) + delete query[i]; + + for(size_t i = 0; i < managers.size(); ++i) + delete managers[i]; + + std::cout.setf(std::ios_base::left, std::ios_base::adjustfield); + size_t w = 7; + + std::cout << "collision timing summary" << std::endl; + std::cout << env_size << " objs, " << query_size << " queries" << std::endl; + std::cout << "register time" << std::endl; + for(size_t i = 0; i < ts.size(); ++i) + std::cout << std::setw(w) << ts[i].records[0] << " "; + std::cout << std::endl; + + std::cout << "setup time" << std::endl; + for(size_t i = 0; i < ts.size(); ++i) + std::cout << std::setw(w) << ts[i].records[1] << " "; + std::cout << std::endl; + + std::cout << "self collision time" << std::endl; + for(size_t i = 0; i < ts.size(); ++i) + std::cout << std::setw(w) << ts[i].records[2] << " "; + std::cout << std::endl; + + std::cout << "collision time" << std::endl; + for(size_t i = 0; i < ts.size(); ++i) + { + S tmp = 0; + for(size_t j = 3; j < ts[i].records.size(); ++j) + tmp += ts[i].records[j]; + std::cout << std::setw(w) << tmp << " "; + } + std::cout << std::endl; + + + std::cout << "overall time" << std::endl; + for(size_t i = 0; i < ts.size(); ++i) + std::cout << std::setw(w) << ts[i].overall_time << " "; + std::cout << std::endl; + std::cout << std::endl; + +} + +//============================================================================== +int main(int argc, char* argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test/test_fcl_broadphase_distance.cpp b/test/test_fcl_broadphase_distance.cpp new file mode 100644 index 000000000..df29c1b2c --- /dev/null +++ b/test/test_fcl_broadphase_distance.cpp @@ -0,0 +1,584 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#include + +#include "fcl/config.h" +#include "fcl/broadphase/broadphase.h" +#include "fcl/broadphase/sparse_hash_table.h" +#include "fcl/broadphase/spatial_hash.h" +#include "fcl/shape/geometric_shape_to_BVH_model.h" +#include "test_fcl_utility.h" + +#if USE_GOOGLEHASH +#include +#include +#include +#endif + +#include +#include + +using namespace fcl; + +/// @brief Generate environment with 3 * n objects for self distance, so we try to make sure none of them collide with each other. +template +void generateSelfDistanceEnvironments(std::vector*>& env, S env_scale, std::size_t n); + +/// @brief Generate environment with 3 * n objects for self distance, but all in meshes. +template +void generateSelfDistanceEnvironmentsMesh(std::vector*>& env, S env_scale, std::size_t n); + +/// @brief test for broad phase distance +template +void broad_phase_distance_test(S env_scale, std::size_t env_size, std::size_t query_size, bool use_mesh = false); + +/// @brief test for broad phase self distance +template +void broad_phase_self_distance_test(S env_scale, std::size_t env_size, bool use_mesh = false); + +template +S getDELTA() { return 0.01; } + +#if USE_GOOGLEHASH +template +struct GoogleSparseHashTable : public google::sparse_hash_map, std::equal_to > {}; + +template +struct GoogleDenseHashTable : public google::dense_hash_map, std::equal_to > +{ + GoogleDenseHashTable() : google::dense_hash_map, std::equal_to >() + { + this->set_empty_key(nullptr); + } +}; +#endif + +/// check broad phase distance +GTEST_TEST(FCL_BROADPHASE, test_core_bf_broad_phase_distance) +{ +#ifdef NDEBUG + broad_phase_distance_test(200, 100, 100); + broad_phase_distance_test(200, 1000, 100); + broad_phase_distance_test(2000, 100, 100); + broad_phase_distance_test(2000, 1000, 100); +#else + broad_phase_distance_test(200, 10, 10); + broad_phase_distance_test(200, 100, 10); + broad_phase_distance_test(2000, 10, 10); + broad_phase_distance_test(2000, 100, 10); +#endif +} + +/// check broad phase self distance +GTEST_TEST(FCL_BROADPHASE, test_core_bf_broad_phase_self_distance) +{ +#ifdef NDEBUG + broad_phase_self_distance_test(200, 512); + broad_phase_self_distance_test(200, 1000); + broad_phase_self_distance_test(200, 5000); +#else + broad_phase_self_distance_test(200, 256); + broad_phase_self_distance_test(200, 500); + broad_phase_self_distance_test(200, 2500); +#endif +} + +/// check broad phase distance +GTEST_TEST(FCL_BROADPHASE, test_core_mesh_bf_broad_phase_distance_mesh) +{ +#ifdef NDEBUG + broad_phase_distance_test(200, 100, 100, true); + broad_phase_distance_test(200, 1000, 100, true); + broad_phase_distance_test(2000, 100, 100, true); + broad_phase_distance_test(2000, 1000, 100, true); +#else + broad_phase_distance_test(200, 2, 2, true); + broad_phase_distance_test(200, 4, 2, true); + broad_phase_distance_test(2000, 2, 2, true); + broad_phase_distance_test(2000, 4, 2, true); +#endif +} + +/// check broad phase self distance +GTEST_TEST(FCL_BROADPHASE, test_core_mesh_bf_broad_phase_self_distance_mesh) +{ +#ifdef NDEBUG + broad_phase_self_distance_test(200, 512, true); + broad_phase_self_distance_test(200, 1000, true); + broad_phase_self_distance_test(200, 5000, true); +#else + broad_phase_self_distance_test(200, 128, true); + broad_phase_self_distance_test(200, 250, true); + broad_phase_self_distance_test(200, 1250, true); +#endif +} + +template +void generateSelfDistanceEnvironments(std::vector*>& env, S env_scale, std::size_t n) +{ + unsigned int n_edge = std::floor(std::pow(n, 1/3.0)); + + S step_size = env_scale * 2 / n_edge; + S delta_size = step_size * 0.05; + S single_size = step_size - 2 * delta_size; + + unsigned int i = 0; + for(; i < n_edge * n_edge * n_edge / 4; ++i) + { + int x = i % (n_edge * n_edge); + int y = (i - n_edge * n_edge * x) % n_edge; + int z = i - n_edge * n_edge * x - n_edge * y; + + Box* box = new Box(single_size, single_size, single_size); + env.push_back(new CollisionObject(std::shared_ptr>(box), + Transform3(Translation3(Vector3(x * step_size + delta_size + 0.5 * single_size - env_scale, + y * step_size + delta_size + 0.5 * single_size - env_scale, + z * step_size + delta_size + 0.5 * single_size - env_scale))))); + } + + for(; i < n_edge * n_edge * n_edge / 4; ++i) + { + int x = i % (n_edge * n_edge); + int y = (i - n_edge * n_edge * x) % n_edge; + int z = i - n_edge * n_edge * x - n_edge * y; + + Sphere* sphere = new Sphere(single_size / 2); + env.push_back(new CollisionObject(std::shared_ptr>(sphere), + Transform3(Translation3(Vector3(x * step_size + delta_size + 0.5 * single_size - env_scale, + y * step_size + delta_size + 0.5 * single_size - env_scale, + z * step_size + delta_size + 0.5 * single_size - env_scale))))); + } + + for(; i < n_edge * n_edge * n_edge / 4; ++i) + { + int x = i % (n_edge * n_edge); + int y = (i - n_edge * n_edge * x) % n_edge; + int z = i - n_edge * n_edge * x - n_edge * y; + + Ellipsoid* ellipsoid = new Ellipsoid(single_size / 2, single_size / 2, single_size / 2); + env.push_back(new CollisionObject(std::shared_ptr>(ellipsoid), + Transform3(Translation3(Vector3(x * step_size + delta_size + 0.5 * single_size - env_scale, + y * step_size + delta_size + 0.5 * single_size - env_scale, + z * step_size + delta_size + 0.5 * single_size - env_scale))))); + } + + for(; i < n_edge * n_edge * n_edge / 4; ++i) + { + int x = i % (n_edge * n_edge); + int y = (i - n_edge * n_edge * x) % n_edge; + int z = i - n_edge * n_edge * x - n_edge * y; + + Cylinder* cylinder = new Cylinder(single_size / 2, single_size); + env.push_back(new CollisionObject(std::shared_ptr>(cylinder), + Transform3(Translation3(Vector3(x * step_size + delta_size + 0.5 * single_size - env_scale, + y * step_size + delta_size + 0.5 * single_size - env_scale, + z * step_size + delta_size + 0.5 * single_size - env_scale))))); + } + + for(; i < n_edge * n_edge * n_edge / 4; ++i) + { + int x = i % (n_edge * n_edge); + int y = (i - n_edge * n_edge * x) % n_edge; + int z = i - n_edge * n_edge * x - n_edge * y; + + Cone* cone = new Cone(single_size / 2, single_size); + env.push_back(new CollisionObject(std::shared_ptr>(cone), + Transform3(Translation3(Vector3(x * step_size + delta_size + 0.5 * single_size - env_scale, + y * step_size + delta_size + 0.5 * single_size - env_scale, + z * step_size + delta_size + 0.5 * single_size - env_scale))))); + } +} + +template +void generateSelfDistanceEnvironmentsMesh(std::vector*>& env, S env_scale, std::size_t n) +{ + unsigned int n_edge = std::floor(std::pow(n, 1/3.0)); + + S step_size = env_scale * 2 / n_edge; + S delta_size = step_size * 0.05; + S single_size = step_size - 2 * delta_size; + + std::size_t i = 0; + for(; i < n_edge * n_edge * n_edge / 4; ++i) + { + int x = i % (n_edge * n_edge); + int y = (i - n_edge * n_edge * x) % n_edge; + int z = i - n_edge * n_edge * x - n_edge * y; + + Box box(single_size, single_size, single_size); + BVHModel>* model = new BVHModel>(); + generateBVHModel(*model, box, Transform3::Identity()); + env.push_back(new CollisionObject(std::shared_ptr>(model), + Transform3(Translation3(Vector3(x * step_size + delta_size + 0.5 * single_size - env_scale, + y * step_size + delta_size + 0.5 * single_size - env_scale, + z * step_size + delta_size + 0.5 * single_size - env_scale))))); + } + + for(; i < n_edge * n_edge * n_edge / 4; ++i) + { + int x = i % (n_edge * n_edge); + int y = (i - n_edge * n_edge * x) % n_edge; + int z = i - n_edge * n_edge * x - n_edge * y; + + Sphere sphere(single_size / 2); + BVHModel>* model = new BVHModel>(); + generateBVHModel(*model, sphere, Transform3::Identity(), 16, 16); + env.push_back(new CollisionObject(std::shared_ptr>(model), + Transform3(Translation3(Vector3(x * step_size + delta_size + 0.5 * single_size - env_scale, + y * step_size + delta_size + 0.5 * single_size - env_scale, + z * step_size + delta_size + 0.5 * single_size - env_scale))))); + } + + for(; i < n_edge * n_edge * n_edge / 4; ++i) + { + int x = i % (n_edge * n_edge); + int y = (i - n_edge * n_edge * x) % n_edge; + int z = i - n_edge * n_edge * x - n_edge * y; + + Ellipsoid ellipsoid(single_size / 2, single_size / 2, single_size / 2); + BVHModel>* model = new BVHModel>(); + generateBVHModel(*model, ellipsoid, Transform3::Identity(), 16, 16); + env.push_back(new CollisionObject(std::shared_ptr>(model), + Transform3(Translation3(Vector3(x * step_size + delta_size + 0.5 * single_size - env_scale, + y * step_size + delta_size + 0.5 * single_size - env_scale, + z * step_size + delta_size + 0.5 * single_size - env_scale))))); + } + + for(; i < n_edge * n_edge * n_edge / 4; ++i) + { + int x = i % (n_edge * n_edge); + int y = (i - n_edge * n_edge * x) % n_edge; + int z = i - n_edge * n_edge * x - n_edge * y; + + Cylinder cylinder(single_size / 2, single_size); + BVHModel>* model = new BVHModel>(); + generateBVHModel(*model, cylinder, Transform3::Identity(), 16, 16); + env.push_back(new CollisionObject(std::shared_ptr>(model), + Transform3(Translation3(Vector3(x * step_size + delta_size + 0.5 * single_size - env_scale, + y * step_size + delta_size + 0.5 * single_size - env_scale, + z * step_size + delta_size + 0.5 * single_size - env_scale))))); + } + + for(; i < n_edge * n_edge * n_edge / 4; ++i) + { + int x = i % (n_edge * n_edge); + int y = (i - n_edge * n_edge * x) % n_edge; + int z = i - n_edge * n_edge * x - n_edge * y; + + Cone cone(single_size / 2, single_size); + BVHModel>* model = new BVHModel>(); + generateBVHModel(*model, cone, Transform3::Identity(), 16, 16); + env.push_back(new CollisionObject(std::shared_ptr>(model), + Transform3(Translation3(Vector3(x * step_size + delta_size + 0.5 * single_size - env_scale, + y * step_size + delta_size + 0.5 * single_size - env_scale, + z * step_size + delta_size + 0.5 * single_size - env_scale))))); + } +} + +template +void broad_phase_self_distance_test(S env_scale, std::size_t env_size, bool use_mesh) +{ + std::vector ts; + std::vector timers; + + std::vector*> env; + if(use_mesh) + generateSelfDistanceEnvironmentsMesh(env, env_scale, env_size); + else + generateSelfDistanceEnvironments(env, env_scale, env_size); + + std::vector*> managers; + + managers.push_back(new NaiveCollisionManager()); + managers.push_back(new SSaPCollisionManager()); + managers.push_back(new SaPCollisionManager()); + managers.push_back(new IntervalTreeCollisionManager()); + + Vector3 lower_limit, upper_limit; + SpatialHashingCollisionManager::computeBound(env, lower_limit, upper_limit); + S cell_size = std::min(std::min((upper_limit[0] - lower_limit[0]) / 5, (upper_limit[1] - lower_limit[1]) / 5), (upper_limit[2] - lower_limit[2]) / 5); + // managers.push_back(new SpatialHashingCollisionManager(cell_size, lower_limit, upper_limit)); + managers.push_back(new SpatialHashingCollisionManager, CollisionObject*, SpatialHash> >(cell_size, lower_limit, upper_limit)); +#if USE_GOOGLEHASH + managers.push_back(new SpatialHashingCollisionManager, CollisionObject*, SpatialHash, GoogleSparseHashTable> >(cell_size, lower_limit, upper_limit)); + managers.push_back(new SpatialHashingCollisionManager, CollisionObject*, SpatialHash, GoogleDenseHashTable> >(cell_size, lower_limit, upper_limit)); +#endif + managers.push_back(new DynamicAABBTreeCollisionManager()); + managers.push_back(new DynamicAABBTreeCollisionManager_Array()); + + { + DynamicAABBTreeCollisionManager* m = new DynamicAABBTreeCollisionManager(); + m->tree_init_level = 2; + managers.push_back(m); + } + + { + DynamicAABBTreeCollisionManager_Array* m = new DynamicAABBTreeCollisionManager_Array(); + m->tree_init_level = 2; + managers.push_back(m); + } + + ts.resize(managers.size()); + timers.resize(managers.size()); + + for(size_t i = 0; i < managers.size(); ++i) + { + timers[i].start(); + managers[i]->registerObjects(env); + timers[i].stop(); + ts[i].push_back(timers[i].getElapsedTime()); + } + + for(size_t i = 0; i < managers.size(); ++i) + { + timers[i].start(); + managers[i]->setup(); + timers[i].stop(); + ts[i].push_back(timers[i].getElapsedTime()); + } + + + std::vector> self_data(managers.size()); + + for(size_t i = 0; i < self_data.size(); ++i) + { + timers[i].start(); + managers[i]->distance(&self_data[i], defaultDistanceFunction); + timers[i].stop(); + ts[i].push_back(timers[i].getElapsedTime()); + // std::cout << self_data[i].result.min_distance << " "; + } + // std::cout << std::endl; + + for(size_t i = 1; i < managers.size(); ++i) + EXPECT_TRUE(fabs(self_data[0].result.min_distance - self_data[i].result.min_distance) < getDELTA() || + fabs(self_data[0].result.min_distance - self_data[i].result.min_distance) / fabs(self_data[0].result.min_distance) < getDELTA()); + + for(size_t i = 0; i < env.size(); ++i) + delete env[i]; + + for(size_t i = 0; i < managers.size(); ++i) + delete managers[i]; + + std::cout.setf(std::ios_base::left, std::ios_base::adjustfield); + size_t w = 7; + + std::cout << "self distance timing summary" << std::endl; + std::cout << env.size() << " objs" << std::endl; + std::cout << "register time" << std::endl; + for(size_t i = 0; i < ts.size(); ++i) + std::cout << std::setw(w) << ts[i].records[0] << " "; + std::cout << std::endl; + + std::cout << "setup time" << std::endl; + for(size_t i = 0; i < ts.size(); ++i) + std::cout << std::setw(w) << ts[i].records[1] << " "; + std::cout << std::endl; + + std::cout << "self distance time" << std::endl; + for(size_t i = 0; i < ts.size(); ++i) + std::cout << std::setw(w) << ts[i].records[2] << " "; + std::cout << std::endl; + + std::cout << "overall time" << std::endl; + for(size_t i = 0; i < ts.size(); ++i) + std::cout << std::setw(w) << ts[i].overall_time << " "; + std::cout << std::endl; + std::cout << std::endl; +} + +template +void broad_phase_distance_test(S env_scale, std::size_t env_size, std::size_t query_size, bool use_mesh) +{ + std::vector ts; + std::vector timers; + + std::vector*> env; + if(use_mesh) + generateEnvironmentsMesh(env, env_scale, env_size); + else + generateEnvironments(env, env_scale, env_size); + + std::vector*> query; + + BroadPhaseCollisionManager* manager = new NaiveCollisionManager(); + for(std::size_t i = 0; i < env.size(); ++i) + manager->registerObject(env[i]); + manager->setup(); + + while(1) + { + std::vector*> candidates; + if(use_mesh) + generateEnvironmentsMesh(candidates, env_scale, query_size); + else + generateEnvironments(candidates, env_scale, query_size); + + for(std::size_t i = 0; i < candidates.size(); ++i) + { + CollisionData query_data; + manager->collide(candidates[i], &query_data, defaultCollisionFunction); + if(query_data.result.numContacts() == 0) + query.push_back(candidates[i]); + else + delete candidates[i]; + if(query.size() == query_size) break; + } + + if(query.size() == query_size) break; + } + + delete manager; + + std::vector*> managers; + + managers.push_back(new NaiveCollisionManager()); + managers.push_back(new SSaPCollisionManager()); + managers.push_back(new SaPCollisionManager()); + managers.push_back(new IntervalTreeCollisionManager()); + + Vector3 lower_limit, upper_limit; + SpatialHashingCollisionManager::computeBound(env, lower_limit, upper_limit); + S cell_size = std::min(std::min((upper_limit[0] - lower_limit[0]) / 20, (upper_limit[1] - lower_limit[1]) / 20), (upper_limit[2] - lower_limit[2])/20); + // managers.push_back(new SpatialHashingCollisionManager(cell_size, lower_limit, upper_limit)); + managers.push_back(new SpatialHashingCollisionManager, CollisionObject*, SpatialHash> >(cell_size, lower_limit, upper_limit)); +#if USE_GOOGLEHASH + managers.push_back(new SpatialHashingCollisionManager, CollisionObject*, SpatialHash, GoogleSparseHashTable> >(cell_size, lower_limit, upper_limit)); + managers.push_back(new SpatialHashingCollisionManager, CollisionObject*, SpatialHash, GoogleDenseHashTable> >(cell_size, lower_limit, upper_limit)); +#endif + managers.push_back(new DynamicAABBTreeCollisionManager()); + managers.push_back(new DynamicAABBTreeCollisionManager_Array()); + + { + DynamicAABBTreeCollisionManager* m = new DynamicAABBTreeCollisionManager(); + m->tree_init_level = 2; + managers.push_back(m); + } + + { + DynamicAABBTreeCollisionManager_Array* m = new DynamicAABBTreeCollisionManager_Array(); + m->tree_init_level = 2; + managers.push_back(m); + } + + ts.resize(managers.size()); + timers.resize(managers.size()); + + for(size_t i = 0; i < managers.size(); ++i) + { + timers[i].start(); + managers[i]->registerObjects(env); + timers[i].stop(); + ts[i].push_back(timers[i].getElapsedTime()); + } + + for(size_t i = 0; i < managers.size(); ++i) + { + timers[i].start(); + managers[i]->setup(); + timers[i].stop(); + ts[i].push_back(timers[i].getElapsedTime()); + } + + + for(size_t i = 0; i < query.size(); ++i) + { + std::vector> query_data(managers.size()); + for(size_t j = 0; j < managers.size(); ++j) + { + timers[j].start(); + managers[j]->distance(query[i], &query_data[j], defaultDistanceFunction); + timers[j].stop(); + ts[j].push_back(timers[j].getElapsedTime()); + // std::cout << query_data[j].result.min_distance << " "; + } + // std::cout << std::endl; + + for(size_t j = 1; j < managers.size(); ++j) + EXPECT_TRUE(fabs(query_data[0].result.min_distance - query_data[j].result.min_distance) < getDELTA() || + fabs(query_data[0].result.min_distance - query_data[j].result.min_distance) / fabs(query_data[0].result.min_distance) < getDELTA()); + } + + + for(std::size_t i = 0; i < env.size(); ++i) + delete env[i]; + for(std::size_t i = 0; i < query.size(); ++i) + delete query[i]; + + for(size_t i = 0; i < managers.size(); ++i) + delete managers[i]; + + + std::cout.setf(std::ios_base::left, std::ios_base::adjustfield); + size_t w = 7; + + std::cout << "distance timing summary" << std::endl; + std::cout << env_size << " objs, " << query_size << " queries" << std::endl; + std::cout << "register time" << std::endl; + for(size_t i = 0; i < ts.size(); ++i) + std::cout << std::setw(w) << ts[i].records[0] << " "; + std::cout << std::endl; + + std::cout << "setup time" << std::endl; + for(size_t i = 0; i < ts.size(); ++i) + std::cout << std::setw(w) << ts[i].records[1] << " "; + std::cout << std::endl; + + std::cout << "distance time" << std::endl; + for(size_t i = 0; i < ts.size(); ++i) + { + S tmp = 0; + for(size_t j = 2; j < ts[i].records.size(); ++j) + tmp += ts[i].records[j]; + std::cout << std::setw(w) << tmp << " "; + } + std::cout << std::endl; + + std::cout << "overall time" << std::endl; + for(size_t i = 0; i < ts.size(); ++i) + std::cout << std::setw(w) << ts[i].overall_time << " "; + std::cout << std::endl; + std::cout << std::endl; +} + +//============================================================================== +int main(int argc, char* argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test/test_fcl_bvh_models.cpp b/test/test_fcl_bvh_models.cpp index a3cedff60..de9cd194e 100644 --- a/test/test_fcl_bvh_models.cpp +++ b/test/test_fcl_bvh_models.cpp @@ -47,6 +47,8 @@ using namespace fcl; template void testBVHModelPointCloud() { + using S = typename BV::S; + std::shared_ptr > model(new BVHModel); if (model->getNodeType() != BV_AABB @@ -60,11 +62,11 @@ void testBVHModelPointCloud() return; } - Box box; - double a = box.side[0]; - double b = box.side[1]; - double c = box.side[2]; - std::vector points(8); + Box box; + auto a = box.side[0]; + auto b = box.side[1]; + auto c = box.side[2]; + std::vector> points(8); points[0] << 0.5 * a, -0.5 * b, 0.5 * c; points[1] << 0.5 * a, 0.5 * b, 0.5 * c; points[2] << -0.5 * a, 0.5 * b, 0.5 * c; @@ -98,13 +100,15 @@ void testBVHModelPointCloud() template void testBVHModelTriangles() { + using S = typename BV::S; + std::shared_ptr > model(new BVHModel); - Box box; + Box box; - double a = box.side[0]; - double b = box.side[1]; - double c = box.side[2]; - std::vector points(8); + auto a = box.side[0]; + auto b = box.side[1]; + auto c = box.side[2]; + std::vector> points(8); std::vector tri_indices(12); points[0] << 0.5 * a, -0.5 * b, 0.5 * c; points[1] << 0.5 * a, 0.5 * b, 0.5 * c; @@ -152,13 +156,15 @@ void testBVHModelTriangles() template void testBVHModelSubModel() { + using S = typename BV::S; + std::shared_ptr > model(new BVHModel); - Box box; + Box box; - double a = box.side[0]; - double b = box.side[1]; - double c = box.side[2]; - std::vector points(8); + auto a = box.side[0]; + auto b = box.side[1]; + auto c = box.side[2]; + std::vector> points(8); std::vector tri_indices(12); points[0] << 0.5 * a, -0.5 * b, 0.5 * c; points[1] << 0.5 * a, 0.5 * b, 0.5 * c; @@ -210,14 +216,23 @@ void testBVHModel() GTEST_TEST(FCL_BVH_MODELS, building_bvh_models) { - testBVHModel(); - testBVHModel(); - testBVHModel(); - testBVHModel(); - testBVHModel(); - testBVHModel >(); - testBVHModel >(); - testBVHModel >(); +// testBVHModel>(); +// testBVHModel>(); +// testBVHModel>(); +// testBVHModel>(); +// testBVHModel>(); +// testBVHModel >(); +// testBVHModel >(); +// testBVHModel >(); + + testBVHModel>(); + testBVHModel>(); + testBVHModel>(); + testBVHModel>(); + testBVHModel>(); + testBVHModel >(); + testBVHModel >(); + testBVHModel >(); } //============================================================================== diff --git a/test/test_fcl_capsule_box_1.cpp b/test/test_fcl_capsule_box_1.cpp index ef8caae9b..2c4ce8b0f 100644 --- a/test/test_fcl_capsule_box_1.cpp +++ b/test/test_fcl_capsule_box_1.cpp @@ -42,37 +42,39 @@ #include #include -GTEST_TEST(FCL_GEOMETRIC_SHAPES, distance_capsule_box) +template +void test_distance_capsule_box() { - typedef std::shared_ptr CollisionGeometryPtr_t; + using CollisionGeometryPtr_t = std::shared_ptr>; + // Capsule of radius 2 and of height 4 - CollisionGeometryPtr_t capsuleGeometry (new fcl::Capsule (2., 4.)); + CollisionGeometryPtr_t capsuleGeometry (new fcl::Capsule (2., 4.)); // Box of size 1 by 2 by 4 - CollisionGeometryPtr_t boxGeometry (new fcl::Box (1., 2., 4.)); + CollisionGeometryPtr_t boxGeometry (new fcl::Box (1., 2., 4.)); // Enable computation of nearest points - fcl::DistanceRequest distanceRequest (true); - fcl::DistanceResult distanceResult; - - fcl::Transform3d tf1(Eigen::Translation3d(fcl::Vector3d (3., 0, 0))); - fcl::Transform3d tf2 = fcl::Transform3d::Identity(); - fcl::CollisionObject capsule (capsuleGeometry, tf1); - fcl::CollisionObject box (boxGeometry, tf2); + fcl::DistanceRequest distanceRequest (true); + fcl::DistanceResult distanceResult; + + fcl::Transform3 tf1(fcl::Translation3(fcl::Vector3 (3., 0, 0))); + fcl::Transform3 tf2 = fcl::Transform3::Identity(); + fcl::CollisionObject capsule (capsuleGeometry, tf1); + fcl::CollisionObject box (boxGeometry, tf2); // test distance fcl::distance (&capsule, &box, distanceRequest, distanceResult); // Nearest point on capsule - fcl::Vector3d o1 (distanceResult.nearest_points [0]); + fcl::Vector3 o1 (distanceResult.nearest_points [0]); // Nearest point on box - fcl::Vector3d o2 (distanceResult.nearest_points [1]); + fcl::Vector3 o2 (distanceResult.nearest_points [1]); EXPECT_NEAR (distanceResult.min_distance, 0.5, 1e-4); EXPECT_NEAR (o1 [0], -2.0, 1e-4); EXPECT_NEAR (o1 [1], 0.0, 1e-4); EXPECT_NEAR (o2 [0], 0.5, 1e-4); - EXPECT_NEAR (o1 [1], 0.0, 1e-4); + EXPECT_NEAR (o1 [1], 0.0, 1e-4); // TODO(JS): maybe o2 rather than o1? // Move capsule above box - tf1 = Eigen::Translation3d(fcl::Vector3d (0., 0., 8.)); + tf1 = fcl::Translation3(fcl::Vector3 (0., 0., 8.)); capsule.setTransform (tf1); // test distance @@ -92,8 +94,8 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, distance_capsule_box) EXPECT_NEAR (o2 [2], 2.0, 1e-4); // Rotate capsule around y axis by pi/2 and move it behind box - tf1.translation() = fcl::Vector3d(-10., 0., 0.); - tf1.linear() = fcl::Quaternion3d(sqrt(2)/2, 0, sqrt(2)/2, 0).toRotationMatrix(); + tf1.translation() = fcl::Vector3(-10., 0., 0.); + tf1.linear() = fcl::Quaternion(sqrt(2)/2, 0, sqrt(2)/2, 0).toRotationMatrix(); capsule.setTransform (tf1); // test distance @@ -111,6 +113,12 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, distance_capsule_box) EXPECT_NEAR (o2 [2], 0.0, 1e-4); } +GTEST_TEST(FCL_GEOMETRIC_SHAPES, distance_capsule_box) +{ +// test_distance_capsule_box(); + test_distance_capsule_box(); +} + //============================================================================== int main(int argc, char* argv[]) { diff --git a/test/test_fcl_capsule_box_2.cpp b/test/test_fcl_capsule_box_2.cpp index cfa9a6aca..5414c7a3a 100644 --- a/test/test_fcl_capsule_box_2.cpp +++ b/test/test_fcl_capsule_box_2.cpp @@ -42,30 +42,32 @@ #include #include -GTEST_TEST(FCL_GEOMETRIC_SHAPES, distance_capsule_box) +template +void test_distance_capsule_box() { - typedef std::shared_ptr CollisionGeometryPtr_t; - // Capsule of radius 2 and of height 4 - CollisionGeometryPtr_t capsuleGeometry (new fcl::Capsule (2., 4.)); - // Box of size 1 by 2 by 4 - CollisionGeometryPtr_t boxGeometry (new fcl::Box (1., 2., 4.)); + using CollisionGeometryPtr_t = std::shared_ptr>; + + // Capsule of radius 2 and of height 4 + CollisionGeometryPtr_t capsuleGeometry (new fcl::Capsule (2., 4.)); + // Box of size 1 by 2 by 4 + CollisionGeometryPtr_t boxGeometry (new fcl::Box (1., 2., 4.)); // Enable computation of nearest points - fcl::DistanceRequest distanceRequest (true); - fcl::DistanceResult distanceResult; + fcl::DistanceRequest distanceRequest (true); + fcl::DistanceResult distanceResult; // Rotate capsule around y axis by pi/2 and move it behind box - fcl::Transform3d tf1 = Eigen::Translation3d(fcl::Vector3d(-10., 0.8, 1.5)) - *fcl::Quaternion3d(sqrt(2)/2, 0, sqrt(2)/2, 0); - fcl::Transform3d tf2 = fcl::Transform3d::Identity(); - fcl::CollisionObject capsule (capsuleGeometry, tf1); - fcl::CollisionObject box (boxGeometry, tf2); + fcl::Transform3 tf1 = fcl::Translation3(fcl::Vector3(-10., 0.8, 1.5)) + *fcl::Quaternion(sqrt(2)/2, 0, sqrt(2)/2, 0); + fcl::Transform3 tf2 = fcl::Transform3::Identity(); + fcl::CollisionObject capsule (capsuleGeometry, tf1); + fcl::CollisionObject box (boxGeometry, tf2); // test distance distanceResult.clear (); fcl::distance (&capsule, &box, distanceRequest, distanceResult); - fcl::Vector3d o1 = distanceResult.nearest_points [0]; - fcl::Vector3d o2 = distanceResult.nearest_points [1]; + fcl::Vector3 o1 = distanceResult.nearest_points [0]; + fcl::Vector3 o2 = distanceResult.nearest_points [1]; EXPECT_NEAR (distanceResult.min_distance, 5.5, 1e-4); // Disabled broken test lines. Please see #25. @@ -78,6 +80,12 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, distance_capsule_box) // EXPECT_NEAR (o2 [2], 1.5, 1e-4); } +GTEST_TEST(FCL_GEOMETRIC_SHAPES, distance_capsule_box) +{ +// test_distance_capsule_box(); + test_distance_capsule_box(); +} + //============================================================================== int main(int argc, char* argv[]) { diff --git a/test/test_fcl_capsule_capsule.cpp b/test/test_fcl_capsule_capsule.cpp index 77fa1fe86..164343b59 100644 --- a/test/test_fcl_capsule_capsule.cpp +++ b/test/test_fcl_capsule_capsule.cpp @@ -40,119 +40,147 @@ #include "fcl/math/constants.h" #include "fcl/collision.h" #include "fcl/shape/geometric_shapes.h" -#include "fcl/narrowphase/narrowphase.h" +#include "fcl/narrowphase/gjk_solver_indep.h" +#include "fcl/narrowphase/gjk_solver_libccd.h" #include using namespace fcl; -GTEST_TEST(FCL_CAPSULE_CAPSULE, distance_capsulecapsule_origin) +//============================================================================== +template +void test_distance_capsulecapsule_origin() { + GJKSolver_indep solver; + Capsule s1(5, 10); + Capsule s2(5, 10); - GJKSolver_indep solver; - Capsule s1(5, 10); - Capsule s2(5, 10); - - Vector3d closest_p1, closest_p2; + Vector3 closest_p1, closest_p2; - Transform3d transform = Transform3d::Identity(); - Transform3d transform2 = Transform3d::Identity(); - transform2.translation() = Vector3d(20.1, 0,0); + Transform3 transform = Transform3::Identity(); + Transform3 transform2 = Transform3::Identity(); + transform2.translation() = Vector3(20.1, 0,0); bool res; - FCL_REAL dist; + S dist; - res = solver.shapeDistance(s1, transform, s2, transform2, &dist, &closest_p1, &closest_p2); + res = solver.template shapeDistance, Capsule>(s1, transform, s2, transform2, &dist, &closest_p1, &closest_p2); std::cerr << "applied transformation of two caps: " << transform.translation() << " & " << transform2.translation() << std::endl; std::cerr << "computed points in caps to caps" << closest_p1 << " & " << closest_p2 << "with dist: " << dist << std::endl; EXPECT_TRUE(std::abs(dist - 10.1) < 0.001); EXPECT_TRUE(res); - } - -GTEST_TEST(FCL_CAPSULE_CAPSULE, distance_capsulecapsule_transformXY) +//============================================================================== +template +void test_distance_capsulecapsule_transformXY() { + GJKSolver_indep solver; + Capsule s1(5, 10); + Capsule s2(5, 10); - GJKSolver_indep solver; - Capsule s1(5, 10); - Capsule s2(5, 10); + Vector3 closest_p1, closest_p2; - Vector3d closest_p1, closest_p2; - - Transform3d transform = Transform3d::Identity(); - Transform3d transform2 = Transform3d::Identity(); - transform2.translation() = Vector3d(20, 20,0); + Transform3 transform = Transform3::Identity(); + Transform3 transform2 = Transform3::Identity(); + transform2.translation() = Vector3(20, 20,0); bool res; - FCL_REAL dist; + S dist; - res = solver.shapeDistance(s1, transform, s2, transform2, &dist, &closest_p1, &closest_p2); + res = solver.template shapeDistance, Capsule>(s1, transform, s2, transform2, &dist, &closest_p1, &closest_p2); std::cerr << "applied transformation of two caps: " << transform.translation() << " & " << transform2.translation() << std::endl; std::cerr << "computed points in caps to caps" << closest_p1 << " & " << closest_p2 << "with dist: " << dist << std::endl; - FCL_REAL expected = std::sqrt(FCL_REAL(800)) - 10; + S expected = std::sqrt(S(800)) - 10; EXPECT_TRUE(std::abs(expected-dist) < 0.01); EXPECT_TRUE(res); - } -GTEST_TEST(FCL_CAPSULE_CAPSULE, distance_capsulecapsule_transformZ) +//============================================================================== +template +void test_distance_capsulecapsule_transformZ() { + GJKSolver_indep solver; + Capsule s1(5, 10); + Capsule s2(5, 10); - GJKSolver_indep solver; - Capsule s1(5, 10); - Capsule s2(5, 10); - - Vector3d closest_p1, closest_p2; + Vector3 closest_p1, closest_p2; - Transform3d transform = Transform3d::Identity(); - Transform3d transform2 = Transform3d::Identity(); - transform2.translation() = Vector3d(0,0,20.1); + Transform3 transform = Transform3::Identity(); + Transform3 transform2 = Transform3::Identity(); + transform2.translation() = Vector3(0,0,20.1); bool res; - FCL_REAL dist; + S dist; - res = solver.shapeDistance(s1, transform, s2, transform2, &dist, &closest_p1, &closest_p2); + res = solver.template shapeDistance, Capsule>(s1, transform, s2, transform2, &dist, &closest_p1, &closest_p2); std::cerr << "applied transformation of two caps: " << transform.translation() << " & " << transform2.translation() << std::endl; std::cerr << "computed points in caps to caps" << closest_p1 << " & " << closest_p2 << "with dist: " << dist << std::endl; EXPECT_TRUE(std::abs(dist - 0.1) < 0.001); EXPECT_TRUE(res); - } - -GTEST_TEST(FCL_CAPSULE_CAPSULE, distance_capsulecapsule_transformZ2) +//============================================================================== +template +void test_distance_capsulecapsule_transformZ2() { - const FCL_REAL Pi = constants::pi; + const S Pi = constants::pi(); - GJKSolver_indep solver; - Capsule s1(5, 10); - Capsule s2(5, 10); + GJKSolver_indep solver; + Capsule s1(5, 10); + Capsule s2(5, 10); - Vector3d closest_p1, closest_p2; + Vector3 closest_p1, closest_p2; - Transform3d transform = Transform3d::Identity(); - Transform3d transform2 = Transform3d::Identity(); - transform2.translation() = Vector3d(0,0,25.1); - Matrix3d rot2( - Eigen::AngleAxisd(0, Vector3d::UnitX()) - * Eigen::AngleAxisd(Pi/2, Vector3d::UnitY()) - * Eigen::AngleAxisd(0, Vector3d::UnitZ())); + Transform3 transform = Transform3::Identity(); + Transform3 transform2 = Transform3::Identity(); + transform2.translation() = Vector3(0,0,25.1); + Matrix3 rot2( + AngleAxis(0, Vector3::UnitX()) + * AngleAxis(Pi/2, Vector3::UnitY()) + * AngleAxis(0, Vector3::UnitZ())); transform2.linear() = rot2; bool res; - FCL_REAL dist; + S dist; - res = solver.shapeDistance(s1, transform, s2, transform2, &dist, &closest_p1, &closest_p2); + res = solver.template shapeDistance, Capsule>(s1, transform, s2, transform2, &dist, &closest_p1, &closest_p2); std::cerr << "applied transformation of two caps: " << transform.translation() << " & " << transform2.translation() << std::endl; std::cerr << "applied transformation of two caps: " << transform.linear() << " & " << transform2.linear() << std::endl; std::cerr << "computed points in caps to caps" << closest_p1 << " & " << closest_p2 << "with dist: " << dist << std::endl; EXPECT_TRUE(std::abs(dist - 5.1) < 0.001); EXPECT_TRUE(res); +} +//============================================================================== +GTEST_TEST(FCL_CAPSULE_CAPSULE, distance_capsulecapsule_origin) +{ +// test_distance_capsulecapsule_origin(); + test_distance_capsulecapsule_origin(); +} + +//============================================================================== +GTEST_TEST(FCL_CAPSULE_CAPSULE, distance_capsulecapsule_transformXY) +{ +// test_distance_capsulecapsule_transformXY(); + test_distance_capsulecapsule_transformXY(); +} + +//============================================================================== +GTEST_TEST(FCL_CAPSULE_CAPSULE, distance_capsulecapsule_transformZ) +{ +// test_distance_capsulecapsule_transformZ(); + test_distance_capsulecapsule_transformZ(); +} + +//============================================================================== +GTEST_TEST(FCL_CAPSULE_CAPSULE, distance_capsulecapsule_transformZ2) +{ +// test_distance_capsulecapsule_transformZ2(); + test_distance_capsulecapsule_transformZ2(); } //============================================================================== diff --git a/test/test_fcl_collision.cpp b/test/test_fcl_collision.cpp index 59117be73..774e5fe65 100644 --- a/test/test_fcl_collision.cpp +++ b/test/test_fcl_collision.cpp @@ -37,189 +37,202 @@ #include -#include "fcl/traversal/traversal_node_bvhs.h" -#include "fcl/traversal/traversal_node_setup.h" #include "fcl/collision_node.h" #include "fcl/collision.h" -#include "fcl/BV/BV.h" +#include "fcl/BV/convert_bv.h" #include "fcl/shape/geometric_shapes.h" -#include "fcl/narrowphase/narrowphase.h" +#include "fcl/narrowphase/gjk_solver_indep.h" +#include "fcl/narrowphase/gjk_solver_libccd.h" #include "test_fcl_utility.h" #include "fcl_resources/config.h" using namespace fcl; template -bool collide_Test(const Transform3d& tf, - const std::vector& vertices1, const std::vector& triangles1, - const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, bool verbose = true); +bool collide_Test(const Transform3& tf, + const std::vector>& vertices1, const std::vector& triangles1, + const std::vector>& vertices2, const std::vector& triangles2, SplitMethodType split_method, bool verbose = true); template -bool collide_Test2(const Transform3d& tf, - const std::vector& vertices1, const std::vector& triangles1, - const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, bool verbose = true); +bool collide_Test2(const Transform3& tf, + const std::vector>& vertices1, const std::vector& triangles1, + const std::vector>& vertices2, const std::vector& triangles2, SplitMethodType split_method, bool verbose = true); template -bool collide_Test_Oriented(const Transform3d& tf, - const std::vector& vertices1, const std::vector& triangles1, - const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, bool verbose = true); +bool collide_Test_Oriented(const Transform3& tf, + const std::vector>& vertices1, const std::vector& triangles1, + const std::vector>& vertices2, const std::vector& triangles2, SplitMethodType split_method, bool verbose = true); template -bool test_collide_func(const Transform3d& tf, - const std::vector& vertices1, const std::vector& triangles1, - const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method); +bool test_collide_func(const Transform3& tf, + const std::vector>& vertices1, const std::vector& triangles1, + const std::vector>& vertices2, const std::vector& triangles2, SplitMethodType split_method); int num_max_contacts = std::numeric_limits::max(); bool enable_contact = true; -std::vector global_pairs; -std::vector global_pairs_now; +template +std::vector>& global_pairs() +{ + static std::vector> static_global_pairs; + return static_global_pairs; +} -GTEST_TEST(FCL_COLLISION, OBB_Box_test) +template +std::vector>& global_pairs_now() { - FCL_REAL r_extents[] = {-1000, -1000, -1000, 1000, 1000, 1000}; - std::vector rotate_transform; + static std::vector> static_global_pairs_now; + return static_global_pairs_now; +} + +template +void test_OBB_Box_test() +{ + S r_extents[] = {-1000, -1000, -1000, 1000, 1000, 1000}; + Eigen::aligned_vector> rotate_transform; generateRandomTransforms(r_extents, rotate_transform, 1); - - AABB aabb1; - aabb1.min_ = Vector3d(-600, -600, -600); - aabb1.max_ = Vector3d(600, 600, 600); - OBB obb1; + AABB aabb1; + aabb1.min_ = Vector3(-600, -600, -600); + aabb1.max_ = Vector3(600, 600, 600); + + OBB obb1; convertBV(aabb1, rotate_transform[0], obb1); - Box box1; - Transform3d box1_tf; + Box box1; + Transform3 box1_tf; constructBox(aabb1, rotate_transform[0], box1, box1_tf); - FCL_REAL extents[] = {-1000, -1000, -1000, 1000, 1000, 1000}; + S extents[] = {-1000, -1000, -1000, 1000, 1000, 1000}; std::size_t n = 1000; - std::vector transforms; + Eigen::aligned_vector> transforms; generateRandomTransforms(extents, transforms, n); for(std::size_t i = 0; i < transforms.size(); ++i) { - AABB aabb; + AABB aabb; aabb.min_ = aabb1.min_ * 0.5; - aabb.max_ = aabb1.max_ * 0.5; + aabb.max_ = aabb1.max_ * 0.5; - OBB obb2; + OBB obb2; convertBV(aabb, transforms[i], obb2); - - Box box2; - Transform3d box2_tf; + + Box box2; + Transform3 box2_tf; constructBox(aabb, transforms[i], box2, box2_tf); - GJKSolver_libccd solver; + GJKSolver_libccd solver; bool overlap_obb = obb1.overlap(obb2); - bool overlap_box = solver.shapeIntersect(box1, box1_tf, box2, box2_tf, NULL); - + bool overlap_box = solver.shapeIntersect(box1, box1_tf, box2, box2_tf, nullptr); + EXPECT_TRUE(overlap_obb == overlap_box); } } -GTEST_TEST(FCL_COLLISION, OBB_shape_test) +template +void test_OBB_shape_test() { - FCL_REAL r_extents[] = {-1000, -1000, -1000, 1000, 1000, 1000}; - std::vector rotate_transform; + S r_extents[] = {-1000, -1000, -1000, 1000, 1000, 1000}; + Eigen::aligned_vector> rotate_transform; generateRandomTransforms(r_extents, rotate_transform, 1); - - AABB aabb1; - aabb1.min_ = Vector3d(-600, -600, -600); - aabb1.max_ = Vector3d(600, 600, 600); - OBB obb1; + AABB aabb1; + aabb1.min_ = Vector3(-600, -600, -600); + aabb1.max_ = Vector3(600, 600, 600); + + OBB obb1; convertBV(aabb1, rotate_transform[0], obb1); - Box box1; - Transform3d box1_tf; + Box box1; + Transform3 box1_tf; constructBox(aabb1, rotate_transform[0], box1, box1_tf); - FCL_REAL extents[] = {-1000, -1000, -1000, 1000, 1000, 1000}; + S extents[] = {-1000, -1000, -1000, 1000, 1000, 1000}; std::size_t n = 1000; - std::vector transforms; + Eigen::aligned_vector> transforms; generateRandomTransforms(extents, transforms, n); for(std::size_t i = 0; i < transforms.size(); ++i) { - FCL_REAL len = (aabb1.max_[0] - aabb1.min_[0]) * 0.5; - OBB obb2; - GJKSolver_libccd solver; - - { - Sphere sphere(len); + S len = (aabb1.max_[0] - aabb1.min_[0]) * 0.5; + OBB obb2; + GJKSolver_libccd solver; + + { + Sphere sphere(len); computeBV(sphere, transforms[i], obb2); - + bool overlap_obb = obb1.overlap(obb2); - bool overlap_sphere = solver.shapeIntersect(box1, box1_tf, sphere, transforms[i], NULL); + bool overlap_sphere = solver.shapeIntersect(box1, box1_tf, sphere, transforms[i], nullptr); EXPECT_TRUE(overlap_obb >= overlap_sphere); } { - Ellipsoid ellipsoid(len, len, len); + Ellipsoid ellipsoid(len, len, len); computeBV(ellipsoid, transforms[i], obb2); bool overlap_obb = obb1.overlap(obb2); - bool overlap_ellipsoid = solver.shapeIntersect(box1, box1_tf, ellipsoid, transforms[i], NULL); + bool overlap_ellipsoid = solver.shapeIntersect(box1, box1_tf, ellipsoid, transforms[i], nullptr); EXPECT_TRUE(overlap_obb >= overlap_ellipsoid); } { - Capsule capsule(len, 2 * len); + Capsule capsule(len, 2 * len); computeBV(capsule, transforms[i], obb2); - + bool overlap_obb = obb1.overlap(obb2); - bool overlap_capsule = solver.shapeIntersect(box1, box1_tf, capsule, transforms[i], NULL); + bool overlap_capsule = solver.shapeIntersect(box1, box1_tf, capsule, transforms[i], nullptr); EXPECT_TRUE(overlap_obb >= overlap_capsule); } { - Cone cone(len, 2 * len); + Cone cone(len, 2 * len); computeBV(cone, transforms[i], obb2); - + bool overlap_obb = obb1.overlap(obb2); - bool overlap_cone = solver.shapeIntersect(box1, box1_tf, cone, transforms[i], NULL); + bool overlap_cone = solver.shapeIntersect(box1, box1_tf, cone, transforms[i], nullptr); EXPECT_TRUE(overlap_obb >= overlap_cone); } { - Cylinder cylinder(len, 2 * len); + Cylinder cylinder(len, 2 * len); computeBV(cylinder, transforms[i], obb2); - + bool overlap_obb = obb1.overlap(obb2); - bool overlap_cylinder = solver.shapeIntersect(box1, box1_tf, cylinder, transforms[i], NULL); + bool overlap_cylinder = solver.shapeIntersect(box1, box1_tf, cylinder, transforms[i], nullptr); EXPECT_TRUE(overlap_obb >= overlap_cylinder); } } } -GTEST_TEST(FCL_COLLISION, OBB_AABB_test) +template +void test_OBB_AABB_test() { - FCL_REAL extents[] = {-1000, -1000, -1000, 1000, 1000, 1000}; + S extents[] = {-1000, -1000, -1000, 1000, 1000, 1000}; std::size_t n = 1000; - std::vector transforms; + Eigen::aligned_vector> transforms; generateRandomTransforms(extents, transforms, n); - AABB aabb1; - aabb1.min_ = Vector3d(-600, -600, -600); - aabb1.max_ = Vector3d(600, 600, 600); - - OBB obb1; - convertBV(aabb1, Transform3d::Identity(), obb1); - + AABB aabb1; + aabb1.min_ = Vector3(-600, -600, -600); + aabb1.max_ = Vector3(600, 600, 600); + + OBB obb1; + convertBV(aabb1, Transform3::Identity(), obb1); + for(std::size_t i = 0; i < transforms.size(); ++i) { - AABB aabb; + AABB aabb; aabb.min_ = aabb1.min_ * 0.5; - aabb.max_ = aabb1.max_ * 0.5; + aabb.max_ = aabb1.max_ * 0.5; + + AABB aabb2 = translate(aabb, transforms[i].translation()); - AABB aabb2 = translate(aabb, transforms[i].translation()); - - OBB obb2; - convertBV(aabb, Transform3d(Eigen::Translation3d(transforms[i].translation())), obb2); + OBB obb2; + convertBV(aabb, Transform3(Translation3(transforms[i].translation())), obb2); bool overlap_aabb = aabb1.overlap(aabb2); bool overlap_obb = obb1.overlap(obb2); @@ -236,20 +249,21 @@ GTEST_TEST(FCL_COLLISION, OBB_AABB_test) std::cout << std::endl; } -GTEST_TEST(FCL_COLLISION, mesh_mesh) +template +void test_mesh_mesh() { - std::vector p1, p2; + std::vector> p1, p2; std::vector t1, t2; - + loadOBJFile(TEST_RESOURCES_DIR"/env.obj", p1, t1); loadOBJFile(TEST_RESOURCES_DIR"/rob.obj", p2, t2); - std::vector transforms; - FCL_REAL extents[] = {-3000, -3000, 0, 3000, 3000, 3000}; -#if FCL_BUILD_TYPE_DEBUG - std::size_t n = 1; -#else + Eigen::aligned_vector> transforms; + S extents[] = {-3000, -3000, 0, 3000, 3000, 3000}; +#ifdef NDEBUG std::size_t n = 10; +#else + std::size_t n = 1; #endif bool verbose = false; @@ -258,572 +272,597 @@ GTEST_TEST(FCL_COLLISION, mesh_mesh) // collision for(std::size_t i = 0; i < transforms.size(); ++i) { - global_pairs.clear(); - global_pairs_now.clear(); + global_pairs().clear(); + global_pairs_now().clear(); - collide_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); + collide_Test>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); - collide_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + collide_Test>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } - collide_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + collide_Test>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } - collide_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + collide_Test>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } - collide_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + collide_Test>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } - collide_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + collide_Test>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } - collide_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + collide_Test>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } - collide_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + collide_Test>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } - collide_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + collide_Test>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } - collide_Test >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + collide_Test >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } - collide_Test >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + collide_Test >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } - collide_Test >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + collide_Test >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } - collide_Test >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + collide_Test >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } - collide_Test >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + collide_Test >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } - collide_Test >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + collide_Test >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } - collide_Test >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + collide_Test >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } - collide_Test >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + collide_Test >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } - collide_Test >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + collide_Test >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } - collide_Test2(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + collide_Test2>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } - collide_Test2(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + collide_Test2>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } - collide_Test2(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + collide_Test2>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } - collide_Test2(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + collide_Test2>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } - collide_Test2(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + collide_Test2>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } - collide_Test2(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + collide_Test2>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } - collide_Test2(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + collide_Test2>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } - collide_Test2(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + collide_Test2>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } - collide_Test2(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + collide_Test2>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } - collide_Test2 >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + collide_Test2 >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } - collide_Test2 >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + collide_Test2 >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } - collide_Test2 >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + collide_Test2 >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } - collide_Test2 >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + collide_Test2 >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } - collide_Test2 >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + collide_Test2 >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } - collide_Test2 >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + collide_Test2 >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } - collide_Test2 >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + collide_Test2 >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } - collide_Test2 >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + collide_Test2 >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } - collide_Test2 >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + collide_Test2 >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } - collide_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); + collide_Test_Oriented, MeshCollisionTraversalNodeOBB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } - collide_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); + collide_Test_Oriented, MeshCollisionTraversalNodeOBB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } - collide_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + collide_Test_Oriented, MeshCollisionTraversalNodeOBB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } - collide_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + collide_Test_Oriented, MeshCollisionTraversalNodeRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } - collide_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + collide_Test_Oriented, MeshCollisionTraversalNodeRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } - collide_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + collide_Test_Oriented, MeshCollisionTraversalNodeRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } - test_collide_func(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + test_collide_func>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN); + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } - test_collide_func(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + test_collide_func>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN); + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } - test_collide_func(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + test_collide_func>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN); + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } - - collide_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + + collide_Test>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } - collide_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + collide_Test>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } - collide_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + collide_Test>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } - collide_Test2(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + collide_Test2>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } - collide_Test2(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + collide_Test2>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } - collide_Test2(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + collide_Test2>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } - - collide_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + + collide_Test_Oriented, MeshCollisionTraversalNodekIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } - collide_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + collide_Test_Oriented, MeshCollisionTraversalNodekIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } - collide_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + collide_Test_Oriented, MeshCollisionTraversalNodekIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } - test_collide_func(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + test_collide_func>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN); + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } - test_collide_func(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + test_collide_func>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN); + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } - test_collide_func(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + test_collide_func>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER); + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } - collide_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + collide_Test>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } - collide_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + collide_Test>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } - collide_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + collide_Test>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } - collide_Test2(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + collide_Test2>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } - collide_Test2(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + collide_Test2>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } - collide_Test2(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + collide_Test2>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } - - collide_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + + collide_Test_Oriented, MeshCollisionTraversalNodeOBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } - collide_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + collide_Test_Oriented, MeshCollisionTraversalNodeOBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } - collide_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + collide_Test_Oriented, MeshCollisionTraversalNodeOBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } - test_collide_func(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + test_collide_func>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN); + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } - test_collide_func(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + test_collide_func>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN); + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } - test_collide_func(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER); - EXPECT_TRUE(global_pairs.size() == global_pairs_now.size()); - for(std::size_t j = 0; j < global_pairs.size(); ++j) + test_collide_func>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER); + EXPECT_TRUE(global_pairs().size() == global_pairs_now().size()); + for(std::size_t j = 0; j < global_pairs().size(); ++j) { - EXPECT_TRUE(global_pairs[j].b1 == global_pairs_now[j].b1); - EXPECT_TRUE(global_pairs[j].b2 == global_pairs_now[j].b2); + EXPECT_TRUE(global_pairs()[j].b1 == global_pairs_now()[j].b1); + EXPECT_TRUE(global_pairs()[j].b2 == global_pairs_now()[j].b2); } } } +GTEST_TEST(FCL_COLLISION, OBB_Box_test) +{ +// test_OBB_Box_test(); + test_OBB_Box_test(); +} + +GTEST_TEST(FCL_COLLISION, OBB_shape_test) +{ +// test_OBB_shape_test(); + test_OBB_shape_test(); +} + +GTEST_TEST(FCL_COLLISION, OBB_AABB_test) +{ +// test_OBB_AABB_test(); + test_OBB_AABB_test(); +} + +GTEST_TEST(FCL_COLLISION, mesh_mesh) +{ +// test_mesh_mesh(); + test_mesh_mesh(); +} template -bool collide_Test2(const Transform3d& tf, - const std::vector& vertices1, const std::vector& triangles1, - const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, bool verbose) +bool collide_Test2(const Transform3& tf, + const std::vector>& vertices1, const std::vector& triangles1, + const std::vector>& vertices2, const std::vector& triangles2, SplitMethodType split_method, bool verbose) { + using S = typename BV::S; + BVHModel m1; BVHModel m2; m1.bv_splitter.reset(new BVSplitter(split_method)); m2.bv_splitter.reset(new BVSplitter(split_method)); - std::vector vertices1_new(vertices1.size()); + std::vector> vertices1_new(vertices1.size()); for(unsigned int i = 0; i < vertices1_new.size(); ++i) { vertices1_new[i] = tf * vertices1[i]; @@ -838,14 +877,14 @@ bool collide_Test2(const Transform3d& tf, m2.addSubModel(vertices2, triangles2); m2.endModel(); - Transform3d pose1 = Transform3d::Identity(); - Transform3d pose2 = Transform3d::Identity(); + Transform3 pose1 = Transform3::Identity(); + Transform3 pose2 = Transform3::Identity(); - CollisionResult local_result; + CollisionResult local_result; MeshCollisionTraversalNode node; if(!initialize(node, m1, pose1, m2, pose2, - CollisionRequest(num_max_contacts, enable_contact), local_result)) + CollisionRequest(num_max_contacts, enable_contact), local_result)) std::cout << "initialize error" << std::endl; node.enable_statistics = verbose; @@ -855,15 +894,15 @@ bool collide_Test2(const Transform3d& tf, if(local_result.numContacts() > 0) { - if(global_pairs.size() == 0) + if(global_pairs().size() == 0) { - local_result.getContacts(global_pairs); - std::sort(global_pairs.begin(), global_pairs.end()); + local_result.getContacts(global_pairs()); + std::sort(global_pairs().begin(), global_pairs().end()); } else { - local_result.getContacts(global_pairs_now); - std::sort(global_pairs_now.begin(), global_pairs_now.end()); + local_result.getContacts(global_pairs_now()); + std::sort(global_pairs_now().begin(), global_pairs_now().end()); } @@ -881,10 +920,12 @@ bool collide_Test2(const Transform3d& tf, } template -bool collide_Test(const Transform3d& tf, - const std::vector& vertices1, const std::vector& triangles1, - const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, bool verbose) +bool collide_Test(const Transform3& tf, + const std::vector>& vertices1, const std::vector& triangles1, + const std::vector>& vertices2, const std::vector& triangles2, SplitMethodType split_method, bool verbose) { + using S = typename BV::S; + BVHModel m1; BVHModel m2; m1.bv_splitter.reset(new BVSplitter(split_method)); @@ -898,14 +939,14 @@ bool collide_Test(const Transform3d& tf, m2.addSubModel(vertices2, triangles2); m2.endModel(); - Transform3d pose1(tf); - Transform3d pose2 = Transform3d::Identity(); + Transform3 pose1(tf); + Transform3 pose2 = Transform3::Identity(); - CollisionResult local_result; + CollisionResult local_result; MeshCollisionTraversalNode node; if(!initialize(node, m1, pose1, m2, pose2, - CollisionRequest(num_max_contacts, enable_contact), local_result)) + CollisionRequest(num_max_contacts, enable_contact), local_result)) std::cout << "initialize error" << std::endl; node.enable_statistics = verbose; @@ -915,15 +956,15 @@ bool collide_Test(const Transform3d& tf, if(local_result.numContacts() > 0) { - if(global_pairs.size() == 0) + if(global_pairs().size() == 0) { - local_result.getContacts(global_pairs); - std::sort(global_pairs.begin(), global_pairs.end()); + local_result.getContacts(global_pairs()); + std::sort(global_pairs().begin(), global_pairs().end()); } else { - local_result.getContacts(global_pairs_now); - std::sort(global_pairs_now.begin(), global_pairs_now.end()); + local_result.getContacts(global_pairs_now()); + std::sort(global_pairs_now().begin(), global_pairs_now().end()); } if(verbose) @@ -940,10 +981,12 @@ bool collide_Test(const Transform3d& tf, } template -bool collide_Test_Oriented(const Transform3d& tf, - const std::vector& vertices1, const std::vector& triangles1, - const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, bool verbose) +bool collide_Test_Oriented(const Transform3& tf, + const std::vector>& vertices1, const std::vector& triangles1, + const std::vector>& vertices2, const std::vector& triangles2, SplitMethodType split_method, bool verbose) { + using S = typename BV::S; + BVHModel m1; BVHModel m2; m1.bv_splitter.reset(new BVSplitter(split_method)); @@ -957,13 +1000,13 @@ bool collide_Test_Oriented(const Transform3d& tf, m2.addSubModel(vertices2, triangles2); m2.endModel(); - Transform3d pose1(tf); - Transform3d pose2 = Transform3d::Identity(); + Transform3 pose1(tf); + Transform3 pose2 = Transform3::Identity(); - CollisionResult local_result; + CollisionResult local_result; TraversalNode node; - if(!initialize(node, (const BVHModel&)m1, pose1, (const BVHModel&)m2, pose2, - CollisionRequest(num_max_contacts, enable_contact), local_result)) + if(!initialize(node, (const BVHModel&)m1, pose1, (const BVHModel&)m2, pose2, + CollisionRequest(num_max_contacts, enable_contact), local_result)) std::cout << "initialize error" << std::endl; node.enable_statistics = verbose; @@ -972,15 +1015,15 @@ bool collide_Test_Oriented(const Transform3d& tf, if(local_result.numContacts() > 0) { - if(global_pairs.size() == 0) + if(global_pairs().size() == 0) { - local_result.getContacts(global_pairs); - std::sort(global_pairs.begin(), global_pairs.end()); + local_result.getContacts(global_pairs()); + std::sort(global_pairs().begin(), global_pairs().end()); } else { - local_result.getContacts(global_pairs_now); - std::sort(global_pairs_now.begin(), global_pairs_now.end()); + local_result.getContacts(global_pairs_now()); + std::sort(global_pairs_now().begin(), global_pairs_now().end()); } if(verbose) @@ -998,10 +1041,12 @@ bool collide_Test_Oriented(const Transform3d& tf, template -bool test_collide_func(const Transform3d& tf, - const std::vector& vertices1, const std::vector& triangles1, - const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method) +bool test_collide_func(const Transform3& tf, + const std::vector>& vertices1, const std::vector& triangles1, + const std::vector>& vertices2, const std::vector& triangles2, SplitMethodType split_method) { + using S = typename BV::S; + BVHModel m1; BVHModel m2; m1.bv_splitter.reset(new BVSplitter(split_method)); @@ -1015,27 +1060,26 @@ bool test_collide_func(const Transform3d& tf, m2.addSubModel(vertices2, triangles2); m2.endModel(); - Transform3d pose1(tf); - Transform3d pose2 = Transform3d::Identity(); + Transform3 pose1(tf); + Transform3 pose2 = Transform3::Identity(); + + std::vector> contacts; - std::vector contacts; + CollisionRequest request(num_max_contacts, enable_contact); + CollisionResult result; + int num_contacts = collide(&m1, pose1, &m2, pose2, request, result); - CollisionRequest request(num_max_contacts, enable_contact); - CollisionResult result; - int num_contacts = collide(&m1, pose1, &m2, pose2, - request, result); - result.getContacts(contacts); - global_pairs_now.resize(num_contacts); + global_pairs_now().resize(num_contacts); for(int i = 0; i < num_contacts; ++i) { - global_pairs_now[i].b1 = contacts[i].b1; - global_pairs_now[i].b2 = contacts[i].b2; + global_pairs_now()[i].b1 = contacts[i].b1; + global_pairs_now()[i].b2 = contacts[i].b2; } - std::sort(global_pairs_now.begin(), global_pairs_now.end()); + std::sort(global_pairs_now().begin(), global_pairs_now().end()); if(num_contacts > 0) return true; else return false; diff --git a/test/test_fcl_distance.cpp b/test/test_fcl_distance.cpp index 93f8cb7e9..37568c982 100644 --- a/test/test_fcl_distance.cpp +++ b/test/test_fcl_distance.cpp @@ -37,8 +37,7 @@ #include -#include "fcl/traversal/traversal_node_bvhs.h" -#include "fcl/traversal/traversal_node_setup.h" +#include "fcl/traversal/traversal_nodes.h" #include "fcl/collision_node.h" #include "test_fcl_utility.h" #include "fcl_resources/config.h" @@ -46,52 +45,55 @@ using namespace fcl; bool verbose = false; -FCL_REAL DELTA = 0.001; +template +S DELTA() { return 0.001; } template -void distance_Test(const Transform3d& tf, - const std::vector& vertices1, const std::vector& triangles1, - const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, +void distance_Test(const Transform3& tf, + const std::vector>& vertices1, const std::vector& triangles1, + const std::vector>& vertices2, const std::vector& triangles2, SplitMethodType split_method, int qsize, - DistanceRes& distance_result, + DistanceRes& distance_result, bool verbose = true); -bool collide_Test_OBB(const Transform3d& tf, - const std::vector& vertices1, const std::vector& triangles1, - const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, bool verbose); - +template +bool collide_Test_OBB(const Transform3& tf, + const std::vector>& vertices1, const std::vector& triangles1, + const std::vector>& vertices2, const std::vector& triangles2, SplitMethodType split_method, bool verbose); template -void distance_Test_Oriented(const Transform3d& tf, - const std::vector& vertices1, const std::vector& triangles1, - const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, +void distance_Test_Oriented(const Transform3& tf, + const std::vector>& vertices1, const std::vector& triangles1, + const std::vector>& vertices2, const std::vector& triangles2, SplitMethodType split_method, int qsize, - DistanceRes& distance_result, + DistanceRes& distance_result, bool verbose = true); -inline bool nearlyEqual(const Vector3d& a, const Vector3d& b) +template +bool nearlyEqual(const Vector3& a, const Vector3& b) { - if(fabs(a[0] - b[0]) > DELTA) return false; - if(fabs(a[1] - b[1]) > DELTA) return false; - if(fabs(a[2] - b[2]) > DELTA) return false; + if(fabs(a[0] - b[0]) > DELTA()) return false; + if(fabs(a[1] - b[1]) > DELTA()) return false; + if(fabs(a[2] - b[2]) > DELTA()) return false; return true; } -GTEST_TEST(FCL_DISTANCE, mesh_distance) +template +void test_mesh_distance() { - std::vector p1, p2; + std::vector> p1, p2; std::vector t1, t2; loadOBJFile(TEST_RESOURCES_DIR"/env.obj", p1, t1); loadOBJFile(TEST_RESOURCES_DIR"/rob.obj", p2, t2); - std::vector transforms; // t0 - FCL_REAL extents[] = {-3000, -3000, 0, 3000, 3000, 3000}; -#if FCL_BUILD_TYPE_DEBUG - std::size_t n = 1; -#else + Eigen::aligned_vector> transforms; // t0 + S extents[] = {-3000, -3000, 0, 3000, 3000, 3000}; +#ifdef NDEBUG std::size_t n = 10; +#else + std::size_t n = 1; #endif generateRandomTransforms(extents, transforms, n); @@ -99,7 +101,7 @@ GTEST_TEST(FCL_DISTANCE, mesh_distance) double dis_time = 0; double col_time = 0; - DistanceRes res, res_now; + DistanceRes res, res_now; for(std::size_t i = 0; i < transforms.size(); ++i) { Timer timer_col; @@ -110,187 +112,187 @@ GTEST_TEST(FCL_DISTANCE, mesh_distance) Timer timer_dist; timer_dist.start(); - distance_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res, verbose); + distance_Test_Oriented, MeshDistanceTraversalNodeRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res, verbose); timer_dist.stop(); dis_time += timer_dist.getElapsedTimeInSec(); - distance_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, res_now, verbose); + distance_Test_Oriented, MeshDistanceTraversalNodeRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, res_now, verbose); + + EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA()); + EXPECT_TRUE(fabs(res.distance) < DELTA() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + + distance_Test_Oriented, MeshDistanceTraversalNodeRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, res_now, verbose); + + EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA()); + EXPECT_TRUE(fabs(res.distance) < DELTA() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + + distance_Test_Oriented, MeshDistanceTraversalNodeRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, res_now, verbose); + + EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA()); + EXPECT_TRUE(fabs(res.distance) < DELTA() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + + distance_Test_Oriented, MeshDistanceTraversalNodeRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 20, res_now, verbose); + + EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA()); + EXPECT_TRUE(fabs(res.distance) < DELTA() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + + distance_Test_Oriented, MeshDistanceTraversalNodeRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, res_now, verbose); + + EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA()); + EXPECT_TRUE(fabs(res.distance) < DELTA() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + + distance_Test_Oriented, MeshDistanceTraversalNodekIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res_now, verbose); - EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA); - EXPECT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA()); + EXPECT_TRUE(fabs(res.distance) < DELTA() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); - distance_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, res_now, verbose); + distance_Test_Oriented, MeshDistanceTraversalNodekIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, res_now, verbose); - EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA); - EXPECT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA()); + EXPECT_TRUE(fabs(res.distance) < DELTA() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); - distance_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, res_now, verbose); + distance_Test_Oriented, MeshDistanceTraversalNodekIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, res_now, verbose); - EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA); - EXPECT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA()); + EXPECT_TRUE(fabs(res.distance) < DELTA() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); - distance_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 20, res_now, verbose); + distance_Test_Oriented, MeshDistanceTraversalNodekIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, res_now, verbose); - EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA); - EXPECT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA()); + EXPECT_TRUE(fabs(res.distance) < DELTA() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); - distance_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, res_now, verbose); + distance_Test_Oriented, MeshDistanceTraversalNodekIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, res_now, verbose); - EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA); - EXPECT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); - - distance_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res_now, verbose); + EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA()); + EXPECT_TRUE(fabs(res.distance) < DELTA() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); - EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA); - EXPECT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); - - distance_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, res_now, verbose); - - EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA); - EXPECT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + distance_Test_Oriented, MeshDistanceTraversalNodekIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 20, res_now, verbose); - distance_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, res_now, verbose); - - EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA); - EXPECT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA()); + EXPECT_TRUE(fabs(res.distance) < DELTA() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); - distance_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, res_now, verbose); - - EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA); - EXPECT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + distance_Test_Oriented, MeshDistanceTraversalNodeOBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res_now, verbose); - distance_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, res_now, verbose); - - EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA); - EXPECT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA()); + EXPECT_TRUE(fabs(res.distance) < DELTA() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); - distance_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 20, res_now, verbose); - - EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA); - EXPECT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + distance_Test_Oriented, MeshDistanceTraversalNodeOBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, res_now, verbose); - distance_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res_now, verbose); + EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA()); + EXPECT_TRUE(fabs(res.distance) < DELTA() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); - EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA); - EXPECT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); - - distance_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, res_now, verbose); - - EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA); - EXPECT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + distance_Test_Oriented, MeshDistanceTraversalNodeOBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, res_now, verbose); - distance_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, res_now, verbose); - - EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA); - EXPECT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA()); + EXPECT_TRUE(fabs(res.distance) < DELTA() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); - distance_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, res_now, verbose); - - EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA); - EXPECT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + distance_Test_Oriented, MeshDistanceTraversalNodeOBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, res_now, verbose); - distance_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, res_now, verbose); - - EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA); - EXPECT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA()); + EXPECT_TRUE(fabs(res.distance) < DELTA() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); - distance_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 20, res_now, verbose); - - EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA); - EXPECT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + distance_Test_Oriented, MeshDistanceTraversalNodeOBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, res_now, verbose); + EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA()); + EXPECT_TRUE(fabs(res.distance) < DELTA() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + distance_Test_Oriented, MeshDistanceTraversalNodeOBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 20, res_now, verbose); - distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res_now, verbose); + EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA()); + EXPECT_TRUE(fabs(res.distance) < DELTA() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); - EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA); - EXPECT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); - distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, res_now, verbose); - EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA); - EXPECT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + distance_Test>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res_now, verbose); - distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, res_now, verbose); + EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA()); + EXPECT_TRUE(fabs(res.distance) < DELTA() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); - EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA); - EXPECT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + distance_Test>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, res_now, verbose); - distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, res_now, verbose); + EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA()); + EXPECT_TRUE(fabs(res.distance) < DELTA() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); - EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA); - EXPECT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + distance_Test>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, res_now, verbose); - distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 20, res_now, verbose); + EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA()); + EXPECT_TRUE(fabs(res.distance) < DELTA() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); - EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA); - EXPECT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + distance_Test>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, res_now, verbose); - distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, res_now, verbose); + EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA()); + EXPECT_TRUE(fabs(res.distance) < DELTA() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); - EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA); - EXPECT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); - - distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res_now, verbose); + distance_Test>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 20, res_now, verbose); - EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA); - EXPECT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); - - distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, res_now, verbose); + EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA()); + EXPECT_TRUE(fabs(res.distance) < DELTA() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); - EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA); - EXPECT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); - - distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, res_now, verbose); + distance_Test>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, res_now, verbose); - EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA); - EXPECT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA()); + EXPECT_TRUE(fabs(res.distance) < DELTA() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); - distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, res_now, verbose); + distance_Test>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res_now, verbose); - EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA); - EXPECT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA()); + EXPECT_TRUE(fabs(res.distance) < DELTA() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); - distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, res_now, verbose); + distance_Test>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, res_now, verbose); - EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA); - EXPECT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA()); + EXPECT_TRUE(fabs(res.distance) < DELTA() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); - distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 20, res_now, verbose); + distance_Test>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, res_now, verbose); - EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA); - EXPECT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA()); + EXPECT_TRUE(fabs(res.distance) < DELTA() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + distance_Test>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, res_now, verbose); - distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res_now, verbose); + EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA()); + EXPECT_TRUE(fabs(res.distance) < DELTA() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); - EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA); - EXPECT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); - - distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, res_now, verbose); + distance_Test>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, res_now, verbose); - EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA); - EXPECT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); - - distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, res_now, verbose); + EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA()); + EXPECT_TRUE(fabs(res.distance) < DELTA() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); - EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA); - EXPECT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + distance_Test>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 20, res_now, verbose); - distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, res_now, verbose); + EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA()); + EXPECT_TRUE(fabs(res.distance) < DELTA() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); - EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA); - EXPECT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); - distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, res_now, verbose); + distance_Test>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res_now, verbose); - EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA); - EXPECT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA()); + EXPECT_TRUE(fabs(res.distance) < DELTA() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); - distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 20, res_now, verbose); + distance_Test>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, res_now, verbose); - EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA); - EXPECT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA()); + EXPECT_TRUE(fabs(res.distance) < DELTA() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + + distance_Test>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, res_now, verbose); + + EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA()); + EXPECT_TRUE(fabs(res.distance) < DELTA() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + + distance_Test>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, res_now, verbose); + + EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA()); + EXPECT_TRUE(fabs(res.distance) < DELTA() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + + distance_Test>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, res_now, verbose); + + EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA()); + EXPECT_TRUE(fabs(res.distance) < DELTA() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); + + distance_Test>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 20, res_now, verbose); + + EXPECT_TRUE(fabs(res.distance - res_now.distance) < DELTA()); + EXPECT_TRUE(fabs(res.distance) < DELTA() || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); } @@ -298,14 +300,22 @@ GTEST_TEST(FCL_DISTANCE, mesh_distance) std::cout << "collision timing: " << col_time << " sec" << std::endl; } +GTEST_TEST(FCL_DISTANCE, mesh_distance) +{ +// test_mesh_distance(); + test_mesh_distance(); +} + template -void distance_Test_Oriented(const Transform3d& tf, - const std::vector& vertices1, const std::vector& triangles1, - const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, +void distance_Test_Oriented(const Transform3& tf, + const std::vector>& vertices1, const std::vector& triangles1, + const std::vector>& vertices2, const std::vector& triangles2, SplitMethodType split_method, int qsize, - DistanceRes& distance_result, + DistanceRes& distance_result, bool verbose) { + using S = typename BV::S; + BVHModel m1; BVHModel m2; m1.bv_splitter.reset(new BVSplitter(split_method)); @@ -320,18 +330,18 @@ void distance_Test_Oriented(const Transform3d& tf, m2.addSubModel(vertices2, triangles2); m2.endModel(); - DistanceResult local_result; + DistanceResult local_result; TraversalNode node; - if(!initialize(node, (const BVHModel&)m1, tf, (const BVHModel&)m2, Transform3d::Identity(), DistanceRequest(true), local_result)) + if(!initialize(node, (const BVHModel&)m1, tf, (const BVHModel&)m2, Transform3::Identity(), DistanceRequest(true), local_result)) std::cout << "initialize error" << std::endl; node.enable_statistics = verbose; - distance(&node, NULL, qsize); + distance(&node, nullptr, qsize); // points are in local coordinate, to global coordinate - Vector3d p1 = local_result.nearest_points[0]; - Vector3d p2 = local_result.nearest_points[1]; + Vector3 p1 = local_result.nearest_points[0]; + Vector3 p2 = local_result.nearest_points[1]; distance_result.distance = local_result.min_distance; @@ -349,13 +359,15 @@ void distance_Test_Oriented(const Transform3d& tf, } template -void distance_Test(const Transform3d& tf, - const std::vector& vertices1, const std::vector& triangles1, - const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, +void distance_Test(const Transform3& tf, + const std::vector>& vertices1, const std::vector& triangles1, + const std::vector>& vertices2, const std::vector& triangles2, SplitMethodType split_method, int qsize, - DistanceRes& distance_result, + DistanceRes& distance_result, bool verbose) { + using S = typename BV::S; + BVHModel m1; BVHModel m2; m1.bv_splitter.reset(new BVSplitter(split_method)); @@ -370,18 +382,18 @@ void distance_Test(const Transform3d& tf, m2.addSubModel(vertices2, triangles2); m2.endModel(); - Transform3d pose1(tf); - Transform3d pose2 = Transform3d::Identity(); + Transform3 pose1(tf); + Transform3 pose2 = Transform3::Identity(); - DistanceResult local_result; + DistanceResult local_result; MeshDistanceTraversalNode node; - if(!initialize(node, m1, pose1, m2, pose2, DistanceRequest(true), local_result)) + if(!initialize(node, m1, pose1, m2, pose2, DistanceRequest(true), local_result)) std::cout << "initialize error" << std::endl; node.enable_statistics = verbose; - distance(&node, NULL, qsize); + distance(&node, nullptr, qsize); distance_result.distance = local_result.min_distance; distance_result.p1 = local_result.nearest_points[0]; @@ -397,15 +409,15 @@ void distance_Test(const Transform3d& tf, } } - -bool collide_Test_OBB(const Transform3d& tf, - const std::vector& vertices1, const std::vector& triangles1, - const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, bool verbose) +template +bool collide_Test_OBB(const Transform3& tf, + const std::vector>& vertices1, const std::vector& triangles1, + const std::vector>& vertices2, const std::vector& triangles2, SplitMethodType split_method, bool verbose) { - BVHModel m1; - BVHModel m2; - m1.bv_splitter.reset(new BVSplitter(split_method)); - m2.bv_splitter.reset(new BVSplitter(split_method)); + BVHModel> m1; + BVHModel> m2; + m1.bv_splitter.reset(new BVSplitter>(split_method)); + m2.bv_splitter.reset(new BVSplitter>(split_method)); m1.beginModel(); m1.addSubModel(vertices1, triangles1); @@ -415,10 +427,10 @@ bool collide_Test_OBB(const Transform3d& tf, m2.addSubModel(vertices2, triangles2); m2.endModel(); - CollisionResult local_result; - MeshCollisionTraversalNodeOBB node; - if(!initialize(node, (const BVHModel&)m1, tf, (const BVHModel&)m2, Transform3d::Identity(), - CollisionRequest(), local_result)) + CollisionResult local_result; + MeshCollisionTraversalNodeOBB node; + if(!initialize(node, (const BVHModel>&)m1, tf, (const BVHModel>&)m2, Transform3::Identity(), + CollisionRequest(), local_result)) std::cout << "initialize error" << std::endl; node.enable_statistics = verbose; diff --git a/test/test_fcl_frontlist.cpp b/test/test_fcl_frontlist.cpp index 6e51e5c7b..ea59ad978 100644 --- a/test/test_fcl_frontlist.cpp +++ b/test/test_fcl_frontlist.cpp @@ -37,8 +37,6 @@ #include -#include "fcl/traversal/traversal_node_bvhs.h" -#include "fcl/traversal/traversal_node_setup.h" #include "fcl/collision_node.h" #include "test_fcl_utility.h" @@ -47,162 +45,171 @@ using namespace fcl; template -bool collide_front_list_Test(const Transform3d& tf1, const Transform3d& tf2, - const std::vector& vertices1, const std::vector& triangles1, - const std::vector& vertices2, const std::vector& triangles2, +bool collide_front_list_Test(const Transform3& tf1, const Transform3& tf2, + const std::vector>& vertices1, const std::vector& triangles1, + const std::vector>& vertices2, const std::vector& triangles2, SplitMethodType split_method, bool refit_bottomup, bool verbose); template -bool collide_front_list_Test_Oriented(const Transform3d& tf1, const Transform3d& tf2, - const std::vector& vertices1, const std::vector& triangles1, - const std::vector& vertices2, const std::vector& triangles2, +bool collide_front_list_Test_Oriented(const Transform3& tf1, const Transform3& tf2, + const std::vector>& vertices1, const std::vector& triangles1, + const std::vector>& vertices2, const std::vector& triangles2, SplitMethodType split_method, bool verbose); template -bool collide_Test(const Transform3d& tf, - const std::vector& vertices1, const std::vector& triangles1, - const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, bool verbose); +bool collide_Test(const Transform3& tf, + const std::vector>& vertices1, const std::vector& triangles1, + const std::vector>& vertices2, const std::vector& triangles2, SplitMethodType split_method, bool verbose); // TODO: randomly still have some runtime error -GTEST_TEST(FCL_FRONT_LIST, front_list) +template +void test_front_list() { - std::vector p1, p2; + std::vector> p1, p2; std::vector t1, t2; loadOBJFile(TEST_RESOURCES_DIR"/env.obj", p1, t1); loadOBJFile(TEST_RESOURCES_DIR"/rob.obj", p2, t2); - std::vector transforms; // t0 - std::vector transforms2; // t1 - FCL_REAL extents[] = {-3000, -3000, 0, 3000, 3000, 3000}; - FCL_REAL delta_trans[] = {1, 1, 1}; -#if FCL_BUILD_TYPE_DEBUG - std::size_t n = 1; -#else + Eigen::aligned_vector> transforms; // t0 + Eigen::aligned_vector> transforms2; // t1 + S extents[] = {-3000, -3000, 0, 3000, 3000, 3000}; + S delta_trans[] = {1, 1, 1}; +#ifdef NDEBUG std::size_t n = 10; +#else + std::size_t n = 1; #endif bool verbose = false; - generateRandomTransforms(extents, delta_trans, 0.005 * 2 * 3.1415, transforms, transforms2, n); + generateRandomTransforms(extents, delta_trans, 0.005 * 2 * 3.1415, transforms, transforms2, n); bool res, res2; for(std::size_t i = 0; i < transforms.size(); ++i) { - res = collide_Test(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); - res2 = collide_front_list_Test(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, false, verbose); + res = collide_Test>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); + res2 = collide_front_list_Test>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, false, verbose); EXPECT_TRUE(res == res2); - res = collide_Test(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); - res2 = collide_front_list_Test(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, false, verbose); + res = collide_Test>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); + res2 = collide_front_list_Test>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, false, verbose); EXPECT_TRUE(res == res2); - res = collide_Test(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); - res2 = collide_front_list_Test(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, false, verbose); + res = collide_Test>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); + res2 = collide_front_list_Test>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, false, verbose); EXPECT_TRUE(res == res2); } for(std::size_t i = 0; i < transforms.size(); ++i) { - res = collide_Test(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); - res2 = collide_front_list_Test(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, false, verbose); + res = collide_Test>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); + res2 = collide_front_list_Test>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, false, verbose); EXPECT_TRUE(res == res2); - res = collide_Test(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); - res2 = collide_front_list_Test(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, false, verbose); + res = collide_Test>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); + res2 = collide_front_list_Test>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, false, verbose); EXPECT_TRUE(res == res2); - res = collide_Test(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); - res2 = collide_front_list_Test(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, false, verbose); + res = collide_Test>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); + res2 = collide_front_list_Test>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, false, verbose); EXPECT_TRUE(res == res2); } for(std::size_t i = 0; i < transforms.size(); ++i) { // Disabled broken test lines. Please see #25. - // res = collide_Test(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); - // res2 = collide_front_list_Test(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, false, verbose); + // res = collide_Test>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); + // res2 = collide_front_list_Test>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, false, verbose); // EXPECT_TRUE(res == res2); - res = collide_Test(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); - res2 = collide_front_list_Test(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, false, verbose); + res = collide_Test>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); + res2 = collide_front_list_Test>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, false, verbose); EXPECT_TRUE(res == res2); - res = collide_Test(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); - res2 = collide_front_list_Test(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, false, verbose); + res = collide_Test>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); + res2 = collide_front_list_Test>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, false, verbose); EXPECT_TRUE(res == res2); } for(std::size_t i = 0; i < transforms.size(); ++i) { - res = collide_Test >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); - res2 = collide_front_list_Test >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, false, verbose); + res = collide_Test >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); + res2 = collide_front_list_Test >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, false, verbose); EXPECT_TRUE(res == res2); - res = collide_Test >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); - res2 = collide_front_list_Test >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, false, verbose); + res = collide_Test >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); + res2 = collide_front_list_Test >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, false, verbose); EXPECT_TRUE(res == res2); - res = collide_Test >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); - res2 = collide_front_list_Test >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, false, verbose); + res = collide_Test >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); + res2 = collide_front_list_Test >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, false, verbose); EXPECT_TRUE(res == res2); } for(std::size_t i = 0; i < transforms.size(); ++i) { - res = collide_Test >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); - res2 = collide_front_list_Test >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, false, verbose); + res = collide_Test >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); + res2 = collide_front_list_Test >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, false, verbose); EXPECT_TRUE(res == res2); - res = collide_Test >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); - res2 = collide_front_list_Test >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, false, verbose); + res = collide_Test >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); + res2 = collide_front_list_Test >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, false, verbose); EXPECT_TRUE(res == res2); - res = collide_Test >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); - res2 = collide_front_list_Test >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, false, verbose); + res = collide_Test >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); + res2 = collide_front_list_Test >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, false, verbose); EXPECT_TRUE(res == res2); } for(std::size_t i = 0; i < transforms.size(); ++i) { - res = collide_Test >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); - res2 = collide_front_list_Test >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, false, verbose); + res = collide_Test >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); + res2 = collide_front_list_Test >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, false, verbose); EXPECT_TRUE(res == res2); - res = collide_Test >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); - res2 = collide_front_list_Test >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, false, verbose); + res = collide_Test >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); + res2 = collide_front_list_Test >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, false, verbose); EXPECT_TRUE(res == res2); - res = collide_Test >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); - res2 = collide_front_list_Test >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, false, verbose); + res = collide_Test >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); + res2 = collide_front_list_Test >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, false, verbose); EXPECT_TRUE(res == res2); } for(std::size_t i = 0; i < transforms.size(); ++i) { - res = collide_Test(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); - res2 = collide_front_list_Test_Oriented(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); + res = collide_Test>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); + res2 = collide_front_list_Test_Oriented, MeshCollisionTraversalNodeRSS>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); EXPECT_TRUE(res == res2); - res = collide_Test(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); - res2 = collide_front_list_Test_Oriented(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); + res = collide_Test>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); + res2 = collide_front_list_Test_Oriented, MeshCollisionTraversalNodeRSS>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); EXPECT_TRUE(res == res2); - res = collide_Test(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); - res2 = collide_front_list_Test_Oriented(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); + res = collide_Test>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); + res2 = collide_front_list_Test_Oriented, MeshCollisionTraversalNodeRSS>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); EXPECT_TRUE(res == res2); } for(std::size_t i = 0; i < transforms.size(); ++i) { - res = collide_Test(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); - res2 = collide_front_list_Test_Oriented(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); + res = collide_Test>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); + res2 = collide_front_list_Test_Oriented, MeshCollisionTraversalNodeOBB>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); EXPECT_TRUE(res == res2); - res = collide_Test(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); - res2 = collide_front_list_Test_Oriented(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); + res = collide_Test>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); + res2 = collide_front_list_Test_Oriented, MeshCollisionTraversalNodeOBB>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); EXPECT_TRUE(res == res2); - res = collide_Test(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); - res2 = collide_front_list_Test_Oriented(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); + res = collide_Test>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); + res2 = collide_front_list_Test_Oriented, MeshCollisionTraversalNodeOBB>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); EXPECT_TRUE(res == res2); } } +GTEST_TEST(FCL_FRONT_LIST, front_list) +{ +// test_front_list(); + test_front_list(); +} + template -bool collide_front_list_Test(const Transform3d& tf1, const Transform3d& tf2, - const std::vector& vertices1, const std::vector& triangles1, - const std::vector& vertices2, const std::vector& triangles2, +bool collide_front_list_Test(const Transform3& tf1, const Transform3& tf2, + const std::vector>& vertices1, const std::vector& triangles1, + const std::vector>& vertices2, const std::vector& triangles2, SplitMethodType split_method, bool refit_bottomup, bool verbose) { + using S = typename BV::S; + BVHModel m1; BVHModel m2; m1.bv_splitter.reset(new BVSplitter(split_method)); @@ -211,7 +218,7 @@ bool collide_front_list_Test(const Transform3d& tf1, const Transform3d& tf2, BVHFrontList front_list; - std::vector vertices1_new(vertices1.size()); + std::vector> vertices1_new(vertices1.size()); for(std::size_t i = 0; i < vertices1_new.size(); ++i) { vertices1_new[i] = tf1 * vertices1[i]; @@ -225,14 +232,14 @@ bool collide_front_list_Test(const Transform3d& tf1, const Transform3d& tf2, m2.addSubModel(vertices2, triangles2); m2.endModel(); - Transform3d pose1 = Transform3d::Identity(); - Transform3d pose2 = Transform3d::Identity(); + Transform3 pose1 = Transform3::Identity(); + Transform3 pose2 = Transform3::Identity(); - CollisionResult local_result; + CollisionResult local_result; MeshCollisionTraversalNode node; if(!initialize(node, m1, pose1, m2, pose2, - CollisionRequest(std::numeric_limits::max(), false), local_result)) + CollisionRequest(std::numeric_limits::max(), false), local_result)) std::cout << "initialize error" << std::endl; node.enable_statistics = verbose; @@ -269,11 +276,13 @@ bool collide_front_list_Test(const Transform3d& tf1, const Transform3d& tf2, template -bool collide_front_list_Test_Oriented(const Transform3d& tf1, const Transform3d& tf2, - const std::vector& vertices1, const std::vector& triangles1, - const std::vector& vertices2, const std::vector& triangles2, +bool collide_front_list_Test_Oriented(const Transform3& tf1, const Transform3& tf2, + const std::vector>& vertices1, const std::vector& triangles1, + const std::vector>& vertices2, const std::vector& triangles2, SplitMethodType split_method, bool verbose) { + using S = typename BV::S; + BVHModel m1; BVHModel m2; m1.bv_splitter.reset(new BVSplitter(split_method)); @@ -289,14 +298,14 @@ bool collide_front_list_Test_Oriented(const Transform3d& tf1, const Transform3d& m2.addSubModel(vertices2, triangles2); m2.endModel(); - Transform3d pose1(tf1); - Transform3d pose2 = Transform3d::Identity(); + Transform3 pose1(tf1); + Transform3 pose2 = Transform3::Identity(); - CollisionResult local_result; + CollisionResult local_result; TraversalNode node; if(!initialize(node, (const BVHModel&)m1, pose1, (const BVHModel&)m2, pose2, - CollisionRequest(std::numeric_limits::max(), false), local_result)) + CollisionRequest(std::numeric_limits::max(), false), local_result)) std::cout << "initialize error" << std::endl; node.enable_statistics = verbose; @@ -308,7 +317,7 @@ bool collide_front_list_Test_Oriented(const Transform3d& tf1, const Transform3d& // update the mesh pose1 = tf2; - if(!initialize(node, (const BVHModel&)m1, pose1, (const BVHModel&)m2, pose2, CollisionRequest(), local_result)) + if(!initialize(node, (const BVHModel&)m1, pose1, (const BVHModel&)m2, pose2, CollisionRequest(), local_result)) std::cout << "initialize error" << std::endl; local_result.clear(); @@ -322,10 +331,12 @@ bool collide_front_list_Test_Oriented(const Transform3d& tf1, const Transform3d& template -bool collide_Test(const Transform3d& tf, - const std::vector& vertices1, const std::vector& triangles1, - const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, bool verbose) +bool collide_Test(const Transform3& tf, + const std::vector>& vertices1, const std::vector& triangles1, + const std::vector>& vertices2, const std::vector& triangles2, SplitMethodType split_method, bool verbose) { + using S = typename BV::S; + BVHModel m1; BVHModel m2; m1.bv_splitter.reset(new BVSplitter(split_method)); @@ -339,14 +350,14 @@ bool collide_Test(const Transform3d& tf, m2.addSubModel(vertices2, triangles2); m2.endModel(); - Transform3d pose1(tf); - Transform3d pose2 = Transform3d::Identity(); + Transform3 pose1(tf); + Transform3 pose2 = Transform3::Identity(); - CollisionResult local_result; + CollisionResult local_result; MeshCollisionTraversalNode node; if(!initialize(node, m1, pose1, m2, pose2, - CollisionRequest(std::numeric_limits::max(), false), local_result)) + CollisionRequest(std::numeric_limits::max(), false), local_result)) std::cout << "initialize error" << std::endl; node.enable_statistics = verbose; diff --git a/test/test_fcl_geometric_shapes.cpp b/test/test_fcl_geometric_shapes.cpp index 6c52e98b5..187adabf2 100755 --- a/test/test_fcl_geometric_shapes.cpp +++ b/test/test_fcl_geometric_shapes.cpp @@ -35,9 +35,12 @@ /** \author Jia Pan */ +#include + #include -#include "fcl/narrowphase/narrowphase.h" +#include "fcl/narrowphase/gjk_solver_indep.h" +#include "fcl/narrowphase/gjk_solver_libccd.h" #include "fcl/collision.h" #include "test_fcl_utility.h" #include "fcl/ccd/motion.h" @@ -46,38 +49,68 @@ using namespace fcl; -FCL_REAL extents [6] = {0, 0, 0, 10, 10, 10}; +template +std::array& extents() +{ + static std::array static_extents{ {0, 0, 0, 10, 10, 10} }; + return static_extents; +} -GJKSolver_libccd solver1; -GJKSolver_indep solver2; +template +GJKSolver_libccd& solver1() +{ + static GJKSolver_libccd static_solver1; + return static_solver1; +} -#define EXPECT_TRUE_FALSE(p) EXPECT_TRUE(!(p)) +template +GJKSolver_indep& solver2() +{ + static GJKSolver_indep static_solver2; + return static_solver2; +} -GTEST_TEST(FCL_GEOMETRIC_SHAPES, sphere_shape) +template +S tolerance(); + +template <> +float tolerance() { return 1e-4; } + +template <> +double tolerance() { return 1e-12; } + +template +void test_sphere_shape() { - const double tol = 1e-12; - const double radius = 5.0; - const double pi = constants::pi; + const S radius = 5.0; + const S pi = constants::pi(); - Sphere s(radius); + Sphere s(radius); - const double volume = 4.0 / 3.0 * pi * radius * radius * radius; - EXPECT_NEAR(volume, s.computeVolume(), tol); + const auto volume = 4.0 / 3.0 * pi * radius * radius * radius; + EXPECT_NEAR(volume, s.computeVolume(), tolerance()); } -GTEST_TEST(FCL_GEOMETRIC_SHAPES, gjkcache) +GTEST_TEST(FCL_GEOMETRIC_SHAPES, sphere_shape) { - Cylinder s1(5, 10); - Cone s2(5, 10); +// test_sphere_shape(); + test_sphere_shape(); +} + +template +void test_gjkcache() +{ + Cylinder s1(5, 10); + Cone s2(5, 10); - CollisionRequest request; + CollisionRequest request; request.enable_cached_gjk_guess = true; request.gjk_solver_type = GST_INDEP; - TranslationMotion motion(Transform3d(Eigen::Translation3d(Vector3d(-20.0, -20.0, -20.0))), Transform3d(Eigen::Translation3d(Vector3d(20.0, 20.0, 20.0)))); + TranslationMotion motion(Transform3(Translation3(Vector3(-20.0, -20.0, -20.0))), Transform3(Translation3(Vector3(20.0, 20.0, 20.0)))); int N = 1000; - FCL_REAL dt = 1.0 / (N - 1); + S dt = 1.0 / (N - 1); /// test exploiting spatial coherence Timer timer1; @@ -86,12 +119,12 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, gjkcache) for(int i = 0; i < N; ++i) { motion.integrate(dt * i); - Transform3d tf; + Transform3 tf; motion.getCurrentTransform(tf); - CollisionResult result; + CollisionResult result; - collide(&s1, Transform3d::Identity(), &s2, tf, request, result); + collide(&s1, Transform3::Identity(), &s2, tf, request, result); result1[i] = result.isCollision(); request.cached_gjk_guess = result.cached_gjk_guess; // use cached guess } @@ -106,12 +139,12 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, gjkcache) for(int i = 0; i < N; ++i) { motion.integrate(dt * i); - Transform3d tf; + Transform3 tf; motion.getCurrentTransform(tf); - CollisionResult result; + CollisionResult result; - collide(&s1, Transform3d::Identity(), &s2, tf, request, result); + collide(&s1, Transform3::Identity(), &s2, tf, request, result); result2[i] = result.isCollision(); } @@ -125,25 +158,31 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, gjkcache) } } -template +GTEST_TEST(FCL_GEOMETRIC_SHAPES, gjkcache) +{ +// test_gjkcache(); + test_gjkcache(); +} + +template void printComparisonError(const std::string& comparison_type, - const S1& s1, const Transform3d& tf1, - const S2& s2, const Transform3d& tf2, + const Shape1& s1, const Transform3& tf1, + const Shape2& s2, const Transform3& tf2, GJKSolverType solver_type, - const Vector3d& expected_contact_or_normal, - const Vector3d& actual_contact_or_normal, + const Vector3& expected_contact_or_normal, + const Vector3& actual_contact_or_normal, bool check_opposite_normal, - FCL_REAL tol) + typename Shape1::S tol) { std::cout << "Disagreement between " << comparison_type << " and expected_" << comparison_type << " for " << getNodeTypeName(s1.getNodeType()) << " and " << getNodeTypeName(s2.getNodeType()) << " with '" << getGJKSolverName(solver_type) << "' solver." << std::endl - << "tf1.linear: " << tf1.linear() << std::endl - << "tf1.translation: " << tf1.translation() << std::endl - << "tf2.linear: " << tf2.linear() << std::endl - << "tf2.translation: " << tf2.translation() << std::endl + << "tf1.linear: \n" << tf1.linear() << std::endl + << "tf1.translation: " << tf1.translation().transpose() << std::endl + << "tf2.linear: \n" << tf2.linear() << std::endl + << "tf2.translation: " << tf2.translation().transpose() << std::endl << "expected_" << comparison_type << ": " << expected_contact_or_normal << "actual_" << comparison_type << " : " << actual_contact_or_normal << std::endl; @@ -155,40 +194,40 @@ void printComparisonError(const std::string& comparison_type, << "tolerance: " << tol << std::endl; } -template +template void printComparisonError(const std::string& comparison_type, - const S1& s1, const Transform3d& tf1, - const S2& s2, const Transform3d& tf2, + const Shape1& s1, const Transform3& tf1, + const Shape2& s2, const Transform3& tf2, GJKSolverType solver_type, - FCL_REAL expected_depth, - FCL_REAL actual_depth, - FCL_REAL tol) + typename Shape1::S expected_depth, + typename Shape1::S actual_depth, + typename Shape1::S tol) { std::cout << "Disagreement between " << comparison_type << " and expected_" << comparison_type << " for " << getNodeTypeName(s1.getNodeType()) << " and " << getNodeTypeName(s2.getNodeType()) << " with '" << getGJKSolverName(solver_type) << "' solver." << std::endl - << "tf1.linear: " << tf1.linear() << std::endl - << "tf1.translation: " << tf1.translation() << std::endl - << "tf2.linear: " << tf2.linear() << std::endl - << "tf2.translation: " << tf2.translation() << std::endl + << "tf1.linear: \n" << tf1.linear() << std::endl + << "tf1.translation: " << tf1.translation().transpose() << std::endl + << "tf2.linear: \n" << tf2.linear() << std::endl + << "tf2.translation: " << tf2.translation().transpose() << std::endl << "expected_depth: " << expected_depth << std::endl << "actual_depth : " << actual_depth << std::endl << "difference: " << std::abs(actual_depth - expected_depth) << std::endl << "tolerance: " << tol << std::endl; } -template -bool checkContactPoints(const S1& s1, const Transform3d& tf1, - const S2& s2, const Transform3d& tf2, +template +bool checkContactPointds(const Shape1& s1, const Transform3& tf1, + const Shape2& s2, const Transform3& tf2, GJKSolverType solver_type, - const ContactPoint& expected, const ContactPoint& actual, + const ContactPoint& expected, const ContactPoint& actual, bool check_position = false, bool check_depth = false, bool check_normal = false, bool check_opposite_normal = false, - FCL_REAL tol = 1e-9) + typename Shape1::S tol = 1e-9) { if (check_position) { @@ -218,18 +257,20 @@ bool checkContactPoints(const S1& s1, const Transform3d& tf1, return true; } -template -bool inspectContactPoints(const S1& s1, const Transform3d& tf1, - const S2& s2, const Transform3d& tf2, +template +bool inspectContactPointds(const Shape1& s1, const Transform3& tf1, + const Shape2& s2, const Transform3& tf2, GJKSolverType solver_type, - const std::vector& expected_contacts, - const std::vector& actual_contacts, + const std::vector>& expected_contacts, + const std::vector>& actual_contacts, bool check_position = false, bool check_depth = false, bool check_normal = false, bool check_opposite_normal = false, - FCL_REAL tol = 1e-9) + typename Shape1::S tol = 1e-9) { + using S = typename Shape1::S; + // Check number of contact points bool sameNumContacts = (actual_contacts.size() == expected_contacts.size()); EXPECT_TRUE(sameNumContacts); @@ -242,13 +283,13 @@ bool inspectContactPoints(const S1& s1, const Transform3d& tf1, << "\n" << "[ Shape 1 ]\n" << "Shape type : " << getNodeTypeName(s1.getNodeType()) << "\n" - << "tf1.linear : " << tf1.linear() << "\n" - << "tf1.translation: " << tf1.translation() << "\n" + << "tf1.linear : \n" << tf1.linear() << "\n" + << "tf1.translation: " << tf1.translation().transpose() << "\n" << "\n" << "[ Shape 2 ]\n" << "Shape type : " << getNodeTypeName(s2.getNodeType()) << "\n" - << "tf2.linear : " << tf2.linear() << "\n" - << "tf2.translation: " << tf2.translation() << "\n" + << "tf2.linear : \n" << tf2.linear() << "\n" + << "tf2.translation: " << tf2.translation().transpose() << "\n" << "\n" << "The numbers of expected contacts '" << expected_contacts.size() @@ -268,7 +309,7 @@ bool inspectContactPoints(const S1& s1, const Transform3d& tf1, bool foundAll = true; for (size_t i = 0; i < numContacts; ++i) { - const ContactPoint& expected = expected_contacts[i]; + const ContactPoint& expected = expected_contacts[i]; // Check if expected contact is in the list of actual contacts for (size_t j = 0; j < numContacts; ++j) @@ -276,9 +317,9 @@ bool inspectContactPoints(const S1& s1, const Transform3d& tf1, if (index_to_expected_contacts[j] != -1) continue; - const ContactPoint& actual = actual_contacts[j]; + const ContactPoint& actual = actual_contacts[j]; - bool found = checkContactPoints( + bool found = checkContactPointds( s1, tf1, s2, tf2, solver_type, expected, actual, check_position, @@ -307,21 +348,21 @@ bool inspectContactPoints(const S1& s1, const Transform3d& tf1, << "\n" << "[ Shape 1 ]\n" << "Shape type : " << getNodeTypeName(s1.getNodeType()) << "\n" - << "tf1.linear : " << tf1.linear() << "\n" - << "tf1.translation: " << tf1.translation() << "\n" + << "tf1.linear : \n" << tf1.linear() << "\n" + << "tf1.translation: " << tf1.translation().transpose() << "\n" << "\n" << "[ Shape 2 ]\n" << "Shape type : " << getNodeTypeName(s2.getNodeType()) << "\n" - << "tf2.linear : " << tf2.linear() << "\n" - << "tf2.translation: " << tf2.translation() << "\n" + << "tf2.linear : \n" << tf2.linear() << "\n" + << "tf2.translation: " << tf2.translation().transpose() << "\n" << "\n" << "[ Expected Contacts: " << numContacts << " ]\n"; for (size_t i = 0; i < numContacts; ++i) { - const ContactPoint& expected = expected_contacts[i]; + const ContactPoint& expected = expected_contacts[i]; - std::cout << "(" << i << ") pos: " << expected.pos << ", " - << "normal: " << expected.normal << ", " + std::cout << "(" << i << ") pos: " << expected.pos.transpose() << ", " + << "normal: " << expected.normal.transpose() << ", " << "depth: " << expected.penetration_depth << " ---- "; if (index_to_actual_contacts[i] != -1) @@ -333,10 +374,10 @@ bool inspectContactPoints(const S1& s1, const Transform3d& tf1, << "[ Actual Contacts: " << numContacts << " ]\n"; for (size_t i = 0; i < numContacts; ++i) { - const ContactPoint& actual = actual_contacts[i]; + const ContactPoint& actual = actual_contacts[i]; - std::cout << "(" << i << ") pos: " << actual.pos << ", " - << "normal: " << actual.normal << ", " + std::cout << "(" << i << ") pos: " << actual.pos.transpose() << ", " + << "normal: " << actual.normal.transpose() << ", " << "depth: " << actual.penetration_depth << " ---- "; if (index_to_expected_contacts[i] != -1) @@ -351,14 +392,15 @@ bool inspectContactPoints(const S1& s1, const Transform3d& tf1, return foundAll; } -void getContactPointsFromResult(std::vector& contacts, const CollisionResult& result) +template +void getContactPointdsFromResult(std::vector>& contacts, const CollisionResult& result) { const size_t numContacts = result.numContacts(); contacts.resize(numContacts); for (size_t i = 0; i < numContacts; ++i) { - const Contact& cnt = result.getContact(i); + const auto& cnt = result.getContact(i); contacts[i].pos = cnt.pos; contacts[i].normal = cnt.normal; @@ -366,25 +408,27 @@ void getContactPointsFromResult(std::vector& contacts, const Colli } } -template +template void testShapeIntersection( - const S1& s1, const Transform3d& tf1, - const S2& s2, const Transform3d& tf2, + const Shape1& s1, const Transform3& tf1, + const Shape2& s2, const Transform3& tf2, GJKSolverType solver_type, bool expected_res, - const std::vector& expected_contacts = std::vector(), + const std::vector>& expected_contacts = std::vector>(), bool check_position = true, bool check_depth = true, bool check_normal = true, bool check_opposite_normal = false, - FCL_REAL tol = 1e-9) + typename Shape1::S tol = 1e-9) { - CollisionRequest request; + using S = typename Shape1::S; + + CollisionRequest request; request.gjk_solver_type = solver_type; request.num_max_contacts = std::numeric_limits::max(); - CollisionResult result; + CollisionResult result; - std::vector actual_contacts; + std::vector> actual_contacts; bool res; @@ -393,11 +437,11 @@ void testShapeIntersection( // Check only whether they are colliding or not. if (solver_type == GST_LIBCCD) { - res = solver1.shapeIntersect(s1, tf1, s2, tf2, NULL); + res = solver1().shapeIntersect(s1, tf1, s2, tf2, nullptr); } else if (solver_type == GST_INDEP) { - res = solver2.shapeIntersect(s1, tf1, s2, tf2, NULL); + res = solver2().shapeIntersect(s1, tf1, s2, tf2, nullptr); } else { @@ -409,11 +453,11 @@ void testShapeIntersection( // Check contact information as well if (solver_type == GST_LIBCCD) { - res = solver1.shapeIntersect(s1, tf1, s2, tf2, &actual_contacts); + res = solver1().shapeIntersect(s1, tf1, s2, tf2, &actual_contacts); } else if (solver_type == GST_INDEP) { - res = solver2.shapeIntersect(s1, tf1, s2, tf2, &actual_contacts); + res = solver2().shapeIntersect(s1, tf1, s2, tf2, &actual_contacts); } else { @@ -423,7 +467,7 @@ void testShapeIntersection( EXPECT_EQ(res, expected_res); if (expected_res) { - EXPECT_TRUE(inspectContactPoints(s1, tf1, s2, tf2, solver_type, + EXPECT_TRUE(inspectContactPointds(s1, tf1, s2, tf2, solver_type, expected_contacts, actual_contacts, check_position, check_depth, @@ -446,8 +490,8 @@ void testShapeIntersection( EXPECT_EQ(res, expected_res); if (expected_res) { - getContactPointsFromResult(actual_contacts, result); - EXPECT_TRUE(inspectContactPoints(s1, tf1, s2, tf2, solver_type, + getContactPointdsFromResult(actual_contacts, result); + EXPECT_TRUE(inspectContactPointds(s1, tf1, s2, tf2, solver_type, expected_contacts, actual_contacts, check_position, check_depth, @@ -480,45 +524,46 @@ void testShapeIntersection( // | triangle |/////|////////|///////////|/////////|//////|//////////|///////|////////////| | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_spheresphere) +template +void test_shapeIntersection_spheresphere() { - Sphere s1(20); - Sphere s2(10); + Sphere s1(20); + Sphere s2(10); - Transform3d tf1; - Transform3d tf2; + Transform3 tf1; + Transform3 tf2; - Transform3d transform = Transform3d::Identity(); - generateRandomTransform(extents, transform); + Transform3 transform = Transform3::Identity(); + generateRandomTransform(extents(), transform); - std::vector contacts; + std::vector> contacts; - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(40, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(40, 0, 0))); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(40, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(40, 0, 0))); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(30, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(30, 0, 0))); contacts.resize(1); contacts[0].normal << 1, 0, 0; contacts[0].pos << 20, 0, 0; contacts[0].penetration_depth = 0.0; testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(30.01, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(30.01, 0, 0))); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(30.01, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(30.01, 0, 0))); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(29.9, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(29.9, 0, 0))); contacts.resize(1); contacts[0].normal << 1, 0, 0; contacts[0].pos << 20.0 - 0.1 * 20.0/(20.0 + 10.0), 0, 0; @@ -526,15 +571,15 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_spheresphere) testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(29.9, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(29.9, 0, 0))); contacts.resize(1); - contacts[0].normal = transform.linear() * Vector3d(1, 0, 0); - contacts[0].pos = transform * Vector3d(20.0 - 0.1 * 20.0/(20.0 + 10.0), 0, 0); + contacts[0].normal = transform.linear() * Vector3(1, 0, 0); + contacts[0].pos = transform * Vector3(20.0 - 0.1 * 20.0/(20.0 + 10.0), 0, 0); contacts[0].penetration_depth = 0.1; testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d::Identity(); + tf1 = Transform3::Identity(); + tf2 = Transform3::Identity(); contacts.resize(1); contacts[0].normal.setZero(); // If the centers of two sphere are at the same position, the normal is (0, 0, 0) contacts[0].pos.setZero(); @@ -545,12 +590,12 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_spheresphere) tf2 = transform; contacts.resize(1); contacts[0].normal.setZero(); // If the centers of two sphere are at the same position, the normal is (0, 0, 0) - contacts[0].pos = transform * Vector3d::Zero(); + contacts[0].pos = transform * Vector3::Zero(); contacts[0].penetration_depth = 20.0 + 10.0; testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(-29.9, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(-29.9, 0, 0))); contacts.resize(1); contacts[0].normal << -1, 0, 0; contacts[0].pos << -20.0 + 0.1 * 20.0/(20.0 + 10.0), 0, 0; @@ -558,47 +603,58 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_spheresphere) testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(-29.9, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(-29.9, 0, 0))); contacts.resize(1); - contacts[0].normal = transform.linear() * Vector3d(-1, 0, 0); - contacts[0].pos = transform * Vector3d(-20.0 + 0.1 * 20.0/(20.0 + 10.0), 0, 0); + contacts[0].normal = transform.linear() * Vector3(-1, 0, 0); + contacts[0].pos = transform * Vector3(-20.0 + 0.1 * 20.0/(20.0 + 10.0), 0, 0); contacts[0].penetration_depth = 0.1; testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(-30.0, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(-30.0, 0, 0))); contacts.resize(1); contacts[0].normal << -1, 0, 0; contacts[0].pos << -20, 0, 0; contacts[0].penetration_depth = 0.0; testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(-30.01, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(-30.01, 0, 0))); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(-30.01, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(-30.01, 0, 0))); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); } -bool compareContactPoints1(const Vector3d& c1,const Vector3d& c2) +GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_spheresphere) +{ +// test_shapeIntersection_spheresphere(); + test_shapeIntersection_spheresphere(); +} + +template +bool compareContactPointds1(const Vector3& c1,const Vector3& c2) { return c1[2] < c2[2]; } // Ascending order -bool compareContactPoints2(const ContactPoint& cp1,const ContactPoint& cp2) +template +bool compareContactPointds2(const ContactPoint& cp1,const ContactPoint& cp2) { return cp1.pos[2] < cp2.pos[2]; } // Ascending order -void testBoxBoxContactPoints(const Matrix3d& R) +template +void testBoxBoxContactPointds(const Eigen::MatrixBase& R) { - Box s1(100, 100, 100); - Box s2(10, 20, 30); + using S = typename Derived::RealScalar; + + Box s1(100, 100, 100); + Box s2(10, 20, 30); // Vertices of s2 - std::vector vertices(8); + std::vector> vertices(8); vertices[0] << 1, 1, 1; vertices[1] << 1, 1, -1; vertices[2] << 1, -1, 1; @@ -615,13 +671,13 @@ void testBoxBoxContactPoints(const Matrix3d& R) vertices[i][2] *= 0.5 * s2.side[2]; } - Transform3d tf1 = Transform3d(Eigen::Translation3d(Vector3d(0, 0, -50))); - Transform3d tf2 = Transform3d(R); + Transform3 tf1 = Transform3(Translation3(Vector3(0, 0, -50))); + Transform3 tf2 = Transform3(R); - std::vector contacts; + std::vector> contacts; // Make sure the two boxes are colliding - bool res = solver1.shapeIntersect(s1, tf1, s2, tf2, &contacts); + bool res = solver1().shapeIntersect(s1, tf1, s2, tf2, &contacts); EXPECT_TRUE(res); // Compute global vertices @@ -629,8 +685,8 @@ void testBoxBoxContactPoints(const Matrix3d& R) vertices[i] = tf2 * vertices[i]; // Sort the vertices so that the lowest vertex along z-axis comes first - std::sort(vertices.begin(), vertices.end(), compareContactPoints1); - std::sort(contacts.begin(), contacts.end(), compareContactPoints2); + std::sort(vertices.begin(), vertices.end(), compareContactPointds1); + std::sort(contacts.begin(), contacts.end(), compareContactPointds2); // The lowest n vertex along z-axis should be the contact point size_t numContacts = contacts.size(); @@ -640,27 +696,28 @@ void testBoxBoxContactPoints(const Matrix3d& R) for (size_t i = 0; i < numContacts; ++i) { EXPECT_TRUE(vertices[i].isApprox(contacts[i].pos)); - EXPECT_TRUE(Vector3d(0, 0, 1).isApprox(contacts[i].normal)); + EXPECT_TRUE(Vector3(0, 0, 1).isApprox(contacts[i].normal)); } } -GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_boxbox) +template +void test_shapeIntersection_boxbox() { - Box s1(20, 40, 50); - Box s2(10, 10, 10); + Box s1(20, 40, 50); + Box s2(10, 10, 10); - Transform3d tf1; - Transform3d tf2; + Transform3 tf1; + Transform3 tf2; - Transform3d transform = Transform3d::Identity(); - generateRandomTransform(extents, transform); + Transform3 transform = Transform3::Identity(); + generateRandomTransform(extents(), transform); - std::vector contacts; + std::vector> contacts; - Quaternion3d q(Eigen::AngleAxisd((FCL_REAL)3.140 / 6, Vector3d(0, 0, 1))); + Quaternion q(AngleAxis((S)3.140 / 6, Vector3(0, 0, 1))); - tf1 = Transform3d::Identity(); - tf2 = Transform3d::Identity(); + tf1 = Transform3::Identity(); + tf2 = Transform3::Identity(); // TODO: Need convention for normal when the centers of two objects are at same position. The current result is (1, 0, 0). contacts.resize(4); contacts[0].normal << 1, 0, 0; @@ -673,67 +730,74 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_boxbox) tf2 = transform; // TODO: Need convention for normal when the centers of two objects are at same position. The current result is (1, 0, 0). contacts.resize(4); - contacts[0].normal = transform.linear() * Vector3d(1, 0, 0); - contacts[1].normal = transform.linear() * Vector3d(1, 0, 0); - contacts[2].normal = transform.linear() * Vector3d(1, 0, 0); - contacts[3].normal = transform.linear() * Vector3d(1, 0, 0); + contacts[0].normal = transform.linear() * Vector3(1, 0, 0); + contacts[1].normal = transform.linear() * Vector3(1, 0, 0); + contacts[2].normal = transform.linear() * Vector3(1, 0, 0); + contacts[3].normal = transform.linear() * Vector3(1, 0, 0); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(15, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(15, 0, 0))); contacts.resize(4); - contacts[0].normal = Vector3d(1, 0, 0); - contacts[1].normal = Vector3d(1, 0, 0); - contacts[2].normal = Vector3d(1, 0, 0); - contacts[3].normal = Vector3d(1, 0, 0); + contacts[0].normal = Vector3(1, 0, 0); + contacts[1].normal = Vector3(1, 0, 0); + contacts[2].normal = Vector3(1, 0, 0); + contacts[3].normal = Vector3(1, 0, 0); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(15.01, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(15.01, 0, 0))); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(q); + tf1 = Transform3::Identity(); + tf2 = Transform3(q); contacts.resize(4); - contacts[0].normal = Vector3d(1, 0, 0); - contacts[1].normal = Vector3d(1, 0, 0); - contacts[2].normal = Vector3d(1, 0, 0); - contacts[3].normal = Vector3d(1, 0, 0); + contacts[0].normal = Vector3(1, 0, 0); + contacts[1].normal = Vector3(1, 0, 0); + contacts[2].normal = Vector3(1, 0, 0); + contacts[3].normal = Vector3(1, 0, 0); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true); tf1 = transform; - tf2 = transform * Transform3d(q); + tf2 = transform * Transform3(q); contacts.resize(4); - contacts[0].normal = transform.linear() * Vector3d(1, 0, 0); - contacts[1].normal = transform.linear() * Vector3d(1, 0, 0); - contacts[2].normal = transform.linear() * Vector3d(1, 0, 0); - contacts[3].normal = transform.linear() * Vector3d(1, 0, 0); + contacts[0].normal = transform.linear() * Vector3(1, 0, 0); + contacts[1].normal = transform.linear() * Vector3(1, 0, 0); + contacts[2].normal = transform.linear() * Vector3(1, 0, 0); + contacts[3].normal = transform.linear() * Vector3(1, 0, 0); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true); FCL_UINT32 numTests = 1e+2; for (FCL_UINT32 i = 0; i < numTests; ++i) { - Transform3d tf; - generateRandomTransform(extents, tf); - testBoxBoxContactPoints(tf.linear()); + Transform3 tf; + generateRandomTransform(extents(), tf); + testBoxBoxContactPointds(tf.linear()); } } -GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_spherebox) +GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_boxbox) +{ +// test_shapeIntersection_boxbox(); + test_shapeIntersection_boxbox(); +} + +template +void test_shapeIntersection_spherebox() { - Sphere s1(20); - Box s2(5, 5, 5); + Sphere s1(20); + Box s2(5, 5, 5); - Transform3d tf1; - Transform3d tf2; + Transform3 tf1; + Transform3 tf2; - Transform3d transform = Transform3d::Identity(); - generateRandomTransform(extents, transform); + Transform3 transform = Transform3::Identity(); + generateRandomTransform(extents(), transform); - std::vector contacts; + std::vector> contacts; - tf1 = Transform3d::Identity(); - tf2 = Transform3d::Identity(); + tf1 = Transform3::Identity(); + tf2 = Transform3::Identity(); // TODO: Need convention for normal when the centers of two objects are at same position. The current result is (-1, 0, 0). contacts.resize(1); contacts[0].normal << -1, 0, 0; @@ -745,42 +809,49 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_spherebox) contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(22.5, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(22.5, 0, 0))); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(22.501, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(22.501, 0, 0))); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(22.4, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(22.4, 0, 0))); contacts.resize(1); contacts[0].normal << 1, 0, 0; testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(22.4, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(22.4, 0, 0))); contacts.resize(1); - contacts[0].normal = transform.linear() * Vector3d(1, 0, 0); + contacts[0].normal = transform.linear() * Vector3(1, 0, 0); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true, false, 1e-4); } -GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_spherecapsule) +GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_spherebox) +{ +// test_shapeIntersection_spherebox(); + test_shapeIntersection_spherebox(); +} + +template +void test_shapeIntersection_spherecapsule() { - Sphere s1(20); - Capsule s2(5, 10); + Sphere s1(20); + Capsule s2(5, 10); - Transform3d tf1; - Transform3d tf2; + Transform3 tf1; + Transform3 tf2; - Transform3d transform = Transform3d::Identity(); - generateRandomTransform(extents, transform); + Transform3 transform = Transform3::Identity(); + generateRandomTransform(extents(), transform); - std::vector contacts; + std::vector> contacts; - tf1 = Transform3d::Identity(); - tf2 = Transform3d::Identity(); + tf1 = Transform3::Identity(); + tf2 = Transform3::Identity(); // TODO: Need convention for normal when the centers of two objects are at same position. contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false); @@ -791,55 +862,62 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_spherecapsule) contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(24.9, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(24.9, 0, 0))); contacts.resize(1); contacts[0].normal << 1, 0, 0; testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(24.9, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(24.9, 0, 0))); contacts.resize(1); - contacts[0].normal = transform.linear() * Vector3d(1, 0, 0); + contacts[0].normal = transform.linear() * Vector3(1, 0, 0); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(25, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(25, 0, 0))); contacts.resize(1); contacts[0].normal << 1, 0, 0; testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true); tf1 = transform; - //tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(25, 0, 0))); - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(25 - 1e-6, 0, 0))); + //tf2 = transform * Transform3(Translation3(Vector3(25, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(25 - 1e-6, 0, 0))); contacts.resize(1); - contacts[0].normal = transform.linear() * Vector3d(1, 0, 0); + contacts[0].normal = transform.linear() * Vector3(1, 0, 0); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(25.1, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(25.1, 0, 0))); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(25.1, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(25.1, 0, 0))); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); } -GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_cylindercylinder) +GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_spherecapsule) { - Cylinder s1(5, 10); - Cylinder s2(5, 10); +// test_shapeIntersection_spherecapsule(); + test_shapeIntersection_spherecapsule(); +} - Transform3d tf1; - Transform3d tf2; +template +void test_shapeIntersection_cylindercylinder() +{ + Cylinder s1(5, 10); + Cylinder s2(5, 10); - Transform3d transform = Transform3d::Identity(); - generateRandomTransform(extents, transform); + Transform3 tf1; + Transform3 tf2; - std::vector contacts; + Transform3 transform = Transform3::Identity(); + generateRandomTransform(extents(), transform); - tf1 = Transform3d::Identity(); - tf2 = Transform3d::Identity(); + std::vector> contacts; + + tf1 = Transform3::Identity(); + tf2 = Transform3::Identity(); // TODO: Need convention for normal when the centers of two objects are at same position. contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false); @@ -850,42 +928,49 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_cylindercylinder) contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(9.9, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(9.9, 0, 0))); contacts.resize(1); contacts[0].normal << 1, 0, 0; testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(9.9, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(9.9, 0, 0))); contacts.resize(1); - contacts[0].normal = transform.linear() * Vector3d(1, 0, 0); + contacts[0].normal = transform.linear() * Vector3(1, 0, 0); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true, false, 1e-5); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(10.01, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(10.01, 0, 0))); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(10.01, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(10.01, 0, 0))); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); } -GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_conecone) +GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_cylindercylinder) +{ +// test_shapeIntersection_cylindercylinder(); + test_shapeIntersection_cylindercylinder(); +} + +template +void test_shapeIntersection_conecone() { - Cone s1(5, 10); - Cone s2(5, 10); + Cone s1(5, 10); + Cone s2(5, 10); - Transform3d tf1; - Transform3d tf2; + Transform3 tf1; + Transform3 tf2; - Transform3d transform = Transform3d::Identity(); - generateRandomTransform(extents, transform); + Transform3 transform = Transform3::Identity(); + generateRandomTransform(extents(), transform); - std::vector contacts; + std::vector> contacts; - tf1 = Transform3d::Identity(); - tf2 = Transform3d::Identity(); + tf1 = Transform3::Identity(); + tf2 = Transform3::Identity(); // TODO: Need convention for normal when the centers of two objects are at same position. contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false); @@ -896,54 +981,61 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_conecone) contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(9.9, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(9.9, 0, 0))); contacts.resize(1); contacts[0].normal << 1, 0, 0; testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(9.9, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(9.9, 0, 0))); contacts.resize(1); - contacts[0].normal = transform.linear() * Vector3d(1, 0, 0); - testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true, false, 1e-5); + contacts[0].normal = transform.linear() * Vector3(1, 0, 0); + testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true, false, 5e-5); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(10.001, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(10.001, 0, 0))); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(10.001, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(10.001, 0, 0))); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, 0, 9.9))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, 0, 9.9))); contacts.resize(1); contacts[0].normal << 0, 0, 1; testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, 0, 9.9))); + tf2 = transform * Transform3(Translation3(Vector3(0, 0, 9.9))); contacts.resize(1); - contacts[0].normal = transform.linear() * Vector3d(0, 0, 1); + contacts[0].normal = transform.linear() * Vector3(0, 0, 1); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true, false, 1e-5); } -GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_cylindercone) +GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_conecone) { - Cylinder s1(5, 10); - Cone s2(5, 10); +// test_shapeIntersection_conecone(); + test_shapeIntersection_conecone(); +} - Transform3d tf1; - Transform3d tf2; +template +void test_shapeIntersection_cylindercone() +{ + Cylinder s1(5, 10); + Cone s2(5, 10); - Transform3d transform = Transform3d::Identity(); - generateRandomTransform(extents, transform); + Transform3 tf1; + Transform3 tf2; - std::vector contacts; + Transform3 transform = Transform3::Identity(); + generateRandomTransform(extents(), transform); - tf1 = Transform3d::Identity(); - tf2 = Transform3d::Identity(); + std::vector> contacts; + + tf1 = Transform3::Identity(); + tf2 = Transform3::Identity(); // TODO: Need convention for normal when the centers of two objects are at same position. contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false); @@ -954,89 +1046,96 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_cylindercone) contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(9.9, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(9.9, 0, 0))); contacts.resize(1); contacts[0].normal << 1, 0, 0; testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true, false, 0.061); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(9.9, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(9.9, 0, 0))); contacts.resize(1); - contacts[0].normal = transform.linear() * Vector3d(1, 0, 0); + contacts[0].normal = transform.linear() * Vector3(1, 0, 0); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true, false, 0.46); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(10.01, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(10.01, 0, 0))); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(10.01, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(10.01, 0, 0))); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, 0, 9.9))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, 0, 9.9))); contacts.resize(1); contacts[0].normal << 0, 0, 1; testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, 0, 9.9))); + tf2 = transform * Transform3(Translation3(Vector3(0, 0, 9.9))); contacts.resize(1); - contacts[0].normal = transform.linear() * Vector3d(0, 0, 1); + contacts[0].normal = transform.linear() * Vector3(0, 0, 1); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true, false, 1e-5); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, 0, 10.01))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, 0, 10.01))); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, 0, 10.01))); + tf2 = transform * Transform3(Translation3(Vector3(0, 0, 10.01))); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); } -GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_ellipsoidellipsoid) +GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_cylindercone) +{ +// test_shapeIntersection_cylindercone(); + test_shapeIntersection_cylindercone(); +} + +template +void test_shapeIntersection_ellipsoidellipsoid() { - Ellipsoid s1(20, 40, 50); - Ellipsoid s2(10, 10, 10); + Ellipsoid s1(20, 40, 50); + Ellipsoid s2(10, 10, 10); - Transform3d tf1; - Transform3d tf2; + Transform3 tf1; + Transform3 tf2; - Transform3d transform = Transform3d::Identity(); - generateRandomTransform(extents, transform); - Transform3d identity; + Transform3 transform = Transform3::Identity(); + generateRandomTransform(extents(), transform); + Transform3 identity; - std::vector contacts; + std::vector> contacts; - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(40, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(40, 0, 0))); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(40, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(40, 0, 0))); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(30, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(30, 0, 0))); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(30.01, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(30.01, 0, 0))); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(29.99, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(29.99, 0, 0))); contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(29.9, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(29.9, 0, 0))); contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false); - tf1 = Transform3d::Identity(); - tf2 = Transform3d::Identity(); + tf1 = Transform3::Identity(); + tf2 = Transform3::Identity(); contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false); @@ -1045,161 +1144,189 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_ellipsoidellipsoid) contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(-29.99, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(-29.99, 0, 0))); contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(-29.99, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(-29.99, 0, 0))); contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(-30, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(-30, 0, 0))); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(-30.01, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(-30.01, 0, 0))); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); } -GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_spheretriangle) +GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_ellipsoidellipsoid) +{ +// test_shapeIntersection_ellipsoidellipsoid(); + test_shapeIntersection_ellipsoidellipsoid(); +} + +template +void test_shapeIntersection_spheretriangle() { - Sphere s(10); - Vector3d t[3]; + Sphere s(10); + Vector3 t[3]; t[0] << 20, 0, 0; t[1] << -20, 0, 0; t[2] << 0, 20, 0; - Transform3d transform = Transform3d::Identity(); - generateRandomTransform(extents, transform); + Transform3 transform = Transform3::Identity(); + generateRandomTransform(extents(), transform); - Vector3d normal; + Vector3 normal; bool res; - res = solver1.shapeTriangleIntersect(s, Transform3d::Identity(), t[0], t[1], t[2], NULL, NULL, NULL); + res = solver1().shapeTriangleIntersect(s, Transform3::Identity(), t[0], t[1], t[2], nullptr, nullptr, nullptr); EXPECT_TRUE(res); - res = solver1.shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, NULL, NULL, NULL); + res = solver1().shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, nullptr, nullptr, nullptr); EXPECT_TRUE(res); t[0] << 30, 0, 0; t[1] << 9.9, -20, 0; t[2] << 9.9, 20, 0; - res = solver1.shapeTriangleIntersect(s, Transform3d::Identity(), t[0], t[1], t[2], NULL, NULL, NULL); + res = solver1().shapeTriangleIntersect(s, Transform3::Identity(), t[0], t[1], t[2], nullptr, nullptr, nullptr); EXPECT_TRUE(res); - res = solver1.shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, NULL, NULL, NULL); + res = solver1().shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, nullptr, nullptr, nullptr); EXPECT_TRUE(res); - res = solver1.shapeTriangleIntersect(s, Transform3d::Identity(), t[0], t[1], t[2], NULL, NULL, &normal); + res = solver1().shapeTriangleIntersect(s, Transform3::Identity(), t[0], t[1], t[2], nullptr, nullptr, &normal); EXPECT_TRUE(res); - EXPECT_TRUE(normal.isApprox(Vector3d(1, 0, 0), 1e-9)); + EXPECT_TRUE(normal.isApprox(Vector3(1, 0, 0), 1e-9)); - res = solver1.shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, NULL, NULL, &normal); + res = solver1().shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, nullptr, nullptr, &normal); EXPECT_TRUE(res); - EXPECT_TRUE(normal.isApprox(transform.linear() * Vector3d(1, 0, 0), 1e-9)); + EXPECT_TRUE(normal.isApprox(transform.linear() * Vector3(1, 0, 0), 1e-9)); } -GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacetriangle) +GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_spheretriangle) { - Halfspace hs(Vector3d(1, 0, 0), 0); - Vector3d t[3]; +// test_shapeIntersection_spheretriangle(); + test_shapeIntersection_spheretriangle(); +} + +template +void test_shapeIntersection_halfspacetriangle() +{ + Halfspace hs(Vector3(1, 0, 0), 0); + Vector3 t[3]; t[0] << 20, 0, 0; t[1] << -20, 0, 0; t[2] << 0, 20, 0; - Transform3d transform = Transform3d::Identity(); - generateRandomTransform(extents, transform); + Transform3 transform = Transform3::Identity(); + generateRandomTransform(extents(), transform); - // Vector3d point; - // FCL_REAL depth; - Vector3d normal; + // Vector3 point; + // S depth; + Vector3 normal; bool res; - res = solver1.shapeTriangleIntersect(hs, Transform3d::Identity(), t[0], t[1], t[2], Transform3d::Identity(), NULL, NULL, NULL); + res = solver1().shapeTriangleIntersect(hs, Transform3::Identity(), t[0], t[1], t[2], Transform3::Identity(), nullptr, nullptr, nullptr); EXPECT_TRUE(res); - res = solver1.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, NULL, NULL, NULL); + res = solver1().shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, nullptr, nullptr, nullptr); EXPECT_TRUE(res); t[0] << 20, 0, 0; t[1] << 0, -20, 0; t[2] << 0, 20, 0; - res = solver1.shapeTriangleIntersect(hs, Transform3d::Identity(), t[0], t[1], t[2], Transform3d::Identity(), NULL, NULL, NULL); + res = solver1().shapeTriangleIntersect(hs, Transform3::Identity(), t[0], t[1], t[2], Transform3::Identity(), nullptr, nullptr, nullptr); EXPECT_TRUE(res); - res = solver1.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, NULL, NULL, NULL); + res = solver1().shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, nullptr, nullptr, nullptr); EXPECT_TRUE(res); - res = solver1.shapeTriangleIntersect(hs, Transform3d::Identity(), t[0], t[1], t[2], Transform3d::Identity(), NULL, NULL, &normal); + res = solver1().shapeTriangleIntersect(hs, Transform3::Identity(), t[0], t[1], t[2], Transform3::Identity(), nullptr, nullptr, &normal); EXPECT_TRUE(res); - EXPECT_TRUE(normal.isApprox(Vector3d(1, 0, 0), 1e-9)); + EXPECT_TRUE(normal.isApprox(Vector3(1, 0, 0), 1e-9)); - res = solver1.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, NULL, NULL, &normal); + res = solver1().shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, nullptr, nullptr, &normal); EXPECT_TRUE(res); - EXPECT_TRUE(normal.isApprox(transform.linear() * Vector3d(1, 0, 0), 1e-9)); + EXPECT_TRUE(normal.isApprox(transform.linear() * Vector3(1, 0, 0), 1e-9)); } -GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planetriangle) +GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacetriangle) { - Plane hs(Vector3d(1, 0, 0), 0); - Vector3d t[3]; +// test_shapeIntersection_halfspacetriangle(); + test_shapeIntersection_halfspacetriangle(); +} + +template +void test_shapeIntersection_planetriangle() +{ + Plane hs(Vector3(1, 0, 0), 0); + Vector3 t[3]; t[0] << 20, 0, 0; t[1] << -20, 0, 0; t[2] << 0, 20, 0; - Transform3d transform = Transform3d::Identity(); - generateRandomTransform(extents, transform); + Transform3 transform = Transform3::Identity(); + generateRandomTransform(extents(), transform); - // Vector3d point; - // FCL_REAL depth; - Vector3d normal; + // Vector3 point; + // S depth; + Vector3 normal; bool res; - res = solver1.shapeTriangleIntersect(hs, Transform3d::Identity(), t[0], t[1], t[2], Transform3d::Identity(), NULL, NULL, NULL); + res = solver1().shapeTriangleIntersect(hs, Transform3::Identity(), t[0], t[1], t[2], Transform3::Identity(), nullptr, nullptr, nullptr); EXPECT_TRUE(res); - res = solver1.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, NULL, NULL, NULL); + res = solver1().shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, nullptr, nullptr, nullptr); EXPECT_TRUE(res); t[0] << 20, 0, 0; t[1] << -0.1, -20, 0; t[2] << -0.1, 20, 0; - res = solver1.shapeTriangleIntersect(hs, Transform3d::Identity(), t[0], t[1], t[2], Transform3d::Identity(), NULL, NULL, NULL); + res = solver1().shapeTriangleIntersect(hs, Transform3::Identity(), t[0], t[1], t[2], Transform3::Identity(), nullptr, nullptr, nullptr); EXPECT_TRUE(res); - res = solver1.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, NULL, NULL, NULL); + res = solver1().shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, nullptr, nullptr, nullptr); EXPECT_TRUE(res); - res = solver1.shapeTriangleIntersect(hs, Transform3d::Identity(), t[0], t[1], t[2], Transform3d::Identity(), NULL, NULL, &normal); + res = solver1().shapeTriangleIntersect(hs, Transform3::Identity(), t[0], t[1], t[2], Transform3::Identity(), nullptr, nullptr, &normal); EXPECT_TRUE(res); - EXPECT_TRUE(normal.isApprox(Vector3d(1, 0, 0), 1e-9)); + EXPECT_TRUE(normal.isApprox(Vector3(1, 0, 0), 1e-9)); - res = solver1.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, NULL, NULL, &normal); + res = solver1().shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, nullptr, nullptr, &normal); EXPECT_TRUE(res); - EXPECT_TRUE(normal.isApprox(transform.linear() * Vector3d(1, 0, 0), 1e-9)); + EXPECT_TRUE(normal.isApprox(transform.linear() * Vector3(1, 0, 0), 1e-9)); } -GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacesphere) +GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planetriangle) { - Sphere s(10); - Halfspace hs(Vector3d(1, 0, 0), 0); +// test_shapeIntersection_planetriangle(); + test_shapeIntersection_planetriangle(); +} + +template +void test_shapeIntersection_halfspacesphere() +{ + Sphere s(10); + Halfspace hs(Vector3(1, 0, 0), 0); - Transform3d tf1; - Transform3d tf2; + Transform3 tf1; + Transform3 tf2; - Transform3d transform = Transform3d::Identity(); - generateRandomTransform(extents, transform); + Transform3 transform = Transform3::Identity(); + generateRandomTransform(extents(), transform); - std::vector contacts; + std::vector> contacts; - tf1 = Transform3d::Identity(); - tf2 = Transform3d::Identity(); + tf1 = Transform3::Identity(); + tf2 = Transform3::Identity(); contacts.resize(1); contacts[0].pos << -5, 0, 0; contacts[0].penetration_depth = 10; @@ -1209,13 +1336,13 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacesphere) tf1 = transform; tf2 = transform; contacts.resize(1); - contacts[0].pos = transform * Vector3d(-5, 0, 0); + contacts[0].pos = transform * Vector3(-5, 0, 0); contacts[0].penetration_depth = 10; - contacts[0].normal = transform.linear() * Vector3d(-1, 0, 0); + contacts[0].normal = transform.linear() * Vector3(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(5, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(5, 0, 0))); contacts.resize(1); contacts[0].pos << -2.5, 0, 0; contacts[0].penetration_depth = 15; @@ -1223,15 +1350,15 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacesphere) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(5, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(5, 0, 0))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(-2.5, 0, 0); + contacts[0].pos = transform * Vector3(-2.5, 0, 0); contacts[0].penetration_depth = 15; - contacts[0].normal = transform.linear() * Vector3d(-1, 0, 0); + contacts[0].normal = transform.linear() * Vector3(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(-5, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(-5, 0, 0))); contacts.resize(1); contacts[0].pos << -7.5, 0, 0; contacts[0].penetration_depth = 5; @@ -1239,23 +1366,23 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacesphere) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(-5, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(-5, 0, 0))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(-7.5, 0, 0); + contacts[0].pos = transform * Vector3(-7.5, 0, 0); contacts[0].penetration_depth = 5; - contacts[0].normal = transform.linear() * Vector3d(-1, 0, 0); + contacts[0].normal = transform.linear() * Vector3(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(-10.1, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(-10.1, 0, 0))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(-10.1, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(-10.1, 0, 0))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(10.1, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(10.1, 0, 0))); contacts.resize(1); contacts[0].pos << 0.05, 0, 0; contacts[0].penetration_depth = 20.1; @@ -1263,29 +1390,36 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacesphere) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(10.1, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(10.1, 0, 0))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(0.05, 0, 0); + contacts[0].pos = transform * Vector3(0.05, 0, 0); contacts[0].penetration_depth = 20.1; - contacts[0].normal = transform.linear() * Vector3d(-1, 0, 0); + contacts[0].normal = transform.linear() * Vector3(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); } -GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planesphere) +GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacesphere) +{ +// test_shapeIntersection_halfspacesphere(); + test_shapeIntersection_halfspacesphere(); +} + +template +void test_shapeIntersection_planesphere() { - Sphere s(10); - Plane hs(Vector3d(1, 0, 0), 0); + Sphere s(10); + Plane hs(Vector3(1, 0, 0), 0); - Transform3d tf1; - Transform3d tf2; + Transform3 tf1; + Transform3 tf2; - Transform3d transform = Transform3d::Identity(); - generateRandomTransform(extents, transform); + Transform3 transform = Transform3::Identity(); + generateRandomTransform(extents(), transform); - std::vector contacts; + std::vector> contacts; - tf1 = Transform3d::Identity(); - tf2 = Transform3d::Identity(); + tf1 = Transform3::Identity(); + tf2 = Transform3::Identity(); contacts.resize(1); contacts[0].pos.setZero(); contacts[0].penetration_depth = 10; @@ -1295,13 +1429,13 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planesphere) tf1 = transform; tf2 = transform; contacts.resize(1); - contacts[0].pos = transform * Vector3d(0, 0, 0); + contacts[0].pos = transform * Vector3(0, 0, 0); contacts[0].penetration_depth = 10; - contacts[0].normal = transform.linear() * Vector3d(1, 0, 0); // (1, 0, 0) or (-1, 0, 0) + contacts[0].normal = transform.linear() * Vector3(1, 0, 0); // (1, 0, 0) or (-1, 0, 0) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(5, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(5, 0, 0))); contacts.resize(1); contacts[0].pos << 5, 0, 0; contacts[0].penetration_depth = 5; @@ -1309,15 +1443,15 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planesphere) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(5, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(5, 0, 0))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(5, 0, 0); + contacts[0].pos = transform * Vector3(5, 0, 0); contacts[0].penetration_depth = 5; - contacts[0].normal = transform.linear() * Vector3d(1, 0, 0); + contacts[0].normal = transform.linear() * Vector3(1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(-5, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(-5, 0, 0))); contacts.resize(1); contacts[0].pos << -5, 0, 0; contacts[0].penetration_depth = 5; @@ -1325,45 +1459,52 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planesphere) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(-5, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(-5, 0, 0))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(-5, 0, 0); + contacts[0].pos = transform * Vector3(-5, 0, 0); contacts[0].penetration_depth = 5; - contacts[0].normal = transform.linear() * Vector3d(-1, 0, 0); + contacts[0].normal = transform.linear() * Vector3(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(-10.1, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(-10.1, 0, 0))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(-10.1, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(-10.1, 0, 0))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(10.1, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(10.1, 0, 0))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(10.1, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(10.1, 0, 0))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); } -GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacebox) +GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planesphere) +{ +// test_shapeIntersection_planesphere(); + test_shapeIntersection_planesphere(); +} + +template +void test_shapeIntersection_halfspacebox() { - Box s(5, 10, 20); - Halfspace hs(Vector3d(1, 0, 0), 0); + Box s(5, 10, 20); + Halfspace hs(Vector3(1, 0, 0), 0); - Transform3d tf1; - Transform3d tf2; + Transform3 tf1; + Transform3 tf2; - Transform3d transform = Transform3d::Identity(); - generateRandomTransform(extents, transform); + Transform3 transform = Transform3::Identity(); + generateRandomTransform(extents(), transform); - std::vector contacts; + std::vector> contacts; - tf1 = Transform3d::Identity(); - tf2 = Transform3d::Identity(); + tf1 = Transform3::Identity(); + tf2 = Transform3::Identity(); contacts.resize(1); contacts[0].pos << -1.25, 0, 0; contacts[0].penetration_depth = 2.5; @@ -1373,13 +1514,13 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacebox) tf1 = transform; tf2 = transform; contacts.resize(1); - contacts[0].pos = transform * Vector3d(-1.25, 0, 0); + contacts[0].pos = transform * Vector3(-1.25, 0, 0); contacts[0].penetration_depth = 2.5; - contacts[0].normal = transform.linear() * Vector3d(-1, 0, 0); + contacts[0].normal = transform.linear() * Vector3(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(1.25, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(1.25, 0, 0))); contacts.resize(1); contacts[0].pos << -0.625, 0, 0; contacts[0].penetration_depth = 3.75; @@ -1387,15 +1528,15 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacebox) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(1.25, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(1.25, 0, 0))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(-0.625, 0, 0); + contacts[0].pos = transform * Vector3(-0.625, 0, 0); contacts[0].penetration_depth = 3.75; - contacts[0].normal = transform.linear() * Vector3d(-1, 0, 0); + contacts[0].normal = transform.linear() * Vector3(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(-1.25, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(-1.25, 0, 0))); contacts.resize(1); contacts[0].pos << -1.875, 0, 0; contacts[0].penetration_depth = 1.25; @@ -1403,15 +1544,15 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacebox) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(-1.25, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(-1.25, 0, 0))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(-1.875, 0, 0); + contacts[0].pos = transform * Vector3(-1.875, 0, 0); contacts[0].penetration_depth = 1.25; - contacts[0].normal = transform.linear() * Vector3d(-1, 0, 0); + contacts[0].normal = transform.linear() * Vector3(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(2.51, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(2.51, 0, 0))); contacts.resize(1); contacts[0].pos << 0.005, 0, 0; contacts[0].penetration_depth = 5.01; @@ -1419,42 +1560,49 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacebox) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(2.51, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(2.51, 0, 0))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(0.005, 0, 0); + contacts[0].pos = transform * Vector3(0.005, 0, 0); contacts[0].penetration_depth = 5.01; - contacts[0].normal = transform.linear() * Vector3d(-1, 0, 0); + contacts[0].normal = transform.linear() * Vector3(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(-2.51, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(-2.51, 0, 0))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(-2.51, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(-2.51, 0, 0))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); - tf1 = Transform3d(transform.linear()); - tf2 = Transform3d::Identity(); + tf1 = Transform3(transform.linear()); + tf2 = Transform3::Identity(); contacts.resize(1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, false, false, false); } -GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planebox) +GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacebox) { - Box s(5, 10, 20); - Plane hs(Vector3d(1, 0, 0), 0); +// test_shapeIntersection_halfspacebox(); + test_shapeIntersection_halfspacebox(); +} - Transform3d tf1; - Transform3d tf2; +template +void test_shapeIntersection_planebox() +{ + Box s(5, 10, 20); + Plane hs(Vector3(1, 0, 0), 0); - Transform3d transform = Transform3d::Identity(); - generateRandomTransform(extents, transform); + Transform3 tf1; + Transform3 tf2; - std::vector contacts; + Transform3 transform = Transform3::Identity(); + generateRandomTransform(extents(), transform); - tf1 = Transform3d::Identity(); - tf2 = Transform3d::Identity(); + std::vector> contacts; + + tf1 = Transform3::Identity(); + tf2 = Transform3::Identity(); contacts.resize(1); contacts[0].pos << 0, 0, 0; contacts[0].penetration_depth = 2.5; @@ -1464,13 +1612,13 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planebox) tf1 = transform; tf2 = transform; contacts.resize(1); - contacts[0].pos = transform * Vector3d(0, 0, 0); + contacts[0].pos = transform * Vector3(0, 0, 0); contacts[0].penetration_depth = 2.5; - contacts[0].normal = transform.linear() * Vector3d(1, 0, 0); // (1, 0, 0) or (-1, 0, 0) + contacts[0].normal = transform.linear() * Vector3(1, 0, 0); // (1, 0, 0) or (-1, 0, 0) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(1.25, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(1.25, 0, 0))); contacts.resize(1); contacts[0].pos << 1.25, 0, 0; contacts[0].penetration_depth = 1.25; @@ -1478,15 +1626,15 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planebox) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(1.25, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(1.25, 0, 0))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(1.25, 0, 0); + contacts[0].pos = transform * Vector3(1.25, 0, 0); contacts[0].penetration_depth = 1.25; - contacts[0].normal = transform.linear() * Vector3d(1, 0, 0); + contacts[0].normal = transform.linear() * Vector3(1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(-1.25, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(-1.25, 0, 0))); contacts.resize(1); contacts[0].pos << -1.25, 0, 0; contacts[0].penetration_depth = 1.25; @@ -1494,50 +1642,57 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planebox) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(-1.25, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(-1.25, 0, 0))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(-1.25, 0, 0); + contacts[0].pos = transform * Vector3(-1.25, 0, 0); contacts[0].penetration_depth = 1.25; - contacts[0].normal = transform.linear() * Vector3d(-1, 0, 0); + contacts[0].normal = transform.linear() * Vector3(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(2.51, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(2.51, 0, 0))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(2.51, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(2.51, 0, 0))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(-2.51, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(-2.51, 0, 0))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(-2.51, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(-2.51, 0, 0))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); - tf1 = Transform3d(transform.linear()); - tf2 = Transform3d::Identity(); + tf1 = Transform3(transform.linear()); + tf2 = Transform3::Identity(); contacts.resize(1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, false, false, false); } -GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspaceellipsoid) +GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planebox) +{ +// test_shapeIntersection_planebox(); + test_shapeIntersection_planebox(); +} + +template +void test_shapeIntersection_halfspaceellipsoid() { - Ellipsoid s(5, 10, 20); - Halfspace hs(Vector3d(1, 0, 0), 0); + Ellipsoid s(5, 10, 20); + Halfspace hs(Vector3(1, 0, 0), 0); - Transform3d tf1; - Transform3d tf2; + Transform3 tf1; + Transform3 tf2; - Transform3d transform = Transform3d::Identity(); - generateRandomTransform(extents, transform); + Transform3 transform = Transform3::Identity(); + generateRandomTransform(extents(), transform); - std::vector contacts; + std::vector> contacts; - tf1 = Transform3d::Identity(); - tf2 = Transform3d::Identity(); + tf1 = Transform3::Identity(); + tf2 = Transform3::Identity(); contacts.resize(1); contacts[0].pos << -2.5, 0, 0; contacts[0].penetration_depth = 5.0; @@ -1547,13 +1702,13 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspaceellipsoid) tf1 = transform; tf2 = transform; contacts.resize(1); - contacts[0].pos = transform * Vector3d(-2.5, 0, 0); + contacts[0].pos = transform * Vector3(-2.5, 0, 0); contacts[0].penetration_depth = 5.0; - contacts[0].normal = transform.linear() * Vector3d(-1, 0, 0); + contacts[0].normal = transform.linear() * Vector3(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(1.25, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(1.25, 0, 0))); contacts.resize(1); contacts[0].pos << -1.875, 0, 0; contacts[0].penetration_depth = 6.25; @@ -1561,15 +1716,15 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspaceellipsoid) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(1.25, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(1.25, 0, 0))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(-1.875, 0, 0); + contacts[0].pos = transform * Vector3(-1.875, 0, 0); contacts[0].penetration_depth = 6.25; - contacts[0].normal = transform.linear() * Vector3d(-1, 0, 0); + contacts[0].normal = transform.linear() * Vector3(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(-1.25, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(-1.25, 0, 0))); contacts.resize(1); contacts[0].pos << -3.125, 0, 0; contacts[0].penetration_depth = 3.75; @@ -1577,15 +1732,15 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspaceellipsoid) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(-1.25, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(-1.25, 0, 0))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(-3.125, 0, 0); + contacts[0].pos = transform * Vector3(-3.125, 0, 0); contacts[0].penetration_depth = 3.75; - contacts[0].normal = transform.linear() * Vector3d(-1, 0, 0); + contacts[0].normal = transform.linear() * Vector3(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(5.01, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(5.01, 0, 0))); contacts.resize(1); contacts[0].pos << 0.005, 0, 0; contacts[0].penetration_depth = 10.01; @@ -1593,28 +1748,28 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspaceellipsoid) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(5.01, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(5.01, 0, 0))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(0.005, 0, 0); + contacts[0].pos = transform * Vector3(0.005, 0, 0); contacts[0].penetration_depth = 10.01; - contacts[0].normal = transform.linear() * Vector3d(-1, 0, 0); + contacts[0].normal = transform.linear() * Vector3(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(-5.01, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(-5.01, 0, 0))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(-5.01, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(-5.01, 0, 0))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); - hs = Halfspace(Vector3d(0, 1, 0), 0); + hs = Halfspace(Vector3(0, 1, 0), 0); - tf1 = Transform3d::Identity(); - tf2 = Transform3d::Identity(); + tf1 = Transform3::Identity(); + tf2 = Transform3::Identity(); contacts.resize(1); contacts[0].pos << 0, -5.0, 0; contacts[0].penetration_depth = 10.0; @@ -1624,13 +1779,13 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspaceellipsoid) tf1 = transform; tf2 = transform; contacts.resize(1); - contacts[0].pos = transform * Vector3d(0, -5.0, 0); + contacts[0].pos = transform * Vector3(0, -5.0, 0); contacts[0].penetration_depth = 10.0; - contacts[0].normal = transform.linear() * Vector3d(0, -1, 0); + contacts[0].normal = transform.linear() * Vector3(0, -1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, 1.25, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, 1.25, 0))); contacts.resize(1); contacts[0].pos << 0, -4.375, 0; contacts[0].penetration_depth = 11.25; @@ -1638,15 +1793,15 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspaceellipsoid) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, 1.25, 0))); + tf2 = transform * Transform3(Translation3(Vector3(0, 1.25, 0))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(0, -4.375, 0); + contacts[0].pos = transform * Vector3(0, -4.375, 0); contacts[0].penetration_depth = 11.25; - contacts[0].normal = transform.linear() * Vector3d(0, -1, 0); + contacts[0].normal = transform.linear() * Vector3(0, -1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, -1.25, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, -1.25, 0))); contacts.resize(1); contacts[0].pos << 0, -5.625, 0; contacts[0].penetration_depth = 8.75; @@ -1654,15 +1809,15 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspaceellipsoid) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, -1.25, 0))); + tf2 = transform * Transform3(Translation3(Vector3(0, -1.25, 0))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(0, -5.625, 0); + contacts[0].pos = transform * Vector3(0, -5.625, 0); contacts[0].penetration_depth = 8.75; - contacts[0].normal = transform.linear() * Vector3d(0, -1, 0); + contacts[0].normal = transform.linear() * Vector3(0, -1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, 10.01, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, 10.01, 0))); contacts.resize(1); contacts[0].pos << 0, 0.005, 0; contacts[0].penetration_depth = 20.01; @@ -1670,28 +1825,28 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspaceellipsoid) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, 10.01, 0))); + tf2 = transform * Transform3(Translation3(Vector3(0, 10.01, 0))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(0, 0.005, 0); + contacts[0].pos = transform * Vector3(0, 0.005, 0); contacts[0].penetration_depth = 20.01; - contacts[0].normal = transform.linear() * Vector3d(0, -1, 0); + contacts[0].normal = transform.linear() * Vector3(0, -1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, -10.01, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, -10.01, 0))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, -10.01, 0))); + tf2 = transform * Transform3(Translation3(Vector3(0, -10.01, 0))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); - hs = Halfspace(Vector3d(0, 0, 1), 0); + hs = Halfspace(Vector3(0, 0, 1), 0); - tf1 = Transform3d::Identity(); - tf2 = Transform3d::Identity(); + tf1 = Transform3::Identity(); + tf2 = Transform3::Identity(); contacts.resize(1); contacts[0].pos << 0, 0, -10.0; contacts[0].penetration_depth = 20.0; @@ -1701,13 +1856,13 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspaceellipsoid) tf1 = transform; tf2 = transform; contacts.resize(1); - contacts[0].pos = transform * Vector3d(0, 0, -10.0); + contacts[0].pos = transform * Vector3(0, 0, -10.0); contacts[0].penetration_depth = 20.0; - contacts[0].normal = transform.linear() * Vector3d(0, 0, -1); + contacts[0].normal = transform.linear() * Vector3(0, 0, -1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, 0, 1.25))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, 0, 1.25))); contacts.resize(1); contacts[0].pos << 0, 0, -9.375; contacts[0].penetration_depth = 21.25; @@ -1715,15 +1870,15 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspaceellipsoid) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, 0, 1.25))); + tf2 = transform * Transform3(Translation3(Vector3(0, 0, 1.25))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(0, 0, -9.375); + contacts[0].pos = transform * Vector3(0, 0, -9.375); contacts[0].penetration_depth = 21.25; - contacts[0].normal = transform.linear() * Vector3d(0, 0, -1); + contacts[0].normal = transform.linear() * Vector3(0, 0, -1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, 0, -1.25))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, 0, -1.25))); contacts.resize(1); contacts[0].pos << 0, 0, -10.625; contacts[0].penetration_depth = 18.75; @@ -1731,15 +1886,15 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspaceellipsoid) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, 0, -1.25))); + tf2 = transform * Transform3(Translation3(Vector3(0, 0, -1.25))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(0, 0, -10.625); + contacts[0].pos = transform * Vector3(0, 0, -10.625); contacts[0].penetration_depth = 18.75; - contacts[0].normal = transform.linear() * Vector3d(0, 0, -1); + contacts[0].normal = transform.linear() * Vector3(0, 0, -1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, 0, 20.01))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, 0, 20.01))); contacts.resize(1); contacts[0].pos << 0, 0, 0.005; contacts[0].penetration_depth = 40.01; @@ -1747,37 +1902,44 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspaceellipsoid) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, 0, 20.01))); + tf2 = transform * Transform3(Translation3(Vector3(0, 0, 20.01))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(0, 0, 0.005); + contacts[0].pos = transform * Vector3(0, 0, 0.005); contacts[0].penetration_depth = 40.01; - contacts[0].normal = transform.linear() * Vector3d(0, 0, -1); + contacts[0].normal = transform.linear() * Vector3(0, 0, -1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, 0, -20.01))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, 0, -20.01))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, 0, -20.01))); + tf2 = transform * Transform3(Translation3(Vector3(0, 0, -20.01))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); } -GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planeellipsoid) +GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspaceellipsoid) { - Ellipsoid s(5, 10, 20); - Plane hs(Vector3d(1, 0, 0), 0); +// test_shapeIntersection_halfspaceellipsoid(); + test_shapeIntersection_halfspaceellipsoid(); +} + +template +void test_shapeIntersection_planeellipsoid() +{ + Ellipsoid s(5, 10, 20); + Plane hs(Vector3(1, 0, 0), 0); - Transform3d tf1; - Transform3d tf2; + Transform3 tf1; + Transform3 tf2; - Transform3d transform = Transform3d::Identity(); - generateRandomTransform(extents, transform); + Transform3 transform = Transform3::Identity(); + generateRandomTransform(extents(), transform); - std::vector contacts; + std::vector> contacts; - tf1 = Transform3d::Identity(); - tf2 = Transform3d::Identity(); + tf1 = Transform3::Identity(); + tf2 = Transform3::Identity(); contacts.resize(1); contacts[0].pos << 0, 0, 0; contacts[0].penetration_depth = 5.0; @@ -1787,13 +1949,13 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planeellipsoid) tf1 = transform; tf2 = transform; contacts.resize(1); - contacts[0].pos = transform * Vector3d(0, 0, 0); + contacts[0].pos = transform * Vector3(0, 0, 0); contacts[0].penetration_depth = 5.0; - contacts[0].normal = transform.linear() * Vector3d(-1, 0, 0); + contacts[0].normal = transform.linear() * Vector3(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(1.25, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(1.25, 0, 0))); contacts.resize(1); contacts[0].pos << 1.25, 0, 0; contacts[0].penetration_depth = 3.75; @@ -1801,15 +1963,15 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planeellipsoid) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(1.25, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(1.25, 0, 0))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(1.25, 0, 0); + contacts[0].pos = transform * Vector3(1.25, 0, 0); contacts[0].penetration_depth = 3.75; - contacts[0].normal = transform.linear() * Vector3d(1, 0, 0); + contacts[0].normal = transform.linear() * Vector3(1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(-1.25, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(-1.25, 0, 0))); contacts.resize(1); contacts[0].pos << -1.25, 0, 0; contacts[0].penetration_depth = 3.75; @@ -1817,36 +1979,36 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planeellipsoid) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(-1.25, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(-1.25, 0, 0))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(-1.25, 0, 0); + contacts[0].pos = transform * Vector3(-1.25, 0, 0); contacts[0].penetration_depth = 3.75; - contacts[0].normal = transform.linear() * Vector3d(-1, 0, 0); + contacts[0].normal = transform.linear() * Vector3(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(5.01, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(5.01, 0, 0))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(5.01, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(5.01, 0, 0))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(-5.01, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(-5.01, 0, 0))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(-5.01, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(-5.01, 0, 0))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); - hs = Plane(Vector3d(0, 1, 0), 0); + hs = Plane(Vector3(0, 1, 0), 0); - tf1 = Transform3d::Identity(); - tf2 = Transform3d::Identity(); + tf1 = Transform3::Identity(); + tf2 = Transform3::Identity(); contacts.resize(1); contacts[0].pos << 0, 0.0, 0; contacts[0].penetration_depth = 10.0; @@ -1856,13 +2018,13 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planeellipsoid) tf1 = transform; tf2 = transform; contacts.resize(1); - contacts[0].pos = transform * Vector3d(0, 0, 0); + contacts[0].pos = transform * Vector3(0, 0, 0); contacts[0].penetration_depth = 10.0; - contacts[0].normal = transform.linear() * Vector3d(0, -1, 0); + contacts[0].normal = transform.linear() * Vector3(0, -1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, 1.25, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, 1.25, 0))); contacts.resize(1); contacts[0].pos << 0, 1.25, 0; contacts[0].penetration_depth = 8.75; @@ -1870,15 +2032,15 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planeellipsoid) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, 1.25, 0))); + tf2 = transform * Transform3(Translation3(Vector3(0, 1.25, 0))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(0, 1.25, 0); + contacts[0].pos = transform * Vector3(0, 1.25, 0); contacts[0].penetration_depth = 8.75; - contacts[0].normal = transform.linear() * Vector3d(0, 1, 0); + contacts[0].normal = transform.linear() * Vector3(0, 1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, -1.25, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, -1.25, 0))); contacts.resize(1); contacts[0].pos << 0, -1.25, 0; contacts[0].penetration_depth = 8.75; @@ -1886,36 +2048,36 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planeellipsoid) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, -1.25, 0))); + tf2 = transform * Transform3(Translation3(Vector3(0, -1.25, 0))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(0, -1.25, 0); + contacts[0].pos = transform * Vector3(0, -1.25, 0); contacts[0].penetration_depth = 8.75; - contacts[0].normal = transform.linear() * Vector3d(0, -1, 0); + contacts[0].normal = transform.linear() * Vector3(0, -1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, 10.01, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, 10.01, 0))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, 10.01, 0))); + tf2 = transform * Transform3(Translation3(Vector3(0, 10.01, 0))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, -10.01, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, -10.01, 0))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, -10.01, 0))); + tf2 = transform * Transform3(Translation3(Vector3(0, -10.01, 0))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); - hs = Plane(Vector3d(0, 0, 1), 0); + hs = Plane(Vector3(0, 0, 1), 0); - tf1 = Transform3d::Identity(); - tf2 = Transform3d::Identity(); + tf1 = Transform3::Identity(); + tf2 = Transform3::Identity(); contacts.resize(1); contacts[0].pos << 0, 0, 0; contacts[0].penetration_depth = 20.0; @@ -1925,13 +2087,13 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planeellipsoid) tf1 = transform; tf2 = transform; contacts.resize(1); - contacts[0].pos = transform * Vector3d(0, 0, 0); + contacts[0].pos = transform * Vector3(0, 0, 0); contacts[0].penetration_depth = 20.0; - contacts[0].normal = transform.linear() * Vector3d(0, 0, -1); + contacts[0].normal = transform.linear() * Vector3(0, 0, -1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, 0, 1.25))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, 0, 1.25))); contacts.resize(1); contacts[0].pos << 0, 0, 1.25; contacts[0].penetration_depth = 18.75; @@ -1939,15 +2101,15 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planeellipsoid) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, 0, 1.25))); + tf2 = transform * Transform3(Translation3(Vector3(0, 0, 1.25))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(0, 0, 1.25); + contacts[0].pos = transform * Vector3(0, 0, 1.25); contacts[0].penetration_depth = 18.75; - contacts[0].normal = transform.linear() * Vector3d(0, 0, 1); + contacts[0].normal = transform.linear() * Vector3(0, 0, 1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, 0, -1.25))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, 0, -1.25))); contacts.resize(1); contacts[0].pos << 0, 0, -1.25; contacts[0].penetration_depth = 18.75; @@ -1955,45 +2117,52 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planeellipsoid) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, 0, -1.25))); + tf2 = transform * Transform3(Translation3(Vector3(0, 0, -1.25))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(0, 0, -1.25); + contacts[0].pos = transform * Vector3(0, 0, -1.25); contacts[0].penetration_depth = 18.75; - contacts[0].normal = transform.linear() * Vector3d(0, 0, -1); + contacts[0].normal = transform.linear() * Vector3(0, 0, -1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, 0, 20.01))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, 0, 20.01))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, 0, 20.01))); + tf2 = transform * Transform3(Translation3(Vector3(0, 0, 20.01))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, 0, -20.01))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, 0, -20.01))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, 0, -20.01))); + tf2 = transform * Transform3(Translation3(Vector3(0, 0, -20.01))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); } -GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacecapsule) +GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planeellipsoid) { - Capsule s(5, 10); - Halfspace hs(Vector3d(1, 0, 0), 0); +// test_shapeIntersection_planeellipsoid(); + test_shapeIntersection_planeellipsoid(); +} + +template +void test_shapeIntersection_halfspacecapsule() +{ + Capsule s(5, 10); + Halfspace hs(Vector3(1, 0, 0), 0); - Transform3d tf1; - Transform3d tf2; + Transform3 tf1; + Transform3 tf2; - Transform3d transform = Transform3d::Identity(); - generateRandomTransform(extents, transform); + Transform3 transform = Transform3::Identity(); + generateRandomTransform(extents(), transform); - std::vector contacts; + std::vector> contacts; - tf1 = Transform3d::Identity(); - tf2 = Transform3d::Identity(); + tf1 = Transform3::Identity(); + tf2 = Transform3::Identity(); contacts.resize(1); contacts[0].pos << -2.5, 0, 0; contacts[0].penetration_depth = 5; @@ -2003,13 +2172,13 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacecapsule) tf1 = transform; tf2 = transform; contacts.resize(1); - contacts[0].pos = transform * Vector3d(-2.5, 0, 0); + contacts[0].pos = transform * Vector3(-2.5, 0, 0); contacts[0].penetration_depth = 5; - contacts[0].normal = transform.linear() * Vector3d(-1, 0, 0); + contacts[0].normal = transform.linear() * Vector3(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(2.5, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(2.5, 0, 0))); contacts.resize(1); contacts[0].pos << -1.25, 0, 0; contacts[0].penetration_depth = 7.5; @@ -2017,15 +2186,15 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacecapsule) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(2.5, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(2.5, 0, 0))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(-1.25, 0, 0); + contacts[0].pos = transform * Vector3(-1.25, 0, 0); contacts[0].penetration_depth = 7.5; - contacts[0].normal = transform.linear() * Vector3d(-1, 0, 0); + contacts[0].normal = transform.linear() * Vector3(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(-2.5, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(-2.5, 0, 0))); contacts.resize(1); contacts[0].pos << -3.75, 0, 0; contacts[0].penetration_depth = 2.5; @@ -2033,15 +2202,15 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacecapsule) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(-2.5, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(-2.5, 0, 0))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(-3.75, 0, 0); + contacts[0].pos = transform * Vector3(-3.75, 0, 0); contacts[0].penetration_depth = 2.5; - contacts[0].normal = transform.linear() * Vector3d(-1, 0, 0); + contacts[0].normal = transform.linear() * Vector3(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(5.1, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(5.1, 0, 0))); contacts.resize(1); contacts[0].pos << 0.05, 0, 0; contacts[0].penetration_depth = 10.1; @@ -2049,28 +2218,28 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacecapsule) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(5.1, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(5.1, 0, 0))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(0.05, 0, 0); + contacts[0].pos = transform * Vector3(0.05, 0, 0); contacts[0].penetration_depth = 10.1; - contacts[0].normal = transform.linear() * Vector3d(-1, 0, 0); + contacts[0].normal = transform.linear() * Vector3(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(-5.1, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(-5.1, 0, 0))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(-5.1, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(-5.1, 0, 0))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); - hs = Halfspace(Vector3d(0, 1, 0), 0); + hs = Halfspace(Vector3(0, 1, 0), 0); - tf1 = Transform3d::Identity(); - tf2 = Transform3d::Identity(); + tf1 = Transform3::Identity(); + tf2 = Transform3::Identity(); contacts.resize(1); contacts[0].pos << 0, -2.5, 0; contacts[0].penetration_depth = 5; @@ -2080,13 +2249,13 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacecapsule) tf1 = transform; tf2 = transform; contacts.resize(1); - contacts[0].pos = transform * Vector3d(0, -2.5, 0); + contacts[0].pos = transform * Vector3(0, -2.5, 0); contacts[0].penetration_depth = 5; - contacts[0].normal = transform.linear() * Vector3d(0, -1, 0); + contacts[0].normal = transform.linear() * Vector3(0, -1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, 2.5, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, 2.5, 0))); contacts.resize(1); contacts[0].pos << 0, -1.25, 0; contacts[0].penetration_depth = 7.5; @@ -2094,15 +2263,15 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacecapsule) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, 2.5, 0))); + tf2 = transform * Transform3(Translation3(Vector3(0, 2.5, 0))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(0, -1.25, 0); + contacts[0].pos = transform * Vector3(0, -1.25, 0); contacts[0].penetration_depth = 7.5; - contacts[0].normal = transform.linear() * Vector3d(0, -1, 0); + contacts[0].normal = transform.linear() * Vector3(0, -1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, -2.5, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, -2.5, 0))); contacts.resize(1); contacts[0].pos << 0, -3.75, 0; contacts[0].penetration_depth = 2.5; @@ -2110,15 +2279,15 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacecapsule) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, -2.5, 0))); + tf2 = transform * Transform3(Translation3(Vector3(0, -2.5, 0))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(0, -3.75, 0); + contacts[0].pos = transform * Vector3(0, -3.75, 0); contacts[0].penetration_depth = 2.5; - contacts[0].normal = transform.linear() * Vector3d(0, -1, 0); + contacts[0].normal = transform.linear() * Vector3(0, -1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, 5.1, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, 5.1, 0))); contacts.resize(1); contacts[0].pos << 0, 0.05, 0; contacts[0].penetration_depth = 10.1; @@ -2126,28 +2295,28 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacecapsule) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, 5.1, 0))); + tf2 = transform * Transform3(Translation3(Vector3(0, 5.1, 0))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(0, 0.05, 0); + contacts[0].pos = transform * Vector3(0, 0.05, 0); contacts[0].penetration_depth = 10.1; - contacts[0].normal = transform.linear() * Vector3d(0, -1, 0); + contacts[0].normal = transform.linear() * Vector3(0, -1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, -5.1, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, -5.1, 0))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, -5.1, 0))); + tf2 = transform * Transform3(Translation3(Vector3(0, -5.1, 0))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); - hs = Halfspace(Vector3d(0, 0, 1), 0); + hs = Halfspace(Vector3(0, 0, 1), 0); - tf1 = Transform3d::Identity(); - tf2 = Transform3d::Identity(); + tf1 = Transform3::Identity(); + tf2 = Transform3::Identity(); contacts.resize(1); contacts[0].pos << 0, 0, -5; contacts[0].penetration_depth = 10; @@ -2157,13 +2326,13 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacecapsule) tf1 = transform; tf2 = transform; contacts.resize(1); - contacts[0].pos = transform * Vector3d(0, 0, -5); + contacts[0].pos = transform * Vector3(0, 0, -5); contacts[0].penetration_depth = 10; - contacts[0].normal = transform.linear() * Vector3d(0, 0, -1); + contacts[0].normal = transform.linear() * Vector3(0, 0, -1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, 0, 2.5))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, 0, 2.5))); contacts.resize(1); contacts[0].pos << 0, 0, -3.75; contacts[0].penetration_depth = 12.5; @@ -2171,15 +2340,15 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacecapsule) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, 0, 2.5))); + tf2 = transform * Transform3(Translation3(Vector3(0, 0, 2.5))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(0, 0, -3.75); + contacts[0].pos = transform * Vector3(0, 0, -3.75); contacts[0].penetration_depth = 12.5; - contacts[0].normal = transform.linear() * Vector3d(0, 0, -1); + contacts[0].normal = transform.linear() * Vector3(0, 0, -1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, 0, -2.5))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, 0, -2.5))); contacts.resize(1); contacts[0].pos << 0, 0, -6.25; contacts[0].penetration_depth = 7.5; @@ -2187,15 +2356,15 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacecapsule) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, 0, -2.5))); + tf2 = transform * Transform3(Translation3(Vector3(0, 0, -2.5))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(0, 0, -6.25); + contacts[0].pos = transform * Vector3(0, 0, -6.25); contacts[0].penetration_depth = 7.5; - contacts[0].normal = transform.linear() * Vector3d(0, 0, -1); + contacts[0].normal = transform.linear() * Vector3(0, 0, -1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, 0, 10.1))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, 0, 10.1))); contacts.resize(1); contacts[0].pos << 0, 0, 0.05; contacts[0].penetration_depth = 20.1; @@ -2203,37 +2372,44 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacecapsule) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, 0, 10.1))); + tf2 = transform * Transform3(Translation3(Vector3(0, 0, 10.1))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(0, 0, 0.05); + contacts[0].pos = transform * Vector3(0, 0, 0.05); contacts[0].penetration_depth = 20.1; - contacts[0].normal = transform.linear() * Vector3d(0, 0, -1); + contacts[0].normal = transform.linear() * Vector3(0, 0, -1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, 0, -10.1))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, 0, -10.1))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, 0, -10.1))); + tf2 = transform * Transform3(Translation3(Vector3(0, 0, -10.1))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); } -GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planecapsule) +GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacecapsule) +{ +// test_shapeIntersection_halfspacecapsule(); + test_shapeIntersection_halfspacecapsule(); +} + +template +void test_shapeIntersection_planecapsule() { - Capsule s(5, 10); - Plane hs(Vector3d(1, 0, 0), 0); + Capsule s(5, 10); + Plane hs(Vector3(1, 0, 0), 0); - Transform3d tf1; - Transform3d tf2; + Transform3 tf1; + Transform3 tf2; - Transform3d transform = Transform3d::Identity(); - generateRandomTransform(extents, transform); + Transform3 transform = Transform3::Identity(); + generateRandomTransform(extents(), transform); - std::vector contacts; + std::vector> contacts; - tf1 = Transform3d::Identity(); - tf2 = Transform3d::Identity(); + tf1 = Transform3::Identity(); + tf2 = Transform3::Identity(); contacts.resize(1); contacts[0].pos << 0, 0, 0; contacts[0].penetration_depth = 5; @@ -2243,13 +2419,13 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planecapsule) tf1 = transform; tf2 = transform; contacts.resize(1); - contacts[0].pos = transform * Vector3d(0, 0, 0); + contacts[0].pos = transform * Vector3(0, 0, 0); contacts[0].penetration_depth = 5; - contacts[0].normal = transform.linear() * Vector3d(1, 0, 0); // (1, 0, 0) or (-1, 0, 0) + contacts[0].normal = transform.linear() * Vector3(1, 0, 0); // (1, 0, 0) or (-1, 0, 0) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(2.5, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(2.5, 0, 0))); contacts.resize(1); contacts[0].pos << 2.5, 0, 0; contacts[0].penetration_depth = 2.5; @@ -2257,15 +2433,15 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planecapsule) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(2.5, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(2.5, 0, 0))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(2.5, 0, 0); + contacts[0].pos = transform * Vector3(2.5, 0, 0); contacts[0].penetration_depth = 2.5; - contacts[0].normal = transform.linear() * Vector3d(1, 0, 0); + contacts[0].normal = transform.linear() * Vector3(1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(-2.5, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(-2.5, 0, 0))); contacts.resize(1); contacts[0].pos << -2.5, 0, 0; contacts[0].penetration_depth = 2.5; @@ -2273,36 +2449,36 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planecapsule) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(-2.5, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(-2.5, 0, 0))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(-2.5, 0, 0); + contacts[0].pos = transform * Vector3(-2.5, 0, 0); contacts[0].penetration_depth = 2.5; - contacts[0].normal = transform.linear() * Vector3d(-1, 0, 0); + contacts[0].normal = transform.linear() * Vector3(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(5.1, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(5.1, 0, 0))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(5.1, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(5.1, 0, 0))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(-5.1, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(-5.1, 0, 0))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(-5.1, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(-5.1, 0, 0))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); - hs = Plane(Vector3d(0, 1, 0), 0); + hs = Plane(Vector3(0, 1, 0), 0); - tf1 = Transform3d::Identity(); - tf2 = Transform3d::Identity(); + tf1 = Transform3::Identity(); + tf2 = Transform3::Identity(); contacts.resize(1); contacts[0].pos << 0, 0, 0; contacts[0].penetration_depth = 5; @@ -2312,13 +2488,13 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planecapsule) tf1 = transform; tf2 = transform; contacts.resize(1); - contacts[0].pos = transform * Vector3d(0, 0, 0); + contacts[0].pos = transform * Vector3(0, 0, 0); contacts[0].penetration_depth = 5; - contacts[0].normal = transform.linear() * Vector3d(0, 1, 0); // (0, 1, 0) or (0, -1, 0) + contacts[0].normal = transform.linear() * Vector3(0, 1, 0); // (0, 1, 0) or (0, -1, 0) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, 2.5, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, 2.5, 0))); contacts.resize(1); contacts[0].pos << 0, 2.5, 0; contacts[0].penetration_depth = 2.5; @@ -2326,15 +2502,15 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planecapsule) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, 2.5, 0))); + tf2 = transform * Transform3(Translation3(Vector3(0, 2.5, 0))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(0, 2.5, 0); + contacts[0].pos = transform * Vector3(0, 2.5, 0); contacts[0].penetration_depth = 2.5; - contacts[0].normal = transform.linear() * Vector3d(0, 1, 0); + contacts[0].normal = transform.linear() * Vector3(0, 1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, -2.5, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, -2.5, 0))); contacts.resize(1); contacts[0].pos << 0, -2.5, 0; contacts[0].penetration_depth = 2.5; @@ -2342,36 +2518,36 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planecapsule) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, -2.5, 0))); + tf2 = transform * Transform3(Translation3(Vector3(0, -2.5, 0))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(0, -2.5, 0); + contacts[0].pos = transform * Vector3(0, -2.5, 0); contacts[0].penetration_depth = 2.5; - contacts[0].normal = transform.linear() * Vector3d(0, -1, 0); + contacts[0].normal = transform.linear() * Vector3(0, -1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, 5.1, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, 5.1, 0))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, 5.1, 0))); + tf2 = transform * Transform3(Translation3(Vector3(0, 5.1, 0))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, -5.1, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, -5.1, 0))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, -5.1, 0))); + tf2 = transform * Transform3(Translation3(Vector3(0, -5.1, 0))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); - hs = Plane(Vector3d(0, 0, 1), 0); + hs = Plane(Vector3(0, 0, 1), 0); - tf1 = Transform3d::Identity(); - tf2 = Transform3d::Identity(); + tf1 = Transform3::Identity(); + tf2 = Transform3::Identity(); contacts.resize(1); contacts[0].pos << 0, 0, 0; contacts[0].penetration_depth = 10; @@ -2381,13 +2557,13 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planecapsule) tf1 = transform; tf2 = transform; contacts.resize(1); - contacts[0].pos = transform * Vector3d(0, 0, 0); + contacts[0].pos = transform * Vector3(0, 0, 0); contacts[0].penetration_depth = 10; - contacts[0].normal = transform.linear() * Vector3d(0, 0, 1); // (0, 0, 1) or (0, 0, -1) + contacts[0].normal = transform.linear() * Vector3(0, 0, 1); // (0, 0, 1) or (0, 0, -1) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, 0, 2.5))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, 0, 2.5))); contacts.resize(1); contacts[0].pos << 0, 0, 2.5; contacts[0].penetration_depth = 7.5; @@ -2395,15 +2571,15 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planecapsule) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, 0, 2.5))); + tf2 = transform * Transform3(Translation3(Vector3(0, 0, 2.5))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(0, 0, 2.5); + contacts[0].pos = transform * Vector3(0, 0, 2.5); contacts[0].penetration_depth = 7.5; - contacts[0].normal = transform.linear() * Vector3d(0, 0, 1); + contacts[0].normal = transform.linear() * Vector3(0, 0, 1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, 0, -2.5))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, 0, -2.5))); contacts.resize(1); contacts[0].pos << 0, 0, -2.5; contacts[0].penetration_depth = 7.5; @@ -2411,45 +2587,52 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planecapsule) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, 0, -2.5))); + tf2 = transform * Transform3(Translation3(Vector3(0, 0, -2.5))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(0, 0, -2.5); + contacts[0].pos = transform * Vector3(0, 0, -2.5); contacts[0].penetration_depth = 7.5; - contacts[0].normal = transform.linear() * Vector3d(0, 0, -1); + contacts[0].normal = transform.linear() * Vector3(0, 0, -1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, 0, 10.1))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, 0, 10.1))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, 0, 10.1))); + tf2 = transform * Transform3(Translation3(Vector3(0, 0, 10.1))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, 0, -10.1))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, 0, -10.1))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, 0, -10.1))); + tf2 = transform * Transform3(Translation3(Vector3(0, 0, -10.1))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); } -GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacecylinder) +GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planecapsule) { - Cylinder s(5, 10); - Halfspace hs(Vector3d(1, 0, 0), 0); +// test_shapeIntersection_planecapsule(); + test_shapeIntersection_planecapsule(); +} - Transform3d tf1; - Transform3d tf2; +template +void test_shapeIntersection_halfspacecylinder() +{ + Cylinder s(5, 10); + Halfspace hs(Vector3(1, 0, 0), 0); - Transform3d transform = Transform3d::Identity(); - generateRandomTransform(extents, transform); + Transform3 tf1; + Transform3 tf2; - std::vector contacts; + Transform3 transform = Transform3::Identity(); + generateRandomTransform(extents(), transform); - tf1 = Transform3d::Identity(); - tf2 = Transform3d::Identity(); + std::vector> contacts; + + tf1 = Transform3::Identity(); + tf2 = Transform3::Identity(); contacts.resize(1); contacts[0].pos << -2.5, 0, 0; contacts[0].penetration_depth = 5; @@ -2459,13 +2642,13 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacecylinder) tf1 = transform; tf2 = transform; contacts.resize(1); - contacts[0].pos = transform * Vector3d(-2.5, 0, 0); + contacts[0].pos = transform * Vector3(-2.5, 0, 0); contacts[0].penetration_depth = 5; - contacts[0].normal = transform.linear() * Vector3d(-1, 0, 0); + contacts[0].normal = transform.linear() * Vector3(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(2.5, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(2.5, 0, 0))); contacts.resize(1); contacts[0].pos << -1.25, 0, 0; contacts[0].penetration_depth = 7.5; @@ -2473,15 +2656,15 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacecylinder) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(2.5, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(2.5, 0, 0))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(-1.25, 0, 0); + contacts[0].pos = transform * Vector3(-1.25, 0, 0); contacts[0].penetration_depth = 7.5; - contacts[0].normal = transform.linear() * Vector3d(-1, 0, 0); + contacts[0].normal = transform.linear() * Vector3(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(-2.5, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(-2.5, 0, 0))); contacts.resize(1); contacts[0].pos << -3.75, 0, 0; contacts[0].penetration_depth = 2.5; @@ -2489,15 +2672,15 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacecylinder) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(-2.5, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(-2.5, 0, 0))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(-3.75, 0, 0); + contacts[0].pos = transform * Vector3(-3.75, 0, 0); contacts[0].penetration_depth = 2.5; - contacts[0].normal = transform.linear() * Vector3d(-1, 0, 0); + contacts[0].normal = transform.linear() * Vector3(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(5.1, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(5.1, 0, 0))); contacts.resize(1); contacts[0].pos << 0.05, 0, 0; contacts[0].penetration_depth = 10.1; @@ -2505,28 +2688,28 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacecylinder) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(5.1, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(5.1, 0, 0))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(0.05, 0, 0); + contacts[0].pos = transform * Vector3(0.05, 0, 0); contacts[0].penetration_depth = 10.1; - contacts[0].normal = transform.linear() * Vector3d(-1, 0, 0); + contacts[0].normal = transform.linear() * Vector3(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(-5.1, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(-5.1, 0, 0))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(-5.1, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(-5.1, 0, 0))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); - hs = Halfspace(Vector3d(0, 1, 0), 0); + hs = Halfspace(Vector3(0, 1, 0), 0); - tf1 = Transform3d::Identity(); - tf2 = Transform3d::Identity(); + tf1 = Transform3::Identity(); + tf2 = Transform3::Identity(); contacts.resize(1); contacts[0].pos << 0, -2.5, 0; contacts[0].penetration_depth = 5; @@ -2536,13 +2719,13 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacecylinder) tf1 = transform; tf2 = transform; contacts.resize(1); - contacts[0].pos = transform * Vector3d(0, -2.5, 0); + contacts[0].pos = transform * Vector3(0, -2.5, 0); contacts[0].penetration_depth = 5; - contacts[0].normal = transform.linear() * Vector3d(0, -1, 0); + contacts[0].normal = transform.linear() * Vector3(0, -1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, 2.5, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, 2.5, 0))); contacts.resize(1); contacts[0].pos << 0, -1.25, 0; contacts[0].penetration_depth = 7.5; @@ -2550,15 +2733,15 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacecylinder) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, 2.5, 0))); + tf2 = transform * Transform3(Translation3(Vector3(0, 2.5, 0))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(0, -1.25, 0); + contacts[0].pos = transform * Vector3(0, -1.25, 0); contacts[0].penetration_depth = 7.5; - contacts[0].normal = transform.linear() * Vector3d(0, -1, 0); + contacts[0].normal = transform.linear() * Vector3(0, -1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, -2.5, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, -2.5, 0))); contacts.resize(1); contacts[0].pos << 0, -3.75, 0; contacts[0].penetration_depth = 2.5; @@ -2566,15 +2749,15 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacecylinder) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, -2.5, 0))); + tf2 = transform * Transform3(Translation3(Vector3(0, -2.5, 0))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(0, -3.75, 0); + contacts[0].pos = transform * Vector3(0, -3.75, 0); contacts[0].penetration_depth = 2.5; - contacts[0].normal = transform.linear() * Vector3d(0, -1, 0); + contacts[0].normal = transform.linear() * Vector3(0, -1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, 5.1, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, 5.1, 0))); contacts.resize(1); contacts[0].pos << 0, 0.05, 0; contacts[0].penetration_depth = 10.1; @@ -2582,28 +2765,28 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacecylinder) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, 5.1, 0))); + tf2 = transform * Transform3(Translation3(Vector3(0, 5.1, 0))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(0, 0.05, 0); + contacts[0].pos = transform * Vector3(0, 0.05, 0); contacts[0].penetration_depth = 10.1; - contacts[0].normal = transform.linear() * Vector3d(0, -1, 0); + contacts[0].normal = transform.linear() * Vector3(0, -1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, -5.1, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, -5.1, 0))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, -5.1, 0))); + tf2 = transform * Transform3(Translation3(Vector3(0, -5.1, 0))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); - hs = Halfspace(Vector3d(0, 0, 1), 0); + hs = Halfspace(Vector3(0, 0, 1), 0); - tf1 = Transform3d::Identity(); - tf2 = Transform3d::Identity(); + tf1 = Transform3::Identity(); + tf2 = Transform3::Identity(); contacts.resize(1); contacts[0].pos << 0, 0, -2.5; contacts[0].penetration_depth = 5; @@ -2613,13 +2796,13 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacecylinder) tf1 = transform; tf2 = transform; contacts.resize(1); - contacts[0].pos = transform * Vector3d(0, 0, -2.5); + contacts[0].pos = transform * Vector3(0, 0, -2.5); contacts[0].penetration_depth = 5; - contacts[0].normal = transform.linear() * Vector3d(0, 0, -1); + contacts[0].normal = transform.linear() * Vector3(0, 0, -1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, 0, 2.5))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, 0, 2.5))); contacts.resize(1); contacts[0].pos << 0, 0, -1.25; contacts[0].penetration_depth = 7.5; @@ -2627,15 +2810,15 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacecylinder) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, 0, 2.5))); + tf2 = transform * Transform3(Translation3(Vector3(0, 0, 2.5))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(0, 0, -1.25); + contacts[0].pos = transform * Vector3(0, 0, -1.25); contacts[0].penetration_depth = 7.5; - contacts[0].normal = transform.linear() * Vector3d(0, 0, -1); + contacts[0].normal = transform.linear() * Vector3(0, 0, -1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, 0, -2.5))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, 0, -2.5))); contacts.resize(1); contacts[0].pos << 0, 0, -3.75; contacts[0].penetration_depth = 2.5; @@ -2643,15 +2826,15 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacecylinder) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, 0, -2.5))); + tf2 = transform * Transform3(Translation3(Vector3(0, 0, -2.5))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(0, 0, -3.75); + contacts[0].pos = transform * Vector3(0, 0, -3.75); contacts[0].penetration_depth = 2.5; - contacts[0].normal = transform.linear() * Vector3d(0, 0, -1); + contacts[0].normal = transform.linear() * Vector3(0, 0, -1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, 0, 5.1))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, 0, 5.1))); contacts.resize(1); contacts[0].pos << 0, 0, 0.05; contacts[0].penetration_depth = 10.1; @@ -2659,37 +2842,44 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacecylinder) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, 0, 5.1))); + tf2 = transform * Transform3(Translation3(Vector3(0, 0, 5.1))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(0, 0, 0.05); + contacts[0].pos = transform * Vector3(0, 0, 0.05); contacts[0].penetration_depth = 10.1; - contacts[0].normal = transform.linear() * Vector3d(0, 0, -1); + contacts[0].normal = transform.linear() * Vector3(0, 0, -1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, 0, -5.1))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, 0, -5.1))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, 0, -5.1))); + tf2 = transform * Transform3(Translation3(Vector3(0, 0, -5.1))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); } -GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planecylinder) +GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacecylinder) +{ +// test_shapeIntersection_halfspacecylinder(); + test_shapeIntersection_halfspacecylinder(); +} + +template +void test_shapeIntersection_planecylinder() { - Cylinder s(5, 10); - Plane hs(Vector3d(1, 0, 0), 0); + Cylinder s(5, 10); + Plane hs(Vector3(1, 0, 0), 0); - Transform3d tf1; - Transform3d tf2; + Transform3 tf1; + Transform3 tf2; - Transform3d transform = Transform3d::Identity(); - generateRandomTransform(extents, transform); + Transform3 transform = Transform3::Identity(); + generateRandomTransform(extents(), transform); - std::vector contacts; + std::vector> contacts; - tf1 = Transform3d::Identity(); - tf2 = Transform3d::Identity(); + tf1 = Transform3::Identity(); + tf2 = Transform3::Identity(); contacts.resize(1); contacts[0].pos << 0, 0, 0; contacts[0].penetration_depth = 5; @@ -2699,13 +2889,13 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planecylinder) tf1 = transform; tf2 = transform; contacts.resize(1); - contacts[0].pos = transform * Vector3d(0, 0, 0); + contacts[0].pos = transform * Vector3(0, 0, 0); contacts[0].penetration_depth = 5; - contacts[0].normal = transform.linear() * Vector3d(1, 0, 0); // (1, 0, 0) or (-1, 0, 0) + contacts[0].normal = transform.linear() * Vector3(1, 0, 0); // (1, 0, 0) or (-1, 0, 0) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(2.5, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(2.5, 0, 0))); contacts.resize(1); contacts[0].pos << 2.5, 0, 0; contacts[0].penetration_depth = 2.5; @@ -2713,15 +2903,15 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planecylinder) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(2.5, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(2.5, 0, 0))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(2.5, 0, 0); + contacts[0].pos = transform * Vector3(2.5, 0, 0); contacts[0].penetration_depth = 2.5; - contacts[0].normal = transform.linear() * Vector3d(1, 0, 0); + contacts[0].normal = transform.linear() * Vector3(1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(-2.5, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(-2.5, 0, 0))); contacts.resize(1); contacts[0].pos << -2.5, 0, 0; contacts[0].penetration_depth = 2.5; @@ -2729,36 +2919,36 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planecylinder) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(-2.5, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(-2.5, 0, 0))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(-2.5, 0, 0); + contacts[0].pos = transform * Vector3(-2.5, 0, 0); contacts[0].penetration_depth = 2.5; - contacts[0].normal = transform.linear() * Vector3d(-1, 0, 0); + contacts[0].normal = transform.linear() * Vector3(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(5.1, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(5.1, 0, 0))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(5.1, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(5.1, 0, 0))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(-5.1, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(-5.1, 0, 0))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(-5.1, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(-5.1, 0, 0))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); - hs = Plane(Vector3d(0, 1, 0), 0); + hs = Plane(Vector3(0, 1, 0), 0); - tf1 = Transform3d::Identity(); - tf2 = Transform3d::Identity(); + tf1 = Transform3::Identity(); + tf2 = Transform3::Identity(); contacts.resize(1); contacts[0].pos << 0, 0, 0; contacts[0].penetration_depth = 5; @@ -2768,13 +2958,13 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planecylinder) tf1 = transform; tf2 = transform; contacts.resize(1); - contacts[0].pos = transform * Vector3d(0, 0, 0); + contacts[0].pos = transform * Vector3(0, 0, 0); contacts[0].penetration_depth = 5; - contacts[0].normal = transform.linear() * Vector3d(0, 1, 0); // (1, 0, 0) or (-1, 0, 0) + contacts[0].normal = transform.linear() * Vector3(0, 1, 0); // (1, 0, 0) or (-1, 0, 0) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, 2.5, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, 2.5, 0))); contacts.resize(1); contacts[0].pos << 0, 2.5, 0; contacts[0].penetration_depth = 2.5; @@ -2782,15 +2972,15 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planecylinder) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, 2.5, 0))); + tf2 = transform * Transform3(Translation3(Vector3(0, 2.5, 0))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(0, 2.5, 0); + contacts[0].pos = transform * Vector3(0, 2.5, 0); contacts[0].penetration_depth = 2.5; - contacts[0].normal = transform.linear() * Vector3d(0, 1, 0); + contacts[0].normal = transform.linear() * Vector3(0, 1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, -2.5, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, -2.5, 0))); contacts.resize(1); contacts[0].pos << 0, -2.5, 0; contacts[0].penetration_depth = 2.5; @@ -2798,36 +2988,36 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planecylinder) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, -2.5, 0))); + tf2 = transform * Transform3(Translation3(Vector3(0, -2.5, 0))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(0, -2.5, 0); + contacts[0].pos = transform * Vector3(0, -2.5, 0); contacts[0].penetration_depth = 2.5; - contacts[0].normal = transform.linear() * Vector3d(0, -1, 0); + contacts[0].normal = transform.linear() * Vector3(0, -1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, 5.1, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, 5.1, 0))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, 5.1, 0))); + tf2 = transform * Transform3(Translation3(Vector3(0, 5.1, 0))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, -5.1, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, -5.1, 0))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, -5.1, 0))); + tf2 = transform * Transform3(Translation3(Vector3(0, -5.1, 0))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); - hs = Plane(Vector3d(0, 0, 1), 0); + hs = Plane(Vector3(0, 0, 1), 0); - tf1 = Transform3d::Identity(); - tf2 = Transform3d::Identity(); + tf1 = Transform3::Identity(); + tf2 = Transform3::Identity(); contacts.resize(1); contacts[0].pos << 0, 0, 0; contacts[0].penetration_depth = 5; @@ -2837,13 +3027,13 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planecylinder) tf1 = transform; tf2 = transform; contacts.resize(1); - contacts[0].pos = transform * Vector3d(0, 0, 0); + contacts[0].pos = transform * Vector3(0, 0, 0); contacts[0].penetration_depth = 5; - contacts[0].normal = transform.linear() * Vector3d(0, 0, 1); // (1, 0, 0) or (-1, 0, 0) + contacts[0].normal = transform.linear() * Vector3(0, 0, 1); // (1, 0, 0) or (-1, 0, 0) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, 0, 2.5))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, 0, 2.5))); contacts.resize(1); contacts[0].pos << 0, 0, 2.5; contacts[0].penetration_depth = 2.5; @@ -2851,15 +3041,15 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planecylinder) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, 0, 2.5))); + tf2 = transform * Transform3(Translation3(Vector3(0, 0, 2.5))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(0, 0, 2.5); + contacts[0].pos = transform * Vector3(0, 0, 2.5); contacts[0].penetration_depth = 2.5; - contacts[0].normal = transform.linear() * Vector3d(0, 0, 1); + contacts[0].normal = transform.linear() * Vector3(0, 0, 1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, 0, -2.5))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, 0, -2.5))); contacts.resize(1); contacts[0].pos << 0, 0, -2.5; contacts[0].penetration_depth = 2.5; @@ -2867,46 +3057,52 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planecylinder) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, 0, -2.5))); + tf2 = transform * Transform3(Translation3(Vector3(0, 0, -2.5))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(0, 0, -2.5); + contacts[0].pos = transform * Vector3(0, 0, -2.5); contacts[0].penetration_depth = 2.5; - contacts[0].normal = transform.linear() * Vector3d(0, 0, -1); + contacts[0].normal = transform.linear() * Vector3(0, 0, -1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, 0, 10.1))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, 0, 10.1))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, 0, 10.1))); + tf2 = transform * Transform3(Translation3(Vector3(0, 0, 10.1))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, 0, -10.1))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, 0, -10.1))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, 0, -10.1))); + tf2 = transform * Transform3(Translation3(Vector3(0, 0, -10.1))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); } +GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planecylinder) +{ +// test_shapeIntersection_planecylinder(); + test_shapeIntersection_planecylinder(); +} -GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacecone) +template +void test_shapeIntersection_halfspacecone() { - Cone s(5, 10); - Halfspace hs(Vector3d(1, 0, 0), 0); + Cone s(5, 10); + Halfspace hs(Vector3(1, 0, 0), 0); - Transform3d tf1; - Transform3d tf2; + Transform3 tf1; + Transform3 tf2; - Transform3d transform = Transform3d::Identity(); - generateRandomTransform(extents, transform); + Transform3 transform = Transform3::Identity(); + generateRandomTransform(extents(), transform); - std::vector contacts; + std::vector> contacts; - tf1 = Transform3d::Identity(); - tf2 = Transform3d::Identity(); + tf1 = Transform3::Identity(); + tf2 = Transform3::Identity(); contacts.resize(1); contacts[0].pos << -2.5, 0, -5; contacts[0].penetration_depth = 5; @@ -2916,13 +3112,13 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacecone) tf1 = transform; tf2 = transform; contacts.resize(1); - contacts[0].pos = transform * Vector3d(-2.5, 0, -5); + contacts[0].pos = transform * Vector3(-2.5, 0, -5); contacts[0].penetration_depth = 5; - contacts[0].normal = transform.linear() * Vector3d(-1, 0, 0); + contacts[0].normal = transform.linear() * Vector3(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(2.5, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(2.5, 0, 0))); contacts.resize(1); contacts[0].pos << -1.25, 0, -5; contacts[0].penetration_depth = 7.5; @@ -2930,15 +3126,15 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacecone) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(2.5, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(2.5, 0, 0))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(-1.25, 0, -5); + contacts[0].pos = transform * Vector3(-1.25, 0, -5); contacts[0].penetration_depth = 7.5; - contacts[0].normal = transform.linear() * Vector3d(-1, 0, 0); + contacts[0].normal = transform.linear() * Vector3(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(-2.5, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(-2.5, 0, 0))); contacts.resize(1); contacts[0].pos << -3.75, 0, -5; contacts[0].penetration_depth = 2.5; @@ -2946,15 +3142,15 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacecone) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(-2.5, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(-2.5, 0, 0))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(-3.75, 0, -5); + contacts[0].pos = transform * Vector3(-3.75, 0, -5); contacts[0].penetration_depth = 2.5; - contacts[0].normal = transform.linear() * Vector3d(-1, 0, 0); + contacts[0].normal = transform.linear() * Vector3(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(5.1, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(5.1, 0, 0))); contacts.resize(1); contacts[0].pos << 0.05, 0, -5; contacts[0].penetration_depth = 10.1; @@ -2962,28 +3158,28 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacecone) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(5.1, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(5.1, 0, 0))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(0.05, 0, -5); + contacts[0].pos = transform * Vector3(0.05, 0, -5); contacts[0].penetration_depth = 10.1; - contacts[0].normal = transform.linear() * Vector3d(-1, 0, 0); + contacts[0].normal = transform.linear() * Vector3(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(-5.1, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(-5.1, 0, 0))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(-5.1, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(-5.1, 0, 0))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); - hs = Halfspace(Vector3d(0, 1, 0), 0); + hs = Halfspace(Vector3(0, 1, 0), 0); - tf1 = Transform3d::Identity(); - tf2 = Transform3d::Identity(); + tf1 = Transform3::Identity(); + tf2 = Transform3::Identity(); contacts.resize(1); contacts[0].pos << 0, -2.5, -5; contacts[0].penetration_depth = 5; @@ -2993,13 +3189,13 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacecone) tf1 = transform; tf2 = transform; contacts.resize(1); - contacts[0].pos = transform * Vector3d(0, -2.5, -5); + contacts[0].pos = transform * Vector3(0, -2.5, -5); contacts[0].penetration_depth = 5; - contacts[0].normal = transform.linear() * Vector3d(0, -1, 0); + contacts[0].normal = transform.linear() * Vector3(0, -1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, 2.5, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, 2.5, 0))); contacts.resize(1); contacts[0].pos << 0, -1.25, -5; contacts[0].penetration_depth = 7.5; @@ -3007,15 +3203,15 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacecone) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, 2.5, 0))); + tf2 = transform * Transform3(Translation3(Vector3(0, 2.5, 0))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(0, -1.25, -5); + contacts[0].pos = transform * Vector3(0, -1.25, -5); contacts[0].penetration_depth = 7.5; - contacts[0].normal = transform.linear() * Vector3d(0, -1, 0); + contacts[0].normal = transform.linear() * Vector3(0, -1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, -2.5, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, -2.5, 0))); contacts.resize(1); contacts[0].pos << 0, -3.75, -5; contacts[0].penetration_depth = 2.5; @@ -3023,15 +3219,15 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacecone) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, -2.5, 0))); + tf2 = transform * Transform3(Translation3(Vector3(0, -2.5, 0))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(0, -3.75, -5); + contacts[0].pos = transform * Vector3(0, -3.75, -5); contacts[0].penetration_depth = 2.5; - contacts[0].normal = transform.linear() * Vector3d(0, -1, 0); + contacts[0].normal = transform.linear() * Vector3(0, -1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, 5.1, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, 5.1, 0))); contacts.resize(1); contacts[0].pos << 0, 0.05, -5; contacts[0].penetration_depth = 10.1; @@ -3039,28 +3235,28 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacecone) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, 5.1, 0))); + tf2 = transform * Transform3(Translation3(Vector3(0, 5.1, 0))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(0, 0.05, -5); + contacts[0].pos = transform * Vector3(0, 0.05, -5); contacts[0].penetration_depth = 10.1; - contacts[0].normal = transform.linear() * Vector3d(0, -1, 0); + contacts[0].normal = transform.linear() * Vector3(0, -1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, -5.1, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, -5.1, 0))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, -5.1, 0))); + tf2 = transform * Transform3(Translation3(Vector3(0, -5.1, 0))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); - hs = Halfspace(Vector3d(0, 0, 1), 0); + hs = Halfspace(Vector3(0, 0, 1), 0); - tf1 = Transform3d::Identity(); - tf2 = Transform3d::Identity(); + tf1 = Transform3::Identity(); + tf2 = Transform3::Identity(); contacts.resize(1); contacts[0].pos << 0, 0, -2.5; contacts[0].penetration_depth = 5; @@ -3070,13 +3266,13 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacecone) tf1 = transform; tf2 = transform; contacts.resize(1); - contacts[0].pos = transform * Vector3d(0, 0, -2.5); + contacts[0].pos = transform * Vector3(0, 0, -2.5); contacts[0].penetration_depth = 5; - contacts[0].normal = transform.linear() * Vector3d(0, 0, -1); + contacts[0].normal = transform.linear() * Vector3(0, 0, -1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, 0, 2.5))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, 0, 2.5))); contacts.resize(1); contacts[0].pos << 0, 0, -1.25; contacts[0].penetration_depth = 7.5; @@ -3084,15 +3280,15 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacecone) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, 0, 2.5))); + tf2 = transform * Transform3(Translation3(Vector3(0, 0, 2.5))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(0, 0, -1.25); + contacts[0].pos = transform * Vector3(0, 0, -1.25); contacts[0].penetration_depth = 7.5; - contacts[0].normal = transform.linear() * Vector3d(0, 0, -1); + contacts[0].normal = transform.linear() * Vector3(0, 0, -1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, 0, -2.5))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, 0, -2.5))); contacts.resize(1); contacts[0].pos << 0, 0, -3.75; contacts[0].penetration_depth= 2.5; @@ -3100,15 +3296,15 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacecone) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, 0, -2.5))); + tf2 = transform * Transform3(Translation3(Vector3(0, 0, -2.5))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(0, 0, -3.75); + contacts[0].pos = transform * Vector3(0, 0, -3.75); contacts[0].penetration_depth = 2.5; - contacts[0].normal = transform.linear() * Vector3d(0, 0, -1); + contacts[0].normal = transform.linear() * Vector3(0, 0, -1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, 0, 5.1))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, 0, 5.1))); contacts.resize(1); contacts[0].pos << 0, 0, 0.05; contacts[0].penetration_depth = 10.1; @@ -3116,37 +3312,44 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacecone) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, 0, 5.1))); + tf2 = transform * Transform3(Translation3(Vector3(0, 0, 5.1))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(0, 0, 0.05); + contacts[0].pos = transform * Vector3(0, 0, 0.05); contacts[0].penetration_depth = 10.1; - contacts[0].normal = transform.linear() * Vector3d(0, 0, -1); + contacts[0].normal = transform.linear() * Vector3(0, 0, -1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, 0, -5.1))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, 0, -5.1))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, 0, -5.1))); + tf2 = transform * Transform3(Translation3(Vector3(0, 0, -5.1))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); } -GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planecone) +GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacecone) { - Cone s(5, 10); - Plane hs(Vector3d(1, 0, 0), 0); +// test_shapeIntersection_halfspacecone(); + test_shapeIntersection_halfspacecone(); +} + +template +void test_shapeIntersection_planecone() +{ + Cone s(5, 10); + Plane hs(Vector3(1, 0, 0), 0); - Transform3d tf1; - Transform3d tf2; + Transform3 tf1; + Transform3 tf2; - Transform3d transform = Transform3d::Identity(); - generateRandomTransform(extents, transform); + Transform3 transform = Transform3::Identity(); + generateRandomTransform(extents(), transform); - std::vector contacts; + std::vector> contacts; - tf1 = Transform3d::Identity(); - tf2 = Transform3d::Identity(); + tf1 = Transform3::Identity(); + tf2 = Transform3::Identity(); contacts.resize(1); contacts[0].pos << 0, 0, 0; contacts[0].penetration_depth = 5; @@ -3156,13 +3359,13 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planecone) tf1 = transform; tf2 = transform; contacts.resize(1); - contacts[0].pos = transform * Vector3d(0, 0, 0); + contacts[0].pos = transform * Vector3(0, 0, 0); contacts[0].penetration_depth = 5; - contacts[0].normal = transform.linear() * Vector3d(-1, 0, 0); // (1, 0, 0) or (-1, 0, 0) + contacts[0].normal = transform.linear() * Vector3(-1, 0, 0); // (1, 0, 0) or (-1, 0, 0) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(2.5, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(2.5, 0, 0))); contacts.resize(1); contacts[0].pos << 2.5, 0, -2.5; contacts[0].penetration_depth = 2.5; @@ -3170,15 +3373,15 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planecone) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(2.5, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(2.5, 0, 0))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(2.5, 0, -2.5); + contacts[0].pos = transform * Vector3(2.5, 0, -2.5); contacts[0].penetration_depth = 2.5; - contacts[0].normal = transform.linear() * Vector3d(1, 0, 0); + contacts[0].normal = transform.linear() * Vector3(1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(-2.5, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(-2.5, 0, 0))); contacts.resize(1); contacts[0].pos << -2.5, 0, -2.5; contacts[0].penetration_depth = 2.5; @@ -3186,36 +3389,36 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planecone) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(-2.5, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(-2.5, 0, 0))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(-2.5, 0, -2.5); + contacts[0].pos = transform * Vector3(-2.5, 0, -2.5); contacts[0].penetration_depth = 2.5; - contacts[0].normal = transform.linear() * Vector3d(-1, 0, 0); + contacts[0].normal = transform.linear() * Vector3(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(5.1, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(5.1, 0, 0))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(5.1, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(5.1, 0, 0))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(-5.1, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(-5.1, 0, 0))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(-5.1, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(-5.1, 0, 0))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); - hs = Plane(Vector3d(0, 1, 0), 0); + hs = Plane(Vector3(0, 1, 0), 0); - tf1 = Transform3d::Identity(); - tf2 = Transform3d::Identity(); + tf1 = Transform3::Identity(); + tf2 = Transform3::Identity(); contacts.resize(1); contacts[0].pos << 0, 0, 0; contacts[0].penetration_depth = 5; @@ -3225,13 +3428,13 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planecone) tf1 = transform; tf2 = transform; contacts.resize(1); - contacts[0].pos = transform * Vector3d(0, 0, 0); + contacts[0].pos = transform * Vector3(0, 0, 0); contacts[0].penetration_depth = 5; - contacts[0].normal = transform.linear() * Vector3d(0, 1, 0); // (1, 0, 0) or (-1, 0, 0) + contacts[0].normal = transform.linear() * Vector3(0, 1, 0); // (1, 0, 0) or (-1, 0, 0) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, 2.5, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, 2.5, 0))); contacts.resize(1); contacts[0].pos << 0, 2.5, -2.5; contacts[0].penetration_depth = 2.5; @@ -3239,15 +3442,15 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planecone) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, 2.5, 0))); + tf2 = transform * Transform3(Translation3(Vector3(0, 2.5, 0))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(0, 2.5, -2.5); + contacts[0].pos = transform * Vector3(0, 2.5, -2.5); contacts[0].penetration_depth = 2.5; - contacts[0].normal = transform.linear() * Vector3d(0, 1, 0); + contacts[0].normal = transform.linear() * Vector3(0, 1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, -2.5, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, -2.5, 0))); contacts.resize(1); contacts[0].pos << 0, -2.5, -2.5; contacts[0].penetration_depth = 2.5; @@ -3255,36 +3458,36 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planecone) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, -2.5, 0))); + tf2 = transform * Transform3(Translation3(Vector3(0, -2.5, 0))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(0, -2.5, -2.5); + contacts[0].pos = transform * Vector3(0, -2.5, -2.5); contacts[0].penetration_depth = 2.5; - contacts[0].normal = transform.linear() * Vector3d(0, -1, 0); + contacts[0].normal = transform.linear() * Vector3(0, -1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, 5.1, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, 5.1, 0))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, 5.1, 0))); + tf2 = transform * Transform3(Translation3(Vector3(0, 5.1, 0))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, -5.1, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, -5.1, 0))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, -5.1, 0))); + tf2 = transform * Transform3(Translation3(Vector3(0, -5.1, 0))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); - hs = Plane(Vector3d(0, 0, 1), 0); + hs = Plane(Vector3(0, 0, 1), 0); - tf1 = Transform3d::Identity(); - tf2 = Transform3d::Identity(); + tf1 = Transform3::Identity(); + tf2 = Transform3::Identity(); contacts.resize(1); contacts[0].pos << 0, 0, 0; contacts[0].penetration_depth = 5; @@ -3294,13 +3497,13 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planecone) tf1 = transform; tf2 = transform; contacts.resize(1); - contacts[0].pos = transform * Vector3d(0, 0, 0); + contacts[0].pos = transform * Vector3(0, 0, 0); contacts[0].penetration_depth = 5; - contacts[0].normal = transform.linear() * Vector3d(0, 0, 1); // (1, 0, 0) or (-1, 0, 0) + contacts[0].normal = transform.linear() * Vector3(0, 0, 1); // (1, 0, 0) or (-1, 0, 0) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, 0, 2.5))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, 0, 2.5))); contacts.resize(1); contacts[0].pos << 0, 0, 2.5; contacts[0].penetration_depth = 2.5; @@ -3308,15 +3511,15 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planecone) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, 0, 2.5))); + tf2 = transform * Transform3(Translation3(Vector3(0, 0, 2.5))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(0, 0, 2.5); + contacts[0].pos = transform * Vector3(0, 0, 2.5); contacts[0].penetration_depth = 2.5; - contacts[0].normal = transform.linear() * Vector3d(0, 0, 1); + contacts[0].normal = transform.linear() * Vector3(0, 0, 1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, 0, -2.5))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, 0, -2.5))); contacts.resize(1); contacts[0].pos << 0, 0, -2.5; contacts[0].penetration_depth = 2.5; @@ -3324,30 +3527,36 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planecone) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, 0, -2.5))); + tf2 = transform * Transform3(Translation3(Vector3(0, 0, -2.5))); contacts.resize(1); - contacts[0].pos = transform * Vector3d(0, 0, -2.5); + contacts[0].pos = transform * Vector3(0, 0, -2.5); contacts[0].penetration_depth = 2.5; - contacts[0].normal = transform.linear() * Vector3d(0, 0, -1); + contacts[0].normal = transform.linear() * Vector3(0, 0, -1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, 0, 10.1))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, 0, 10.1))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, 0, 10.1))); + tf2 = transform * Transform3(Translation3(Vector3(0, 0, 10.1))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, 0, -10.1))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, 0, -10.1))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, 0, -10.1))); + tf2 = transform * Transform3(Translation3(Vector3(0, 0, -10.1))); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); } +GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planecone) +{ +// test_shapeIntersection_planecone(); + test_shapeIntersection_planecone(); +} + // Shape distance test coverage (libccd) // // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ @@ -3372,338 +3581,387 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planecone) // | triangle |/////|////////|///////////|/////////|//////|//////////|///////|////////////| | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeDistance_spheresphere) +template +void test_shapeDistance_spheresphere() { - Sphere s1(20); - Sphere s2(10); + Sphere s1(20); + Sphere s2(10); - Transform3d transform = Transform3d::Identity(); - //generateRandomTransform(extents, transform); + Transform3 transform = Transform3::Identity(); + //generateRandomTransform(extents(), transform); bool res; - FCL_REAL dist = -1; - Vector3d closest_p1, closest_p2; - res = solver1.shapeDistance(s1, Transform3d::Identity(), s2, Transform3d(Eigen::Translation3d(Vector3d(0, 40, 0))), &dist, &closest_p1, &closest_p2); + S dist = -1; + Vector3 closest_p1, closest_p2; + res = solver1().shapeDistance(s1, Transform3::Identity(), s2, Transform3(Translation3(Vector3(0, 40, 0))), &dist, &closest_p1, &closest_p2); EXPECT_TRUE(fabs(dist - 10) < 0.001); EXPECT_TRUE(res); - res = solver1.shapeDistance(s1, Transform3d::Identity(), s2, Transform3d(Eigen::Translation3d(Vector3d(30.1, 0, 0))), &dist); + res = solver1().shapeDistance(s1, Transform3::Identity(), s2, Transform3(Translation3(Vector3(30.1, 0, 0))), &dist); EXPECT_TRUE(fabs(dist - 0.1) < 0.001); EXPECT_TRUE(res); - res = solver1.shapeDistance(s1, Transform3d::Identity(), s2, Transform3d(Eigen::Translation3d(Vector3d(29.9, 0, 0))), &dist); + res = solver1().shapeDistance(s1, Transform3::Identity(), s2, Transform3(Translation3(Vector3(29.9, 0, 0))), &dist); EXPECT_TRUE(dist < 0); - EXPECT_TRUE_FALSE(res); + EXPECT_FALSE(res); - res = solver1.shapeDistance(s1, Transform3d(Eigen::Translation3d(Vector3d(40, 0, 0))), s2, Transform3d::Identity(), &dist); + res = solver1().shapeDistance(s1, Transform3(Translation3(Vector3(40, 0, 0))), s2, Transform3::Identity(), &dist); EXPECT_TRUE(fabs(dist - 10) < 0.001); EXPECT_TRUE(res); - res = solver1.shapeDistance(s1, Transform3d(Eigen::Translation3d(Vector3d(30.1, 0, 0))), s2, Transform3d::Identity(), &dist); + res = solver1().shapeDistance(s1, Transform3(Translation3(Vector3(30.1, 0, 0))), s2, Transform3::Identity(), &dist); EXPECT_TRUE(fabs(dist - 0.1) < 0.001); EXPECT_TRUE(res); - res = solver1.shapeDistance(s1, Transform3d(Eigen::Translation3d(Vector3d(29.9, 0, 0))), s2, Transform3d::Identity(), &dist); + res = solver1().shapeDistance(s1, Transform3(Translation3(Vector3(29.9, 0, 0))), s2, Transform3::Identity(), &dist); EXPECT_TRUE(dist < 0); - EXPECT_TRUE_FALSE(res); + EXPECT_FALSE(res); - res = solver1.shapeDistance(s1, transform, s2, transform * Transform3d(Eigen::Translation3d(Vector3d(40, 0, 0))), &dist); + res = solver1().shapeDistance(s1, transform, s2, transform * Transform3(Translation3(Vector3(40, 0, 0))), &dist); // this is one problem: the precise is low sometimes EXPECT_TRUE(fabs(dist - 10) < 0.1); EXPECT_TRUE(res); - res = solver1.shapeDistance(s1, transform, s2, transform * Transform3d(Eigen::Translation3d(Vector3d(30.1, 0, 0))), &dist); + res = solver1().shapeDistance(s1, transform, s2, transform * Transform3(Translation3(Vector3(30.1, 0, 0))), &dist); EXPECT_TRUE(fabs(dist - 0.1) < 0.06); EXPECT_TRUE(res); - res = solver1.shapeDistance(s1, transform, s2, transform * Transform3d(Eigen::Translation3d(Vector3d(29.9, 0, 0))), &dist); + res = solver1().shapeDistance(s1, transform, s2, transform * Transform3(Translation3(Vector3(29.9, 0, 0))), &dist); EXPECT_TRUE(dist < 0); - EXPECT_TRUE_FALSE(res); + EXPECT_FALSE(res); - res = solver1.shapeDistance(s1, transform * Transform3d(Eigen::Translation3d(Vector3d(40, 0, 0))), s2, transform, &dist); + res = solver1().shapeDistance(s1, transform * Transform3(Translation3(Vector3(40, 0, 0))), s2, transform, &dist); EXPECT_TRUE(fabs(dist - 10) < 0.1); EXPECT_TRUE(res); - res = solver1.shapeDistance(s1, transform * Transform3d(Eigen::Translation3d(Vector3d(30.1, 0, 0))), s2, transform, &dist); + res = solver1().shapeDistance(s1, transform * Transform3(Translation3(Vector3(30.1, 0, 0))), s2, transform, &dist); EXPECT_TRUE(fabs(dist - 0.1) < 0.1); EXPECT_TRUE(res); - res = solver1.shapeDistance(s1, transform * Transform3d(Eigen::Translation3d(Vector3d(29.9, 0, 0))), s2, transform, &dist); + res = solver1().shapeDistance(s1, transform * Transform3(Translation3(Vector3(29.9, 0, 0))), s2, transform, &dist); EXPECT_TRUE(dist < 0); - EXPECT_TRUE_FALSE(res); + EXPECT_FALSE(res); } -GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeDistance_boxbox) +GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeDistance_spheresphere) +{ +// test_shapeDistance_spheresphere(); + test_shapeDistance_spheresphere(); +} + +template +void test_shapeDistance_boxbox() { - Box s1(20, 40, 50); - Box s2(10, 10, 10); - Vector3d closest_p1, closest_p2; + Box s1(20, 40, 50); + Box s2(10, 10, 10); + Vector3 closest_p1, closest_p2; - Transform3d transform = Transform3d::Identity(); - //generateRandomTransform(extents, transform); + Transform3 transform = Transform3::Identity(); + //generateRandomTransform(extents(), transform); bool res; - FCL_REAL dist; + S dist; - res = solver1.shapeDistance(s1, Transform3d::Identity(), s2, Transform3d::Identity(), &dist); + res = solver1().shapeDistance(s1, Transform3::Identity(), s2, Transform3::Identity(), &dist); EXPECT_TRUE(dist < 0); - EXPECT_TRUE_FALSE(res); + EXPECT_FALSE(res); - res = solver1.shapeDistance(s1, transform, s2, transform, &dist); + res = solver1().shapeDistance(s1, transform, s2, transform, &dist); EXPECT_TRUE(dist < 0); - EXPECT_TRUE_FALSE(res); + EXPECT_FALSE(res); - res = solver1.shapeDistance(s2, Transform3d::Identity(), s2, Transform3d(Eigen::Translation3d(Vector3d(10.1, 0, 0))), &dist, &closest_p1, &closest_p2); + res = solver1().shapeDistance(s2, Transform3::Identity(), s2, Transform3(Translation3(Vector3(10.1, 0, 0))), &dist, &closest_p1, &closest_p2); EXPECT_TRUE(fabs(dist - 0.1) < 0.001); EXPECT_TRUE(res); - res = solver1.shapeDistance(s2, Transform3d::Identity(), s2, Transform3d(Eigen::Translation3d(Vector3d(20.1, 0, 0))), &dist, &closest_p1, &closest_p2); + res = solver1().shapeDistance(s2, Transform3::Identity(), s2, Transform3(Translation3(Vector3(20.1, 0, 0))), &dist, &closest_p1, &closest_p2); EXPECT_TRUE(fabs(dist - 10.1) < 0.001); EXPECT_TRUE(res); - res = solver1.shapeDistance(s2, Transform3d::Identity(), s2, Transform3d(Eigen::Translation3d(Vector3d(0, 20.2, 0))), &dist, &closest_p1, &closest_p2); + res = solver1().shapeDistance(s2, Transform3::Identity(), s2, Transform3(Translation3(Vector3(0, 20.2, 0))), &dist, &closest_p1, &closest_p2); EXPECT_TRUE(fabs(dist - 10.2) < 0.001); EXPECT_TRUE(res); - res = solver1.shapeDistance(s2, Transform3d::Identity(), s2, Transform3d(Eigen::Translation3d(Vector3d(10.1, 10.1, 0))), &dist, &closest_p1, &closest_p2); + res = solver1().shapeDistance(s2, Transform3::Identity(), s2, Transform3(Translation3(Vector3(10.1, 10.1, 0))), &dist, &closest_p1, &closest_p2); EXPECT_TRUE(fabs(dist - 0.1 * 1.414) < 0.001); EXPECT_TRUE(res); - res = solver2.shapeDistance(s2, Transform3d::Identity(), s2, Transform3d(Eigen::Translation3d(Vector3d(10.1, 0, 0))), &dist, &closest_p1, &closest_p2); + res = solver2().shapeDistance(s2, Transform3::Identity(), s2, Transform3(Translation3(Vector3(10.1, 0, 0))), &dist, &closest_p1, &closest_p2); EXPECT_TRUE(fabs(dist - 0.1) < 0.001); EXPECT_TRUE(res); - res = solver2.shapeDistance(s2, Transform3d::Identity(), s2, Transform3d(Eigen::Translation3d(Vector3d(20.1, 0, 0))), &dist, &closest_p1, &closest_p2); + res = solver2().shapeDistance(s2, Transform3::Identity(), s2, Transform3(Translation3(Vector3(20.1, 0, 0))), &dist, &closest_p1, &closest_p2); EXPECT_TRUE(fabs(dist - 10.1) < 0.001); EXPECT_TRUE(res); - res = solver2.shapeDistance(s2, Transform3d::Identity(), s2, Transform3d(Eigen::Translation3d(Vector3d(0, 20.1, 0))), &dist, &closest_p1, &closest_p2); + res = solver2().shapeDistance(s2, Transform3::Identity(), s2, Transform3(Translation3(Vector3(0, 20.1, 0))), &dist, &closest_p1, &closest_p2); EXPECT_TRUE(fabs(dist - 10.1) < 0.001); EXPECT_TRUE(res); - res = solver2.shapeDistance(s2, Transform3d::Identity(), s2, Transform3d(Eigen::Translation3d(Vector3d(10.1, 10.1, 0))), &dist, &closest_p1, &closest_p2); + res = solver2().shapeDistance(s2, Transform3::Identity(), s2, Transform3(Translation3(Vector3(10.1, 10.1, 0))), &dist, &closest_p1, &closest_p2); EXPECT_TRUE(fabs(dist - 0.1 * 1.414) < 0.001); EXPECT_TRUE(res); - res = solver1.shapeDistance(s1, transform, s2, transform * Transform3d(Eigen::Translation3d(Vector3d(15.1, 0, 0))), &dist); + res = solver1().shapeDistance(s1, transform, s2, transform * Transform3(Translation3(Vector3(15.1, 0, 0))), &dist); EXPECT_TRUE(fabs(dist - 0.1) < 0.001); EXPECT_TRUE(res); - res = solver1.shapeDistance(s1, Transform3d::Identity(), s2, Transform3d(Eigen::Translation3d(Vector3d(20, 0, 0))), &dist); + res = solver1().shapeDistance(s1, Transform3::Identity(), s2, Transform3(Translation3(Vector3(20, 0, 0))), &dist); EXPECT_TRUE(fabs(dist - 5) < 0.001); EXPECT_TRUE(res); - res = solver1.shapeDistance(s1, transform, s2, transform * Transform3d(Eigen::Translation3d(Vector3d(20, 0, 0))), &dist); + res = solver1().shapeDistance(s1, transform, s2, transform * Transform3(Translation3(Vector3(20, 0, 0))), &dist); EXPECT_TRUE(fabs(dist - 5) < 0.001); EXPECT_TRUE(res); } -GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeDistance_boxsphere) +GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeDistance_boxbox) +{ +// test_shapeDistance_boxbox(); + test_shapeDistance_boxbox(); +} + +template +void test_shapeDistance_boxsphere() { - Sphere s1(20); - Box s2(5, 5, 5); + Sphere s1(20); + Box s2(5, 5, 5); - Transform3d transform = Transform3d::Identity(); - generateRandomTransform(extents, transform); + Transform3 transform = Transform3::Identity(); + generateRandomTransform(extents(), transform); bool res; - FCL_REAL dist; + S dist; - res = solver1.shapeDistance(s1, Transform3d::Identity(), s2, Transform3d::Identity(), &dist); + res = solver1().shapeDistance(s1, Transform3::Identity(), s2, Transform3::Identity(), &dist); EXPECT_TRUE(dist < 0); - EXPECT_TRUE_FALSE(res); + EXPECT_FALSE(res); - res = solver1.shapeDistance(s1, transform, s2, transform, &dist); + res = solver1().shapeDistance(s1, transform, s2, transform, &dist); EXPECT_TRUE(dist < 0); - EXPECT_TRUE_FALSE(res); + EXPECT_FALSE(res); - res = solver1.shapeDistance(s1, Transform3d::Identity(), s2, Transform3d(Eigen::Translation3d(Vector3d(22.6, 0, 0))), &dist); + res = solver1().shapeDistance(s1, Transform3::Identity(), s2, Transform3(Translation3(Vector3(22.6, 0, 0))), &dist); EXPECT_TRUE(fabs(dist - 0.1) < 0.001); EXPECT_TRUE(res); - res = solver1.shapeDistance(s1, transform, s2, transform * Transform3d(Eigen::Translation3d(Vector3d(22.6, 0, 0))), &dist); + res = solver1().shapeDistance(s1, transform, s2, transform * Transform3(Translation3(Vector3(22.6, 0, 0))), &dist); EXPECT_TRUE(fabs(dist - 0.1) < 0.05); EXPECT_TRUE(res); - res = solver1.shapeDistance(s1, Transform3d::Identity(), s2, Transform3d(Eigen::Translation3d(Vector3d(40, 0, 0))), &dist); + res = solver1().shapeDistance(s1, Transform3::Identity(), s2, Transform3(Translation3(Vector3(40, 0, 0))), &dist); EXPECT_TRUE(fabs(dist - 17.5) < 0.001); EXPECT_TRUE(res); - res = solver1.shapeDistance(s1, transform, s2, transform * Transform3d(Eigen::Translation3d(Vector3d(40, 0, 0))), &dist); + res = solver1().shapeDistance(s1, transform, s2, transform * Transform3(Translation3(Vector3(40, 0, 0))), &dist); EXPECT_TRUE(fabs(dist - 17.5) < 0.001); EXPECT_TRUE(res); } -GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeDistance_cylindercylinder) +GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeDistance_boxsphere) +{ +// test_shapeDistance_boxsphere(); + test_shapeDistance_boxsphere(); +} + +template +void test_shapeDistance_cylindercylinder() { - Cylinder s1(5, 10); - Cylinder s2(5, 10); + Cylinder s1(5, 10); + Cylinder s2(5, 10); - Transform3d transform = Transform3d::Identity(); - generateRandomTransform(extents, transform); + Transform3 transform = Transform3::Identity(); + generateRandomTransform(extents(), transform); bool res; - FCL_REAL dist; + S dist; - res = solver1.shapeDistance(s1, Transform3d::Identity(), s2, Transform3d::Identity(), &dist); + res = solver1().shapeDistance(s1, Transform3::Identity(), s2, Transform3::Identity(), &dist); EXPECT_TRUE(dist < 0); - EXPECT_TRUE_FALSE(res); + EXPECT_FALSE(res); - res = solver1.shapeDistance(s1, transform, s2, transform, &dist); + res = solver1().shapeDistance(s1, transform, s2, transform, &dist); EXPECT_TRUE(dist < 0); - EXPECT_TRUE_FALSE(res); + EXPECT_FALSE(res); - res = solver1.shapeDistance(s1, Transform3d::Identity(), s2, Transform3d(Eigen::Translation3d(Vector3d(10.1, 0, 0))), &dist); + res = solver1().shapeDistance(s1, Transform3::Identity(), s2, Transform3(Translation3(Vector3(10.1, 0, 0))), &dist); EXPECT_TRUE(fabs(dist - 0.1) < 0.001); EXPECT_TRUE(res); - res = solver1.shapeDistance(s1, transform, s2, transform * Transform3d(Eigen::Translation3d(Vector3d(10.1, 0, 0))), &dist); + res = solver1().shapeDistance(s1, transform, s2, transform * Transform3(Translation3(Vector3(10.1, 0, 0))), &dist); EXPECT_TRUE(fabs(dist - 0.1) < 0.001); EXPECT_TRUE(res); - res = solver1.shapeDistance(s1, Transform3d::Identity(), s2, Transform3d(Eigen::Translation3d(Vector3d(40, 0, 0))), &dist); + res = solver1().shapeDistance(s1, Transform3::Identity(), s2, Transform3(Translation3(Vector3(40, 0, 0))), &dist); EXPECT_TRUE(fabs(dist - 30) < 0.001); EXPECT_TRUE(res); - res = solver1.shapeDistance(s1, transform, s2, transform * Transform3d(Eigen::Translation3d(Vector3d(40, 0, 0))), &dist); + res = solver1().shapeDistance(s1, transform, s2, transform * Transform3(Translation3(Vector3(40, 0, 0))), &dist); EXPECT_TRUE(fabs(dist - 30) < 0.001); EXPECT_TRUE(res); } -GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeDistance_conecone) +GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeDistance_cylindercylinder) { - Cone s1(5, 10); - Cone s2(5, 10); +// test_shapeDistance_cylindercylinder(); + test_shapeDistance_cylindercylinder(); +} - Transform3d transform = Transform3d::Identity(); - generateRandomTransform(extents, transform); +template +void test_shapeDistance_conecone() +{ + Cone s1(5, 10); + Cone s2(5, 10); + + Transform3 transform = Transform3::Identity(); + generateRandomTransform(extents(), transform); bool res; - FCL_REAL dist; + S dist; - res = solver1.shapeDistance(s1, Transform3d::Identity(), s2, Transform3d::Identity(), &dist); + res = solver1().shapeDistance(s1, Transform3::Identity(), s2, Transform3::Identity(), &dist); EXPECT_TRUE(dist < 0); - EXPECT_TRUE_FALSE(res); + EXPECT_FALSE(res); - res = solver1.shapeDistance(s1, transform, s2, transform, &dist); + res = solver1().shapeDistance(s1, transform, s2, transform, &dist); EXPECT_TRUE(dist < 0); - EXPECT_TRUE_FALSE(res); + EXPECT_FALSE(res); - res = solver1.shapeDistance(s1, Transform3d::Identity(), s2, Transform3d(Eigen::Translation3d(Vector3d(10.1, 0, 0))), &dist); + res = solver1().shapeDistance(s1, Transform3::Identity(), s2, Transform3(Translation3(Vector3(10.1, 0, 0))), &dist); EXPECT_TRUE(fabs(dist - 0.1) < 0.001); EXPECT_TRUE(res); - res = solver1.shapeDistance(s1, transform, s2, transform * Transform3d(Eigen::Translation3d(Vector3d(10.1, 0, 0))), &dist); + res = solver1().shapeDistance(s1, transform, s2, transform * Transform3(Translation3(Vector3(10.1, 0, 0))), &dist); EXPECT_TRUE(fabs(dist - 0.1) < 0.001); EXPECT_TRUE(res); - res = solver1.shapeDistance(s1, Transform3d::Identity(), s2, Transform3d(Eigen::Translation3d(Vector3d(0, 0, 40))), &dist); + res = solver1().shapeDistance(s1, Transform3::Identity(), s2, Transform3(Translation3(Vector3(0, 0, 40))), &dist); EXPECT_TRUE(fabs(dist - 30) < 1); EXPECT_TRUE(res); - res = solver1.shapeDistance(s1, transform, s2, transform * Transform3d(Eigen::Translation3d(Vector3d(0, 0, 40))), &dist); + res = solver1().shapeDistance(s1, transform, s2, transform * Transform3(Translation3(Vector3(0, 0, 40))), &dist); EXPECT_TRUE(fabs(dist - 30) < 1); EXPECT_TRUE(res); } -GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeDistance_conecylinder) +GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeDistance_conecone) { - Cylinder s1(5, 10); - Cone s2(5, 10); +// test_shapeDistance_conecone(); + test_shapeDistance_conecone(); +} + +template +void test_shapeDistance_conecylinder() +{ + Cylinder s1(5, 10); + Cone s2(5, 10); - Transform3d transform = Transform3d::Identity(); - generateRandomTransform(extents, transform); + Transform3 transform = Transform3::Identity(); + generateRandomTransform(extents(), transform); bool res; - FCL_REAL dist; + S dist; - res = solver1.shapeDistance(s1, Transform3d::Identity(), s2, Transform3d::Identity(), &dist); + res = solver1().shapeDistance(s1, Transform3::Identity(), s2, Transform3::Identity(), &dist); EXPECT_TRUE(dist < 0); - EXPECT_TRUE_FALSE(res); + EXPECT_FALSE(res); - res = solver1.shapeDistance(s1, transform, s2, transform, &dist); + res = solver1().shapeDistance(s1, transform, s2, transform, &dist); EXPECT_TRUE(dist < 0); - EXPECT_TRUE_FALSE(res); + EXPECT_FALSE(res); - res = solver1.shapeDistance(s1, Transform3d::Identity(), s2, Transform3d(Eigen::Translation3d(Vector3d(10.1, 0, 0))), &dist); + res = solver1().shapeDistance(s1, Transform3::Identity(), s2, Transform3(Translation3(Vector3(10.1, 0, 0))), &dist); EXPECT_TRUE(fabs(dist - 0.1) < 0.01); EXPECT_TRUE(res); - res = solver1.shapeDistance(s1, transform, s2, transform * Transform3d(Eigen::Translation3d(Vector3d(10.1, 0, 0))), &dist); + res = solver1().shapeDistance(s1, transform, s2, transform * Transform3(Translation3(Vector3(10.1, 0, 0))), &dist); EXPECT_TRUE(fabs(dist - 0.1) < 0.02); EXPECT_TRUE(res); - res = solver1.shapeDistance(s1, Transform3d::Identity(), s2, Transform3d(Eigen::Translation3d(Vector3d(40, 0, 0))), &dist); + res = solver1().shapeDistance(s1, Transform3::Identity(), s2, Transform3(Translation3(Vector3(40, 0, 0))), &dist); EXPECT_TRUE(fabs(dist - 30) < 0.01); EXPECT_TRUE(res); - res = solver1.shapeDistance(s1, transform, s2, transform * Transform3d(Eigen::Translation3d(Vector3d(40, 0, 0))), &dist); + res = solver1().shapeDistance(s1, transform, s2, transform * Transform3(Translation3(Vector3(40, 0, 0))), &dist); EXPECT_TRUE(fabs(dist - 30) < 0.1); EXPECT_TRUE(res); } -GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeDistance_ellipsoidellipsoid) +GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeDistance_conecylinder) +{ +// test_shapeDistance_conecylinder(); + test_shapeDistance_conecylinder(); +} + +template +void test_shapeDistance_ellipsoidellipsoid() { - Ellipsoid s1(20, 40, 50); - Ellipsoid s2(10, 10, 10); + Ellipsoid s1(20, 40, 50); + Ellipsoid s2(10, 10, 10); - Transform3d transform = Transform3d::Identity(); - generateRandomTransform(extents, transform); + Transform3 transform = Transform3::Identity(); + generateRandomTransform(extents(), transform); bool res; - FCL_REAL dist = -1; - Vector3d closest_p1, closest_p2; + S dist = -1; + Vector3 closest_p1, closest_p2; - res = solver1.shapeDistance(s1, Transform3d::Identity(), s2, Transform3d(Eigen::Translation3d(Vector3d(40, 0, 0))), &dist, &closest_p1, &closest_p2); + res = solver1().shapeDistance(s1, Transform3::Identity(), s2, Transform3(Translation3(Vector3(40, 0, 0))), &dist, &closest_p1, &closest_p2); EXPECT_TRUE(fabs(dist - 10) < 0.001); EXPECT_TRUE(res); - res = solver1.shapeDistance(s1, Transform3d::Identity(), s2, Transform3d(Eigen::Translation3d(Vector3d(30.1, 0, 0))), &dist); + res = solver1().shapeDistance(s1, Transform3::Identity(), s2, Transform3(Translation3(Vector3(30.1, 0, 0))), &dist); EXPECT_TRUE(fabs(dist - 0.1) < 0.001); EXPECT_TRUE(res); - res = solver1.shapeDistance(s1, Transform3d::Identity(), s2, Transform3d(Eigen::Translation3d(Vector3d(29.9, 0, 0))), &dist); + res = solver1().shapeDistance(s1, Transform3::Identity(), s2, Transform3(Translation3(Vector3(29.9, 0, 0))), &dist); EXPECT_TRUE(dist < 0); - EXPECT_TRUE_FALSE(res); + EXPECT_FALSE(res); - res = solver1.shapeDistance(s1, Transform3d(Eigen::Translation3d(Vector3d(40, 0, 0))), s2, Transform3d::Identity(), &dist); + res = solver1().shapeDistance(s1, Transform3(Translation3(Vector3(40, 0, 0))), s2, Transform3::Identity(), &dist); EXPECT_TRUE(fabs(dist - 10) < 0.001); EXPECT_TRUE(res); - res = solver1.shapeDistance(s1, Transform3d(Eigen::Translation3d(Vector3d(30.1, 0, 0))), s2, Transform3d::Identity(), &dist); + res = solver1().shapeDistance(s1, Transform3(Translation3(Vector3(30.1, 0, 0))), s2, Transform3::Identity(), &dist); EXPECT_TRUE(fabs(dist - 0.1) < 0.001); EXPECT_TRUE(res); - res = solver1.shapeDistance(s1, Transform3d(Eigen::Translation3d(Vector3d(29.9, 0, 0))), s2, Transform3d::Identity(), &dist); + res = solver1().shapeDistance(s1, Transform3(Translation3(Vector3(29.9, 0, 0))), s2, Transform3::Identity(), &dist); EXPECT_TRUE(dist < 0); - EXPECT_TRUE_FALSE(res); + EXPECT_FALSE(res); - res = solver1.shapeDistance(s1, transform, s2, transform * Transform3d(Eigen::Translation3d(Vector3d(40, 0, 0))), &dist); + res = solver1().shapeDistance(s1, transform, s2, transform * Transform3(Translation3(Vector3(40, 0, 0))), &dist); EXPECT_TRUE(fabs(dist - 10) < 0.001); EXPECT_TRUE(res); - res = solver1.shapeDistance(s1, transform, s2, transform * Transform3d(Eigen::Translation3d(Vector3d(30.1, 0, 0))), &dist); + res = solver1().shapeDistance(s1, transform, s2, transform * Transform3(Translation3(Vector3(30.1, 0, 0))), &dist); EXPECT_TRUE(fabs(dist - 0.1) < 0.001); EXPECT_TRUE(res); - res = solver1.shapeDistance(s1, transform, s2, transform * Transform3d(Eigen::Translation3d(Vector3d(29.9, 0, 0))), &dist); + res = solver1().shapeDistance(s1, transform, s2, transform * Transform3(Translation3(Vector3(29.9, 0, 0))), &dist); EXPECT_TRUE(dist < 0); - EXPECT_TRUE_FALSE(res); + EXPECT_FALSE(res); - res = solver1.shapeDistance(s1, transform * Transform3d(Eigen::Translation3d(Vector3d(40, 0, 0))), s2, transform, &dist); + res = solver1().shapeDistance(s1, transform * Transform3(Translation3(Vector3(40, 0, 0))), s2, transform, &dist); EXPECT_TRUE(fabs(dist - 10) < 0.001); EXPECT_TRUE(res); - res = solver1.shapeDistance(s1, transform * Transform3d(Eigen::Translation3d(Vector3d(30.1, 0, 0))), s2, transform, &dist); + res = solver1().shapeDistance(s1, transform * Transform3(Translation3(Vector3(30.1, 0, 0))), s2, transform, &dist); EXPECT_TRUE(fabs(dist - 0.1) < 0.001); EXPECT_TRUE(res); - res = solver1.shapeDistance(s1, transform * Transform3d(Eigen::Translation3d(Vector3d(29.9, 0, 0))), s2, transform, &dist); + res = solver1().shapeDistance(s1, transform * Transform3(Translation3(Vector3(29.9, 0, 0))), s2, transform, &dist); EXPECT_TRUE(dist < 0); - EXPECT_TRUE_FALSE(res); + EXPECT_FALSE(res); +} + +GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeDistance_ellipsoidellipsoid) +{ +// test_shapeDistance_ellipsoidellipsoid(); + test_shapeDistance_ellipsoidellipsoid(); } // Shape intersection test coverage (built-in GJK) @@ -3730,45 +3988,46 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeDistance_ellipsoidellipsoid) // | triangle |/////|////////|///////////|/////////|//////|//////////|///////|////////////| | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersectionGJK_spheresphere) +template +void test_shapeIntersectionGJK_spheresphere() { - Sphere s1(20); - Sphere s2(10); + Sphere s1(20); + Sphere s2(10); - Transform3d tf1; - Transform3d tf2; + Transform3 tf1; + Transform3 tf2; - Transform3d transform = Transform3d::Identity(); - generateRandomTransform(extents, transform); + Transform3 transform = Transform3::Identity(); + generateRandomTransform(extents(), transform); - std::vector contacts; + std::vector> contacts; - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(40, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(40, 0, 0))); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(40, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(40, 0, 0))); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(30, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(30, 0, 0))); contacts.resize(1); contacts[0].normal << 1, 0, 0; contacts[0].pos << 20, 0, 0; contacts[0].penetration_depth = 0.0; testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(30.01, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(30.01, 0, 0))); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(30.01, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(30.01, 0, 0))); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(29.9, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(29.9, 0, 0))); contacts.resize(1); contacts[0].normal << 1, 0, 0; contacts[0].pos << 20.0 - 0.1 * 20.0/(20.0 + 10.0), 0, 0; @@ -3776,15 +4035,15 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersectionGJK_spheresphere) testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(29.9, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(29.9, 0, 0))); contacts.resize(1); - contacts[0].normal = transform.linear() * Vector3d(1, 0, 0); - contacts[0].pos = transform * Vector3d(20.0 - 0.1 * 20.0/(20.0 + 10.0), 0, 0); + contacts[0].normal = transform.linear() * Vector3(1, 0, 0); + contacts[0].pos = transform * Vector3(20.0 - 0.1 * 20.0/(20.0 + 10.0), 0, 0); contacts[0].penetration_depth = 0.1; testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d::Identity(); + tf1 = Transform3::Identity(); + tf2 = Transform3::Identity(); contacts.resize(1); contacts[0].normal.setZero(); // If the centers of two sphere are at the same position, the normal is (0, 0, 0) contacts[0].pos.setZero(); @@ -3795,12 +4054,12 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersectionGJK_spheresphere) tf2 = transform; contacts.resize(1); contacts[0].normal.setZero(); // If the centers of two sphere are at the same position, the normal is (0, 0, 0) - contacts[0].pos = transform * Vector3d::Zero(); + contacts[0].pos = transform * Vector3::Zero(); contacts[0].penetration_depth = 20.0 + 10.0; testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(-29.9, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(-29.9, 0, 0))); contacts.resize(1); contacts[0].normal << -1, 0, 0; contacts[0].pos << -20.0 + 0.1 * 20.0/(20.0 + 10.0), 0, 0; @@ -3808,47 +4067,54 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersectionGJK_spheresphere) testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(-29.9, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(-29.9, 0, 0))); contacts.resize(1); - contacts[0].normal = transform.linear() * Vector3d(-1, 0, 0); - contacts[0].pos = transform * Vector3d(-20.0 + 0.1 * 20.0/(20.0 + 10.0), 0, 0); + contacts[0].normal = transform.linear() * Vector3(-1, 0, 0); + contacts[0].pos = transform * Vector3(-20.0 + 0.1 * 20.0/(20.0 + 10.0), 0, 0); contacts[0].penetration_depth = 0.1; testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(-30.0, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(-30.0, 0, 0))); contacts.resize(1); contacts[0].normal << -1, 0, 0; contacts[0].pos << -20, 0, 0; contacts[0].penetration_depth = 0.0; testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(-30.01, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(-30.01, 0, 0))); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(-30.01, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(-30.01, 0, 0))); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false); } -GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersectionGJK_boxbox) +GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersectionGJK_spheresphere) { - Box s1(20, 40, 50); - Box s2(10, 10, 10); +// test_shapeIntersectionGJK_spheresphere(); + test_shapeIntersectionGJK_spheresphere(); +} - Transform3d tf1; - Transform3d tf2; +template +void test_shapeIntersectionGJK_boxbox() +{ + Box s1(20, 40, 50); + Box s2(10, 10, 10); - Transform3d transform = Transform3d::Identity(); - generateRandomTransform(extents, transform); + Transform3 tf1; + Transform3 tf2; - std::vector contacts; + Transform3 transform = Transform3::Identity(); + generateRandomTransform(extents(), transform); - Quaternion3d q(Eigen::AngleAxisd((FCL_REAL)3.140 / 6, Vector3d(0, 0, 1))); + std::vector> contacts; - tf1 = Transform3d::Identity(); - tf2 = Transform3d::Identity(); + Quaternion q(AngleAxis((S)3.140 / 6, Vector3(0, 0, 1))); + + tf1 = Transform3::Identity(); + tf2 = Transform3::Identity(); // TODO: Need convention for normal when the centers of two objects are at same position. The current result is (1, 0, 0). contacts.resize(4); contacts[0].normal << 1, 0, 0; @@ -3861,59 +4127,66 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersectionGJK_boxbox) tf2 = transform; // TODO: Need convention for normal when the centers of two objects are at same position. The current result is (1, 0, 0). contacts.resize(4); - contacts[0].normal = transform.linear() * Vector3d(1, 0, 0); - contacts[1].normal = transform.linear() * Vector3d(1, 0, 0); - contacts[2].normal = transform.linear() * Vector3d(1, 0, 0); - contacts[3].normal = transform.linear() * Vector3d(1, 0, 0); + contacts[0].normal = transform.linear() * Vector3(1, 0, 0); + contacts[1].normal = transform.linear() * Vector3(1, 0, 0); + contacts[2].normal = transform.linear() * Vector3(1, 0, 0); + contacts[3].normal = transform.linear() * Vector3(1, 0, 0); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(15, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(15, 0, 0))); contacts.resize(4); - contacts[0].normal = Vector3d(1, 0, 0); - contacts[1].normal = Vector3d(1, 0, 0); - contacts[2].normal = Vector3d(1, 0, 0); - contacts[3].normal = Vector3d(1, 0, 0); + contacts[0].normal = Vector3(1, 0, 0); + contacts[1].normal = Vector3(1, 0, 0); + contacts[2].normal = Vector3(1, 0, 0); + contacts[3].normal = Vector3(1, 0, 0); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(15.01, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(15.01, 0, 0))); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(q); + tf1 = Transform3::Identity(); + tf2 = Transform3(q); contacts.resize(4); - contacts[0].normal = Vector3d(1, 0, 0); - contacts[1].normal = Vector3d(1, 0, 0); - contacts[2].normal = Vector3d(1, 0, 0); - contacts[3].normal = Vector3d(1, 0, 0); + contacts[0].normal = Vector3(1, 0, 0); + contacts[1].normal = Vector3(1, 0, 0); + contacts[2].normal = Vector3(1, 0, 0); + contacts[3].normal = Vector3(1, 0, 0); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true); tf1 = transform; - tf2 = transform * Transform3d(q); + tf2 = transform * Transform3(q); contacts.resize(4); - contacts[0].normal = transform.linear() * Vector3d(1, 0, 0); - contacts[1].normal = transform.linear() * Vector3d(1, 0, 0); - contacts[2].normal = transform.linear() * Vector3d(1, 0, 0); - contacts[3].normal = transform.linear() * Vector3d(1, 0, 0); + contacts[0].normal = transform.linear() * Vector3(1, 0, 0); + contacts[1].normal = transform.linear() * Vector3(1, 0, 0); + contacts[2].normal = transform.linear() * Vector3(1, 0, 0); + contacts[3].normal = transform.linear() * Vector3(1, 0, 0); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true); } -GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersectionGJK_spherebox) +GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersectionGJK_boxbox) +{ +// test_shapeIntersectionGJK_boxbox(); + test_shapeIntersectionGJK_boxbox(); +} + +template +void test_shapeIntersectionGJK_spherebox() { - Sphere s1(20); - Box s2(5, 5, 5); + Sphere s1(20); + Box s2(5, 5, 5); - Transform3d tf1; - Transform3d tf2; + Transform3 tf1; + Transform3 tf2; - Transform3d transform = Transform3d::Identity(); - generateRandomTransform(extents, transform); + Transform3 transform = Transform3::Identity(); + generateRandomTransform(extents(), transform); - std::vector contacts; + std::vector> contacts; - tf1 = Transform3d::Identity(); - tf2 = Transform3d::Identity(); + tf1 = Transform3::Identity(); + tf2 = Transform3::Identity(); // TODO: Need convention for normal when the centers of two objects are at same position. contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); @@ -3924,45 +4197,52 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersectionGJK_spherebox) contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(22.5, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(22.5, 0, 0))); contacts.resize(1); contacts[0].normal << 1, 0, 0; testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true, false, 1e-7); // built-in GJK solver requires larger tolerance than libccd tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(22.51, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(22.51, 0, 0))); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(22.4, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(22.4, 0, 0))); contacts.resize(1); contacts[0].normal << 1, 0, 0; testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true, false, 1e-2); // built-in GJK solver requires larger tolerance than libccd tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(22.4, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(22.4, 0, 0))); contacts.resize(1); - // contacts[0].normal = transform.linear() * Vector3d(1, 0, 0); + // contacts[0].normal = transform.linear() * Vector3(1, 0, 0); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); // built-in GJK solver returns incorrect normal. } -GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersectionGJK_spherecapsule) +GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersectionGJK_spherebox) { - Sphere s1(20); - Capsule s2(5, 10); +// test_shapeIntersectionGJK_spherebox(); + test_shapeIntersectionGJK_spherebox(); +} + +template +void test_shapeIntersectionGJK_spherecapsule() +{ + Sphere s1(20); + Capsule s2(5, 10); - Transform3d tf1; - Transform3d tf2; + Transform3 tf1; + Transform3 tf2; - Transform3d transform = Transform3d::Identity(); - generateRandomTransform(extents, transform); + Transform3 transform = Transform3::Identity(); + generateRandomTransform(extents(), transform); - std::vector contacts; + std::vector> contacts; - tf1 = Transform3d::Identity(); - tf2 = Transform3d::Identity(); + tf1 = Transform3::Identity(); + tf2 = Transform3::Identity(); // TODO: Need convention for normal when the centers of two objects are at same position. contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); @@ -3973,44 +4253,51 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersectionGJK_spherecapsule) contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(24.9, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(24.9, 0, 0))); contacts.resize(1); contacts[0].normal << 1, 0, 0; testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(24.9, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(24.9, 0, 0))); contacts.resize(1); - contacts[0].normal = transform.linear() * Vector3d(1, 0, 0); + contacts[0].normal = transform.linear() * Vector3(1, 0, 0); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(25, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(25, 0, 0))); contacts.resize(1); contacts[0].normal << 1, 0, 0; testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(25.1, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(25.1, 0, 0))); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false); } -GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersectionGJK_cylindercylinder) +GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersectionGJK_spherecapsule) { - Cylinder s1(5, 10); - Cylinder s2(5, 10); +// test_shapeIntersectionGJK_spherecapsule(); + test_shapeIntersectionGJK_spherecapsule(); +} + +template +void test_shapeIntersectionGJK_cylindercylinder() +{ + Cylinder s1(5, 10); + Cylinder s2(5, 10); - Transform3d tf1; - Transform3d tf2; + Transform3 tf1; + Transform3 tf2; - Transform3d transform = Transform3d::Identity(); - generateRandomTransform(extents, transform); + Transform3 transform = Transform3::Identity(); + generateRandomTransform(extents(), transform); - std::vector contacts; + std::vector> contacts; - tf1 = Transform3d::Identity(); - tf2 = Transform3d::Identity(); + tf1 = Transform3::Identity(); + tf2 = Transform3::Identity(); // TODO: Need convention for normal when the centers of two objects are at same position. contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); @@ -4021,45 +4308,52 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersectionGJK_cylindercylinder) contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(9.9, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(9.9, 0, 0))); contacts.resize(1); contacts[0].normal << 1, 0, 0; testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true, false, 3e-1); // built-in GJK solver requires larger tolerance than libccd tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(9.9, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(9.9, 0, 0))); contacts.resize(1); - // contacts[0].normal = transform.linear() * Vector3d(1, 0, 0); + // contacts[0].normal = transform.linear() * Vector3(1, 0, 0); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); // built-in GJK solver returns incorrect normal. - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(10, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(10, 0, 0))); contacts.resize(1); contacts[0].normal << 1, 0, 0; testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(10.01, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(10.01, 0, 0))); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false); } -GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersectionGJK_conecone) +GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersectionGJK_cylindercylinder) +{ +// test_shapeIntersectionGJK_cylindercylinder(); + test_shapeIntersectionGJK_cylindercylinder(); +} + +template +void test_shapeIntersectionGJK_conecone() { - Cone s1(5, 10); - Cone s2(5, 10); + Cone s1(5, 10); + Cone s2(5, 10); - Transform3d tf1; - Transform3d tf2; + Transform3 tf1; + Transform3 tf2; - Transform3d transform = Transform3d::Identity(); - generateRandomTransform(extents, transform); + Transform3 transform = Transform3::Identity(); + generateRandomTransform(extents(), transform); - std::vector contacts; + std::vector> contacts; - tf1 = Transform3d::Identity(); - tf2 = Transform3d::Identity(); + tf1 = Transform3::Identity(); + tf2 = Transform3::Identity(); // TODO: Need convention for normal when the centers of two objects are at same position. contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); @@ -4070,55 +4364,62 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersectionGJK_conecone) contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(9.9, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(9.9, 0, 0))); contacts.resize(1); contacts[0].normal << 1, 0, 0; testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true, false, 5.7e-1); // built-in GJK solver requires larger tolerance than libccd tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(9.9, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(9.9, 0, 0))); contacts.resize(1); - // contacts[0].normal = transform.linear() * Vector3d(1, 0, 0); + // contacts[0].normal = transform.linear() * Vector3(1, 0, 0); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); // built-in GJK solver returns incorrect normal. - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(10.1, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(10.1, 0, 0))); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(10.1, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(10.1, 0, 0))); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, 0, 9.9))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, 0, 9.9))); contacts.resize(1); contacts[0].normal << 0, 0, 1; testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, 0, 9.9))); + tf2 = transform * Transform3(Translation3(Vector3(0, 0, 9.9))); contacts.resize(1); - // contacts[0].normal = transform.linear() * Vector3d(0, 0, 1); + // contacts[0].normal = transform.linear() * Vector3(0, 0, 1); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); // built-in GJK solver returns incorrect normal. } -GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersectionGJK_cylindercone) +GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersectionGJK_conecone) { - Cylinder s1(5, 10); - Cone s2(5, 10); +// test_shapeIntersectionGJK_conecone(); + test_shapeIntersectionGJK_conecone(); +} - Transform3d tf1; - Transform3d tf2; +template +void test_shapeIntersectionGJK_cylindercone() +{ + Cylinder s1(5, 10); + Cone s2(5, 10); - Transform3d transform = Transform3d::Identity(); - generateRandomTransform(extents, transform); + Transform3 tf1; + Transform3 tf2; - std::vector contacts; - tf1 = Transform3d::Identity(); - tf2 = Transform3d::Identity(); + Transform3 transform = Transform3::Identity(); + generateRandomTransform(extents(), transform); + + std::vector> contacts; + tf1 = Transform3::Identity(); + tf2 = Transform3::Identity(); // TODO: Need convention for normal when the centers of two objects are at same position. contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); @@ -4129,93 +4430,100 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersectionGJK_cylindercone) contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(9.9, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(9.9, 0, 0))); contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(9.9, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(9.9, 0, 0))); contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(10, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(10, 0, 0))); contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(10, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(10, 0, 0))); contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, 0, 9.9))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, 0, 9.9))); contacts.resize(1); contacts[0].normal << 0, 0, 1; testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, 0, 9.9))); + tf2 = transform * Transform3(Translation3(Vector3(0, 0, 9.9))); contacts.resize(1); - // contacts[0].normal = transform.linear() * Vector3d(1, 0, 0); + // contacts[0].normal = transform.linear() * Vector3(1, 0, 0); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); // built-in GJK solver returns incorrect normal. - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(0, 0, 10))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(0, 0, 10))); contacts.resize(1); contacts[0].normal << 0, 0, 1; testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(0, 0, 10.1))); + tf2 = transform * Transform3(Translation3(Vector3(0, 0, 10.1))); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false); } -GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersectionGJK_ellipsoidellipsoid) +GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersectionGJK_cylindercone) { - Ellipsoid s1(20, 40, 50); - Ellipsoid s2(10, 10, 10); +// test_shapeIntersectionGJK_cylindercone(); + test_shapeIntersectionGJK_cylindercone(); +} - Transform3d tf1; - Transform3d tf2; +template +void test_shapeIntersectionGJK_ellipsoidellipsoid() +{ + Ellipsoid s1(20, 40, 50); + Ellipsoid s2(10, 10, 10); - Transform3d transform = Transform3d::Identity(); - generateRandomTransform(extents, transform); - Transform3d identity; + Transform3 tf1; + Transform3 tf2; - std::vector contacts; + Transform3 transform = Transform3::Identity(); + generateRandomTransform(extents(), transform); + Transform3 identity; - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(40, 0, 0))); + std::vector> contacts; + + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(40, 0, 0))); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(40, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(40, 0, 0))); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(30, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(30, 0, 0))); contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(30.01, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(30.01, 0, 0))); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(29.99, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(29.99, 0, 0))); contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(29.9, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(29.9, 0, 0))); contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); - tf1 = Transform3d::Identity(); - tf2 = Transform3d::Identity(); + tf1 = Transform3::Identity(); + tf2 = Transform3::Identity(); contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); @@ -4224,146 +4532,173 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersectionGJK_ellipsoidellipsoid) contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(-29.99, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(-29.99, 0, 0))); contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(-29.99, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(-29.99, 0, 0))); contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); - tf1 = Transform3d::Identity(); - tf2 = Transform3d(Eigen::Translation3d(Vector3d(-30, 0, 0))); + tf1 = Transform3::Identity(); + tf2 = Transform3(Translation3(Vector3(-30, 0, 0))); contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); tf1 = transform; - tf2 = transform * Transform3d(Eigen::Translation3d(Vector3d(-30.01, 0, 0))); + tf2 = transform * Transform3(Translation3(Vector3(-30.01, 0, 0))); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false); } -GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersectionGJK_spheretriangle) +GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersectionGJK_ellipsoidellipsoid) { - Sphere s(10); - Vector3d t[3]; +// test_shapeIntersectionGJK_ellipsoidellipsoid(); + test_shapeIntersectionGJK_ellipsoidellipsoid(); +} + +template +void test_shapeIntersectionGJK_spheretriangle() +{ + Sphere s(10); + Vector3 t[3]; t[0] << 20, 0, 0; t[1] << -20, 0, 0; t[2] << 0, 20, 0; - Transform3d transform = Transform3d::Identity(); - generateRandomTransform(extents, transform); + Transform3 transform = Transform3::Identity(); + generateRandomTransform(extents(), transform); - // Vector3d point; - // FCL_REAL depth; - Vector3d normal; + // Vector3 point; + // S depth; + Vector3 normal; bool res; - res = solver2.shapeTriangleIntersect(s, Transform3d::Identity(), t[0], t[1], t[2], NULL, NULL, NULL); + res = solver2().shapeTriangleIntersect(s, Transform3::Identity(), t[0], t[1], t[2], nullptr, nullptr, nullptr); EXPECT_TRUE(res); - res = solver2.shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, NULL, NULL, NULL); + res = solver2().shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, nullptr, nullptr, nullptr); EXPECT_TRUE(res); t[0] << 30, 0, 0; t[1] << 9.9, -20, 0; t[2] << 9.9, 20, 0; - res = solver2.shapeTriangleIntersect(s, Transform3d::Identity(), t[0], t[1], t[2], NULL, NULL, NULL); + res = solver2().shapeTriangleIntersect(s, Transform3::Identity(), t[0], t[1], t[2], nullptr, nullptr, nullptr); EXPECT_TRUE(res); - res = solver2.shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, NULL, NULL, NULL); + res = solver2().shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, nullptr, nullptr, nullptr); EXPECT_TRUE(res); - res = solver2.shapeTriangleIntersect(s, Transform3d::Identity(), t[0], t[1], t[2], NULL, NULL, &normal); + res = solver2().shapeTriangleIntersect(s, Transform3::Identity(), t[0], t[1], t[2], nullptr, nullptr, &normal); EXPECT_TRUE(res); - EXPECT_TRUE(normal.isApprox(Vector3d(1, 0, 0), 1e-9)); + EXPECT_TRUE(normal.isApprox(Vector3(1, 0, 0), 1e-9)); - res = solver2.shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, NULL, NULL, &normal); + res = solver2().shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, nullptr, nullptr, &normal); EXPECT_TRUE(res); - EXPECT_TRUE(normal.isApprox(transform.linear() * Vector3d(1, 0, 0), 1e-9)); + EXPECT_TRUE(normal.isApprox(transform.linear() * Vector3(1, 0, 0), 1e-9)); } -GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersectionGJK_halfspacetriangle) +GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersectionGJK_spheretriangle) { - Halfspace hs(Vector3d(1, 0, 0), 0); - Vector3d t[3]; +// test_shapeIntersectionGJK_spheretriangle(); + test_shapeIntersectionGJK_spheretriangle(); +} + +template +void test_shapeIntersectionGJK_halfspacetriangle() +{ + Halfspace hs(Vector3(1, 0, 0), 0); + Vector3 t[3]; t[0] << 20, 0, 0; t[1] << -20, 0, 0; t[2] << 0, 20, 0; - Transform3d transform = Transform3d::Identity(); - generateRandomTransform(extents, transform); + Transform3 transform = Transform3::Identity(); + generateRandomTransform(extents(), transform); - // Vector3d point; - // FCL_REAL depth; - Vector3d normal; + // Vector3 point; + // S depth; + Vector3 normal; bool res; - res = solver2.shapeTriangleIntersect(hs, Transform3d::Identity(), t[0], t[1], t[2], Transform3d::Identity(), NULL, NULL, NULL); + res = solver2().shapeTriangleIntersect(hs, Transform3::Identity(), t[0], t[1], t[2], Transform3::Identity(), nullptr, nullptr, nullptr); EXPECT_TRUE(res); - res = solver2.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, NULL, NULL, NULL); + res = solver2().shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, nullptr, nullptr, nullptr); EXPECT_TRUE(res); t[0] << 20, 0, 0; t[1] << -0.1, -20, 0; t[2] << -0.1, 20, 0; - res = solver2.shapeTriangleIntersect(hs, Transform3d::Identity(), t[0], t[1], t[2], Transform3d::Identity(), NULL, NULL, NULL); + res = solver2().shapeTriangleIntersect(hs, Transform3::Identity(), t[0], t[1], t[2], Transform3::Identity(), nullptr, nullptr, nullptr); EXPECT_TRUE(res); - res = solver2.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, NULL, NULL, NULL); + res = solver2().shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, nullptr, nullptr, nullptr); EXPECT_TRUE(res); - res = solver2.shapeTriangleIntersect(hs, Transform3d::Identity(), t[0], t[1], t[2], Transform3d::Identity(), NULL, NULL, &normal); + res = solver2().shapeTriangleIntersect(hs, Transform3::Identity(), t[0], t[1], t[2], Transform3::Identity(), nullptr, nullptr, &normal); EXPECT_TRUE(res); - EXPECT_TRUE(normal.isApprox(Vector3d(1, 0, 0), 1e-9)); + EXPECT_TRUE(normal.isApprox(Vector3(1, 0, 0), 1e-9)); - res = solver2.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, NULL, NULL, &normal); + res = solver2().shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, nullptr, nullptr, &normal); EXPECT_TRUE(res); - EXPECT_TRUE(normal.isApprox(transform.linear() * Vector3d(1, 0, 0), 1e-9)); + EXPECT_TRUE(normal.isApprox(transform.linear() * Vector3(1, 0, 0), 1e-9)); } -GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersectionGJK_planetriangle) +GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersectionGJK_halfspacetriangle) { - Plane hs(Vector3d(1, 0, 0), 0); - Vector3d t[3]; +// test_shapeIntersectionGJK_halfspacetriangle(); + test_shapeIntersectionGJK_halfspacetriangle(); +} + +template +void test_shapeIntersectionGJK_planetriangle() +{ + Plane hs(Vector3(1, 0, 0), 0); + Vector3 t[3]; t[0] << 20, 0, 0; t[1] << -20, 0, 0; t[2] << 0, 20, 0; - Transform3d transform = Transform3d::Identity(); - generateRandomTransform(extents, transform); + Transform3 transform = Transform3::Identity(); + generateRandomTransform(extents(), transform); - // Vector3d point; - // FCL_REAL depth; - Vector3d normal; + // Vector3 point; + // S depth; + Vector3 normal; bool res; - res = solver1.shapeTriangleIntersect(hs, Transform3d::Identity(), t[0], t[1], t[2], Transform3d::Identity(), NULL, NULL, NULL); + res = solver1().shapeTriangleIntersect(hs, Transform3::Identity(), t[0], t[1], t[2], Transform3::Identity(), nullptr, nullptr, nullptr); EXPECT_TRUE(res); - res = solver1.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, NULL, NULL, NULL); + res = solver1().shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, nullptr, nullptr, nullptr); EXPECT_TRUE(res); t[0] << 20, 0, 0; t[1] << -0.1, -20, 0; t[2] << -0.1, 20, 0; - res = solver2.shapeTriangleIntersect(hs, Transform3d::Identity(), t[0], t[1], t[2], Transform3d::Identity(), NULL, NULL, NULL); + res = solver2().shapeTriangleIntersect(hs, Transform3::Identity(), t[0], t[1], t[2], Transform3::Identity(), nullptr, nullptr, nullptr); EXPECT_TRUE(res); - res = solver2.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, NULL, NULL, NULL); + res = solver2().shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, nullptr, nullptr, nullptr); EXPECT_TRUE(res); - res = solver2.shapeTriangleIntersect(hs, Transform3d::Identity(), t[0], t[1], t[2], Transform3d::Identity(), NULL, NULL, &normal); + res = solver2().shapeTriangleIntersect(hs, Transform3::Identity(), t[0], t[1], t[2], Transform3::Identity(), nullptr, nullptr, &normal); EXPECT_TRUE(res); - EXPECT_TRUE(normal.isApprox(Vector3d(1, 0, 0), 1e-9)); + EXPECT_TRUE(normal.isApprox(Vector3(1, 0, 0), 1e-9)); - res = solver2.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, NULL, NULL, &normal); + res = solver2().shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, nullptr, nullptr, &normal); EXPECT_TRUE(res); - EXPECT_TRUE(normal.isApprox(transform.linear() * Vector3d(1, 0, 0), 1e-9)); + EXPECT_TRUE(normal.isApprox(transform.linear() * Vector3(1, 0, 0), 1e-9)); +} + +GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersectionGJK_planetriangle) +{ +// test_shapeIntersectionGJK_planetriangle(); + test_shapeIntersectionGJK_planetriangle(); } // Shape distance test coverage (built-in GJK) @@ -4390,287 +4725,331 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersectionGJK_planetriangle) // | triangle |/////|////////|///////////|/////////|//////|//////////|///////|////////////| | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ -GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeDistanceGJK_spheresphere) +template +void test_shapeDistanceGJK_spheresphere() { - Sphere s1(20); - Sphere s2(10); + Sphere s1(20); + Sphere s2(10); - Transform3d transform = Transform3d::Identity(); - generateRandomTransform(extents, transform); + Transform3 transform = Transform3::Identity(); + generateRandomTransform(extents(), transform); bool res; - FCL_REAL dist = -1; + S dist = -1; - res = solver2.shapeDistance(s1, Transform3d::Identity(), s2, Transform3d(Eigen::Translation3d(Vector3d(40, 0, 0))), &dist); + res = solver2().shapeDistance(s1, Transform3::Identity(), s2, Transform3(Translation3(Vector3(40, 0, 0))), &dist); EXPECT_TRUE(fabs(dist - 10) < 0.001); EXPECT_TRUE(res); - res = solver2.shapeDistance(s1, Transform3d::Identity(), s2, Transform3d(Eigen::Translation3d(Vector3d(30.1, 0, 0))), &dist); + res = solver2().shapeDistance(s1, Transform3::Identity(), s2, Transform3(Translation3(Vector3(30.1, 0, 0))), &dist); EXPECT_TRUE(fabs(dist - 0.1) < 0.001); EXPECT_TRUE(res); - res = solver2.shapeDistance(s1, Transform3d::Identity(), s2, Transform3d(Eigen::Translation3d(Vector3d(29.9, 0, 0))), &dist); + res = solver2().shapeDistance(s1, Transform3::Identity(), s2, Transform3(Translation3(Vector3(29.9, 0, 0))), &dist); EXPECT_TRUE(dist < 0); - EXPECT_TRUE_FALSE(res); + EXPECT_FALSE(res); - res = solver2.shapeDistance(s1, Transform3d(Eigen::Translation3d(Vector3d(40, 0, 0))), s2, Transform3d::Identity(), &dist); + res = solver2().shapeDistance(s1, Transform3(Translation3(Vector3(40, 0, 0))), s2, Transform3::Identity(), &dist); EXPECT_TRUE(fabs(dist - 10) < 0.001); EXPECT_TRUE(res); - res = solver2.shapeDistance(s1, Transform3d(Eigen::Translation3d(Vector3d(30.1, 0, 0))), s2, Transform3d::Identity(), &dist); + res = solver2().shapeDistance(s1, Transform3(Translation3(Vector3(30.1, 0, 0))), s2, Transform3::Identity(), &dist); EXPECT_TRUE(fabs(dist - 0.1) < 0.001); EXPECT_TRUE(res); - res = solver2.shapeDistance(s1, Transform3d(Eigen::Translation3d(Vector3d(29.9, 0, 0))), s2, Transform3d::Identity(), &dist); + res = solver2().shapeDistance(s1, Transform3(Translation3(Vector3(29.9, 0, 0))), s2, Transform3::Identity(), &dist); EXPECT_TRUE(dist < 0); - EXPECT_TRUE_FALSE(res); + EXPECT_FALSE(res); - res = solver2.shapeDistance(s1, transform, s2, transform * Transform3d(Eigen::Translation3d(Vector3d(40, 0, 0))), &dist); + res = solver2().shapeDistance(s1, transform, s2, transform * Transform3(Translation3(Vector3(40, 0, 0))), &dist); EXPECT_TRUE(fabs(dist - 10) < 0.001); EXPECT_TRUE(res); - res = solver2.shapeDistance(s1, transform, s2, transform * Transform3d(Eigen::Translation3d(Vector3d(30.1, 0, 0))), &dist); + res = solver2().shapeDistance(s1, transform, s2, transform * Transform3(Translation3(Vector3(30.1, 0, 0))), &dist); EXPECT_TRUE(fabs(dist - 0.1) < 0.001); EXPECT_TRUE(res); - res = solver2.shapeDistance(s1, transform, s2, transform * Transform3d(Eigen::Translation3d(Vector3d(29.9, 0, 0))), &dist); + res = solver2().shapeDistance(s1, transform, s2, transform * Transform3(Translation3(Vector3(29.9, 0, 0))), &dist); EXPECT_TRUE(dist < 0); - EXPECT_TRUE_FALSE(res); + EXPECT_FALSE(res); - res = solver2.shapeDistance(s1, transform * Transform3d(Eigen::Translation3d(Vector3d(40, 0, 0))), s2, transform, &dist); + res = solver2().shapeDistance(s1, transform * Transform3(Translation3(Vector3(40, 0, 0))), s2, transform, &dist); EXPECT_TRUE(fabs(dist - 10) < 0.001); EXPECT_TRUE(res); - res = solver2.shapeDistance(s1, transform * Transform3d(Eigen::Translation3d(Vector3d(30.1, 0, 0))), s2, transform, &dist); + res = solver2().shapeDistance(s1, transform * Transform3(Translation3(Vector3(30.1, 0, 0))), s2, transform, &dist); EXPECT_TRUE(fabs(dist - 0.1) < 0.001); EXPECT_TRUE(res); - res = solver2.shapeDistance(s1, transform * Transform3d(Eigen::Translation3d(Vector3d(29.9, 0, 0))), s2, transform, &dist); + res = solver2().shapeDistance(s1, transform * Transform3(Translation3(Vector3(29.9, 0, 0))), s2, transform, &dist); EXPECT_TRUE(dist < 0); - EXPECT_TRUE_FALSE(res); + EXPECT_FALSE(res); } -GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeDistanceGJK_boxbox) +GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeDistanceGJK_spheresphere) +{ +// test_shapeDistanceGJK_spheresphere(); + test_shapeDistanceGJK_spheresphere(); +} + +template +void test_shapeDistanceGJK_boxbox() { - Box s1(20, 40, 50); - Box s2(10, 10, 10); + Box s1(20, 40, 50); + Box s2(10, 10, 10); - Transform3d transform = Transform3d::Identity(); - generateRandomTransform(extents, transform); + Transform3 transform = Transform3::Identity(); + generateRandomTransform(extents(), transform); bool res; - FCL_REAL dist; + S dist; - res = solver2.shapeDistance(s1, Transform3d::Identity(), s2, Transform3d::Identity(), &dist); + res = solver2().shapeDistance(s1, Transform3::Identity(), s2, Transform3::Identity(), &dist); EXPECT_TRUE(dist < 0); - EXPECT_TRUE_FALSE(res); + EXPECT_FALSE(res); - res = solver2.shapeDistance(s1, transform, s2, transform, &dist); + res = solver2().shapeDistance(s1, transform, s2, transform, &dist); EXPECT_TRUE(dist < 0); - EXPECT_TRUE_FALSE(res); + EXPECT_FALSE(res); - res = solver2.shapeDistance(s1, Transform3d::Identity(), s2, Transform3d(Eigen::Translation3d(Vector3d(15.1, 0, 0))), &dist); + res = solver2().shapeDistance(s1, Transform3::Identity(), s2, Transform3(Translation3(Vector3(15.1, 0, 0))), &dist); EXPECT_TRUE(fabs(dist - 0.1) < 0.001); EXPECT_TRUE(res); - res = solver2.shapeDistance(s1, transform, s2, transform * Transform3d(Eigen::Translation3d(Vector3d(15.1, 0, 0))), &dist); + res = solver2().shapeDistance(s1, transform, s2, transform * Transform3(Translation3(Vector3(15.1, 0, 0))), &dist); EXPECT_TRUE(fabs(dist - 0.1) < 0.001); EXPECT_TRUE(res); - res = solver2.shapeDistance(s1, Transform3d::Identity(), s2, Transform3d(Eigen::Translation3d(Vector3d(20, 0, 0))), &dist); + res = solver2().shapeDistance(s1, Transform3::Identity(), s2, Transform3(Translation3(Vector3(20, 0, 0))), &dist); EXPECT_TRUE(fabs(dist - 5) < 0.001); EXPECT_TRUE(res); - res = solver2.shapeDistance(s1, transform, s2, transform * Transform3d(Eigen::Translation3d(Vector3d(20, 0, 0))), &dist); + res = solver2().shapeDistance(s1, transform, s2, transform * Transform3(Translation3(Vector3(20, 0, 0))), &dist); EXPECT_TRUE(fabs(dist - 5) < 0.001); EXPECT_TRUE(res); } -GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeDistanceGJK_boxsphere) +GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeDistanceGJK_boxbox) { - Sphere s1(20); - Box s2(5, 5, 5); +// test_shapeDistanceGJK_boxbox(); + test_shapeDistanceGJK_boxbox(); +} - Transform3d transform = Transform3d::Identity(); - generateRandomTransform(extents, transform); +template +void test_shapeDistanceGJK_boxsphere() +{ + Sphere s1(20); + Box s2(5, 5, 5); + + Transform3 transform = Transform3::Identity(); + generateRandomTransform(extents(), transform); bool res; - FCL_REAL dist; + S dist; - res = solver2.shapeDistance(s1, Transform3d::Identity(), s2, Transform3d::Identity(), &dist); + res = solver2().shapeDistance(s1, Transform3::Identity(), s2, Transform3::Identity(), &dist); EXPECT_TRUE(dist < 0); - EXPECT_TRUE_FALSE(res); + EXPECT_FALSE(res); - res = solver2.shapeDistance(s1, transform, s2, transform, &dist); + res = solver2().shapeDistance(s1, transform, s2, transform, &dist); EXPECT_TRUE(dist < 0); - EXPECT_TRUE_FALSE(res); + EXPECT_FALSE(res); - res = solver2.shapeDistance(s1, Transform3d::Identity(), s2, Transform3d(Eigen::Translation3d(Vector3d(22.6, 0, 0))), &dist); + res = solver2().shapeDistance(s1, Transform3::Identity(), s2, Transform3(Translation3(Vector3(22.6, 0, 0))), &dist); EXPECT_TRUE(fabs(dist - 0.1) < 0.01); EXPECT_TRUE(res); - res = solver2.shapeDistance(s1, transform, s2, transform * Transform3d(Eigen::Translation3d(Vector3d(22.6, 0, 0))), &dist); + res = solver2().shapeDistance(s1, transform, s2, transform * Transform3(Translation3(Vector3(22.6, 0, 0))), &dist); EXPECT_TRUE(fabs(dist - 0.1) < 0.01); EXPECT_TRUE(res); - res = solver2.shapeDistance(s1, Transform3d::Identity(), s2, Transform3d(Eigen::Translation3d(Vector3d(40, 0, 0))), &dist); + res = solver2().shapeDistance(s1, Transform3::Identity(), s2, Transform3(Translation3(Vector3(40, 0, 0))), &dist); EXPECT_TRUE(fabs(dist - 17.5) < 0.001); EXPECT_TRUE(res); - res = solver2.shapeDistance(s1, transform, s2, transform * Transform3d(Eigen::Translation3d(Vector3d(40, 0, 0))), &dist); + res = solver2().shapeDistance(s1, transform, s2, transform * Transform3(Translation3(Vector3(40, 0, 0))), &dist); EXPECT_TRUE(fabs(dist - 17.5) < 0.001); EXPECT_TRUE(res); } -GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeDistanceGJK_cylindercylinder) +GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeDistanceGJK_boxsphere) { - Cylinder s1(5, 10); - Cylinder s2(5, 10); +// test_shapeDistanceGJK_boxsphere(); + test_shapeDistanceGJK_boxsphere(); +} - Transform3d transform = Transform3d::Identity(); - generateRandomTransform(extents, transform); +template +void test_shapeDistanceGJK_cylindercylinder() +{ + Cylinder s1(5, 10); + Cylinder s2(5, 10); + + Transform3 transform = Transform3::Identity(); + generateRandomTransform(extents(), transform); bool res; - FCL_REAL dist; + S dist; - res = solver2.shapeDistance(s1, Transform3d::Identity(), s2, Transform3d::Identity(), &dist); + res = solver2().shapeDistance(s1, Transform3::Identity(), s2, Transform3::Identity(), &dist); EXPECT_TRUE(dist < 0); - EXPECT_TRUE_FALSE(res); + EXPECT_FALSE(res); - res = solver2.shapeDistance(s1, transform, s2, transform, &dist); + res = solver2().shapeDistance(s1, transform, s2, transform, &dist); EXPECT_TRUE(dist < 0); - EXPECT_TRUE_FALSE(res); + EXPECT_FALSE(res); - res = solver2.shapeDistance(s1, Transform3d::Identity(), s2, Transform3d(Eigen::Translation3d(Vector3d(10.1, 0, 0))), &dist); + res = solver2().shapeDistance(s1, Transform3::Identity(), s2, Transform3(Translation3(Vector3(10.1, 0, 0))), &dist); EXPECT_TRUE(fabs(dist - 0.1) < 0.001); EXPECT_TRUE(res); - res = solver2.shapeDistance(s1, transform, s2, transform * Transform3d(Eigen::Translation3d(Vector3d(10.1, 0, 0))), &dist); + res = solver2().shapeDistance(s1, transform, s2, transform * Transform3(Translation3(Vector3(10.1, 0, 0))), &dist); EXPECT_TRUE(fabs(dist - 0.1) < 0.001); EXPECT_TRUE(res); - res = solver2.shapeDistance(s1, Transform3d::Identity(), s2, Transform3d(Eigen::Translation3d(Vector3d(40, 0, 0))), &dist); + res = solver2().shapeDistance(s1, Transform3::Identity(), s2, Transform3(Translation3(Vector3(40, 0, 0))), &dist); EXPECT_TRUE(fabs(dist - 30) < 0.001); EXPECT_TRUE(res); - res = solver2.shapeDistance(s1, transform, s2, transform * Transform3d(Eigen::Translation3d(Vector3d(40, 0, 0))), &dist); + res = solver2().shapeDistance(s1, transform, s2, transform * Transform3(Translation3(Vector3(40, 0, 0))), &dist); EXPECT_TRUE(fabs(dist - 30) < 0.001); EXPECT_TRUE(res); } -GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeDistanceGJK_conecone) +GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeDistanceGJK_cylindercylinder) { - Cone s1(5, 10); - Cone s2(5, 10); +// test_shapeDistanceGJK_cylindercylinder(); + test_shapeDistanceGJK_cylindercylinder(); +} + +template +void test_shapeDistanceGJK_conecone() +{ + Cone s1(5, 10); + Cone s2(5, 10); - Transform3d transform = Transform3d::Identity(); - generateRandomTransform(extents, transform); + Transform3 transform = Transform3::Identity(); + generateRandomTransform(extents(), transform); bool res; - FCL_REAL dist; + S dist; - res = solver2.shapeDistance(s1, Transform3d::Identity(), s2, Transform3d::Identity(), &dist); + res = solver2().shapeDistance(s1, Transform3::Identity(), s2, Transform3::Identity(), &dist); EXPECT_TRUE(dist < 0); - EXPECT_TRUE_FALSE(res); + EXPECT_FALSE(res); - res = solver2.shapeDistance(s1, transform, s2, transform, &dist); + res = solver2().shapeDistance(s1, transform, s2, transform, &dist); EXPECT_TRUE(dist < 0); - EXPECT_TRUE_FALSE(res); + EXPECT_FALSE(res); - res = solver2.shapeDistance(s1, Transform3d::Identity(), s2, Transform3d(Eigen::Translation3d(Vector3d(10.1, 0, 0))), &dist); + res = solver2().shapeDistance(s1, Transform3::Identity(), s2, Transform3(Translation3(Vector3(10.1, 0, 0))), &dist); EXPECT_TRUE(fabs(dist - 0.1) < 0.001); EXPECT_TRUE(res); - res = solver2.shapeDistance(s1, transform, s2, transform * Transform3d(Eigen::Translation3d(Vector3d(10.1, 0, 0))), &dist); + res = solver2().shapeDistance(s1, transform, s2, transform * Transform3(Translation3(Vector3(10.1, 0, 0))), &dist); EXPECT_TRUE(fabs(dist - 0.1) < 0.001); EXPECT_TRUE(res); - res = solver2.shapeDistance(s1, Transform3d::Identity(), s2, Transform3d(Eigen::Translation3d(Vector3d(0, 0, 40))), &dist); + res = solver2().shapeDistance(s1, Transform3::Identity(), s2, Transform3(Translation3(Vector3(0, 0, 40))), &dist); EXPECT_TRUE(fabs(dist - 30) < 0.001); EXPECT_TRUE(res); - res = solver2.shapeDistance(s1, transform, s2, transform * Transform3d(Eigen::Translation3d(Vector3d(0, 0, 40))), &dist); + res = solver2().shapeDistance(s1, transform, s2, transform * Transform3(Translation3(Vector3(0, 0, 40))), &dist); EXPECT_TRUE(fabs(dist - 30) < 0.001); EXPECT_TRUE(res); } -GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeDistanceGJK_ellipsoidellipsoid) +GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeDistanceGJK_conecone) +{ +// test_shapeDistanceGJK_conecone(); + test_shapeDistanceGJK_conecone(); +} + +template +void test_shapeDistanceGJK_ellipsoidellipsoid() { - Ellipsoid s1(20, 40, 50); - Ellipsoid s2(10, 10, 10); + Ellipsoid s1(20, 40, 50); + Ellipsoid s2(10, 10, 10); - Transform3d transform = Transform3d::Identity(); - generateRandomTransform(extents, transform); + Transform3 transform = Transform3::Identity(); + generateRandomTransform(extents(), transform); bool res; - FCL_REAL dist = -1; + S dist = -1; - res = solver2.shapeDistance(s1, Transform3d::Identity(), s2, Transform3d(Eigen::Translation3d(Vector3d(40, 0, 0))), &dist); + res = solver2().shapeDistance(s1, Transform3::Identity(), s2, Transform3(Translation3(Vector3(40, 0, 0))), &dist); EXPECT_TRUE(fabs(dist - 10) < 0.001); EXPECT_TRUE(res); - res = solver2.shapeDistance(s1, Transform3d::Identity(), s2, Transform3d(Eigen::Translation3d(Vector3d(30.1, 0, 0))), &dist); + res = solver2().shapeDistance(s1, Transform3::Identity(), s2, Transform3(Translation3(Vector3(30.1, 0, 0))), &dist); EXPECT_TRUE(fabs(dist - 0.1) < 0.001); EXPECT_TRUE(res); - res = solver2.shapeDistance(s1, Transform3d::Identity(), s2, Transform3d(Eigen::Translation3d(Vector3d(29.9, 0, 0))), &dist); + res = solver2().shapeDistance(s1, Transform3::Identity(), s2, Transform3(Translation3(Vector3(29.9, 0, 0))), &dist); EXPECT_TRUE(dist < 0); - EXPECT_TRUE_FALSE(res); + EXPECT_FALSE(res); - res = solver2.shapeDistance(s1, Transform3d(Eigen::Translation3d(Vector3d(40, 0, 0))), s2, Transform3d::Identity(), &dist); + res = solver2().shapeDistance(s1, Transform3(Translation3(Vector3(40, 0, 0))), s2, Transform3::Identity(), &dist); EXPECT_TRUE(fabs(dist - 10) < 0.001); EXPECT_TRUE(res); - res = solver2.shapeDistance(s1, Transform3d(Eigen::Translation3d(Vector3d(30.1, 0, 0))), s2, Transform3d::Identity(), &dist); + res = solver2().shapeDistance(s1, Transform3(Translation3(Vector3(30.1, 0, 0))), s2, Transform3::Identity(), &dist); EXPECT_TRUE(fabs(dist - 0.1) < 0.001); EXPECT_TRUE(res); - res = solver2.shapeDistance(s1, Transform3d(Eigen::Translation3d(Vector3d(29.9, 0, 0))), s2, Transform3d::Identity(), &dist); + res = solver2().shapeDistance(s1, Transform3(Translation3(Vector3(29.9, 0, 0))), s2, Transform3::Identity(), &dist); EXPECT_TRUE(dist < 0); - EXPECT_TRUE_FALSE(res); + EXPECT_FALSE(res); - res = solver2.shapeDistance(s1, transform, s2, transform * Transform3d(Eigen::Translation3d(Vector3d(40, 0, 0))), &dist); + res = solver2().shapeDistance(s1, transform, s2, transform * Transform3(Translation3(Vector3(40, 0, 0))), &dist); EXPECT_TRUE(fabs(dist - 10) < 0.001); EXPECT_TRUE(res); - res = solver2.shapeDistance(s1, transform, s2, transform * Transform3d(Eigen::Translation3d(Vector3d(30.1, 0, 0))), &dist); + res = solver2().shapeDistance(s1, transform, s2, transform * Transform3(Translation3(Vector3(30.1, 0, 0))), &dist); EXPECT_TRUE(fabs(dist - 0.1) < 0.001); EXPECT_TRUE(res); - res = solver2.shapeDistance(s1, transform, s2, transform * Transform3d(Eigen::Translation3d(Vector3d(29.9, 0, 0))), &dist); + res = solver2().shapeDistance(s1, transform, s2, transform * Transform3(Translation3(Vector3(29.9, 0, 0))), &dist); EXPECT_TRUE(dist < 0); - EXPECT_TRUE_FALSE(res); + EXPECT_FALSE(res); - res = solver2.shapeDistance(s1, transform * Transform3d(Eigen::Translation3d(Vector3d(40, 0, 0))), s2, transform, &dist); + res = solver2().shapeDistance(s1, transform * Transform3(Translation3(Vector3(40, 0, 0))), s2, transform, &dist); EXPECT_TRUE(fabs(dist - 10) < 0.001); EXPECT_TRUE(res); - res = solver2.shapeDistance(s1, transform * Transform3d(Eigen::Translation3d(Vector3d(30.1, 0, 0))), s2, transform, &dist); + res = solver2().shapeDistance(s1, transform * Transform3(Translation3(Vector3(30.1, 0, 0))), s2, transform, &dist); EXPECT_TRUE(fabs(dist - 0.1) < 0.001); EXPECT_TRUE(res); - res = solver2.shapeDistance(s1, transform * Transform3d(Eigen::Translation3d(Vector3d(29.9, 0, 0))), s2, transform, &dist); + res = solver2().shapeDistance(s1, transform * Transform3(Translation3(Vector3(29.9, 0, 0))), s2, transform, &dist); EXPECT_TRUE(dist < 0); - EXPECT_TRUE_FALSE(res); + EXPECT_FALSE(res); } -template -void testReversibleShapeIntersection(const S1& s1, const S2& s2, FCL_REAL distance) +GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeDistanceGJK_ellipsoidellipsoid) +{ +// test_shapeDistanceGJK_ellipsoidellipsoid(); + test_shapeDistanceGJK_ellipsoidellipsoid(); +} + +template +void testReversibleShapeIntersection(const Shape1& s1, const Shape2& s2, typename Shape2::S distance) { - Transform3d tf1(Eigen::Translation3d(Vector3d(-0.5 * distance, 0.0, 0.0))); - Transform3d tf2(Eigen::Translation3d(Vector3d(+0.5 * distance, 0.0, 0.0))); + using S = typename Shape2::S; - std::vector contactsA; - std::vector contactsB; + Transform3 tf1(Translation3(Vector3(-0.5 * distance, 0.0, 0.0))); + Transform3 tf2(Translation3(Vector3(+0.5 * distance, 0.0, 0.0))); + + std::vector> contactsA; + std::vector> contactsB; bool resA; bool resB; const double tol = 1e-6; - resA = solver1.shapeIntersect(s1, tf1, s2, tf2, &contactsA); - resB = solver1.shapeIntersect(s2, tf2, s1, tf1, &contactsB); + resA = solver1().shapeIntersect(s1, tf1, s2, tf2, &contactsA); + resB = solver1().shapeIntersect(s2, tf2, s1, tf1, &contactsB); // normal should be opposite for (size_t i = 0; i < contactsB.size(); ++i) @@ -4678,12 +5057,12 @@ void testReversibleShapeIntersection(const S1& s1, const S2& s2, FCL_REAL distan EXPECT_TRUE(resA); EXPECT_TRUE(resB); - EXPECT_TRUE(inspectContactPoints(s1, tf1, s2, tf2, GST_LIBCCD, + EXPECT_TRUE(inspectContactPointds(s1, tf1, s2, tf2, GST_LIBCCD, contactsA, contactsB, true, true, true, false, tol)); - resA = solver2.shapeIntersect(s1, tf1, s2, tf2, &contactsA); - resB = solver2.shapeIntersect(s2, tf2, s1, tf1, &contactsB); + resA = solver2().shapeIntersect(s1, tf1, s2, tf2, &contactsA); + resB = solver2().shapeIntersect(s2, tf2, s1, tf1, &contactsB); // normal should be opposite for (size_t i = 0; i < contactsB.size(); ++i) @@ -4691,29 +5070,30 @@ void testReversibleShapeIntersection(const S1& s1, const S2& s2, FCL_REAL distan EXPECT_TRUE(resA); EXPECT_TRUE(resB); - EXPECT_TRUE(inspectContactPoints(s1, tf1, s2, tf2, GST_INDEP, + EXPECT_TRUE(inspectContactPointds(s1, tf1, s2, tf2, GST_INDEP, contactsA, contactsB, true, true, true, false, tol)); } -GTEST_TEST(FCL_GEOMETRIC_SHAPES, reversibleShapeIntersection_allshapes) +template +void test_reversibleShapeIntersection_allshapes() { // This test check whether a shape intersection algorithm is called for the // reverse case as well. For example, if FCL has sphere-capsule intersection // algorithm, then this algorithm should be called for capsule-sphere case. // Prepare all kinds of primitive shapes (8) -- box, sphere, ellipsoid, capsule, cone, cylinder, plane, halfspace - Box box(10, 10, 10); - Sphere sphere(5); - Ellipsoid ellipsoid(5, 5, 5); - Capsule capsule(5, 10); - Cone cone(5, 10); - Cylinder cylinder(5, 10); - Plane plane(Vector3d::Zero(), 0.0); - Halfspace halfspace(Vector3d::Zero(), 0.0); + Box box(10, 10, 10); + Sphere sphere(5); + Ellipsoid ellipsoid(5, 5, 5); + Capsule capsule(5, 10); + Cone cone(5, 10); + Cylinder cylinder(5, 10); + Plane plane(Vector3::Zero(), 0.0); + Halfspace halfspace(Vector3::Zero(), 0.0); // Use sufficiently short distance so that all the primitive shapes can intersect - FCL_REAL distance = 5.0; + S distance = 5.0; // If new shape intersection algorithm is added for two distinct primitive // shapes, uncomment associated lines. For example, box-sphere intersection @@ -4755,26 +5135,34 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, reversibleShapeIntersection_allshapes) testReversibleShapeIntersection(plane, halfspace, distance); } -template -void testReversibleShapeDistance(const S1& s1, const S2& s2, FCL_REAL distance) +GTEST_TEST(FCL_GEOMETRIC_SHAPES, reversibleShapeIntersection_allshapes) +{ +// test_reversibleShapeIntersection_allshapes(); + test_reversibleShapeIntersection_allshapes(); +} + +template +void testReversibleShapeDistance(const Shape1& s1, const Shape2& s2, typename Shape2::S distance) { - Transform3d tf1(Eigen::Translation3d(Vector3d(-0.5 * distance, 0.0, 0.0))); - Transform3d tf2(Eigen::Translation3d(Vector3d(+0.5 * distance, 0.0, 0.0))); + using S = typename Shape2::S; - FCL_REAL distA; - FCL_REAL distB; - Vector3d p1A; - Vector3d p1B; - Vector3d p2A; - Vector3d p2B; + Transform3 tf1(Translation3(Vector3(-0.5 * distance, 0.0, 0.0))); + Transform3 tf2(Translation3(Vector3(+0.5 * distance, 0.0, 0.0))); + + S distA; + S distB; + Vector3 p1A; + Vector3 p1B; + Vector3 p2A; + Vector3 p2B; bool resA; bool resB; const double tol = 1e-6; - resA = solver1.shapeDistance(s1, tf1, s2, tf2, &distA, &p1A, &p2A); - resB = solver1.shapeDistance(s2, tf2, s1, tf1, &distB, &p1B, &p2B); + resA = solver1().shapeDistance(s1, tf1, s2, tf2, &distA, &p1A, &p2A); + resB = solver1().shapeDistance(s2, tf2, s1, tf1, &distB, &p1B, &p2B); EXPECT_TRUE(resA); EXPECT_TRUE(resB); @@ -4782,8 +5170,8 @@ void testReversibleShapeDistance(const S1& s1, const S2& s2, FCL_REAL distance) EXPECT_TRUE(p1A.isApprox(p2B, tol)); // closest points should in reverse order EXPECT_TRUE(p2A.isApprox(p1B, tol)); - resA = solver2.shapeDistance(s1, tf1, s2, tf2, &distA, &p1A, &p2A); - resB = solver2.shapeDistance(s2, tf2, s1, tf1, &distB, &p1B, &p2B); + resA = solver2().shapeDistance(s1, tf1, s2, tf2, &distA, &p1A, &p2A); + resB = solver2().shapeDistance(s2, tf2, s1, tf1, &distB, &p1B, &p2B); EXPECT_TRUE(resA); EXPECT_TRUE(resB); @@ -4792,24 +5180,25 @@ void testReversibleShapeDistance(const S1& s1, const S2& s2, FCL_REAL distance) EXPECT_TRUE(p2A.isApprox(p1B, tol)); } -GTEST_TEST(FCL_GEOMETRIC_SHAPES, reversibleShapeDistance_allshapes) +template +void test_reversibleShapeDistance_allshapes() { // This test check whether a shape distance algorithm is called for the // reverse case as well. For example, if FCL has sphere-capsule distance // algorithm, then this algorithm should be called for capsule-sphere case. // Prepare all kinds of primitive shapes (8) -- box, sphere, ellipsoid, capsule, cone, cylinder, plane, halfspace - Box box(10, 10, 10); - Sphere sphere(5); - Ellipsoid ellipsoid(5, 5, 5); - Capsule capsule(5, 10); - Cone cone(5, 10); - Cylinder cylinder(5, 10); - Plane plane(Vector3d::Zero(), 0.0); - Halfspace halfspace(Vector3d::Zero(), 0.0); + Box box(10, 10, 10); + Sphere sphere(5); + Ellipsoid ellipsoid(5, 5, 5); + Capsule capsule(5, 10); + Cone cone(5, 10); + Cylinder cylinder(5, 10); + Plane plane(Vector3::Zero(), 0.0); + Halfspace halfspace(Vector3::Zero(), 0.0); // Use sufficiently long distance so that all the primitive shapes CANNOT intersect - FCL_REAL distance = 15.0; + S distance = 15.0; // If new shape distance algorithm is added for two distinct primitive // shapes, uncomment associated lines. For example, box-sphere intersection @@ -4851,6 +5240,12 @@ GTEST_TEST(FCL_GEOMETRIC_SHAPES, reversibleShapeDistance_allshapes) // testReversibleShapeDistance(plane, halfspace, distance); } +GTEST_TEST(FCL_GEOMETRIC_SHAPES, reversibleShapeDistance_allshapes) +{ +// test_reversibleShapeDistance_allshapes(); + test_reversibleShapeDistance_allshapes(); +} + //============================================================================== int main(int argc, char* argv[]) { diff --git a/test/test_fcl_math.cpp b/test/test_fcl_math.cpp index c342ddd88..6afdbbdef 100644 --- a/test/test_fcl_math.cpp +++ b/test/test_fcl_math.cpp @@ -41,73 +41,16 @@ using namespace fcl; -GTEST_TEST(FCL_MATH, vec_test_basic_vec32) +template +void test_vec_test_basic_vector() { - using Vec3f32 = Vector3; + Vector3 v1(1.0, 2.0, 3.0); + EXPECT_TRUE(v1[0] == (S)1.0); + EXPECT_TRUE(v1[1] == (S)2.0); + EXPECT_TRUE(v1[2] == (S)3.0); - Vec3f32 v1(1.0f, 2.0f, 3.0f); - EXPECT_TRUE(v1[0] == 1.0f); - EXPECT_TRUE(v1[1] == 2.0f); - EXPECT_TRUE(v1[2] == 3.0f); - - Vec3f32 v2 = v1; - Vec3f32 v3(3.3f, 4.3f, 5.3f); - v1 += v3; - EXPECT_TRUE(v1.isApprox(v2 + v3)); - v1 -= v3; - EXPECT_TRUE(v1.isApprox(v2)); - v1 -= v3; - EXPECT_TRUE(v1.isApprox(v2 - v3)); - v1 += v3; - - v1.array() *= v3.array(); - EXPECT_TRUE(v1.array().isApprox(v2.array() * v3.array())); - v1.array() /= v3.array(); - EXPECT_TRUE(v1.isApprox(v2)); - v1.array() /= v3.array(); - EXPECT_TRUE(v1.array().isApprox(v2.array() / v3.array())); - v1.array() *= v3.array(); - - v1 *= 2.0f; - EXPECT_TRUE(v1.isApprox(v2 * 2.0f)); - v1 /= 2.0f; - EXPECT_TRUE(v1.isApprox(v2)); - v1 /= 2.0f; - EXPECT_TRUE(v1.isApprox(v2 / 2.0f)); - v1 *= 2.0f; - - v1.array() += 2.0f; - EXPECT_TRUE(v1.array().isApprox(v2.array() + 2.0f)); - v1.array() -= 2.0f; - EXPECT_TRUE(v1.isApprox(v2)); - v1.array() -= 2.0f; - EXPECT_TRUE(v1.array().isApprox(v2.array() - 2.0f)); - v1.array() += 2.0f; - - EXPECT_TRUE((-Vec3f32(1.0f, 2.0f, 3.0f)).isApprox(Vec3f32(-1.0f, -2.0f, -3.0f))); - - v1 = Vec3f32(1.0f, 2.0f, 3.0f); - v2 = Vec3f32(3.0f, 4.0f, 5.0f); - EXPECT_TRUE((v1.cross(v2)).isApprox(Vec3f32(-2.0f, 4.0f, -2.0f))); - EXPECT_TRUE(std::abs(v1.dot(v2) - 26) < 1e-5); - - v1 = Vec3f32(3.0f, 4.0f, 5.0f); - EXPECT_TRUE(std::abs(v1.squaredNorm() - 50.0) < 1e-5); - EXPECT_TRUE(std::abs(v1.norm() - sqrt(50.0)) < 1e-5); - EXPECT_TRUE(v1.normalized().isApprox(v1 / v1.norm())); -} - -GTEST_TEST(FCL_MATH, vec_test_basic_vec64) -{ - using Vec3f64 = Vector3; - - Vec3f64 v1(1.0, 2.0, 3.0); - EXPECT_TRUE(v1[0] == 1.0); - EXPECT_TRUE(v1[1] == 2.0); - EXPECT_TRUE(v1[2] == 3.0); - - Vec3f64 v2 = v1; - Vec3f64 v3(3.3, 4.3, 5.3); + Vector3 v2 = v1; + Vector3 v3(3.3, 4.3, 5.3); v1 += v3; EXPECT_TRUE(v1.isApprox(v2 + v3)); v1 -= v3; @@ -140,38 +83,51 @@ GTEST_TEST(FCL_MATH, vec_test_basic_vec64) EXPECT_TRUE(v1.array().isApprox(v2.array() - 2.0)); v1.array() += 2.0; - EXPECT_TRUE((-Vec3f64(1.0, 2.0, 3.0)) == (Vec3f64(-1.0, -2.0, -3.0))); + EXPECT_TRUE((-Vector3(1.0, 2.0, 3.0)) == (Vector3(-1.0, -2.0, -3.0))); - v1 = Vec3f64(1.0, 2.0, 3.0); - v2 = Vec3f64(3.0, 4.0, 5.0); - EXPECT_TRUE((v1.cross(v2)).isApprox(Vec3f64(-2.0, 4.0, -2.0))); + v1 = Vector3(1.0, 2.0, 3.0); + v2 = Vector3(3.0, 4.0, 5.0); + EXPECT_TRUE((v1.cross(v2)).isApprox(Vector3(-2.0, 4.0, -2.0))); EXPECT_TRUE(std::abs(v1.dot(v2) - 26) < 1e-5); - v1 = Vec3f64(3.0, 4.0, 5.0); + v1 = Vector3(3.0, 4.0, 5.0); EXPECT_TRUE(std::abs(v1.squaredNorm() - 50.0) < 1e-5); EXPECT_TRUE(std::abs(v1.norm() - sqrt(50.0)) < 1e-5); EXPECT_TRUE(v1.normalized().isApprox(v1 / v1.norm())); - v1 = Vec3f64(1.0, 2.0, 3.0); - v2 = Vec3f64(3.0, 4.0, 5.0); - EXPECT_TRUE((v1.cross(v2)).isApprox(Vec3f64(-2.0, 4.0, -2.0))); + v1 = Vector3(1.0, 2.0, 3.0); + v2 = Vector3(3.0, 4.0, 5.0); + EXPECT_TRUE((v1.cross(v2)).isApprox(Vector3(-2.0, 4.0, -2.0))); EXPECT_TRUE(v1.dot(v2) == 26); } -GTEST_TEST(FCL_MATH, morton) +GTEST_TEST(FCL_MATH, vec_test_basic_vector3) +{ +// test_vec_test_basic_vector(); + test_vec_test_basic_vector(); +} + +template +void test_morton() { - AABB bbox(Vector3d(0, 0, 0), Vector3d(1000, 1000, 1000)); - morton_functor> F1(bbox); - morton_functor> F2(bbox); - morton_functor F3(bbox); // 60 bits - morton_functor F4(bbox); // 30 bits + AABB bbox(Vector3(0, 0, 0), Vector3(1000, 1000, 1000)); + morton_functor> F1(bbox); + morton_functor> F2(bbox); + morton_functor F3(bbox); // 60 bits + morton_functor F4(bbox); // 30 bits - Vector3d p(254, 873, 674); + Vector3 p(254, 873, 674); EXPECT_TRUE(F1(p).to_ulong() == F4(p)); EXPECT_TRUE(F2(p).to_ullong() == F3(p)); } +GTEST_TEST(FCL_MATH, morton) +{ +// test_morton(); + test_morton(); +} + //============================================================================== int main(int argc, char* argv[]) { diff --git a/test/test_fcl_octomap.cpp b/test/test_fcl_octomap.cpp deleted file mode 100644 index 95322e346..000000000 --- a/test/test_fcl_octomap.cpp +++ /dev/null @@ -1,847 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#include - -#include "fcl/config.h" -#include "fcl/octree.h" -#include "fcl/traversal/traversal_node_octree.h" -#include "fcl/collision.h" -#include "fcl/broadphase/broadphase.h" -#include "fcl/shape/geometric_shape_to_BVH_model.h" -#include "test_fcl_utility.h" -#include "fcl_resources/config.h" - -using namespace fcl; - - -struct TStruct -{ - std::vector records; - double overall_time; - - TStruct() { overall_time = 0; } - - void push_back(double t) - { - records.push_back(t); - overall_time += t; - } -}; - -/// @brief Generate environment with 3 * n objects: n spheres, n boxes and n cylinders -void generateEnvironments(std::vector& env, double env_scale, std::size_t n); - -/// @brief Generate environment with 3 * n objects: n spheres, n boxes and n cylinders, all in mesh -void generateEnvironmentsMesh(std::vector& env, double env_scale, std::size_t n); - -/// @brief Generate boxes from the octomap -void generateBoxesFromOctomap(std::vector& env, OcTree& tree); - -/// @brief Generate boxes from the octomap -void generateBoxesFromOctomapMesh(std::vector& env, OcTree& tree); - -/// @brief Generate an octree -octomap::OcTree* generateOcTree(double resolution = 0.1); - -/// @brief Octomap collision with an environment with 3 * env_size objects -void octomap_collision_test(double env_scale, std::size_t env_size, bool exhaustive, std::size_t num_max_contacts, bool use_mesh, bool use_mesh_octomap, double resolution = 0.1); - -/// @brief Octomap collision with an environment with 3 * env_size objects, compute cost -void octomap_cost_test(double env_scale, std::size_t env_size, std::size_t num_max_cost_sources, bool use_mesh, bool use_mesh_octomap, double resolution = 0.1); - -/// @brief Octomap distance with an environment with 3 * env_size objects -void octomap_distance_test(double env_scale, std::size_t env_size, bool use_mesh, bool use_mesh_octomap, double resolution = 0.1); - -/// @brief Octomap collision with an environment mesh with 3 * env_size objects, asserting that correct triangle ids -/// are returned when performing collision tests -void octomap_collision_test_mesh_triangle_id(double env_scale, std::size_t env_size, std::size_t num_max_contacts, double resolution = 0.1); - - -template -void octomap_collision_test_BVH(std::size_t n, bool exhaustive, double resolution = 0.1); - - -template -void octomap_distance_test_BVH(std::size_t n, double resolution = 0.1); - -GTEST_TEST(FCL_OCTOMAP, test_octomap_cost) -{ -#if FCL_BUILD_TYPE_DEBUG - octomap_cost_test(200, 10, 10, false, false, 0.1); - octomap_cost_test(200, 100, 10, false, false, 0.1); -#else - octomap_cost_test(200, 100, 10, false, false); - octomap_cost_test(200, 1000, 10, false, false); -#endif -} - -GTEST_TEST(FCL_OCTOMAP, test_octomap_cost_mesh) -{ -#if FCL_BUILD_TYPE_DEBUG - octomap_cost_test(200, 2, 4, true, false, 1.0); - octomap_cost_test(200, 5, 4, true, false, 1.0); -#else - octomap_cost_test(200, 100, 10, true, false); - octomap_cost_test(200, 1000, 10, true, false); -#endif -} - -GTEST_TEST(FCL_OCTOMAP, test_octomap_collision) -{ -#if FCL_BUILD_TYPE_DEBUG - octomap_collision_test(200, 10, false, 10, false, false, 0.1); - octomap_collision_test(200, 100, false, 10, false, false, 0.1); - octomap_collision_test(200, 10, true, 1, false, false, 0.1); - octomap_collision_test(200, 100, true, 1, false, false, 0.1); -#else - octomap_collision_test(200, 100, false, 10, false, false, 0.1); - octomap_collision_test(200, 1000, false, 10, false, false, 0.1); - octomap_collision_test(200, 100, true, 1, false, false, 0.1); - octomap_collision_test(200, 1000, true, 1, false, false, 0.1); -#endif -} - -GTEST_TEST(FCL_OCTOMAP, test_octomap_collision_mesh) -{ -#if FCL_BUILD_TYPE_DEBUG - octomap_collision_test(200, 4, false, 1, true, true, 1.0); - octomap_collision_test(200, 4, true, 1, true, true, 1.0); -#else - octomap_collision_test(200, 100, false, 10, true, true, 0.1); - octomap_collision_test(200, 1000, false, 10, true, true, 0.1); - octomap_collision_test(200, 100, true, 1, true, true, 0.1); - octomap_collision_test(200, 1000, true, 1, true, true, 0.1); -#endif -} - -GTEST_TEST(FCL_OCTOMAP, test_octomap_collision_mesh_triangle_id) -{ -#if FCL_BUILD_TYPE_DEBUG - octomap_collision_test_mesh_triangle_id(1, 10, 10000, 1.0); -#else - octomap_collision_test_mesh_triangle_id(1, 30, 100000, 0.1); -#endif -} - - -GTEST_TEST(FCL_OCTOMAP, test_octomap_collision_mesh_octomap_box) -{ -#if FCL_BUILD_TYPE_DEBUG - octomap_collision_test(200, 4, false, 4, true, false, 1.0); - octomap_collision_test(200, 4, true, 1, true, false, 1.0); -#else - octomap_collision_test(200, 100, false, 10, true, false, 0.1); - octomap_collision_test(200, 1000, false, 10, true, false, 0.1); - octomap_collision_test(200, 100, true, 1, true, false, 0.1); - octomap_collision_test(200, 1000, true, 1, true, false, 0.1); -#endif -} - -GTEST_TEST(FCL_OCTOMAP, test_octomap_distance) -{ -#if FCL_BUILD_TYPE_DEBUG - octomap_distance_test(200, 2, false, false, 1.0); - octomap_distance_test(200, 10, false, false, 1.0); -#else - octomap_distance_test(200, 100, false, false, 0.1); - octomap_distance_test(200, 1000, false, false, 0.1); -#endif -} - -GTEST_TEST(FCL_OCTOMAP, test_octomap_distance_mesh) -{ -#if FCL_BUILD_TYPE_DEBUG - octomap_distance_test(200, 2, true, true, 1.0); - octomap_distance_test(200, 5, true, true, 1.0); -#else - octomap_distance_test(200, 100, true, true, 0.1); - octomap_distance_test(200, 1000, true, true, 0.1); -#endif -} - -GTEST_TEST(FCL_OCTOMAP, test_octomap_distance_mesh_octomap_box) -{ -#if FCL_BUILD_TYPE_DEBUG - octomap_distance_test(200, 2, true, false, 1.0); - octomap_distance_test(200, 5, true, false, 1.0); -#else - octomap_distance_test(200, 100, true, false, 0.1); - octomap_distance_test(200, 1000, true, false, 0.1); -#endif -} - -GTEST_TEST(FCL_OCTOMAP, test_octomap_bvh_obb_collision_obb) -{ -#if FCL_BUILD_TYPE_DEBUG - octomap_collision_test_BVH(1, false, 1.0); - octomap_collision_test_BVH(1, true, 1.0); -#else - octomap_collision_test_BVH(5, false, 0.1); - octomap_collision_test_BVH(5, true, 0.1); -#endif -} - -GTEST_TEST(FCL_OCTOMAP, test_octomap_bvh_rss_d_distance_rss) -{ -#if FCL_BUILD_TYPE_DEBUG - octomap_distance_test_BVH(1, 1.0); -#else - octomap_distance_test_BVH(5, 0.1); -#endif -} - -GTEST_TEST(FCL_OCTOMAP, test_octomap_bvh_obb_d_distance_obb) -{ -#if FCL_BUILD_TYPE_DEBUG - octomap_distance_test_BVH(1, 1.0); -#else - octomap_distance_test_BVH(5, 0.1); -#endif -} - -GTEST_TEST(FCL_OCTOMAP, test_octomap_bvh_kios_d_distance_kios) -{ -#if FCL_BUILD_TYPE_DEBUG - octomap_distance_test_BVH(1, 1.0); -#else - octomap_distance_test_BVH(5, 0.1); -#endif -} - -template -void octomap_collision_test_BVH(std::size_t n, bool exhaustive, double resolution) -{ - std::vector p1; - std::vector t1; - - loadOBJFile(TEST_RESOURCES_DIR"/env.obj", p1, t1); - - BVHModel* m1 = new BVHModel(); - std::shared_ptr m1_ptr(m1); - - m1->beginModel(); - m1->addSubModel(p1, t1); - m1->endModel(); - - OcTree* tree = new OcTree(std::shared_ptr(generateOcTree(resolution))); - std::shared_ptr tree_ptr(tree); - - std::vector transforms; - FCL_REAL extents[] = {-10, -10, 10, 10, 10, 10}; - - generateRandomTransforms(extents, transforms, n); - - for(std::size_t i = 0; i < n; ++i) - { - Transform3d tf(transforms[i]); - - CollisionObject obj1(m1_ptr, tf); - CollisionObject obj2(tree_ptr, tf); - CollisionData cdata; - if(exhaustive) cdata.request.num_max_contacts = 100000; - - defaultCollisionFunction(&obj1, &obj2, &cdata); - - - std::vector boxes; - generateBoxesFromOctomap(boxes, *tree); - for(std::size_t j = 0; j < boxes.size(); ++j) - boxes[j]->setTransform(tf * boxes[j]->getTransform()); - - DynamicAABBTreeCollisionManager* manager = new DynamicAABBTreeCollisionManager(); - manager->registerObjects(boxes); - manager->setup(); - - CollisionData cdata2; - if(exhaustive) cdata2.request.num_max_contacts = 100000; - manager->collide(&obj1, &cdata2, defaultCollisionFunction); - - for(std::size_t j = 0; j < boxes.size(); ++j) - delete boxes[j]; - delete manager; - - if(exhaustive) - { - std::cout << cdata.result.numContacts() << " " << cdata2.result.numContacts() << std::endl; - EXPECT_TRUE(cdata.result.numContacts() == cdata2.result.numContacts()); - } - else - { - std::cout << (cdata.result.numContacts() > 0) << " " << (cdata2.result.numContacts() > 0) << std::endl; - EXPECT_TRUE((cdata.result.numContacts() > 0) == (cdata2.result.numContacts() > 0)); - } - } -} - - -template -void octomap_distance_test_BVH(std::size_t n, double resolution) -{ - std::vector p1; - std::vector t1; - - loadOBJFile(TEST_RESOURCES_DIR"/env.obj", p1, t1); - - BVHModel* m1 = new BVHModel(); - std::shared_ptr m1_ptr(m1); - - m1->beginModel(); - m1->addSubModel(p1, t1); - m1->endModel(); - - OcTree* tree = new OcTree(std::shared_ptr(generateOcTree(resolution))); - std::shared_ptr tree_ptr(tree); - - std::vector transforms; - FCL_REAL extents[] = {-10, -10, 10, 10, 10, 10}; - - generateRandomTransforms(extents, transforms, n); - - for(std::size_t i = 0; i < n; ++i) - { - Transform3d tf(transforms[i]); - - CollisionObject obj1(m1_ptr, tf); - CollisionObject obj2(tree_ptr, tf); - DistanceData cdata; - FCL_REAL dist1 = std::numeric_limits::max(); - defaultDistanceFunction(&obj1, &obj2, &cdata, dist1); - - - std::vector boxes; - generateBoxesFromOctomap(boxes, *tree); - for(std::size_t j = 0; j < boxes.size(); ++j) - boxes[j]->setTransform(tf * boxes[j]->getTransform()); - - DynamicAABBTreeCollisionManager* manager = new DynamicAABBTreeCollisionManager(); - manager->registerObjects(boxes); - manager->setup(); - - DistanceData cdata2; - manager->distance(&obj1, &cdata2, defaultDistanceFunction); - FCL_REAL dist2 = cdata2.result.min_distance; - - for(std::size_t j = 0; j < boxes.size(); ++j) - delete boxes[j]; - delete manager; - - std::cout << dist1 << " " << dist2 << std::endl; - EXPECT_TRUE(std::abs(dist1 - dist2) < 0.001); - } -} - - -void octomap_cost_test(double env_scale, std::size_t env_size, std::size_t num_max_cost_sources, bool use_mesh, bool use_mesh_octomap, double resolution) -{ - std::vector env; - if(use_mesh) - generateEnvironmentsMesh(env, env_scale, env_size); - else - generateEnvironments(env, env_scale, env_size); - - OcTree* tree = new OcTree(std::shared_ptr(generateOcTree(resolution))); - CollisionObject tree_obj((std::shared_ptr(tree))); - - DynamicAABBTreeCollisionManager* manager = new DynamicAABBTreeCollisionManager(); - manager->registerObjects(env); - manager->setup(); - - CollisionData cdata; - cdata.request.enable_cost = true; - cdata.request.num_max_cost_sources = num_max_cost_sources; - - TStruct t1; - Timer timer1; - timer1.start(); - manager->octree_as_geometry_collide = false; - manager->octree_as_geometry_distance = false; - manager->collide(&tree_obj, &cdata, defaultCollisionFunction); - timer1.stop(); - t1.push_back(timer1.getElapsedTime()); - - CollisionData cdata3; - cdata3.request.enable_cost = true; - cdata3.request.num_max_cost_sources = num_max_cost_sources; - - TStruct t3; - Timer timer3; - timer3.start(); - manager->octree_as_geometry_collide = true; - manager->octree_as_geometry_distance = true; - manager->collide(&tree_obj, &cdata3, defaultCollisionFunction); - timer3.stop(); - t3.push_back(timer3.getElapsedTime()); - - TStruct t2; - Timer timer2; - timer2.start(); - std::vector boxes; - if(use_mesh_octomap) - generateBoxesFromOctomapMesh(boxes, *tree); - else - generateBoxesFromOctomap(boxes, *tree); - timer2.stop(); - t2.push_back(timer2.getElapsedTime()); - - timer2.start(); - DynamicAABBTreeCollisionManager* manager2 = new DynamicAABBTreeCollisionManager(); - manager2->registerObjects(boxes); - manager2->setup(); - timer2.stop(); - t2.push_back(timer2.getElapsedTime()); - - - CollisionData cdata2; - cdata2.request.enable_cost = true; - cdata3.request.num_max_cost_sources = num_max_cost_sources; - - timer2.start(); - manager->collide(manager2, &cdata2, defaultCollisionFunction); - timer2.stop(); - t2.push_back(timer2.getElapsedTime()); - - std::cout << cdata.result.numContacts() << " " << cdata3.result.numContacts() << " " << cdata2.result.numContacts() << std::endl; - std::cout << cdata.result.numCostSources() << " " << cdata3.result.numCostSources() << " " << cdata2.result.numCostSources() << std::endl; - - { - std::vector cost_sources; - cdata.result.getCostSources(cost_sources); - for(std::size_t i = 0; i < cost_sources.size(); ++i) - { - std::cout << cost_sources[i].aabb_min.transpose() << " " << cost_sources[i].aabb_max.transpose() << " " << cost_sources[i].cost_density << std::endl; - } - - std::cout << std::endl; - - cdata3.result.getCostSources(cost_sources); - for(std::size_t i = 0; i < cost_sources.size(); ++i) - { - std::cout << cost_sources[i].aabb_min.transpose() << " " << cost_sources[i].aabb_max.transpose() << " " << cost_sources[i].cost_density << std::endl; - } - - std::cout << std::endl; - - cdata2.result.getCostSources(cost_sources); - for(std::size_t i = 0; i < cost_sources.size(); ++i) - { - std::cout << cost_sources[i].aabb_min.transpose() << " " << cost_sources[i].aabb_max.transpose() << " " << cost_sources[i].cost_density << std::endl; - } - - std::cout << std::endl; - - } - - if(use_mesh) EXPECT_TRUE((cdata.result.numContacts() > 0) >= (cdata2.result.numContacts() > 0)); - else EXPECT_TRUE(cdata.result.numContacts() >= cdata2.result.numContacts()); - - delete manager; - delete manager2; - for(std::size_t i = 0; i < boxes.size(); ++i) - delete boxes[i]; - - std::cout << "collision cost" << std::endl; - std::cout << "1) octomap overall time: " << t1.overall_time << std::endl; - std::cout << "1') octomap overall time (as geometry): " << t3.overall_time << std::endl; - std::cout << "2) boxes overall time: " << t2.overall_time << std::endl; - std::cout << " a) to boxes: " << t2.records[0] << std::endl; - std::cout << " b) structure init: " << t2.records[1] << std::endl; - std::cout << " c) collision: " << t2.records[2] << std::endl; - std::cout << "Note: octomap may need more collides when using mesh, because octomap collision uses box primitive inside" << std::endl; -} - - -void octomap_collision_test(double env_scale, std::size_t env_size, bool exhaustive, std::size_t num_max_contacts, bool use_mesh, bool use_mesh_octomap, double resolution) -{ - // srand(1); - std::vector env; - if(use_mesh) - generateEnvironmentsMesh(env, env_scale, env_size); - else - generateEnvironments(env, env_scale, env_size); - - OcTree* tree = new OcTree(std::shared_ptr(generateOcTree(resolution))); - CollisionObject tree_obj((std::shared_ptr(tree))); - - DynamicAABBTreeCollisionManager* manager = new DynamicAABBTreeCollisionManager(); - manager->registerObjects(env); - manager->setup(); - - CollisionData cdata; - if(exhaustive) cdata.request.num_max_contacts = 100000; - else cdata.request.num_max_contacts = num_max_contacts; - - TStruct t1; - Timer timer1; - timer1.start(); - manager->octree_as_geometry_collide = false; - manager->octree_as_geometry_distance = false; - manager->collide(&tree_obj, &cdata, defaultCollisionFunction); - timer1.stop(); - t1.push_back(timer1.getElapsedTime()); - - CollisionData cdata3; - if(exhaustive) cdata3.request.num_max_contacts = 100000; - else cdata3.request.num_max_contacts = num_max_contacts; - - TStruct t3; - Timer timer3; - timer3.start(); - manager->octree_as_geometry_collide = true; - manager->octree_as_geometry_distance = true; - manager->collide(&tree_obj, &cdata3, defaultCollisionFunction); - timer3.stop(); - t3.push_back(timer3.getElapsedTime()); - - TStruct t2; - Timer timer2; - timer2.start(); - std::vector boxes; - if(use_mesh_octomap) - generateBoxesFromOctomapMesh(boxes, *tree); - else - generateBoxesFromOctomap(boxes, *tree); - timer2.stop(); - t2.push_back(timer2.getElapsedTime()); - - timer2.start(); - DynamicAABBTreeCollisionManager* manager2 = new DynamicAABBTreeCollisionManager(); - manager2->registerObjects(boxes); - manager2->setup(); - timer2.stop(); - t2.push_back(timer2.getElapsedTime()); - - - CollisionData cdata2; - if(exhaustive) cdata2.request.num_max_contacts = 100000; - else cdata2.request.num_max_contacts = num_max_contacts; - - timer2.start(); - manager->collide(manager2, &cdata2, defaultCollisionFunction); - timer2.stop(); - t2.push_back(timer2.getElapsedTime()); - - std::cout << cdata.result.numContacts() << " " << cdata3.result.numContacts() << " " << cdata2.result.numContacts() << std::endl; - if(exhaustive) - { - if(use_mesh) EXPECT_TRUE((cdata.result.numContacts() > 0) >= (cdata2.result.numContacts() > 0)); - else EXPECT_TRUE(cdata.result.numContacts() == cdata2.result.numContacts()); - } - else - { - if(use_mesh) EXPECT_TRUE((cdata.result.numContacts() > 0) >= (cdata2.result.numContacts() > 0)); - else EXPECT_TRUE((cdata.result.numContacts() > 0) >= (cdata2.result.numContacts() > 0)); // because AABB return collision when two boxes contact - } - - delete manager; - delete manager2; - for(size_t i = 0; i < boxes.size(); ++i) - delete boxes[i]; - - if(exhaustive) std::cout << "exhaustive collision" << std::endl; - else std::cout << "non exhaustive collision" << std::endl; - std::cout << "1) octomap overall time: " << t1.overall_time << std::endl; - std::cout << "1') octomap overall time (as geometry): " << t3.overall_time << std::endl; - std::cout << "2) boxes overall time: " << t2.overall_time << std::endl; - std::cout << " a) to boxes: " << t2.records[0] << std::endl; - std::cout << " b) structure init: " << t2.records[1] << std::endl; - std::cout << " c) collision: " << t2.records[2] << std::endl; - std::cout << "Note: octomap may need more collides when using mesh, because octomap collision uses box primitive inside" << std::endl; -} - -void octomap_collision_test_mesh_triangle_id(double env_scale, std::size_t env_size, std::size_t num_max_contacts, double resolution) -{ - std::vector env; - generateEnvironmentsMesh(env, env_scale, env_size); - - OcTree* tree = new OcTree(std::shared_ptr(generateOcTree(resolution))); - CollisionObject tree_obj((std::shared_ptr(tree))); - - std::vector boxes; - generateBoxesFromOctomap(boxes, *tree); - for(std::vector::const_iterator cit = env.begin(); - cit != env.end(); ++cit) - { - fcl::CollisionRequest req(num_max_contacts, true); - fcl::CollisionResult cResult; - fcl::collide(&tree_obj, *cit, req, cResult); - for(std::size_t index=0; index* surface = static_cast*> (contact.o2); - EXPECT_TRUE(surface->num_tris > contact.b2); - } - } -} - -void octomap_distance_test(double env_scale, std::size_t env_size, bool use_mesh, bool use_mesh_octomap, double resolution) -{ - // srand(1); - std::vector env; - if(use_mesh) - generateEnvironmentsMesh(env, env_scale, env_size); - else - generateEnvironments(env, env_scale, env_size); - - OcTree* tree = new OcTree(std::shared_ptr(generateOcTree(resolution))); - CollisionObject tree_obj((std::shared_ptr(tree))); - - DynamicAABBTreeCollisionManager* manager = new DynamicAABBTreeCollisionManager(); - manager->registerObjects(env); - manager->setup(); - - DistanceData cdata; - TStruct t1; - Timer timer1; - timer1.start(); - manager->octree_as_geometry_collide = false; - manager->octree_as_geometry_distance = false; - manager->distance(&tree_obj, &cdata, defaultDistanceFunction); - timer1.stop(); - t1.push_back(timer1.getElapsedTime()); - - - DistanceData cdata3; - TStruct t3; - Timer timer3; - timer3.start(); - manager->octree_as_geometry_collide = true; - manager->octree_as_geometry_distance = true; - manager->distance(&tree_obj, &cdata3, defaultDistanceFunction); - timer3.stop(); - t3.push_back(timer3.getElapsedTime()); - - - TStruct t2; - Timer timer2; - timer2.start(); - std::vector boxes; - if(use_mesh_octomap) - generateBoxesFromOctomapMesh(boxes, *tree); - else - generateBoxesFromOctomap(boxes, *tree); - timer2.stop(); - t2.push_back(timer2.getElapsedTime()); - - timer2.start(); - DynamicAABBTreeCollisionManager* manager2 = new DynamicAABBTreeCollisionManager(); - manager2->registerObjects(boxes); - manager2->setup(); - timer2.stop(); - t2.push_back(timer2.getElapsedTime()); - - - DistanceData cdata2; - - timer2.start(); - manager->distance(manager2, &cdata2, defaultDistanceFunction); - timer2.stop(); - t2.push_back(timer2.getElapsedTime()); - - std::cout << cdata.result.min_distance << " " << cdata3.result.min_distance << " " << cdata2.result.min_distance << std::endl; - - if(cdata.result.min_distance < 0) - EXPECT_TRUE(cdata2.result.min_distance <= 0); - else - EXPECT_TRUE(std::abs(cdata.result.min_distance - cdata2.result.min_distance) < 1e-3); - - delete manager; - delete manager2; - for(size_t i = 0; i < boxes.size(); ++i) - delete boxes[i]; - - - std::cout << "1) octomap overall time: " << t1.overall_time << std::endl; - std::cout << "1') octomap overall time (as geometry): " << t3.overall_time << std::endl; - std::cout << "2) boxes overall time: " << t2.overall_time << std::endl; - std::cout << " a) to boxes: " << t2.records[0] << std::endl; - std::cout << " b) structure init: " << t2.records[1] << std::endl; - std::cout << " c) distance: " << t2.records[2] << std::endl; - std::cout << "Note: octomap may need more collides when using mesh, because octomap collision uses box primitive inside" << std::endl; -} - - - -void generateBoxesFromOctomap(std::vector& boxes, OcTree& tree) -{ - std::vector > boxes_ = tree.toBoxes(); - - for(std::size_t i = 0; i < boxes_.size(); ++i) - { - FCL_REAL x = boxes_[i][0]; - FCL_REAL y = boxes_[i][1]; - FCL_REAL z = boxes_[i][2]; - FCL_REAL size = boxes_[i][3]; - FCL_REAL cost = boxes_[i][4]; - FCL_REAL threshold = boxes_[i][5]; - - Box* box = new Box(size, size, size); - box->cost_density = cost; - box->threshold_occupied = threshold; - CollisionObject* obj = new CollisionObject(std::shared_ptr(box), Transform3d(Eigen::Translation3d(Vector3d(x, y, z)))); - boxes.push_back(obj); - } - - std::cout << "boxes size: " << boxes.size() << std::endl; - -} - -void generateEnvironments(std::vector& env, double env_scale, std::size_t n) -{ - FCL_REAL extents[] = {-env_scale, env_scale, -env_scale, env_scale, -env_scale, env_scale}; - std::vector transforms; - - generateRandomTransforms(extents, transforms, n); - for(std::size_t i = 0; i < n; ++i) - { - Box* box = new Box(5, 10, 20); - env.push_back(new CollisionObject(std::shared_ptr(box), transforms[i])); - } - - generateRandomTransforms(extents, transforms, n); - for(std::size_t i = 0; i < n; ++i) - { - Sphere* sphere = new Sphere(30); - env.push_back(new CollisionObject(std::shared_ptr(sphere), transforms[i])); - } - - generateRandomTransforms(extents, transforms, n); - for(std::size_t i = 0; i < n; ++i) - { - Cylinder* cylinder = new Cylinder(10, 40); - env.push_back(new CollisionObject(std::shared_ptr(cylinder), transforms[i])); - } - -} - - -void generateBoxesFromOctomapMesh(std::vector& boxes, OcTree& tree) -{ - std::vector > boxes_ = tree.toBoxes(); - - for(std::size_t i = 0; i < boxes_.size(); ++i) - { - FCL_REAL x = boxes_[i][0]; - FCL_REAL y = boxes_[i][1]; - FCL_REAL z = boxes_[i][2]; - FCL_REAL size = boxes_[i][3]; - FCL_REAL cost = boxes_[i][4]; - FCL_REAL threshold = boxes_[i][5]; - - Box box(size, size, size); - BVHModel* model = new BVHModel(); - generateBVHModel(*model, box, Transform3d::Identity()); - model->cost_density = cost; - model->threshold_occupied = threshold; - CollisionObject* obj = new CollisionObject(std::shared_ptr(model), Transform3d(Eigen::Translation3d(Vector3d(x, y, z)))); - boxes.push_back(obj); - } - - std::cout << "boxes size: " << boxes.size() << std::endl; -} - -void generateEnvironmentsMesh(std::vector& env, double env_scale, std::size_t n) -{ - FCL_REAL extents[] = {-env_scale, env_scale, -env_scale, env_scale, -env_scale, env_scale}; - std::vector transforms; - - generateRandomTransforms(extents, transforms, n); - Box box(5, 10, 20); - for(std::size_t i = 0; i < n; ++i) - { - BVHModel* model = new BVHModel(); - generateBVHModel(*model, box, Transform3d::Identity()); - env.push_back(new CollisionObject(std::shared_ptr(model), transforms[i])); - } - - generateRandomTransforms(extents, transforms, n); - Sphere sphere(30); - for(std::size_t i = 0; i < n; ++i) - { - BVHModel* model = new BVHModel(); - generateBVHModel(*model, sphere, Transform3d::Identity(), 16, 16); - env.push_back(new CollisionObject(std::shared_ptr(model), transforms[i])); - } - - generateRandomTransforms(extents, transforms, n); - Cylinder cylinder(10, 40); - for(std::size_t i = 0; i < n; ++i) - { - BVHModel* model = new BVHModel(); - generateBVHModel(*model, cylinder, Transform3d::Identity(), 16, 16); - env.push_back(new CollisionObject(std::shared_ptr(model), transforms[i])); - } -} - - -octomap::OcTree* generateOcTree(double resolution) -{ - octomap::OcTree* tree = new octomap::OcTree(resolution); - - // insert some measurements of occupied cells - for(int x = -20; x < 20; x++) - { - for(int y = -20; y < 20; y++) - { - for(int z = -20; z < 20; z++) - { - tree->updateNode(octomap::point3d(x * 0.05, y * 0.05, z * 0.05), true); - } - } - } - - // insert some measurements of free cells - for(int x = -30; x < 30; x++) - { - for(int y = -30; y < 30; y++) - { - for(int z = -30; z < 30; z++) - { - tree->updateNode(octomap::point3d(x*0.02 -1.0, y*0.02-1.0, z*0.02-1.0), false); - } - } - } - - return tree; -} - -//============================================================================== -int main(int argc, char* argv[]) -{ - ::testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/test/test_fcl_octomap_collision.cpp b/test/test_fcl_octomap_collision.cpp new file mode 100644 index 000000000..769d99ac9 --- /dev/null +++ b/test/test_fcl_octomap_collision.cpp @@ -0,0 +1,356 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#include + +#include "fcl/config.h" +#include "fcl/octree.h" +#include "fcl/traversal/traversal_nodes.h" +#include "fcl/collision.h" +#include "fcl/broadphase/broadphase.h" +#include "fcl/shape/geometric_shape_to_BVH_model.h" +#include "test_fcl_utility.h" +#include "fcl_resources/config.h" + +using namespace fcl; + +/// @brief Octomap collision with an environment with 3 * env_size objects +template +void octomap_collision_test(S env_scale, std::size_t env_size, bool exhaustive, std::size_t num_max_contacts, bool use_mesh, bool use_mesh_octomap, double resolution = 0.1); + +/// @brief Octomap collision with an environment mesh with 3 * env_size objects, asserting that correct triangle ids +/// are returned when performing collision tests +template +void octomap_collision_test_mesh_triangle_id(S env_scale, std::size_t env_size, std::size_t num_max_contacts, double resolution = 0.1); + +template +void octomap_collision_test_BVH(std::size_t n, bool exhaustive, double resolution = 0.1); + +template +void test_octomap_collision() +{ +#ifdef NDEBUG + octomap_collision_test(200, 100, false, 10, false, false); + octomap_collision_test(200, 1000, false, 10, false, false); + octomap_collision_test(200, 100, true, 1, false, false); + octomap_collision_test(200, 1000, true, 1, false, false); +#else + octomap_collision_test(200, 10, false, 10, false, false, 0.1); + octomap_collision_test(200, 100, false, 10, false, false, 0.1); + octomap_collision_test(200, 10, true, 1, false, false, 0.1); + octomap_collision_test(200, 100, true, 1, false, false, 0.1); +#endif +} + +GTEST_TEST(FCL_OCTOMAP, test_octomap_collision) +{ +// test_octomap_collision(); + test_octomap_collision(); +} + +template +void test_octomap_collision_mesh() +{ +#ifdef NDEBUG + octomap_collision_test(200, 100, false, 10, true, true); + octomap_collision_test(200, 1000, false, 10, true, true); + octomap_collision_test(200, 100, true, 1, true, true); + octomap_collision_test(200, 1000, true, 1, true, true); +#else + octomap_collision_test(200, 4, false, 1, true, true, 1.0); + octomap_collision_test(200, 4, true, 1, true, true, 1.0); +#endif +} + +GTEST_TEST(FCL_OCTOMAP, test_octomap_collision_mesh) +{ +// test_octomap_collision_mesh(); + test_octomap_collision_mesh(); +} + +template +void test_octomap_collision_mesh_triangle_id() +{ +#ifdef NDEBUG + octomap_collision_test_mesh_triangle_id(1, 30, 100000); +#else + octomap_collision_test_mesh_triangle_id(1, 10, 10000, 1.0); +#endif +} + +GTEST_TEST(FCL_OCTOMAP, test_octomap_collision_mesh_triangle_id) +{ +// test_octomap_collision_mesh_triangle_id(); + test_octomap_collision_mesh_triangle_id(); +} + +template +void test_octomap_collision_mesh_octomap_box() +{ +#ifdef NDEBUG + octomap_collision_test(200, 100, false, 10, true, false); + octomap_collision_test(200, 1000, false, 10, true, false); + octomap_collision_test(200, 100, true, 1, true, false); + octomap_collision_test(200, 1000, true, 1, true, false); +#else + octomap_collision_test(200, 4, false, 4, true, false, 1.0); + octomap_collision_test(200, 4, true, 1, true, false, 1.0); +#endif +} + +GTEST_TEST(FCL_OCTOMAP, test_octomap_collision_mesh_octomap_box) +{ +// test_octomap_collision_mesh_octomap_box(); + test_octomap_collision_mesh_octomap_box(); +} + +template +void test_octomap_bvh_obb_collision_obb() +{ +#ifdef NDEBUG + octomap_collision_test_BVH>(5, false); + octomap_collision_test_BVH>(5, true); +#else + octomap_collision_test_BVH>(1, false, 1.0); + octomap_collision_test_BVH>(1, true, 1.0); +#endif +} + +GTEST_TEST(FCL_OCTOMAP, test_octomap_bvh_obb_collision_obb) +{ +// test_octomap_bvh_obb_collision_obb(); + test_octomap_bvh_obb_collision_obb(); +} + +template +void octomap_collision_test_BVH(std::size_t n, bool exhaustive, double resolution) +{ + using S = typename BV::S; + + std::vector> p1; + std::vector t1; + + loadOBJFile(TEST_RESOURCES_DIR"/env.obj", p1, t1); + + BVHModel* m1 = new BVHModel(); + std::shared_ptr> m1_ptr(m1); + + m1->beginModel(); + m1->addSubModel(p1, t1); + m1->endModel(); + + OcTree* tree = new OcTree(std::shared_ptr(generateOcTree(resolution))); + std::shared_ptr> tree_ptr(tree); + + Eigen::aligned_vector> transforms; + S extents[] = {-10, -10, 10, 10, 10, 10}; + + generateRandomTransforms(extents, transforms, n); + + for(std::size_t i = 0; i < n; ++i) + { + Transform3 tf(transforms[i]); + + CollisionObject obj1(m1_ptr, tf); + CollisionObject obj2(tree_ptr, tf); + CollisionData cdata; + if(exhaustive) cdata.request.num_max_contacts = 100000; + + defaultCollisionFunction(&obj1, &obj2, &cdata); + + + std::vector*> boxes; + generateBoxesFromOctomap(boxes, *tree); + for(std::size_t j = 0; j < boxes.size(); ++j) + boxes[j]->setTransform(tf * boxes[j]->getTransform()); + + DynamicAABBTreeCollisionManager* manager = new DynamicAABBTreeCollisionManager(); + manager->registerObjects(boxes); + manager->setup(); + + CollisionData cdata2; + if(exhaustive) cdata2.request.num_max_contacts = 100000; + manager->collide(&obj1, &cdata2, defaultCollisionFunction); + + for(std::size_t j = 0; j < boxes.size(); ++j) + delete boxes[j]; + delete manager; + + if(exhaustive) + { + std::cout << cdata.result.numContacts() << " " << cdata2.result.numContacts() << std::endl; + EXPECT_TRUE(cdata.result.numContacts() == cdata2.result.numContacts()); + } + else + { + std::cout << (cdata.result.numContacts() > 0) << " " << (cdata2.result.numContacts() > 0) << std::endl; + EXPECT_TRUE((cdata.result.numContacts() > 0) == (cdata2.result.numContacts() > 0)); + } + } +} + +template +void octomap_collision_test(S env_scale, std::size_t env_size, bool exhaustive, std::size_t num_max_contacts, bool use_mesh, bool use_mesh_octomap, double resolution) +{ + // srand(1); + std::vector*> env; + if(use_mesh) + generateEnvironmentsMesh(env, env_scale, env_size); + else + generateEnvironments(env, env_scale, env_size); + + OcTree* tree = new OcTree(std::shared_ptr(generateOcTree(resolution))); + CollisionObject tree_obj((std::shared_ptr>(tree))); + + DynamicAABBTreeCollisionManager* manager = new DynamicAABBTreeCollisionManager(); + manager->registerObjects(env); + manager->setup(); + + CollisionData cdata; + if(exhaustive) cdata.request.num_max_contacts = 100000; + else cdata.request.num_max_contacts = num_max_contacts; + + TStruct t1; + Timer timer1; + timer1.start(); + manager->octree_as_geometry_collide = false; + manager->octree_as_geometry_distance = false; + manager->collide(&tree_obj, &cdata, defaultCollisionFunction); + timer1.stop(); + t1.push_back(timer1.getElapsedTime()); + + CollisionData cdata3; + if(exhaustive) cdata3.request.num_max_contacts = 100000; + else cdata3.request.num_max_contacts = num_max_contacts; + + TStruct t3; + Timer timer3; + timer3.start(); + manager->octree_as_geometry_collide = true; + manager->octree_as_geometry_distance = true; + manager->collide(&tree_obj, &cdata3, defaultCollisionFunction); + timer3.stop(); + t3.push_back(timer3.getElapsedTime()); + + TStruct t2; + Timer timer2; + timer2.start(); + std::vector*> boxes; + if(use_mesh_octomap) + generateBoxesFromOctomapMesh(boxes, *tree); + else + generateBoxesFromOctomap(boxes, *tree); + timer2.stop(); + t2.push_back(timer2.getElapsedTime()); + + timer2.start(); + DynamicAABBTreeCollisionManager* manager2 = new DynamicAABBTreeCollisionManager(); + manager2->registerObjects(boxes); + manager2->setup(); + timer2.stop(); + t2.push_back(timer2.getElapsedTime()); + + + CollisionData cdata2; + if(exhaustive) cdata2.request.num_max_contacts = 100000; + else cdata2.request.num_max_contacts = num_max_contacts; + + timer2.start(); + manager->collide(manager2, &cdata2, defaultCollisionFunction); + timer2.stop(); + t2.push_back(timer2.getElapsedTime()); + + std::cout << cdata.result.numContacts() << " " << cdata3.result.numContacts() << " " << cdata2.result.numContacts() << std::endl; + if(exhaustive) + { + if(use_mesh) EXPECT_TRUE((cdata.result.numContacts() > 0) >= (cdata2.result.numContacts() > 0)); + else EXPECT_TRUE(cdata.result.numContacts() == cdata2.result.numContacts()); + } + else + { + if(use_mesh) EXPECT_TRUE((cdata.result.numContacts() > 0) >= (cdata2.result.numContacts() > 0)); + else EXPECT_TRUE((cdata.result.numContacts() > 0) >= (cdata2.result.numContacts() > 0)); // because AABB return collision when two boxes contact + } + + delete manager; + delete manager2; + for(size_t i = 0; i < boxes.size(); ++i) + delete boxes[i]; + + if(exhaustive) std::cout << "exhaustive collision" << std::endl; + else std::cout << "non exhaustive collision" << std::endl; + std::cout << "1) octomap overall time: " << t1.overall_time << std::endl; + std::cout << "1') octomap overall time (as geometry): " << t3.overall_time << std::endl; + std::cout << "2) boxes overall time: " << t2.overall_time << std::endl; + std::cout << " a) to boxes: " << t2.records[0] << std::endl; + std::cout << " b) structure init: " << t2.records[1] << std::endl; + std::cout << " c) collision: " << t2.records[2] << std::endl; + std::cout << "Note: octomap may need more collides when using mesh, because octomap collision uses box primitive inside" << std::endl; +} + +template +void octomap_collision_test_mesh_triangle_id(S env_scale, std::size_t env_size, std::size_t num_max_contacts, double resolution) +{ + std::vector*> env; + generateEnvironmentsMesh(env, env_scale, env_size); + + OcTree* tree = new OcTree(std::shared_ptr(generateOcTree(resolution))); + CollisionObject tree_obj((std::shared_ptr>(tree))); + + std::vector*> boxes; + generateBoxesFromOctomap(boxes, *tree); + for(typename std::vector*>::const_iterator cit = env.begin(); + cit != env.end(); ++cit) + { + fcl::CollisionRequest req(num_max_contacts, true); + fcl::CollisionResult cResult; + fcl::collide(&tree_obj, *cit, req, cResult); + for(std::size_t index=0; index& contact = cResult.getContact(index); + const fcl::BVHModel>* surface = static_cast>*> (contact.o2); + EXPECT_TRUE(surface->num_tris > contact.b2); + } + } +} + +//============================================================================== +int main(int argc, char* argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test/test_fcl_octomap_cost.cpp b/test/test_fcl_octomap_cost.cpp new file mode 100644 index 000000000..0977ff14a --- /dev/null +++ b/test/test_fcl_octomap_cost.cpp @@ -0,0 +1,266 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#include + +#include "fcl/config.h" +#include "fcl/octree.h" +#include "fcl/traversal/traversal_nodes.h" +#include "fcl/collision.h" +#include "fcl/broadphase/broadphase.h" +#include "fcl/shape/geometric_shape_to_BVH_model.h" +#include "test_fcl_utility.h" +#include "fcl_resources/config.h" + +using namespace fcl; + +/// @brief Octomap collision with an environment with 3 * env_size objects, compute cost +template +void octomap_cost_test(S env_scale, std::size_t env_size, std::size_t num_max_cost_sources, bool use_mesh, bool use_mesh_octomap, double resolution = 0.1); + +template +void test_octomap_cost() +{ +#ifdef NDEBUG + octomap_cost_test(200, 100, 10, false, false); + octomap_cost_test(200, 1000, 10, false, false); +#else + octomap_cost_test(200, 10, 10, false, false, 0.1); + octomap_cost_test(200, 100, 10, false, false, 0.1); +#endif +} + +GTEST_TEST(FCL_OCTOMAP, test_octomap_cost) +{ +// test_octomap_cost(); + test_octomap_cost(); +} + +template +void test_octomap_cost_mesh() +{ +#ifdef NDEBUG + octomap_cost_test(200, 100, 10, true, false); + octomap_cost_test(200, 1000, 10, true, false); +#else + octomap_cost_test(200, 2, 4, true, false, 1.0); + octomap_cost_test(200, 5, 4, true, false, 1.0); +#endif +} + +GTEST_TEST(FCL_OCTOMAP, test_octomap_cost_mesh) +{ +// test_octomap_cost_mesh(); + test_octomap_cost_mesh(); +} + +template +void octomap_cost_test(S env_scale, std::size_t env_size, std::size_t num_max_cost_sources, bool use_mesh, bool use_mesh_octomap, double resolution) +{ + std::vector*> env; + if(use_mesh) + generateEnvironmentsMesh(env, env_scale, env_size); + else + generateEnvironments(env, env_scale, env_size); + + OcTree* tree = new OcTree(std::shared_ptr(generateOcTree(resolution))); + CollisionObject tree_obj((std::shared_ptr>(tree))); + + DynamicAABBTreeCollisionManager* manager = new DynamicAABBTreeCollisionManager(); + manager->registerObjects(env); + manager->setup(); + + CollisionData cdata; + cdata.request.enable_cost = true; + cdata.request.num_max_cost_sources = num_max_cost_sources; + + TStruct t1; + Timer timer1; + timer1.start(); + manager->octree_as_geometry_collide = false; + manager->octree_as_geometry_distance = false; + manager->collide(&tree_obj, &cdata, defaultCollisionFunction); + timer1.stop(); + t1.push_back(timer1.getElapsedTime()); + + CollisionData cdata3; + cdata3.request.enable_cost = true; + cdata3.request.num_max_cost_sources = num_max_cost_sources; + + TStruct t3; + Timer timer3; + timer3.start(); + manager->octree_as_geometry_collide = true; + manager->octree_as_geometry_distance = true; + manager->collide(&tree_obj, &cdata3, defaultCollisionFunction); + timer3.stop(); + t3.push_back(timer3.getElapsedTime()); + + TStruct t2; + Timer timer2; + timer2.start(); + std::vector*> boxes; + if(use_mesh_octomap) + generateBoxesFromOctomapMesh(boxes, *tree); + else + generateBoxesFromOctomap(boxes, *tree); + timer2.stop(); + t2.push_back(timer2.getElapsedTime()); + + timer2.start(); + DynamicAABBTreeCollisionManager* manager2 = new DynamicAABBTreeCollisionManager(); + manager2->registerObjects(boxes); + manager2->setup(); + timer2.stop(); + t2.push_back(timer2.getElapsedTime()); + + + CollisionData cdata2; + cdata2.request.enable_cost = true; + cdata3.request.num_max_cost_sources = num_max_cost_sources; + + timer2.start(); + manager->collide(manager2, &cdata2, defaultCollisionFunction); + timer2.stop(); + t2.push_back(timer2.getElapsedTime()); + + std::cout << cdata.result.numContacts() << " " << cdata3.result.numContacts() << " " << cdata2.result.numContacts() << std::endl; + std::cout << cdata.result.numCostSources() << " " << cdata3.result.numCostSources() << " " << cdata2.result.numCostSources() << std::endl; + + { + std::vector> cost_sources; + cdata.result.getCostSources(cost_sources); + for(std::size_t i = 0; i < cost_sources.size(); ++i) + { + std::cout << cost_sources[i].aabb_min.transpose() << " " << cost_sources[i].aabb_max.transpose() << " " << cost_sources[i].cost_density << std::endl; + } + + std::cout << std::endl; + + cdata3.result.getCostSources(cost_sources); + for(std::size_t i = 0; i < cost_sources.size(); ++i) + { + std::cout << cost_sources[i].aabb_min.transpose() << " " << cost_sources[i].aabb_max.transpose() << " " << cost_sources[i].cost_density << std::endl; + } + + std::cout << std::endl; + + cdata2.result.getCostSources(cost_sources); + for(std::size_t i = 0; i < cost_sources.size(); ++i) + { + std::cout << cost_sources[i].aabb_min.transpose() << " " << cost_sources[i].aabb_max.transpose() << " " << cost_sources[i].cost_density << std::endl; + } + + std::cout << std::endl; + + } + + if(use_mesh) EXPECT_TRUE((cdata.result.numContacts() > 0) >= (cdata2.result.numContacts() > 0)); + else EXPECT_TRUE(cdata.result.numContacts() >= cdata2.result.numContacts()); + + delete manager; + delete manager2; + for(std::size_t i = 0; i < boxes.size(); ++i) + delete boxes[i]; + + std::cout << "collision cost" << std::endl; + std::cout << "1) octomap overall time: " << t1.overall_time << std::endl; + std::cout << "1') octomap overall time (as geometry): " << t3.overall_time << std::endl; + std::cout << "2) boxes overall time: " << t2.overall_time << std::endl; + std::cout << " a) to boxes: " << t2.records[0] << std::endl; + std::cout << " b) structure init: " << t2.records[1] << std::endl; + std::cout << " c) collision: " << t2.records[2] << std::endl; + std::cout << "Note: octomap may need more collides when using mesh, because octomap collision uses box primitive inside" << std::endl; +} + +template +void generateBoxesFromOctomap(std::vector*>& boxes, OcTree& tree) +{ + std::vector > boxes_ = tree.toBoxes(); + + for(std::size_t i = 0; i < boxes_.size(); ++i) + { + S x = boxes_[i][0]; + S y = boxes_[i][1]; + S z = boxes_[i][2]; + S size = boxes_[i][3]; + S cost = boxes_[i][4]; + S threshold = boxes_[i][5]; + + Box* box = new Box(size, size, size); + box->cost_density = cost; + box->threshold_occupied = threshold; + CollisionObject* obj = new CollisionObject(std::shared_ptr>(box), Transform3(Translation3(Vector3(x, y, z)))); + boxes.push_back(obj); + } + + std::cout << "boxes size: " << boxes.size() << std::endl; + +} + +template +void generateBoxesFromOctomapMesh(std::vector*>& boxes, OcTree& tree) +{ + std::vector > boxes_ = tree.toBoxes(); + + for(std::size_t i = 0; i < boxes_.size(); ++i) + { + S x = boxes_[i][0]; + S y = boxes_[i][1]; + S z = boxes_[i][2]; + S size = boxes_[i][3]; + S cost = boxes_[i][4]; + S threshold = boxes_[i][5]; + + Box box(size, size, size); + BVHModel>* model = new BVHModel>(); + generateBVHModel(*model, box, Transform3::Identity()); + model->cost_density = cost; + model->threshold_occupied = threshold; + CollisionObject* obj = new CollisionObject(std::shared_ptr>(model), Transform3(Translation3(Vector3(x, y, z)))); + boxes.push_back(obj); + } + + std::cout << "boxes size: " << boxes.size() << std::endl; +} + +//============================================================================== +int main(int argc, char* argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test/test_fcl_octomap_distance.cpp b/test/test_fcl_octomap_distance.cpp new file mode 100644 index 000000000..881f08b3d --- /dev/null +++ b/test/test_fcl_octomap_distance.cpp @@ -0,0 +1,310 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#include + +#include "fcl/config.h" +#include "fcl/octree.h" +#include "fcl/traversal/traversal_nodes.h" +#include "fcl/collision.h" +#include "fcl/broadphase/broadphase.h" +#include "fcl/shape/geometric_shape_to_BVH_model.h" +#include "test_fcl_utility.h" +#include "fcl_resources/config.h" + +using namespace fcl; + +/// @brief Octomap distance with an environment with 3 * env_size objects +template +void octomap_distance_test(S env_scale, std::size_t env_size, bool use_mesh, bool use_mesh_octomap, double resolution = 0.1); + +template +void octomap_distance_test_BVH(std::size_t n, double resolution = 0.1); + +template +void test_octomap_distance() +{ +#ifdef NDEBUG + octomap_distance_test(200, 100, false, false); + octomap_distance_test(200, 1000, false, false); +#else + octomap_distance_test(200, 2, false, false, 1.0); + octomap_distance_test(200, 10, false, false, 1.0); +#endif +} + +GTEST_TEST(FCL_OCTOMAP, test_octomap_distance) +{ +// test_octomap_distance(); + test_octomap_distance(); +} + +template +void test_octomap_distance_mesh() +{ +#ifdef NDEBUG + octomap_distance_test(200, 100, true, true); + octomap_distance_test(200, 1000, true, true); +#else + octomap_distance_test(200, 2, true, true, 1.0); + octomap_distance_test(200, 5, true, true, 1.0); +#endif +} + +GTEST_TEST(FCL_OCTOMAP, test_octomap_distance_mesh) +{ +// test_octomap_distance_mesh(); + test_octomap_distance_mesh(); +} + +template +void test_octomap_distance_mesh_octomap_box() +{ +#ifdef NDEBUG + octomap_distance_test(200, 100, true, false); + octomap_distance_test(200, 1000, true, false); +#else + octomap_distance_test(200, 2, true, false, 1.0); + octomap_distance_test(200, 5, true, false, 1.0); +#endif +} + +GTEST_TEST(FCL_OCTOMAP, test_octomap_distance_mesh_octomap_box) +{ +// test_octomap_distance_mesh_octomap_box(); + test_octomap_distance_mesh_octomap_box(); +} + +template +void test_octomap_bvh_rss_d_distance_rss() +{ +#ifdef NDEBUG + octomap_distance_test_BVH>(5); +#else + octomap_distance_test_BVH>(1, 1.0); +#endif +} + +GTEST_TEST(FCL_OCTOMAP, test_octomap_bvh_rss_d_distance_rss) +{ +// test_octomap_bvh_rss_d_distance_rss(); + test_octomap_bvh_rss_d_distance_rss(); +} + +template +void test_octomap_bvh_obb_d_distance_obb() +{ +#ifdef NDEBUG + octomap_distance_test_BVH>(5); +#else + octomap_distance_test_BVH>(1, 1.0); +#endif +} + +GTEST_TEST(FCL_OCTOMAP, test_octomap_bvh_obb_d_distance_obb) +{ +// test_octomap_bvh_obb_d_distance_obb(); + test_octomap_bvh_obb_d_distance_obb(); +} + +template +void test_octomap_bvh_kios_d_distance_kios() +{ +#ifdef NDEBUG + octomap_distance_test_BVH>(5); +#else + octomap_distance_test_BVH>(1, 1.0); +#endif +} + +GTEST_TEST(FCL_OCTOMAP, test_octomap_bvh_kios_d_distance_kios) +{ +// test_octomap_bvh_kios_d_distance_kios(); + test_octomap_bvh_kios_d_distance_kios(); +} + +template +void octomap_distance_test_BVH(std::size_t n, double resolution) +{ + using S = typename BV::S; + + std::vector> p1; + std::vector t1; + + loadOBJFile(TEST_RESOURCES_DIR"/env.obj", p1, t1); + + BVHModel* m1 = new BVHModel(); + std::shared_ptr> m1_ptr(m1); + + m1->beginModel(); + m1->addSubModel(p1, t1); + m1->endModel(); + + OcTree* tree = new OcTree(std::shared_ptr(generateOcTree(resolution))); + std::shared_ptr> tree_ptr(tree); + + Eigen::aligned_vector> transforms; + S extents[] = {-10, -10, 10, 10, 10, 10}; + + generateRandomTransforms(extents, transforms, n); + + for(std::size_t i = 0; i < n; ++i) + { + Transform3 tf(transforms[i]); + + CollisionObject obj1(m1_ptr, tf); + CollisionObject obj2(tree_ptr, tf); + DistanceData cdata; + S dist1 = std::numeric_limits::max(); + defaultDistanceFunction(&obj1, &obj2, &cdata, dist1); + + + std::vector*> boxes; + generateBoxesFromOctomap(boxes, *tree); + for(std::size_t j = 0; j < boxes.size(); ++j) + boxes[j]->setTransform(tf * boxes[j]->getTransform()); + + DynamicAABBTreeCollisionManager* manager = new DynamicAABBTreeCollisionManager(); + manager->registerObjects(boxes); + manager->setup(); + + DistanceData cdata2; + manager->distance(&obj1, &cdata2, defaultDistanceFunction); + S dist2 = cdata2.result.min_distance; + + for(std::size_t j = 0; j < boxes.size(); ++j) + delete boxes[j]; + delete manager; + + std::cout << dist1 << " " << dist2 << std::endl; + EXPECT_TRUE(std::abs(dist1 - dist2) < 0.001); + } +} + +template +void octomap_distance_test(S env_scale, std::size_t env_size, bool use_mesh, bool use_mesh_octomap, double resolution) +{ + // srand(1); + std::vector*> env; + if(use_mesh) + generateEnvironmentsMesh(env, env_scale, env_size); + else + generateEnvironments(env, env_scale, env_size); + + OcTree* tree = new OcTree(std::shared_ptr(generateOcTree(resolution))); + CollisionObject tree_obj((std::shared_ptr>(tree))); + + DynamicAABBTreeCollisionManager* manager = new DynamicAABBTreeCollisionManager(); + manager->registerObjects(env); + manager->setup(); + + DistanceData cdata; + TStruct t1; + Timer timer1; + timer1.start(); + manager->octree_as_geometry_collide = false; + manager->octree_as_geometry_distance = false; + manager->distance(&tree_obj, &cdata, defaultDistanceFunction); + timer1.stop(); + t1.push_back(timer1.getElapsedTime()); + + + DistanceData cdata3; + TStruct t3; + Timer timer3; + timer3.start(); + manager->octree_as_geometry_collide = true; + manager->octree_as_geometry_distance = true; + manager->distance(&tree_obj, &cdata3, defaultDistanceFunction); + timer3.stop(); + t3.push_back(timer3.getElapsedTime()); + + + TStruct t2; + Timer timer2; + timer2.start(); + std::vector*> boxes; + if(use_mesh_octomap) + generateBoxesFromOctomapMesh(boxes, *tree); + else + generateBoxesFromOctomap(boxes, *tree); + timer2.stop(); + t2.push_back(timer2.getElapsedTime()); + + timer2.start(); + DynamicAABBTreeCollisionManager* manager2 = new DynamicAABBTreeCollisionManager(); + manager2->registerObjects(boxes); + manager2->setup(); + timer2.stop(); + t2.push_back(timer2.getElapsedTime()); + + + DistanceData cdata2; + + timer2.start(); + manager->distance(manager2, &cdata2, defaultDistanceFunction); + timer2.stop(); + t2.push_back(timer2.getElapsedTime()); + + std::cout << cdata.result.min_distance << " " << cdata3.result.min_distance << " " << cdata2.result.min_distance << std::endl; + + if(cdata.result.min_distance < 0) + EXPECT_TRUE(cdata2.result.min_distance <= 0); + else + EXPECT_TRUE(std::abs(cdata.result.min_distance - cdata2.result.min_distance) < 1e-3); + + delete manager; + delete manager2; + for(size_t i = 0; i < boxes.size(); ++i) + delete boxes[i]; + + + std::cout << "1) octomap overall time: " << t1.overall_time << std::endl; + std::cout << "1') octomap overall time (as geometry): " << t3.overall_time << std::endl; + std::cout << "2) boxes overall time: " << t2.overall_time << std::endl; + std::cout << " a) to boxes: " << t2.records[0] << std::endl; + std::cout << " b) structure init: " << t2.records[1] << std::endl; + std::cout << " c) distance: " << t2.records[2] << std::endl; + std::cout << "Note: octomap may need more collides when using mesh, because octomap collision uses box primitive inside" << std::endl; +} + +//============================================================================== +int main(int argc, char* argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test/test_fcl_profiler.cpp b/test/test_fcl_profiler.cpp new file mode 100644 index 000000000..580dd517c --- /dev/null +++ b/test/test_fcl_profiler.cpp @@ -0,0 +1,78 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * Copyright (c) 2016, Toyota Research Institute + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* Author Jeongseok Lee */ + +#include +#include "fcl/common/profiler.h" + +using namespace fcl; + +//============================================================================== +GTEST_TEST(FCL_PROFILER, basic) +{ + detail::Profiler::Start(); + { + detail::Profiler::Begin("Section 1"); + { + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + + detail::Profiler::Begin("Section 1.1"); + { + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + } + detail::Profiler::End("Section 1.1"); + + } + detail::Profiler::End("Section 1"); + + detail::Profiler::Begin("Section 2"); + { + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + } + detail::Profiler::End("Section 2"); + } + detail::Profiler::Stop(); + + detail::Profiler::Status(std::cout); +} + +//============================================================================== +int main(int argc, char* argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test/test_fcl_shape_mesh_consistency.cpp b/test/test_fcl_shape_mesh_consistency.cpp index e08cb89db..eb3f23f64 100644 --- a/test/test_fcl_shape_mesh_consistency.cpp +++ b/test/test_fcl_shape_mesh_consistency.cpp @@ -35,58 +35,64 @@ /** \author Jia Pan */ +#include + #include -#include "fcl/narrowphase/narrowphase.h" +#include "fcl/narrowphase/gjk_solver_indep.h" +#include "fcl/narrowphase/gjk_solver_libccd.h" #include "fcl/shape/geometric_shape_to_BVH_model.h" #include "fcl/distance.h" #include "fcl/collision.h" #include "test_fcl_utility.h" - using namespace fcl; -#define EXPECT_TRUE_FALSE(p) EXPECT_TRUE(!(p)) - -FCL_REAL extents[6] = {0, 0, 0, 10, 10, 10}; +template +std::array& extents() +{ + static std::array static_extents{ {0, 0, 0, 10, 10, 10} }; + return static_extents; +} -GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_distance_spheresphere_libccd) +template +void test_consistency_distance_spheresphere_libccd() { - Sphere s1(20); - Sphere s2(20); - BVHModel s1_rss; - BVHModel s2_rss; + Sphere s1(20); + Sphere s2(20); + BVHModel> s1_rss; + BVHModel> s2_rss; - generateBVHModel(s1_rss, s1, Transform3d::Identity(), 16, 16); - generateBVHModel(s2_rss, s2, Transform3d::Identity(), 16, 16); + generateBVHModel(s1_rss, s1, Transform3::Identity(), 16, 16); + generateBVHModel(s2_rss, s2, Transform3::Identity(), 16, 16); - DistanceRequest request; - DistanceResult res, res1; + DistanceRequest request; + DistanceResult res, res1; - Transform3d pose = Transform3d::Identity(); + Transform3 pose = Transform3::Identity(); - pose.translation() = Vector3d(50, 0, 0); + pose.translation() = Vector3(50, 0, 0); res.clear(); res1.clear(); - distance(&s1, Transform3d::Identity(), &s2, pose, request, res); - distance(&s1_rss, Transform3d::Identity(), &s2_rss, pose, request, res1); + distance(&s1, Transform3::Identity(), &s2, pose, request, res); + distance(&s1_rss, Transform3::Identity(), &s2_rss, pose, request, res1); EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); - distance(&s1, Transform3d::Identity(), &s2_rss, pose, request, res1); + distance(&s1, Transform3::Identity(), &s2_rss, pose, request, res1); EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); - + res1.clear(); - distance(&s1_rss, Transform3d::Identity(), &s2, pose, request, res1); + distance(&s1_rss, Transform3::Identity(), &s2, pose, request, res1); EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); for(std::size_t i = 0; i < 10; ++i) { - Transform3d t; - generateRandomTransform(extents, t); - - Transform3d pose1(t); - Transform3d pose2 = t * pose; + Transform3 t; + generateRandomTransform(extents(), t); + + Transform3 pose1(t); + Transform3 pose2 = t * pose; res.clear(); res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); @@ -102,29 +108,29 @@ GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_distance_spheresphere_libccd) EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); } - pose.translation() = Vector3d(40.1, 0, 0); + pose.translation() = Vector3(40.1, 0, 0); res.clear(), res1.clear(); - distance(&s1, Transform3d::Identity(), &s2, pose, request, res); - distance(&s1_rss, Transform3d::Identity(), &s2_rss, pose, request, res1); + distance(&s1, Transform3::Identity(), &s2, pose, request, res); + distance(&s1_rss, Transform3::Identity(), &s2_rss, pose, request, res1); EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); - distance(&s1, Transform3d::Identity(), &s2_rss, pose, request, res1); + distance(&s1, Transform3::Identity(), &s2_rss, pose, request, res1); EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); - distance(&s1_rss, Transform3d::Identity(), &s2, pose, request, res1); + distance(&s1_rss, Transform3::Identity(), &s2, pose, request, res1); EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); for(std::size_t i = 0; i < 10; ++i) { - Transform3d t; - generateRandomTransform(extents, t); - - Transform3d pose1(t); - Transform3d pose2 = t * pose; + Transform3 t; + generateRandomTransform(extents(), t); + + Transform3 pose1(t); + Transform3 pose2 = t * pose; res.clear(); res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); @@ -141,43 +147,50 @@ GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_distance_spheresphere_libccd) } } -GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_distance_ellipsoidellipsoid_libccd) +GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_distance_spheresphere_libccd) { - Ellipsoid s1(20, 40, 50); - Ellipsoid s2(10, 10, 10); - BVHModel s1_rss; - BVHModel s2_rss; +// test_consistency_distance_spheresphere_libccd(); + test_consistency_distance_spheresphere_libccd(); +} + +template +void test_consistency_distance_ellipsoidellipsoid_libccd() +{ + Ellipsoid s1(20, 40, 50); + Ellipsoid s2(10, 10, 10); + BVHModel> s1_rss; + BVHModel> s2_rss; - generateBVHModel(s1_rss, s1, Transform3d::Identity(), 16, 16); - generateBVHModel(s2_rss, s2, Transform3d::Identity(), 16, 16); + generateBVHModel(s1_rss, s1, Transform3::Identity(), 16, 16); + generateBVHModel(s2_rss, s2, Transform3::Identity(), 16, 16); - DistanceRequest request; - DistanceResult res, res1; + DistanceRequest request; + DistanceResult res, res1; - Transform3d pose = Transform3d::Identity(); + Transform3 pose = Transform3::Identity(); - pose.translation() = Vector3d(40, 0, 0); + pose.translation() = Vector3(40, 0, 0); res.clear(); res1.clear(); - distance(&s1, Transform3d::Identity(), &s2, pose, request, res); - distance(&s1_rss, Transform3d::Identity(), &s2_rss, pose, request, res1); + distance(&s1, Transform3::Identity(), &s2, pose, request, res); + distance(&s1_rss, Transform3::Identity(), &s2_rss, pose, request, res1); EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); - distance(&s1, Transform3d::Identity(), &s2_rss, pose, request, res1); + distance(&s1, Transform3::Identity(), &s2_rss, pose, request, res1); EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); - distance(&s1_rss, Transform3d::Identity(), &s2, pose, request, res1); + distance(&s1_rss, Transform3::Identity(), &s2, pose, request, res1); EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); for(std::size_t i = 0; i < 10; ++i) { - Transform3d t; - generateRandomTransform(extents, t); + Transform3 t; + generateRandomTransform(extents(), t); - Transform3d pose1(t); - Transform3d pose2 = t * pose; + Transform3 pose1(t); + Transform3 pose2 = t * pose; res.clear(); res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); @@ -193,28 +206,28 @@ GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_distance_ellipsoidellipsoid_l EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); } - pose.translation() = Vector3d(30.1, 0, 0); + pose.translation() = Vector3(30.1, 0, 0); res.clear(), res1.clear(); - distance(&s1, Transform3d::Identity(), &s2, pose, request, res); - distance(&s1_rss, Transform3d::Identity(), &s2_rss, pose, request, res1); + distance(&s1, Transform3::Identity(), &s2, pose, request, res); + distance(&s1_rss, Transform3::Identity(), &s2_rss, pose, request, res1); EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); - distance(&s1, Transform3d::Identity(), &s2_rss, pose, request, res1); + distance(&s1, Transform3::Identity(), &s2_rss, pose, request, res1); EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); - distance(&s1_rss, Transform3d::Identity(), &s2, pose, request, res1); + distance(&s1_rss, Transform3::Identity(), &s2, pose, request, res1); EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); for(std::size_t i = 0; i < 10; ++i) { - Transform3d t; - generateRandomTransform(extents, t); + Transform3 t; + generateRandomTransform(extents(), t); - Transform3d pose1(t); - Transform3d pose2 = t * pose; + Transform3 pose1(t); + Transform3 pose2 = t * pose; res.clear(); res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); @@ -231,44 +244,51 @@ GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_distance_ellipsoidellipsoid_l } } -GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_distance_boxbox_libccd) +GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_distance_ellipsoidellipsoid_libccd) { - Box s1(20, 40, 50); - Box s2(10, 10, 10); +// test_consistency_distance_ellipsoidellipsoid_libccd(); + test_consistency_distance_ellipsoidellipsoid_libccd(); +} - BVHModel s1_rss; - BVHModel s2_rss; +template +void test_consistency_distance_boxbox_libccd() +{ + Box s1(20, 40, 50); + Box s2(10, 10, 10); + + BVHModel> s1_rss; + BVHModel> s2_rss; - generateBVHModel(s1_rss, s1, Transform3d::Identity()); - generateBVHModel(s2_rss, s2, Transform3d::Identity()); + generateBVHModel(s1_rss, s1, Transform3::Identity()); + generateBVHModel(s2_rss, s2, Transform3::Identity()); - DistanceRequest request; - DistanceResult res, res1; + DistanceRequest request; + DistanceResult res, res1; - Transform3d pose = Transform3d::Identity(); + Transform3 pose = Transform3::Identity(); - pose.translation() = Vector3d(50, 0, 0); + pose.translation() = Vector3(50, 0, 0); res.clear(); res1.clear(); - distance(&s1, Transform3d::Identity(), &s2, pose, request, res); - distance(&s1_rss, Transform3d::Identity(), &s2_rss, pose, request, res1); + distance(&s1, Transform3::Identity(), &s2, pose, request, res); + distance(&s1_rss, Transform3::Identity(), &s2_rss, pose, request, res1); EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); - + res1.clear(); - distance(&s1, Transform3d::Identity(), &s2_rss, pose, request, res1); + distance(&s1, Transform3::Identity(), &s2_rss, pose, request, res1); EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); res1.clear(); - distance(&s1_rss, Transform3d::Identity(), &s2, pose, request, res1); + distance(&s1_rss, Transform3::Identity(), &s2, pose, request, res1); EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); for(std::size_t i = 0; i < 10; ++i) { - Transform3d t; - generateRandomTransform(extents, t); - - Transform3d pose1(t); - Transform3d pose2 = t * pose; + Transform3 t; + generateRandomTransform(extents(), t); + + Transform3 pose1(t); + Transform3 pose2 = t * pose; res.clear(); res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); @@ -284,28 +304,28 @@ GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_distance_boxbox_libccd) EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); } - pose.translation() = Vector3d(15.1, 0, 0); - + pose.translation() = Vector3(15.1, 0, 0); + res.clear(); res1.clear(); - distance(&s1, Transform3d::Identity(), &s2, pose, request, res); - distance(&s1_rss, Transform3d::Identity(), &s2_rss, pose, request, res1); + distance(&s1, Transform3::Identity(), &s2, pose, request, res); + distance(&s1_rss, Transform3::Identity(), &s2_rss, pose, request, res1); EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); - + res1.clear(); - distance(&s1, Transform3d::Identity(), &s2_rss, pose, request, res1); + distance(&s1, Transform3::Identity(), &s2_rss, pose, request, res1); EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); - distance(&s1_rss, Transform3d::Identity(), &s2, pose, request, res1); + distance(&s1_rss, Transform3::Identity(), &s2, pose, request, res1); EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); for(std::size_t i = 0; i < 10; ++i) { - Transform3d t; - generateRandomTransform(extents, t); - - Transform3d pose1(t); - Transform3d pose2 = t * pose; + Transform3 t; + generateRandomTransform(extents(), t); + + Transform3 pose1(t); + Transform3 pose2 = t * pose; res.clear(); res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); @@ -322,44 +342,51 @@ GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_distance_boxbox_libccd) } } -GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_distance_cylindercylinder_libccd) +GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_distance_boxbox_libccd) { - Cylinder s1(5, 10); - Cylinder s2(5, 10); +// test_consistency_distance_boxbox_libccd(); + test_consistency_distance_boxbox_libccd(); +} - BVHModel s1_rss; - BVHModel s2_rss; +template +void test_consistency_distance_cylindercylinder_libccd() +{ + Cylinder s1(5, 10); + Cylinder s2(5, 10); - generateBVHModel(s1_rss, s1, Transform3d::Identity(), 16, 16); - generateBVHModel(s2_rss, s2, Transform3d::Identity(), 16, 16); + BVHModel> s1_rss; + BVHModel> s2_rss; - DistanceRequest request; - DistanceResult res, res1; + generateBVHModel(s1_rss, s1, Transform3::Identity(), 16, 16); + generateBVHModel(s2_rss, s2, Transform3::Identity(), 16, 16); - Transform3d pose = Transform3d::Identity(); + DistanceRequest request; + DistanceResult res, res1; - pose.translation() = Vector3d(20, 0, 0); + Transform3 pose = Transform3::Identity(); + + pose.translation() = Vector3(20, 0, 0); res.clear(); res1.clear(); - distance(&s1, Transform3d::Identity(), &s2, pose, request, res); - distance(&s1_rss, Transform3d::Identity(), &s2_rss, pose, request, res1); + distance(&s1, Transform3::Identity(), &s2, pose, request, res); + distance(&s1_rss, Transform3::Identity(), &s2_rss, pose, request, res1); EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); - + res1.clear(); - distance(&s1, Transform3d::Identity(), &s2_rss, pose, request, res1); + distance(&s1, Transform3::Identity(), &s2_rss, pose, request, res1); EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); - + res1.clear(); - distance(&s1_rss, Transform3d::Identity(), &s2, pose, request, res1); + distance(&s1_rss, Transform3::Identity(), &s2, pose, request, res1); EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); - + for(std::size_t i = 0; i < 10; ++i) { - Transform3d t; - generateRandomTransform(extents, t); - - Transform3d pose1(t); - Transform3d pose2 = t * pose; + Transform3 t; + generateRandomTransform(extents(), t); + + Transform3 pose1(t); + Transform3 pose2 = t * pose; res.clear(); res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); @@ -374,29 +401,29 @@ GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_distance_cylindercylinder_lib distance(&s1_rss, pose1, &s2, pose2, request, res1); EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); } - - pose.translation() = Vector3d(15, 0, 0); // libccd cannot use small value here :( - + + pose.translation() = Vector3(15, 0, 0); // libccd cannot use small value here :( + res.clear(); res1.clear(); - distance(&s1, Transform3d::Identity(), &s2, pose, request, res); - distance(&s1_rss, Transform3d::Identity(), &s2_rss, pose, request, res1); + distance(&s1, Transform3::Identity(), &s2, pose, request, res); + distance(&s1_rss, Transform3::Identity(), &s2_rss, pose, request, res1); EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); - + res1.clear(); - distance(&s1, Transform3d::Identity(), &s2_rss, pose, request, res1); + distance(&s1, Transform3::Identity(), &s2_rss, pose, request, res1); EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); - distance(&s1_rss, Transform3d::Identity(), &s2, pose, request, res1); + distance(&s1_rss, Transform3::Identity(), &s2, pose, request, res1); EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); - + for(std::size_t i = 0; i < 10; ++i) { - Transform3d t; - generateRandomTransform(extents, t); - - Transform3d pose1(t); - Transform3d pose2 = t * pose; + Transform3 t; + generateRandomTransform(extents(), t); + + Transform3 pose1(t); + Transform3 pose2 = t * pose; res.clear(); res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); @@ -413,43 +440,50 @@ GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_distance_cylindercylinder_lib } } -GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_distance_conecone_libccd) +GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_distance_cylindercylinder_libccd) +{ +// test_consistency_distance_cylindercylinder_libccd(); + test_consistency_distance_cylindercylinder_libccd(); +} + +template +void test_consistency_distance_conecone_libccd() { - Cone s1(5, 10); - Cone s2(5, 10); + Cone s1(5, 10); + Cone s2(5, 10); - BVHModel s1_rss; - BVHModel s2_rss; + BVHModel> s1_rss; + BVHModel> s2_rss; - generateBVHModel(s1_rss, s1, Transform3d::Identity(), 16, 16); - generateBVHModel(s2_rss, s2, Transform3d::Identity(), 16, 16); + generateBVHModel(s1_rss, s1, Transform3::Identity(), 16, 16); + generateBVHModel(s2_rss, s2, Transform3::Identity(), 16, 16); - DistanceRequest request; - DistanceResult res, res1; + DistanceRequest request; + DistanceResult res, res1; - Transform3d pose = Transform3d::Identity(); + Transform3 pose = Transform3::Identity(); + + pose.translation() = Vector3(20, 0, 0); - pose.translation() = Vector3d(20, 0, 0); - res.clear(); res1.clear(); - distance(&s1, Transform3d::Identity(), &s2, pose, request, res); - distance(&s1_rss, Transform3d::Identity(), &s2_rss, pose, request, res1); + distance(&s1, Transform3::Identity(), &s2, pose, request, res); + distance(&s1_rss, Transform3::Identity(), &s2_rss, pose, request, res1); EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); - + res1.clear(); - distance(&s1, Transform3d::Identity(), &s2_rss, pose, request, res1); + distance(&s1, Transform3::Identity(), &s2_rss, pose, request, res1); EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); - - distance(&s1_rss, Transform3d::Identity(), &s2, pose, request, res1); + + distance(&s1_rss, Transform3::Identity(), &s2, pose, request, res1); EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); - + for(std::size_t i = 0; i < 10; ++i) { - Transform3d t; - generateRandomTransform(extents, t); - - Transform3d pose1(t); - Transform3d pose2 = t * pose; + Transform3 t; + generateRandomTransform(extents(), t); + + Transform3 pose1(t); + Transform3 pose2 = t * pose; res.clear(); res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); @@ -464,29 +498,29 @@ GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_distance_conecone_libccd) distance(&s1_rss, pose1, &s2, pose2, request, res1); EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); } - - pose.translation() = Vector3d(15, 0, 0); // libccd cannot use small value here :( - + + pose.translation() = Vector3(15, 0, 0); // libccd cannot use small value here :( + res.clear(); res1.clear(); - distance(&s1, Transform3d::Identity(), &s2, pose, request, res); - distance(&s1_rss, Transform3d::Identity(), &s2_rss, pose, request, res1); + distance(&s1, Transform3::Identity(), &s2, pose, request, res); + distance(&s1_rss, Transform3::Identity(), &s2_rss, pose, request, res1); EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); - + res1.clear(); - distance(&s1, Transform3d::Identity(), &s2_rss, pose, request, res1); + distance(&s1, Transform3::Identity(), &s2_rss, pose, request, res1); EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); - distance(&s1_rss, Transform3d::Identity(), &s2, pose, request, res1); + distance(&s1_rss, Transform3::Identity(), &s2, pose, request, res1); EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); - + for(std::size_t i = 0; i < 10; ++i) { - Transform3d t; - generateRandomTransform(extents, t); - - Transform3d pose1(t); - Transform3d pose2 = t * pose; + Transform3 t; + generateRandomTransform(extents(), t); + + Transform3 pose1(t); + Transform3 pose2 = t * pose; res.clear(); res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); @@ -503,46 +537,52 @@ GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_distance_conecone_libccd) } } +GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_distance_conecone_libccd) +{ +// test_consistency_distance_conecone_libccd(); + test_consistency_distance_conecone_libccd(); +} -GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_distance_spheresphere_GJK) +template +void test_consistency_distance_spheresphere_GJK() { - Sphere s1(20); - Sphere s2(20); - BVHModel s1_rss; - BVHModel s2_rss; + Sphere s1(20); + Sphere s2(20); + BVHModel> s1_rss; + BVHModel> s2_rss; - generateBVHModel(s1_rss, s1, Transform3d::Identity(), 16, 16); - generateBVHModel(s2_rss, s2, Transform3d::Identity(), 16, 16); + generateBVHModel(s1_rss, s1, Transform3::Identity(), 16, 16); + generateBVHModel(s2_rss, s2, Transform3::Identity(), 16, 16); - DistanceRequest request; + DistanceRequest request; request.gjk_solver_type = GST_INDEP; - DistanceResult res, res1; + DistanceResult res, res1; - Transform3d pose = Transform3d::Identity(); + Transform3 pose = Transform3::Identity(); - pose.translation() = Vector3d(50, 0, 0); + pose.translation() = Vector3(50, 0, 0); res.clear(); res1.clear(); - distance(&s1, Transform3d::Identity(), &s2, pose, request, res); - distance(&s1_rss, Transform3d::Identity(), &s2_rss, pose, request, res1); + distance(&s1, Transform3::Identity(), &s2, pose, request, res); + distance(&s1_rss, Transform3::Identity(), &s2_rss, pose, request, res1); EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); - distance(&s1, Transform3d::Identity(), &s2_rss, pose, request, res1); + distance(&s1, Transform3::Identity(), &s2_rss, pose, request, res1); EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); - + res1.clear(); - distance(&s1_rss, Transform3d::Identity(), &s2, pose, request, res1); + distance(&s1_rss, Transform3::Identity(), &s2, pose, request, res1); EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); for(std::size_t i = 0; i < 10; ++i) { - Transform3d t; - generateRandomTransform(extents, t); - - Transform3d pose1(t); - Transform3d pose2 = t * pose; + Transform3 t; + generateRandomTransform(extents(), t); + + Transform3 pose1(t); + Transform3 pose2 = t * pose; res.clear(); res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); @@ -558,29 +598,29 @@ GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_distance_spheresphere_GJK) EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); } - pose.translation() = Vector3d(40.1, 0, 0); + pose.translation() = Vector3(40.1, 0, 0); res.clear(); res1.clear(); - distance(&s1, Transform3d::Identity(), &s2, pose, request, res); - distance(&s1_rss, Transform3d::Identity(), &s2_rss, pose, request, res1); + distance(&s1, Transform3::Identity(), &s2, pose, request, res); + distance(&s1_rss, Transform3::Identity(), &s2_rss, pose, request, res1); EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); - distance(&s1, Transform3d::Identity(), &s2_rss, pose, request, res1); + distance(&s1, Transform3::Identity(), &s2_rss, pose, request, res1); EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); - distance(&s1_rss, Transform3d::Identity(), &s2, pose, request, res1); + distance(&s1_rss, Transform3::Identity(), &s2, pose, request, res1); EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 4); for(std::size_t i = 0; i < 10; ++i) { - Transform3d t; - generateRandomTransform(extents, t); - - Transform3d pose1(t); - Transform3d pose2 = t * pose; + Transform3 t; + generateRandomTransform(extents(), t); + + Transform3 pose1(t); + Transform3 pose2 = t * pose; res.clear(); res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); @@ -597,45 +637,52 @@ GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_distance_spheresphere_GJK) } } -GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_distance_ellipsoidellipsoid_GJK) +GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_distance_spheresphere_GJK) +{ +// test_consistency_distance_spheresphere_GJK(); + test_consistency_distance_spheresphere_GJK(); +} + +template +void test_consistency_distance_ellipsoidellipsoid_GJK() { - Ellipsoid s1(20, 40, 50); - Ellipsoid s2(10, 10, 10); - BVHModel s1_rss; - BVHModel s2_rss; + Ellipsoid s1(20, 40, 50); + Ellipsoid s2(10, 10, 10); + BVHModel> s1_rss; + BVHModel> s2_rss; - generateBVHModel(s1_rss, s1, Transform3d::Identity(), 16, 16); - generateBVHModel(s2_rss, s2, Transform3d::Identity(), 16, 16); + generateBVHModel(s1_rss, s1, Transform3::Identity(), 16, 16); + generateBVHModel(s2_rss, s2, Transform3::Identity(), 16, 16); - DistanceRequest request; + DistanceRequest request; request.gjk_solver_type = GST_INDEP; - DistanceResult res, res1; + DistanceResult res, res1; - Transform3d pose = Transform3d::Identity(); + Transform3 pose = Transform3::Identity(); - pose.translation() = Vector3d(40, 0, 0); + pose.translation() = Vector3(40, 0, 0); res.clear(); res1.clear(); - distance(&s1, Transform3d::Identity(), &s2, pose, request, res); - distance(&s1_rss, Transform3d::Identity(), &s2_rss, pose, request, res1); + distance(&s1, Transform3::Identity(), &s2, pose, request, res); + distance(&s1_rss, Transform3::Identity(), &s2_rss, pose, request, res1); EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); - distance(&s1, Transform3d::Identity(), &s2_rss, pose, request, res1); + distance(&s1, Transform3::Identity(), &s2_rss, pose, request, res1); EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); - distance(&s1_rss, Transform3d::Identity(), &s2, pose, request, res1); + distance(&s1_rss, Transform3::Identity(), &s2, pose, request, res1); EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); for(std::size_t i = 0; i < 10; ++i) { - Transform3d t; - generateRandomTransform(extents, t); + Transform3 t; + generateRandomTransform(extents(), t); - Transform3d pose1(t); - Transform3d pose2 = t * pose; + Transform3 pose1(t); + Transform3 pose2 = t * pose; res.clear(); res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); @@ -651,29 +698,29 @@ GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_distance_ellipsoidellipsoid_G EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); } - pose.translation() = Vector3d(30.1, 0, 0); + pose.translation() = Vector3(30.1, 0, 0); res.clear(); res1.clear(); - distance(&s1, Transform3d::Identity(), &s2, pose, request, res); - distance(&s1_rss, Transform3d::Identity(), &s2_rss, pose, request, res1); + distance(&s1, Transform3::Identity(), &s2, pose, request, res); + distance(&s1_rss, Transform3::Identity(), &s2_rss, pose, request, res1); EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); - distance(&s1, Transform3d::Identity(), &s2_rss, pose, request, res1); + distance(&s1, Transform3::Identity(), &s2_rss, pose, request, res1); EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); - distance(&s1_rss, Transform3d::Identity(), &s2, pose, request, res1); + distance(&s1_rss, Transform3::Identity(), &s2, pose, request, res1); EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 4); for(std::size_t i = 0; i < 10; ++i) { - Transform3d t; - generateRandomTransform(extents, t); + Transform3 t; + generateRandomTransform(extents(), t); - Transform3d pose1(t); - Transform3d pose2 = t * pose; + Transform3 pose1(t); + Transform3 pose2 = t * pose; res.clear(); res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); @@ -690,45 +737,52 @@ GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_distance_ellipsoidellipsoid_G } } -GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_distance_boxbox_GJK) +GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_distance_ellipsoidellipsoid_GJK) +{ +// test_consistency_distance_ellipsoidellipsoid_GJK(); + test_consistency_distance_ellipsoidellipsoid_GJK(); +} + +template +void test_consistency_distance_boxbox_GJK() { - Box s1(20, 40, 50); - Box s2(10, 10, 10); + Box s1(20, 40, 50); + Box s2(10, 10, 10); - BVHModel s1_rss; - BVHModel s2_rss; + BVHModel> s1_rss; + BVHModel> s2_rss; - generateBVHModel(s1_rss, s1, Transform3d::Identity()); - generateBVHModel(s2_rss, s2, Transform3d::Identity()); + generateBVHModel(s1_rss, s1, Transform3::Identity()); + generateBVHModel(s2_rss, s2, Transform3::Identity()); - DistanceRequest request; + DistanceRequest request; request.gjk_solver_type = GST_INDEP; - DistanceResult res, res1; + DistanceResult res, res1; - Transform3d pose = Transform3d::Identity(); + Transform3 pose = Transform3::Identity(); - pose.translation() = Vector3d(50, 0, 0); + pose.translation() = Vector3(50, 0, 0); res.clear(); res1.clear(); - distance(&s1, Transform3d::Identity(), &s2, pose, request, res); - distance(&s1_rss, Transform3d::Identity(), &s2_rss, pose, request, res1); + distance(&s1, Transform3::Identity(), &s2, pose, request, res); + distance(&s1_rss, Transform3::Identity(), &s2_rss, pose, request, res1); EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); - + res1.clear(); - distance(&s1, Transform3d::Identity(), &s2_rss, pose, request, res1); + distance(&s1, Transform3::Identity(), &s2_rss, pose, request, res1); EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); res1.clear(); - distance(&s1_rss, Transform3d::Identity(), &s2, pose, request, res1); + distance(&s1_rss, Transform3::Identity(), &s2, pose, request, res1); EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); for(std::size_t i = 0; i < 10; ++i) { - Transform3d t; - generateRandomTransform(extents, t); - - Transform3d pose1(t); - Transform3d pose2 = t * pose; + Transform3 t; + generateRandomTransform(extents(), t); + + Transform3 pose1(t); + Transform3 pose2 = t * pose; res.clear(); res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); @@ -744,28 +798,28 @@ GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_distance_boxbox_GJK) EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); } - pose.translation() = Vector3d(15.1, 0, 0); - + pose.translation() = Vector3(15.1, 0, 0); + res.clear(); res1.clear(); - distance(&s1, Transform3d::Identity(), &s2, pose, request, res); - distance(&s1_rss, Transform3d::Identity(), &s2_rss, pose, request, res1); + distance(&s1, Transform3::Identity(), &s2, pose, request, res); + distance(&s1_rss, Transform3::Identity(), &s2_rss, pose, request, res1); EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); - + res1.clear(); - distance(&s1, Transform3d::Identity(), &s2_rss, pose, request, res1); + distance(&s1, Transform3::Identity(), &s2_rss, pose, request, res1); EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); - distance(&s1_rss, Transform3d::Identity(), &s2, pose, request, res1); + distance(&s1_rss, Transform3::Identity(), &s2, pose, request, res1); EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); for(std::size_t i = 0; i < 10; ++i) { - Transform3d t; - generateRandomTransform(extents, t); - - Transform3d pose1(t); - Transform3d pose2 = t * pose; + Transform3 t; + generateRandomTransform(extents(), t); + + Transform3 pose1(t); + Transform3 pose2 = t * pose; res.clear(); res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); @@ -782,45 +836,52 @@ GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_distance_boxbox_GJK) } } -GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_distance_cylindercylinder_GJK) +GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_distance_boxbox_GJK) +{ +// test_consistency_distance_boxbox_GJK(); + test_consistency_distance_boxbox_GJK(); +} + +template +void test_consistency_distance_cylindercylinder_GJK() { - Cylinder s1(5, 10); - Cylinder s2(5, 10); + Cylinder s1(5, 10); + Cylinder s2(5, 10); - BVHModel s1_rss; - BVHModel s2_rss; + BVHModel> s1_rss; + BVHModel> s2_rss; - generateBVHModel(s1_rss, s1, Transform3d::Identity(), 16, 16); - generateBVHModel(s2_rss, s2, Transform3d::Identity(), 16, 16); - - DistanceRequest request; + generateBVHModel(s1_rss, s1, Transform3::Identity(), 16, 16); + generateBVHModel(s2_rss, s2, Transform3::Identity(), 16, 16); + + DistanceRequest request; request.gjk_solver_type = GST_INDEP; - DistanceResult res, res1; + DistanceResult res, res1; + + Transform3 pose = Transform3::Identity(); - Transform3d pose = Transform3d::Identity(); + pose.translation() = Vector3(20, 0, 0); - pose.translation() = Vector3d(20, 0, 0); - res.clear(); res1.clear(); - distance(&s1, Transform3d::Identity(), &s2, pose, request, res); - distance(&s1_rss, Transform3d::Identity(), &s2_rss, pose, request, res1); + distance(&s1, Transform3::Identity(), &s2, pose, request, res); + distance(&s1_rss, Transform3::Identity(), &s2_rss, pose, request, res1); EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); - + res1.clear(); - distance(&s1, Transform3d::Identity(), &s2_rss, pose, request, res1); + distance(&s1, Transform3::Identity(), &s2_rss, pose, request, res1); EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); - + res1.clear(); - distance(&s1_rss, Transform3d::Identity(), &s2, pose, request, res1); + distance(&s1_rss, Transform3::Identity(), &s2, pose, request, res1); EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); - + for(std::size_t i = 0; i < 10; ++i) { - Transform3d t; - generateRandomTransform(extents, t); - - Transform3d pose1(t); - Transform3d pose2 = t * pose; + Transform3 t; + generateRandomTransform(extents(), t); + + Transform3 pose1(t); + Transform3 pose2 = t * pose; res.clear(); res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); @@ -844,29 +905,29 @@ GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_distance_cylindercylinder_GJK else EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); } - - pose.translation() = Vector3d(10.1, 0, 0); - + + pose.translation() = Vector3(10.1, 0, 0); + res.clear(); res1.clear(); - distance(&s1, Transform3d::Identity(), &s2, pose, request, res); - distance(&s1_rss, Transform3d::Identity(), &s2_rss, pose, request, res1); + distance(&s1, Transform3::Identity(), &s2, pose, request, res); + distance(&s1_rss, Transform3::Identity(), &s2_rss, pose, request, res1); EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); - + res1.clear(); - distance(&s1, Transform3d::Identity(), &s2_rss, pose, request, res1); + distance(&s1, Transform3::Identity(), &s2_rss, pose, request, res1); EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); - distance(&s1_rss, Transform3d::Identity(), &s2, pose, request, res1); + distance(&s1_rss, Transform3::Identity(), &s2, pose, request, res1); EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); - + for(std::size_t i = 0; i < 10; ++i) { - Transform3d t; - generateRandomTransform(extents, t); - - Transform3d pose1(t); - Transform3d pose2 = t * pose; + Transform3 t; + generateRandomTransform(extents(), t); + + Transform3 pose1(t); + Transform3 pose2 = t * pose; res.clear(); res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); @@ -883,45 +944,52 @@ GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_distance_cylindercylinder_GJK } } -GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_distance_conecone_GJK) +GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_distance_cylindercylinder_GJK) +{ +// test_consistency_distance_cylindercylinder_GJK(); + test_consistency_distance_cylindercylinder_GJK(); +} + +template +void test_consistency_distance_conecone_GJK() { - Cone s1(5, 10); - Cone s2(5, 10); + Cone s1(5, 10); + Cone s2(5, 10); - BVHModel s1_rss; - BVHModel s2_rss; + BVHModel> s1_rss; + BVHModel> s2_rss; - generateBVHModel(s1_rss, s1, Transform3d::Identity(), 16, 16); - generateBVHModel(s2_rss, s2, Transform3d::Identity(), 16, 16); + generateBVHModel(s1_rss, s1, Transform3::Identity(), 16, 16); + generateBVHModel(s2_rss, s2, Transform3::Identity(), 16, 16); - DistanceRequest request; + DistanceRequest request; request.gjk_solver_type = GST_INDEP; - DistanceResult res, res1; + DistanceResult res, res1; - Transform3d pose = Transform3d::Identity(); + Transform3 pose = Transform3::Identity(); - pose.translation() = Vector3d(20, 0, 0); + pose.translation() = Vector3(20, 0, 0); res.clear(); res1.clear(); - distance(&s1, Transform3d::Identity(), &s2, pose, request, res); - distance(&s1_rss, Transform3d::Identity(), &s2_rss, pose, request, res1); + distance(&s1, Transform3::Identity(), &s2, pose, request, res); + distance(&s1_rss, Transform3::Identity(), &s2_rss, pose, request, res1); EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); - + res1.clear(); - distance(&s1, Transform3d::Identity(), &s2_rss, pose, request, res1); + distance(&s1, Transform3::Identity(), &s2_rss, pose, request, res1); EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); - + res1.clear(); - distance(&s1_rss, Transform3d::Identity(), &s2, pose, request, res1); + distance(&s1_rss, Transform3::Identity(), &s2, pose, request, res1); EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); - + for(std::size_t i = 0; i < 10; ++i) { - Transform3d t; - generateRandomTransform(extents, t); - - Transform3d pose1(t); - Transform3d pose2 = t * pose; + Transform3 t; + generateRandomTransform(extents(), t); + + Transform3 pose1(t); + Transform3 pose2 = t * pose; res.clear(); res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); @@ -936,29 +1004,29 @@ GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_distance_conecone_GJK) distance(&s1_rss, pose1, &s2, pose2, request, res1); EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); } - - pose.translation() = Vector3d(10.1, 0, 0); - + + pose.translation() = Vector3(10.1, 0, 0); + res.clear(); res1.clear(); - distance(&s1, Transform3d::Identity(), &s2, pose, request, res); - distance(&s1_rss, Transform3d::Identity(), &s2_rss, pose, request, res1); + distance(&s1, Transform3::Identity(), &s2, pose, request, res); + distance(&s1_rss, Transform3::Identity(), &s2_rss, pose, request, res1); EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); - + res1.clear(); - distance(&s1, Transform3d::Identity(), &s2_rss, pose, request, res1); + distance(&s1, Transform3::Identity(), &s2_rss, pose, request, res1); EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); - distance(&s1_rss, Transform3d::Identity(), &s2, pose, request, res1); + distance(&s1_rss, Transform3::Identity(), &s2, pose, request, res1); EXPECT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); - + for(std::size_t i = 0; i < 10; ++i) { - Transform3d t; - generateRandomTransform(extents, t); - - Transform3d pose1(t); - Transform3d pose2 = t * pose; + Transform3 t; + generateRandomTransform(extents(), t); + + Transform3 pose1(t); + Transform3 pose2 = t * pose; res.clear(); res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); @@ -975,30 +1043,35 @@ GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_distance_conecone_GJK) } } +GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_distance_conecone_GJK) +{ +// test_consistency_distance_conecone_GJK(); + test_consistency_distance_conecone_GJK(); +} - -GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_collision_spheresphere_libccd) +template +void test_consistency_collision_spheresphere_libccd() { - Sphere s1(20); - Sphere s2(10); - BVHModel s1_aabb; - BVHModel s2_aabb; - BVHModel s1_obb; - BVHModel s2_obb; + Sphere s1(20); + Sphere s2(10); + BVHModel> s1_aabb; + BVHModel> s2_aabb; + BVHModel> s1_obb; + BVHModel> s2_obb; - generateBVHModel(s1_aabb, s1, Transform3d::Identity(), 16, 16); - generateBVHModel(s2_aabb, s2, Transform3d::Identity(), 16, 16); - generateBVHModel(s1_obb, s1, Transform3d::Identity(), 16, 16); - generateBVHModel(s2_obb, s2, Transform3d::Identity(), 16, 16); + generateBVHModel(s1_aabb, s1, Transform3::Identity(), 16, 16); + generateBVHModel(s2_aabb, s2, Transform3::Identity(), 16, 16); + generateBVHModel(s1_obb, s1, Transform3::Identity(), 16, 16); + generateBVHModel(s2_obb, s2, Transform3::Identity(), 16, 16); - CollisionRequest request; - CollisionResult result; + CollisionRequest request; + CollisionResult result; bool res; - Transform3d pose = Transform3d::Identity(); - Transform3d pose_aabb = Transform3d::Identity(); - Transform3d pose_obb = Transform3d::Identity(); + Transform3 pose = Transform3::Identity(); + Transform3 pose_aabb = Transform3::Identity(); + Transform3 pose_obb = Transform3::Identity(); // s2 is within s1 @@ -1007,217 +1080,224 @@ GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_collision_spheresphere_libccd // s1 is mesh, s2 is shape --> collision free // all are reasonable result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2_aabb, pose_aabb, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2_obb, pose_obb, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2, pose, &s1_aabb, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2, pose, &s1_obb, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); - pose.translation() = Vector3d(40, 0, 0); - pose_aabb.translation() = Vector3d(40, 0, 0); - pose_obb.translation() = Vector3d(40, 0, 0); + pose.translation() = Vector3(40, 0, 0); + pose_aabb.translation() = Vector3(40, 0, 0); + pose_obb.translation() = Vector3(40, 0, 0); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2_obb, pose_obb, &s1, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_aabb, pose_aabb, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1, Transform3::Identity(), &s2_aabb, pose_aabb, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_obb, pose_obb, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1, Transform3::Identity(), &s2_obb, pose_obb, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2, pose, &s1_aabb, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2, pose, &s1_obb, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); - pose.translation() = Vector3d(30, 0, 0); - pose_aabb.translation() = Vector3d(30, 0, 0); - pose_obb.translation() = Vector3d(30, 0, 0); + pose.translation() = Vector3(30, 0, 0); + pose_aabb.translation() = Vector3(30, 0, 0); + pose_obb.translation() = Vector3(30, 0, 0); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2_obb, pose_obb, &s1, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_aabb, pose_aabb, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1, Transform3::Identity(), &s2_aabb, pose_aabb, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_obb, pose_obb, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1, Transform3::Identity(), &s2_obb, pose_obb, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2, pose, &s1_aabb, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2, pose, &s1_obb, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); - pose.translation() = Vector3d(29.9, 0, 0); - pose_aabb.translation() = Vector3d(29.8, 0, 0); // 29.9 fails, result depends on mesh precision - pose_obb.translation() = Vector3d(29.8, 0, 0); // 29.9 fails, result depends on mesh precision + pose.translation() = Vector3(29.9, 0, 0); + pose_aabb.translation() = Vector3(29.8, 0, 0); // 29.9 fails, result depends on mesh precision + pose_obb.translation() = Vector3(29.8, 0, 0); // 29.9 fails, result depends on mesh precision result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2_aabb, pose_aabb, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2_obb, pose_obb, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); - pose.translation() = Vector3d(-29.9, 0, 0); - pose_aabb.translation() = Vector3d(-29.8, 0, 0); // 29.9 fails, result depends on mesh precision - pose_obb.translation() = Vector3d(-29.8, 0, 0); // 29.9 fails, result depends on mesh precision + pose.translation() = Vector3(-29.9, 0, 0); + pose_aabb.translation() = Vector3(-29.8, 0, 0); // 29.9 fails, result depends on mesh precision + pose_obb.translation() = Vector3(-29.8, 0, 0); // 29.9 fails, result depends on mesh precision result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2_aabb, pose_aabb, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2_obb, pose_obb, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); - pose.translation() = Vector3d(-30, 0, 0); - pose_aabb.translation() = Vector3d(-30, 0, 0); - pose_obb.translation() = Vector3d(-30, 0, 0); + pose.translation() = Vector3(-30, 0, 0); + pose_aabb.translation() = Vector3(-30, 0, 0); + pose_obb.translation() = Vector3(-30, 0, 0); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2_obb, pose_obb, &s1, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_aabb, pose_aabb, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1, Transform3::Identity(), &s2_aabb, pose_aabb, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_obb, pose_obb, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1, Transform3::Identity(), &s2_obb, pose_obb, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2, pose, &s1_aabb, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2, pose, &s1_obb, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); } -GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_collision_ellipsoidellipsoid_libccd) +GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_collision_spheresphere_libccd) { - Ellipsoid s1(20, 40, 50); - Ellipsoid s2(10, 10, 10); - BVHModel s1_aabb; - BVHModel s2_aabb; - BVHModel s1_obb; - BVHModel s2_obb; +// test_consistency_collision_spheresphere_libccd(); + test_consistency_collision_spheresphere_libccd(); +} - generateBVHModel(s1_aabb, s1, Transform3d::Identity(), 16, 16); - generateBVHModel(s2_aabb, s2, Transform3d::Identity(), 16, 16); - generateBVHModel(s1_obb, s1, Transform3d::Identity(), 16, 16); - generateBVHModel(s2_obb, s2, Transform3d::Identity(), 16, 16); +template +void test_consistency_collision_ellipsoidellipsoid_libccd() +{ + Ellipsoid s1(20, 40, 50); + Ellipsoid s2(10, 10, 10); + BVHModel> s1_aabb; + BVHModel> s2_aabb; + BVHModel> s1_obb; + BVHModel> s2_obb; + + generateBVHModel(s1_aabb, s1, Transform3::Identity(), 16, 16); + generateBVHModel(s2_aabb, s2, Transform3::Identity(), 16, 16); + generateBVHModel(s1_obb, s1, Transform3::Identity(), 16, 16); + generateBVHModel(s2_obb, s2, Transform3::Identity(), 16, 16); - CollisionRequest request; - CollisionResult result; + CollisionRequest request; + CollisionResult result; bool res; - Transform3d pose = Transform3d::Identity(); - Transform3d pose_aabb = Transform3d::Identity(); - Transform3d pose_obb = Transform3d::Identity(); + Transform3 pose = Transform3::Identity(); + Transform3 pose_aabb = Transform3::Identity(); + Transform3 pose_obb = Transform3::Identity(); // s2 is within s1 @@ -1226,220 +1306,227 @@ GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_collision_ellipsoidellipsoid_ // s1 is mesh, s2 is shape --> collision free // all are reasonable result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2_aabb, pose_aabb, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2_obb, pose_obb, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2, pose, &s1_aabb, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2, pose, &s1_obb, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); - pose.translation() = Vector3d(40, 0, 0); - pose_aabb.translation() = Vector3d(40, 0, 0); - pose_obb.translation() = Vector3d(40, 0, 0); + pose.translation() = Vector3(40, 0, 0); + pose_aabb.translation() = Vector3(40, 0, 0); + pose_obb.translation() = Vector3(40, 0, 0); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2_obb, pose_obb, &s1, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_aabb, pose_aabb, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1, Transform3::Identity(), &s2_aabb, pose_aabb, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_obb, pose_obb, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1, Transform3::Identity(), &s2_obb, pose_obb, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2, pose, &s1_aabb, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2, pose, &s1_obb, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); - pose.translation() = Vector3d(30, 0, 0); - pose_aabb.translation() = Vector3d(30, 0, 0); - pose_obb.translation() = Vector3d(30, 0, 0); + pose.translation() = Vector3(30, 0, 0); + pose_aabb.translation() = Vector3(30, 0, 0); + pose_obb.translation() = Vector3(30, 0, 0); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); // libccd cannot detect collision when two ellipsoid is exactly touching each other + res = (collide(&s1, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); // libccd cannot detect collision when two ellipsoid is exactly touching each other result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2, Transform3d(Eigen::Translation3d(Vector3d(29.999, 0, 0))), request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2, Transform3(Translation3(Vector3(29.999, 0, 0))), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2_obb, pose_obb, &s1, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_aabb, pose_aabb, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1, Transform3::Identity(), &s2_aabb, pose_aabb, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_obb, pose_obb, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1, Transform3::Identity(), &s2_obb, pose_obb, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2, pose, &s1_aabb, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2, pose, &s1_obb, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); - pose.translation() = Vector3d(29.9, 0, 0); - pose_aabb.translation() = Vector3d(29.9, 0, 0); // 29.9 fails, result depends on mesh precision - pose_obb.translation() = Vector3d(29.9, 0, 0); // 29.9 fails, result depends on mesh precision + pose.translation() = Vector3(29.9, 0, 0); + pose_aabb.translation() = Vector3(29.9, 0, 0); // 29.9 fails, result depends on mesh precision + pose_obb.translation() = Vector3(29.9, 0, 0); // 29.9 fails, result depends on mesh precision result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2_aabb, pose_aabb, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2_obb, pose_obb, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); - pose.translation() = Vector3d(-29.9, 0, 0); - pose_aabb.translation() = Vector3d(-29.9, 0, 0); - pose_obb.translation() = Vector3d(-29.9, 0, 0); + pose.translation() = Vector3(-29.9, 0, 0); + pose_aabb.translation() = Vector3(-29.9, 0, 0); + pose_obb.translation() = Vector3(-29.9, 0, 0); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2_aabb, pose_aabb, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2_obb, pose_obb, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); - pose.translation() = Vector3d(-30, 0, 0); - pose_aabb.translation() = Vector3d(-30, 0, 0); - pose_obb.translation() = Vector3d(-30, 0, 0); + pose.translation() = Vector3(-30, 0, 0); + pose_aabb.translation() = Vector3(-30, 0, 0); + pose_obb.translation() = Vector3(-30, 0, 0); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2_obb, pose_obb, &s1, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_aabb, pose_aabb, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1, Transform3::Identity(), &s2_aabb, pose_aabb, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_obb, pose_obb, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1, Transform3::Identity(), &s2_obb, pose_obb, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2, pose, &s1_aabb, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2, pose, &s1_obb, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); } -GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_collision_boxbox_libccd) +GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_collision_ellipsoidellipsoid_libccd) { - Box s1(20, 40, 50); - Box s2(10, 10, 10); +// test_consistency_collision_ellipsoidellipsoid_libccd(); + test_consistency_collision_ellipsoidellipsoid_libccd(); +} - BVHModel s1_aabb; - BVHModel s2_aabb; - BVHModel s1_obb; - BVHModel s2_obb; +template +void test_consistency_collision_boxbox_libccd() +{ + Box s1(20, 40, 50); + Box s2(10, 10, 10); + + BVHModel> s1_aabb; + BVHModel> s2_aabb; + BVHModel> s1_obb; + BVHModel> s2_obb; - generateBVHModel(s1_aabb, s1, Transform3d::Identity()); - generateBVHModel(s2_aabb, s2, Transform3d::Identity()); - generateBVHModel(s1_obb, s1, Transform3d::Identity()); - generateBVHModel(s2_obb, s2, Transform3d::Identity()); + generateBVHModel(s1_aabb, s1, Transform3::Identity()); + generateBVHModel(s2_aabb, s2, Transform3::Identity()); + generateBVHModel(s1_obb, s1, Transform3::Identity()); + generateBVHModel(s2_obb, s2, Transform3::Identity()); - CollisionRequest request; - CollisionResult result; + CollisionRequest request; + CollisionResult result; bool res; - Transform3d pose = Transform3d::Identity(); - Transform3d pose_aabb = Transform3d::Identity(); - Transform3d pose_obb = Transform3d::Identity(); + Transform3 pose = Transform3::Identity(); + Transform3 pose_aabb = Transform3::Identity(); + Transform3 pose_obb = Transform3::Identity(); // s2 is within s1 // both are shapes --> collision @@ -1447,121 +1534,128 @@ GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_collision_boxbox_libccd) // s1 is mesh, s2 is shape --> collision free // all are reasonable result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2_aabb, pose_aabb, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2_obb, pose_obb, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2, pose, &s1_aabb, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2, pose, &s1_obb, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); - pose.translation() = Vector3d(15.01, 0, 0); - pose_aabb.translation() = Vector3d(15.01, 0, 0); - pose_obb.translation() = Vector3d(15.01, 0, 0); + pose.translation() = Vector3(15.01, 0, 0); + pose_aabb.translation() = Vector3(15.01, 0, 0); + pose_obb.translation() = Vector3(15.01, 0, 0); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2_obb, pose_obb, &s1, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_aabb, pose_aabb, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1, Transform3::Identity(), &s2_aabb, pose_aabb, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_obb, pose_obb, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1, Transform3::Identity(), &s2_obb, pose_obb, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2, pose, &s1_aabb, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2, pose, &s1_obb, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); - pose.translation() = Vector3d(14.99, 0, 0); - pose_aabb.translation() = Vector3d(14.99, 0, 0); - pose_obb.translation() = Vector3d(14.99, 0, 0); + pose.translation() = Vector3(14.99, 0, 0); + pose_aabb.translation() = Vector3(14.99, 0, 0); + pose_obb.translation() = Vector3(14.99, 0, 0); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2_aabb, pose_aabb, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2_obb, pose_obb, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); } -GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_collision_spherebox_libccd) +GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_collision_boxbox_libccd) +{ +// test_consistency_collision_boxbox_libccd(); + test_consistency_collision_boxbox_libccd(); +} + +template +void test_consistency_collision_spherebox_libccd() { - Sphere s1(20); - Box s2(5, 5, 5); + Sphere s1(20); + Box s2(5, 5, 5); - BVHModel s1_aabb; - BVHModel s2_aabb; - BVHModel s1_obb; - BVHModel s2_obb; + BVHModel> s1_aabb; + BVHModel> s2_aabb; + BVHModel> s1_obb; + BVHModel> s2_obb; - generateBVHModel(s1_aabb, s1, Transform3d::Identity(), 16, 16); - generateBVHModel(s2_aabb, s2, Transform3d::Identity()); - generateBVHModel(s1_obb, s1, Transform3d::Identity(), 16, 16); - generateBVHModel(s2_obb, s2, Transform3d::Identity()); + generateBVHModel(s1_aabb, s1, Transform3::Identity(), 16, 16); + generateBVHModel(s2_aabb, s2, Transform3::Identity()); + generateBVHModel(s1_obb, s1, Transform3::Identity(), 16, 16); + generateBVHModel(s2_obb, s2, Transform3::Identity()); - CollisionRequest request; - CollisionResult result; + CollisionRequest request; + CollisionResult result; bool res; - Transform3d pose = Transform3d::Identity(); - Transform3d pose_aabb = Transform3d::Identity(); - Transform3d pose_obb = Transform3d::Identity(); + Transform3 pose = Transform3::Identity(); + Transform3 pose_aabb = Transform3::Identity(); + Transform3 pose_obb = Transform3::Identity(); // s2 is within s1 // both are shapes --> collision @@ -1569,367 +1663,385 @@ GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_collision_spherebox_libccd) // s1 is mesh, s2 is shape --> collision free // all are reasonable result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2_aabb, pose_aabb, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2_obb, pose_obb, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2, pose, &s1_aabb, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2, pose, &s1_obb, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); - pose.translation() = Vector3d(22.4, 0, 0); - pose_aabb.translation() = Vector3d(22.4, 0, 0); - pose_obb.translation() = Vector3d(22.4, 0, 0); + pose.translation() = Vector3(22.4, 0, 0); + pose_aabb.translation() = Vector3(22.4, 0, 0); + pose_obb.translation() = Vector3(22.4, 0, 0); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2_aabb, pose_aabb, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2_obb, pose_obb, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); - pose.translation() = Vector3d(22.51, 0, 0); - pose_aabb.translation() = Vector3d(22.51, 0, 0); - pose_obb.translation() = Vector3d(22.51, 0, 0); + pose.translation() = Vector3(22.51, 0, 0); + pose_aabb.translation() = Vector3(22.51, 0, 0); + pose_obb.translation() = Vector3(22.51, 0, 0); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2_obb, pose_obb, &s1, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_aabb, pose_aabb, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1, Transform3::Identity(), &s2_aabb, pose_aabb, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_obb, pose_obb, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1, Transform3::Identity(), &s2_obb, pose_obb, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2, pose, &s1_aabb, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2, pose, &s1_obb, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); } -GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_collision_cylindercylinder_libccd) +GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_collision_spherebox_libccd) { - Cylinder s1(5, 10); - Cylinder s2(5, 10); +// test_consistency_collision_spherebox_libccd(); + test_consistency_collision_spherebox_libccd(); +} - BVHModel s1_aabb; - BVHModel s2_aabb; - BVHModel s1_obb; - BVHModel s2_obb; +template +void test_consistency_collision_cylindercylinder_libccd() +{ + Cylinder s1(5, 10); + Cylinder s2(5, 10); + + BVHModel> s1_aabb; + BVHModel> s2_aabb; + BVHModel> s1_obb; + BVHModel> s2_obb; - generateBVHModel(s1_aabb, s1, Transform3d::Identity(), 16, 16); - generateBVHModel(s2_aabb, s2, Transform3d::Identity(), 16, 16); - generateBVHModel(s1_obb, s1, Transform3d::Identity(), 16, 16); - generateBVHModel(s2_obb, s2, Transform3d::Identity(), 16, 16); + generateBVHModel(s1_aabb, s1, Transform3::Identity(), 16, 16); + generateBVHModel(s2_aabb, s2, Transform3::Identity(), 16, 16); + generateBVHModel(s1_obb, s1, Transform3::Identity(), 16, 16); + generateBVHModel(s2_obb, s2, Transform3::Identity(), 16, 16); - CollisionRequest request; - CollisionResult result; + CollisionRequest request; + CollisionResult result; bool res; - Transform3d pose = Transform3d::Identity(); - Transform3d pose_aabb = Transform3d::Identity(); - Transform3d pose_obb = Transform3d::Identity(); + Transform3 pose = Transform3::Identity(); + Transform3 pose_aabb = Transform3::Identity(); + Transform3 pose_obb = Transform3::Identity(); - pose.translation() = Vector3d(9.99, 0, 0); - pose_aabb.translation() = Vector3d(9.99, 0, 0); - pose_obb.translation() = Vector3d(9.99, 0, 0); + pose.translation() = Vector3(9.99, 0, 0); + pose_aabb.translation() = Vector3(9.99, 0, 0); + pose_obb.translation() = Vector3(9.99, 0, 0); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2_aabb, pose_aabb, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2_obb, pose_obb, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); - pose.translation() = Vector3d(10.01, 0, 0); - pose_aabb.translation() = Vector3d(10.01, 0, 0); - pose_obb.translation() = Vector3d(10.01, 0, 0); + pose.translation() = Vector3(10.01, 0, 0); + pose_aabb.translation() = Vector3(10.01, 0, 0); + pose_obb.translation() = Vector3(10.01, 0, 0); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2_obb, pose_obb, &s1, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_aabb, pose_aabb, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1, Transform3::Identity(), &s2_aabb, pose_aabb, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_obb, pose_obb, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1, Transform3::Identity(), &s2_obb, pose_obb, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2, pose, &s1_aabb, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2, pose, &s1_obb, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); } -GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_collision_conecone_libccd) +GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_collision_cylindercylinder_libccd) { - Cone s1(5, 10); - Cone s2(5, 10); +// test_consistency_collision_cylindercylinder_libccd(); + test_consistency_collision_cylindercylinder_libccd(); +} - BVHModel s1_aabb; - BVHModel s2_aabb; - BVHModel s1_obb; - BVHModel s2_obb; +template +void test_consistency_collision_conecone_libccd() +{ + Cone s1(5, 10); + Cone s2(5, 10); - generateBVHModel(s1_aabb, s1, Transform3d::Identity(), 16, 16); - generateBVHModel(s2_aabb, s2, Transform3d::Identity(), 16, 16); - generateBVHModel(s1_obb, s1, Transform3d::Identity(), 16, 16); - generateBVHModel(s2_obb, s2, Transform3d::Identity(), 16, 16); + BVHModel> s1_aabb; + BVHModel> s2_aabb; + BVHModel> s1_obb; + BVHModel> s2_obb; - CollisionRequest request; - CollisionResult result; + generateBVHModel(s1_aabb, s1, Transform3::Identity(), 16, 16); + generateBVHModel(s2_aabb, s2, Transform3::Identity(), 16, 16); + generateBVHModel(s1_obb, s1, Transform3::Identity(), 16, 16); + generateBVHModel(s2_obb, s2, Transform3::Identity(), 16, 16); + + CollisionRequest request; + CollisionResult result; bool res; - Transform3d pose = Transform3d::Identity(); - Transform3d pose_aabb = Transform3d::Identity(); - Transform3d pose_obb = Transform3d::Identity(); + Transform3 pose = Transform3::Identity(); + Transform3 pose_aabb = Transform3::Identity(); + Transform3 pose_obb = Transform3::Identity(); - pose.translation() = Vector3d(9.9, 0, 0); - pose_aabb.translation() = Vector3d(9.9, 0, 0); - pose_obb.translation() = Vector3d(9.9, 0, 0); + pose.translation() = Vector3(9.9, 0, 0); + pose_aabb.translation() = Vector3(9.9, 0, 0); + pose_obb.translation() = Vector3(9.9, 0, 0); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2_aabb, pose_aabb, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2_obb, pose_obb, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); - pose.translation() = Vector3d(10.1, 0, 0); - pose_aabb.translation() = Vector3d(10.1, 0, 0); - pose_obb.translation() = Vector3d(10.1, 0, 0); + pose.translation() = Vector3(10.1, 0, 0); + pose_aabb.translation() = Vector3(10.1, 0, 0); + pose_obb.translation() = Vector3(10.1, 0, 0); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2_obb, pose_obb, &s1, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_aabb, pose_aabb, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1, Transform3::Identity(), &s2_aabb, pose_aabb, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_obb, pose_obb, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1, Transform3::Identity(), &s2_obb, pose_obb, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2, pose, &s1_aabb, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2, pose, &s1_obb, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); - pose.translation() = Vector3d(0, 0, 9.9); - pose_aabb.translation() = Vector3d(0, 0, 9.9); - pose_obb.translation() = Vector3d(0, 0, 9.9); + pose.translation() = Vector3(0, 0, 9.9); + pose_aabb.translation() = Vector3(0, 0, 9.9); + pose_obb.translation() = Vector3(0, 0, 9.9); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2_aabb, pose_aabb, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2_obb, pose_obb, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); - pose.translation() = Vector3d(0, 0, 10.1); - pose_aabb.translation() = Vector3d(0, 0, 10.1); - pose_obb.translation() = Vector3d(0, 0, 10.1); + pose.translation() = Vector3(0, 0, 10.1); + pose_aabb.translation() = Vector3(0, 0, 10.1); + pose_obb.translation() = Vector3(0, 0, 10.1); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2_obb, pose_obb, &s1, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_aabb, pose_aabb, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1, Transform3::Identity(), &s2_aabb, pose_aabb, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_obb, pose_obb, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1, Transform3::Identity(), &s2_obb, pose_obb, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2, pose, &s1_aabb, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2, pose, &s1_obb, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); } +GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_collision_conecone_libccd) +{ +// test_consistency_collision_conecone_libccd(); + test_consistency_collision_conecone_libccd(); +} - - -GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_collision_spheresphere_GJK) +template +void test_consistency_collision_spheresphere_GJK() { - Sphere s1(20); - Sphere s2(10); - BVHModel s1_aabb; - BVHModel s2_aabb; - BVHModel s1_obb; - BVHModel s2_obb; - - generateBVHModel(s1_aabb, s1, Transform3d::Identity(), 16, 16); - generateBVHModel(s2_aabb, s2, Transform3d::Identity(), 16, 16); - generateBVHModel(s1_obb, s1, Transform3d::Identity(), 16, 16); - generateBVHModel(s2_obb, s2, Transform3d::Identity(), 16, 16); - - CollisionRequest request; + Sphere s1(20); + Sphere s2(10); + BVHModel> s1_aabb; + BVHModel> s2_aabb; + BVHModel> s1_obb; + BVHModel> s2_obb; + + generateBVHModel(s1_aabb, s1, Transform3::Identity(), 16, 16); + generateBVHModel(s2_aabb, s2, Transform3::Identity(), 16, 16); + generateBVHModel(s1_obb, s1, Transform3::Identity(), 16, 16); + generateBVHModel(s2_obb, s2, Transform3::Identity(), 16, 16); + + CollisionRequest request; request.gjk_solver_type = GST_INDEP; - CollisionResult result; + CollisionResult result; bool res; - Transform3d pose = Transform3d::Identity(); - Transform3d pose_aabb = Transform3d::Identity(); - Transform3d pose_obb = Transform3d::Identity(); + Transform3 pose = Transform3::Identity(); + Transform3 pose_aabb = Transform3::Identity(); + Transform3 pose_obb = Transform3::Identity(); // s2 is within s1 @@ -1938,219 +2050,226 @@ GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_collision_spheresphere_GJK) // s1 is mesh, s2 is shape --> collision free // all are reasonable result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2_aabb, pose_aabb, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2_obb, pose_obb, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2, pose, &s1_aabb, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2, pose, &s1_obb, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); - pose.translation() = Vector3d(40, 0, 0); - pose_aabb.translation() = Vector3d(40, 0, 0); - pose_obb.translation() = Vector3d(40, 0, 0); + pose.translation() = Vector3(40, 0, 0); + pose_aabb.translation() = Vector3(40, 0, 0); + pose_obb.translation() = Vector3(40, 0, 0); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2_obb, pose_obb, &s1, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_aabb, pose_aabb, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1, Transform3::Identity(), &s2_aabb, pose_aabb, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_obb, pose_obb, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1, Transform3::Identity(), &s2_obb, pose_obb, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2, pose, &s1_aabb, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2, pose, &s1_obb, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); - pose.translation() = Vector3d(30, 0, 0); - pose_aabb.translation() = Vector3d(30, 0, 0); - pose_obb.translation() = Vector3d(30, 0, 0); + pose.translation() = Vector3(30, 0, 0); + pose_aabb.translation() = Vector3(30, 0, 0); + pose_obb.translation() = Vector3(30, 0, 0); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2_obb, pose_obb, &s1, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_aabb, pose_aabb, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1, Transform3::Identity(), &s2_aabb, pose_aabb, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_obb, pose_obb, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1, Transform3::Identity(), &s2_obb, pose_obb, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2, pose, &s1_aabb, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2, pose, &s1_obb, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); - pose.translation() = Vector3d(29.9, 0, 0); - pose_aabb.translation() = Vector3d(29.8, 0, 0); // 29.9 fails, result depends on mesh precision - pose_obb.translation() = Vector3d(29.8, 0, 0); // 29.9 fails, result depends on mesh precision + pose.translation() = Vector3(29.9, 0, 0); + pose_aabb.translation() = Vector3(29.8, 0, 0); // 29.9 fails, result depends on mesh precision + pose_obb.translation() = Vector3(29.8, 0, 0); // 29.9 fails, result depends on mesh precision result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2_aabb, pose_aabb, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2_obb, pose_obb, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); - pose.translation() = Vector3d(-29.9, 0, 0); - pose_aabb.translation() = Vector3d(-29.8, 0, 0); // 29.9 fails, result depends on mesh precision - pose_obb.translation() = Vector3d(-29.8, 0, 0); // 29.9 fails, result depends on mesh precision + pose.translation() = Vector3(-29.9, 0, 0); + pose_aabb.translation() = Vector3(-29.8, 0, 0); // 29.9 fails, result depends on mesh precision + pose_obb.translation() = Vector3(-29.8, 0, 0); // 29.9 fails, result depends on mesh precision result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2_aabb, pose_aabb, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2_obb, pose_obb, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); - pose.translation() = Vector3d(-30, 0, 0); - pose_aabb.translation() = Vector3d(-30, 0, 0); - pose_obb.translation() = Vector3d(-30, 0, 0); + pose.translation() = Vector3(-30, 0, 0); + pose_aabb.translation() = Vector3(-30, 0, 0); + pose_obb.translation() = Vector3(-30, 0, 0); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2_obb, pose_obb, &s1, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_aabb, pose_aabb, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1, Transform3::Identity(), &s2_aabb, pose_aabb, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_obb, pose_obb, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1, Transform3::Identity(), &s2_obb, pose_obb, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2, pose, &s1_aabb, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2, pose, &s1_obb, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); } -GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_collision_ellipsoidellipsoid_GJK) +GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_collision_spheresphere_GJK) +{ +// test_consistency_collision_spheresphere_GJK(); + test_consistency_collision_spheresphere_GJK(); +} + +template +void test_consistency_collision_ellipsoidellipsoid_GJK() { - Ellipsoid s1(20, 40, 50); - Ellipsoid s2(10, 10, 10); - BVHModel s1_aabb; - BVHModel s2_aabb; - BVHModel s1_obb; - BVHModel s2_obb; - - generateBVHModel(s1_aabb, s1, Transform3d::Identity(), 16, 16); - generateBVHModel(s2_aabb, s2, Transform3d::Identity(), 16, 16); - generateBVHModel(s1_obb, s1, Transform3d::Identity(), 16, 16); - generateBVHModel(s2_obb, s2, Transform3d::Identity(), 16, 16); - - CollisionRequest request; + Ellipsoid s1(20, 40, 50); + Ellipsoid s2(10, 10, 10); + BVHModel> s1_aabb; + BVHModel> s2_aabb; + BVHModel> s1_obb; + BVHModel> s2_obb; + + generateBVHModel(s1_aabb, s1, Transform3::Identity(), 16, 16); + generateBVHModel(s2_aabb, s2, Transform3::Identity(), 16, 16); + generateBVHModel(s1_obb, s1, Transform3::Identity(), 16, 16); + generateBVHModel(s2_obb, s2, Transform3::Identity(), 16, 16); + + CollisionRequest request; request.gjk_solver_type = GST_INDEP; - CollisionResult result; + CollisionResult result; bool res; - Transform3d pose = Transform3d::Identity(); - Transform3d pose_aabb = Transform3d::Identity(); - Transform3d pose_obb = Transform3d::Identity(); + Transform3 pose = Transform3::Identity(); + Transform3 pose_aabb = Transform3::Identity(); + Transform3 pose_obb = Transform3::Identity(); // s2 is within s1 @@ -2159,220 +2278,227 @@ GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_collision_ellipsoidellipsoid_ // s1 is mesh, s2 is shape --> collision free // all are reasonable result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2_aabb, pose_aabb, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2_obb, pose_obb, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2, pose, &s1_aabb, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2, pose, &s1_obb, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); - pose.translation() = Vector3d(40, 0, 0); - pose_aabb.translation() = Vector3d(40, 0, 0); - pose_obb.translation() = Vector3d(40, 0, 0); + pose.translation() = Vector3(40, 0, 0); + pose_aabb.translation() = Vector3(40, 0, 0); + pose_obb.translation() = Vector3(40, 0, 0); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2_obb, pose_obb, &s1, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_aabb, pose_aabb, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1, Transform3::Identity(), &s2_aabb, pose_aabb, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_obb, pose_obb, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1, Transform3::Identity(), &s2_obb, pose_obb, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2, pose, &s1_aabb, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2, pose, &s1_obb, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); - pose.translation() = Vector3d(30, 0, 0); - pose_aabb.translation() = Vector3d(30, 0, 0); - pose_obb.translation() = Vector3d(30, 0, 0); + pose.translation() = Vector3(30, 0, 0); + pose_aabb.translation() = Vector3(30, 0, 0); + pose_obb.translation() = Vector3(30, 0, 0); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2_obb, pose_obb, &s1, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_aabb, pose_aabb, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1, Transform3::Identity(), &s2_aabb, pose_aabb, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_obb, pose_obb, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1, Transform3::Identity(), &s2_obb, pose_obb, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2, pose, &s1_aabb, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2, pose, &s1_obb, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); - pose.translation() = Vector3d(29.9, 0, 0); - pose_aabb.translation() = Vector3d(29.9, 0, 0); - pose_obb.translation() = Vector3d(29.9, 0, 0); + pose.translation() = Vector3(29.9, 0, 0); + pose_aabb.translation() = Vector3(29.9, 0, 0); + pose_obb.translation() = Vector3(29.9, 0, 0); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2_aabb, pose_aabb, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2_obb, pose_obb, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); - pose.translation() = Vector3d(-29.9, 0, 0); - pose_aabb.translation() = Vector3d(-29.9, 0, 0); - pose_obb.translation() = Vector3d(-29.9, 0, 0); + pose.translation() = Vector3(-29.9, 0, 0); + pose_aabb.translation() = Vector3(-29.9, 0, 0); + pose_obb.translation() = Vector3(-29.9, 0, 0); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2_aabb, pose_aabb, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2_obb, pose_obb, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); - pose.translation() = Vector3d(-30, 0, 0); - pose_aabb.translation() = Vector3d(-30, 0, 0); - pose_obb.translation() = Vector3d(-30, 0, 0); + pose.translation() = Vector3(-30, 0, 0); + pose_aabb.translation() = Vector3(-30, 0, 0); + pose_obb.translation() = Vector3(-30, 0, 0); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2_obb, pose_obb, &s1, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_aabb, pose_aabb, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1, Transform3::Identity(), &s2_aabb, pose_aabb, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_obb, pose_obb, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1, Transform3::Identity(), &s2_obb, pose_obb, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2, pose, &s1_aabb, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2, pose, &s1_obb, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); } -GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_collision_boxbox_GJK) +GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_collision_ellipsoidellipsoid_GJK) +{ +// test_consistency_collision_ellipsoidellipsoid_GJK(); + test_consistency_collision_ellipsoidellipsoid_GJK(); +} + +template +void test_consistency_collision_boxbox_GJK() { - Box s1(20, 40, 50); - Box s2(10, 10, 10); + Box s1(20, 40, 50); + Box s2(10, 10, 10); - BVHModel s1_aabb; - BVHModel s2_aabb; - BVHModel s1_obb; - BVHModel s2_obb; + BVHModel> s1_aabb; + BVHModel> s2_aabb; + BVHModel> s1_obb; + BVHModel> s2_obb; - generateBVHModel(s1_aabb, s1, Transform3d::Identity()); - generateBVHModel(s2_aabb, s2, Transform3d::Identity()); - generateBVHModel(s1_obb, s1, Transform3d::Identity()); - generateBVHModel(s2_obb, s2, Transform3d::Identity()); + generateBVHModel(s1_aabb, s1, Transform3::Identity()); + generateBVHModel(s2_aabb, s2, Transform3::Identity()); + generateBVHModel(s1_obb, s1, Transform3::Identity()); + generateBVHModel(s2_obb, s2, Transform3::Identity()); - CollisionRequest request; + CollisionRequest request; request.gjk_solver_type = GST_INDEP; - CollisionResult result; + CollisionResult result; bool res; - Transform3d pose = Transform3d::Identity(); - Transform3d pose_aabb = Transform3d::Identity(); - Transform3d pose_obb = Transform3d::Identity(); + Transform3 pose = Transform3::Identity(); + Transform3 pose_aabb = Transform3::Identity(); + Transform3 pose_obb = Transform3::Identity(); // s2 is within s1 // both are shapes --> collision @@ -2380,123 +2506,130 @@ GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_collision_boxbox_GJK) // s1 is mesh, s2 is shape --> collision free // all are reasonable result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2_aabb, pose_aabb, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2_obb, pose_obb, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2, pose, &s1_aabb, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2, pose, &s1_obb, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); - pose.translation() = Vector3d(15.01, 0, 0); - pose_aabb.translation() = Vector3d(15.01, 0, 0); - pose_obb.translation() = Vector3d(15.01, 0, 0); + pose.translation() = Vector3(15.01, 0, 0); + pose_aabb.translation() = Vector3(15.01, 0, 0); + pose_obb.translation() = Vector3(15.01, 0, 0); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2_obb, pose_obb, &s1, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_aabb, pose_aabb, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1, Transform3::Identity(), &s2_aabb, pose_aabb, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_obb, pose_obb, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1, Transform3::Identity(), &s2_obb, pose_obb, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2, pose, &s1_aabb, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2, pose, &s1_obb, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); - pose.translation() = Vector3d(14.99, 0, 0); - pose_aabb.translation() = Vector3d(14.99, 0, 0); - pose_obb.translation() = Vector3d(14.99, 0, 0); + pose.translation() = Vector3(14.99, 0, 0); + pose_aabb.translation() = Vector3(14.99, 0, 0); + pose_obb.translation() = Vector3(14.99, 0, 0); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2_aabb, pose_aabb, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2_obb, pose_obb, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); } -GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_collision_spherebox_GJK) +GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_collision_boxbox_GJK) { - Sphere s1(20); - Box s2(5, 5, 5); +// test_consistency_collision_boxbox_GJK(); + test_consistency_collision_boxbox_GJK(); +} - BVHModel s1_aabb; - BVHModel s2_aabb; - BVHModel s1_obb; - BVHModel s2_obb; +template +void test_consistency_collision_spherebox_GJK() +{ + Sphere s1(20); + Box s2(5, 5, 5); - generateBVHModel(s1_aabb, s1, Transform3d::Identity(), 16, 16); - generateBVHModel(s2_aabb, s2, Transform3d::Identity()); - generateBVHModel(s1_obb, s1, Transform3d::Identity(), 16, 16); - generateBVHModel(s2_obb, s2, Transform3d::Identity()); + BVHModel> s1_aabb; + BVHModel> s2_aabb; + BVHModel> s1_obb; + BVHModel> s2_obb; - CollisionRequest request; + generateBVHModel(s1_aabb, s1, Transform3::Identity(), 16, 16); + generateBVHModel(s2_aabb, s2, Transform3::Identity()); + generateBVHModel(s1_obb, s1, Transform3::Identity(), 16, 16); + generateBVHModel(s2_obb, s2, Transform3::Identity()); + + CollisionRequest request; request.gjk_solver_type = GST_INDEP; - CollisionResult result; + CollisionResult result; bool res; - Transform3d pose = Transform3d::Identity(); - Transform3d pose_aabb = Transform3d::Identity(); - Transform3d pose_obb = Transform3d::Identity(); + Transform3 pose = Transform3::Identity(); + Transform3 pose_aabb = Transform3::Identity(); + Transform3 pose_obb = Transform3::Identity(); // s2 is within s1 // both are shapes --> collision @@ -2504,342 +2637,362 @@ GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_collision_spherebox_GJK) // s1 is mesh, s2 is shape --> collision free // all are reasonable result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2_aabb, pose_aabb, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2_obb, pose_obb, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2, pose, &s1_aabb, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2, pose, &s1_obb, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); - pose.translation() = Vector3d(22.4, 0, 0); - pose_aabb.translation() = Vector3d(22.4, 0, 0); - pose_obb.translation() = Vector3d(22.4, 0, 0); + pose.translation() = Vector3(22.4, 0, 0); + pose_aabb.translation() = Vector3(22.4, 0, 0); + pose_obb.translation() = Vector3(22.4, 0, 0); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2_aabb, pose_aabb, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2_obb, pose_obb, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); - pose.translation() = Vector3d(22.51, 0, 0); - pose_aabb.translation() = Vector3d(22.51, 0, 0); - pose_obb.translation() = Vector3d(22.51, 0, 0); + pose.translation() = Vector3(22.51, 0, 0); + pose_aabb.translation() = Vector3(22.51, 0, 0); + pose_obb.translation() = Vector3(22.51, 0, 0); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2_obb, pose_obb, &s1, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_aabb, pose_aabb, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1, Transform3::Identity(), &s2_aabb, pose_aabb, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_obb, pose_obb, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1, Transform3::Identity(), &s2_obb, pose_obb, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2, pose, &s1_aabb, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2, pose, &s1_obb, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); } -GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_collision_cylindercylinder_GJK) +GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_collision_spherebox_GJK) { - Cylinder s1(5, 10); - Cylinder s2(5, 10); +// test_consistency_collision_spherebox_GJK(); + test_consistency_collision_spherebox_GJK(); +} - BVHModel s1_aabb; - BVHModel s2_aabb; - BVHModel s1_obb; - BVHModel s2_obb; +template +void test_consistency_collision_cylindercylinder_GJK() +{ + Cylinder s1(5, 10); + Cylinder s2(5, 10); - generateBVHModel(s1_aabb, s1, Transform3d::Identity(), 16, 16); - generateBVHModel(s2_aabb, s2, Transform3d::Identity(), 16, 16); - generateBVHModel(s1_obb, s1, Transform3d::Identity(), 16, 16); - generateBVHModel(s2_obb, s2, Transform3d::Identity(), 16, 16); + BVHModel> s1_aabb; + BVHModel> s2_aabb; + BVHModel> s1_obb; + BVHModel> s2_obb; - CollisionRequest request; + generateBVHModel(s1_aabb, s1, Transform3::Identity(), 16, 16); + generateBVHModel(s2_aabb, s2, Transform3::Identity(), 16, 16); + generateBVHModel(s1_obb, s1, Transform3::Identity(), 16, 16); + generateBVHModel(s2_obb, s2, Transform3::Identity(), 16, 16); + + CollisionRequest request; request.gjk_solver_type = GST_INDEP; - CollisionResult result; + CollisionResult result; bool res; - Transform3d pose = Transform3d::Identity(); - Transform3d pose_aabb = Transform3d::Identity(); - Transform3d pose_obb = Transform3d::Identity(); + Transform3 pose = Transform3::Identity(); + Transform3 pose_aabb = Transform3::Identity(); + Transform3 pose_obb = Transform3::Identity(); - pose.translation() = Vector3d(9.99, 0, 0); - pose_aabb.translation() = Vector3d(9.99, 0, 0); - pose_obb.translation() = Vector3d(9.99, 0, 0); + pose.translation() = Vector3(9.99, 0, 0); + pose_aabb.translation() = Vector3(9.99, 0, 0); + pose_obb.translation() = Vector3(9.99, 0, 0); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2_aabb, pose_aabb, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2_obb, pose_obb, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); - pose.translation() = Vector3d(10.01, 0, 0); - pose_aabb.translation() = Vector3d(10.01, 0, 0); - pose_obb.translation() = Vector3d(10.01, 0, 0); + pose.translation() = Vector3(10.01, 0, 0); + pose_aabb.translation() = Vector3(10.01, 0, 0); + pose_obb.translation() = Vector3(10.01, 0, 0); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2_obb, pose_obb, &s1, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_aabb, pose_aabb, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1, Transform3::Identity(), &s2_aabb, pose_aabb, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_obb, pose_obb, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1, Transform3::Identity(), &s2_obb, pose_obb, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2, pose, &s1_aabb, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2, pose, &s1_obb, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); } -GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_collision_conecone_GJK) +GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_collision_cylindercylinder_GJK) +{ +// test_consistency_collision_cylindercylinder_GJK(); + test_consistency_collision_cylindercylinder_GJK(); +} + +template +void test_consistency_collision_conecone_GJK() { - Cone s1(5, 10); - Cone s2(5, 10); + Cone s1(5, 10); + Cone s2(5, 10); - BVHModel s1_aabb; - BVHModel s2_aabb; - BVHModel s1_obb; - BVHModel s2_obb; + BVHModel> s1_aabb; + BVHModel> s2_aabb; + BVHModel> s1_obb; + BVHModel> s2_obb; - generateBVHModel(s1_aabb, s1, Transform3d::Identity(), 16, 16); - generateBVHModel(s2_aabb, s2, Transform3d::Identity(), 16, 16); - generateBVHModel(s1_obb, s1, Transform3d::Identity(), 16, 16); - generateBVHModel(s2_obb, s2, Transform3d::Identity(), 16, 16); + generateBVHModel(s1_aabb, s1, Transform3::Identity(), 16, 16); + generateBVHModel(s2_aabb, s2, Transform3::Identity(), 16, 16); + generateBVHModel(s1_obb, s1, Transform3::Identity(), 16, 16); + generateBVHModel(s2_obb, s2, Transform3::Identity(), 16, 16); - CollisionRequest request; + CollisionRequest request; request.gjk_solver_type = GST_INDEP; - CollisionResult result; + CollisionResult result; bool res; - Transform3d pose = Transform3d::Identity(); - Transform3d pose_aabb = Transform3d::Identity(); - Transform3d pose_obb = Transform3d::Identity(); + Transform3 pose = Transform3::Identity(); + Transform3 pose_aabb = Transform3::Identity(); + Transform3 pose_obb = Transform3::Identity(); - pose.translation() = Vector3d(9.9, 0, 0); - pose_aabb.translation() = Vector3d(9.9, 0, 0); - pose_obb.translation() = Vector3d(9.9, 0, 0); + pose.translation() = Vector3(9.9, 0, 0); + pose_aabb.translation() = Vector3(9.9, 0, 0); + pose_obb.translation() = Vector3(9.9, 0, 0); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2_aabb, pose_aabb, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2_obb, pose_obb, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); - pose.translation() = Vector3d(10.1, 0, 0); - pose_aabb.translation() = Vector3d(10.1, 0, 0); - pose_obb.translation() = Vector3d(10.1, 0, 0); + pose.translation() = Vector3(10.1, 0, 0); + pose_aabb.translation() = Vector3(10.1, 0, 0); + pose_obb.translation() = Vector3(10.1, 0, 0); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2_obb, pose_obb, &s1, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_aabb, pose_aabb, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1, Transform3::Identity(), &s2_aabb, pose_aabb, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_obb, pose_obb, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1, Transform3::Identity(), &s2_obb, pose_obb, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2, pose, &s1_aabb, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2, pose, &s1_obb, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); - pose.translation() = Vector3d(0, 0, 9.9); - pose_aabb.translation() = Vector3d(0, 0, 9.9); - pose_obb.translation() = Vector3d(0, 0, 9.9); + pose.translation() = Vector3(0, 0, 9.9); + pose_aabb.translation() = Vector3(0, 0, 9.9); + pose_obb.translation() = Vector3(0, 0, 9.9); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2_aabb, pose_aabb, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3::Identity(), &s2_obb, pose_obb, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3d::Identity(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3::Identity(), request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); EXPECT_TRUE(res); - pose.translation() = Vector3d(0, 0, 10.1); - pose_aabb.translation() = Vector3d(0, 0, 10.1); - pose_obb.translation() = Vector3d(0, 0, 10.1); + pose.translation() = Vector3(0, 0, 10.1); + pose_aabb.translation() = Vector3(0, 0, 10.1); + pose_obb.translation() = Vector3(0, 0, 10.1); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2_obb, pose_obb, &s1, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_aabb, pose_aabb, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1, Transform3::Identity(), &s2_aabb, pose_aabb, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1, Transform3d::Identity(), &s2_obb, pose_obb, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1, Transform3::Identity(), &s2_obb, pose_obb, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2, pose, &s1_aabb, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3d::Identity(), request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s2, pose, &s1_obb, Transform3::Identity(), request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3d::Identity(), &s2, pose, request, result) > 0); - EXPECT_TRUE_FALSE(res); + res = (collide(&s1_aabb, Transform3::Identity(), &s2, pose, request, result) > 0); + EXPECT_FALSE(res); +} + +GTEST_TEST(FCL_SHAPE_MESH_CONSISTENCY, consistency_collision_conecone_GJK) +{ +// test_consistency_collision_conecone_GJK(); + test_consistency_collision_conecone_GJK(); } //============================================================================== diff --git a/test/test_fcl_simple.cpp b/test/test_fcl_simple.cpp index f30aa5b78..b43ff84c9 100644 --- a/test/test_fcl_simple.cpp +++ b/test/test_fcl_simple.cpp @@ -41,313 +41,354 @@ #include "fcl/collision.h" #include "fcl/BVH/BVH_model.h" #include "fcl_resources/config.h" -#include "fcl/math/sampling.h" +#include "fcl/math/sampler_r.h" +#include "fcl/math/sampler_se2.h" +#include "fcl/math/sampler_se2_disk.h" +#include "fcl/math/sampler_se3_euler.h" +#include "fcl/math/sampler_se3_euler_ball.h" +#include "fcl/math/sampler_se3_quat.h" +#include "fcl/math/sampler_se3_quat_ball.h" #include "fcl/math/geometry.h" using namespace fcl; -static FCL_REAL epsilon = 1e-6; - -static bool approx(FCL_REAL x, FCL_REAL y) +template +S epsilon() { - return std::abs(x - y) < epsilon; + return 1e-6; } +template <> +float epsilon() +{ + return 1e-4; +} +template +bool approx(S x, S y) +{ + return std::abs(x - y) < epsilon(); +} -template -double distance_Vecnf(const VectorNd& a, const VectorNd& b) +template +S distance_Vecnf(const VectorN& a, const VectorN& b) { - double d = 0; + S d = 0; for(std::size_t i = 0; i < N; ++i) d += (a[i] - b[i]) * (a[i] - b[i]); return d; } - -GTEST_TEST(FCL_SIMPLE, Vec_nf_test) +template +void test_Vec_nf_test() { - VectorNd<4> a; - VectorNd<4> b; + VectorN a; + VectorN b; for(auto i = 0; i < a.size(); ++i) a[i] = i; for(auto i = 0; i < b.size(); ++i) b[i] = 1; - std::cout << a << std::endl; - std::cout << b << std::endl; - std::cout << a + b << std::endl; - std::cout << a - b << std::endl; - std::cout << (a -= b) << std::endl; - std::cout << (a += b) << std::endl; - std::cout << a * 2 << std::endl; - std::cout << a / 2 << std::endl; - std::cout << (a *= 2) << std::endl; - std::cout << (a /= 2) << std::endl; + std::cout << a.transpose() << std::endl; + std::cout << b.transpose() << std::endl; + std::cout << (a + b).transpose() << std::endl; + std::cout << (a - b).transpose() << std::endl; + std::cout << (a -= b).transpose() << std::endl; + std::cout << (a += b).transpose() << std::endl; + std::cout << (a * 2).transpose() << std::endl; + std::cout << (a / 2).transpose() << std::endl; + std::cout << (a *= 2).transpose() << std::endl; + std::cout << (a /= 2).transpose() << std::endl; std::cout << a.dot(b) << std::endl; - VectorNd<8> c = combine(a, b); - std::cout << c << std::endl; + VectorN c = combine(a, b); + std::cout << c.transpose() << std::endl; - VectorNd<4> upper, lower; + VectorN upper, lower; for(int i = 0; i < 4; ++i) upper[i] = 1; - VectorNd<4> aa = VectorNd<4>(1, 2, 1, 2); - std::cout << aa << std::endl; + VectorN aa = VectorN(1, 2, 1, 2); + std::cout << aa.transpose() << std::endl; - SamplerR<4> sampler(lower, upper); + SamplerR sampler(lower, upper); for(std::size_t i = 0; i < 10; ++i) - std::cout << sampler.sample() << std::endl; + std::cout << sampler.sample().transpose() << std::endl; // Disabled broken test lines. Please see #25. // SamplerSE2 sampler2(0, 1, -1, 1); // for(std::size_t i = 0; i < 10; ++i) // std::cout << sampler2.sample() << std::endl; - SamplerSE3Euler sampler3(Vector3d(0, 0, 0), Vector3d(1, 1, 1)); + SamplerSE3Euler sampler3(Vector3(0, 0, 0), Vector3(1, 1, 1)); for(std::size_t i = 0; i < 10; ++i) - std::cout << sampler3.sample() << std::endl; - + std::cout << sampler3.sample().transpose() << std::endl; + } +GTEST_TEST(FCL_SIMPLE, Vec_nf_test) +{ +// test_Vec_nf_test(); + test_Vec_nf_test(); +} -GTEST_TEST(FCL_SIMPLE, projection_test_line) +template +void test_projection_test_line() { - Vector3d v1(0, 0, 0); - Vector3d v2(2, 0, 0); - - Vector3d p(1, 0, 0); - Project::ProjectResult res = Project::projectLine(v1, v2, p); + Vector3 v1(0, 0, 0); + Vector3 v2(2, 0, 0); + + Vector3 p(1, 0, 0); + auto res = Project::projectLine(v1, v2, p); EXPECT_TRUE(res.encode == 3); - EXPECT_TRUE(approx(res.sqr_distance, 0)); - EXPECT_TRUE(approx(res.parameterization[0], 0.5)); - EXPECT_TRUE(approx(res.parameterization[1], 0.5)); - - p = Vector3d(-1, 0, 0); - res = Project::projectLine(v1, v2, p); + EXPECT_TRUE(approx(res.sqr_distance, (S)0)); + EXPECT_TRUE(approx(res.parameterization[0], (S)0.5)); + EXPECT_TRUE(approx(res.parameterization[1], (S)0.5)); + + p = Vector3(-1, 0, 0); + res = Project::projectLine(v1, v2, p); EXPECT_TRUE(res.encode == 1); - EXPECT_TRUE(approx(res.sqr_distance, 1)); - EXPECT_TRUE(approx(res.parameterization[0], 1)); - EXPECT_TRUE(approx(res.parameterization[1], 0)); + EXPECT_TRUE(approx(res.sqr_distance, (S)1)); + EXPECT_TRUE(approx(res.parameterization[0], (S)1)); + EXPECT_TRUE(approx(res.parameterization[1], (S)0)); - p = Vector3d(3, 0, 0); - res = Project::projectLine(v1, v2, p); + p = Vector3(3, 0, 0); + res = Project::projectLine(v1, v2, p); EXPECT_TRUE(res.encode == 2); - EXPECT_TRUE(approx(res.sqr_distance, 1)); - EXPECT_TRUE(approx(res.parameterization[0], 0)); - EXPECT_TRUE(approx(res.parameterization[1], 1)); + EXPECT_TRUE(approx(res.sqr_distance, (S)1)); + EXPECT_TRUE(approx(res.parameterization[0], (S)0)); + EXPECT_TRUE(approx(res.parameterization[1], (S)1)); } -GTEST_TEST(FCL_SIMPLE, projection_test_triangle) +GTEST_TEST(FCL_SIMPLE, projection_test_line) +{ +// test_projection_test_line(); + test_projection_test_line(); +} + +template +void test_projection_test_triangle() { - Vector3d v1(0, 0, 1); - Vector3d v2(0, 1, 0); - Vector3d v3(1, 0, 0); + Vector3 v1(0, 0, 1); + Vector3 v2(0, 1, 0); + Vector3 v3(1, 0, 0); - Vector3d p(1, 1, 1); - Project::ProjectResult res = Project::projectTriangle(v1, v2, v3, p); + Vector3 p(1, 1, 1); + auto res = Project::projectTriangle(v1, v2, v3, p); EXPECT_TRUE(res.encode == 7); - EXPECT_TRUE(approx(res.sqr_distance, 4/3.0)); - EXPECT_TRUE(approx(res.parameterization[0], 1/3.0)); - EXPECT_TRUE(approx(res.parameterization[1], 1/3.0)); - EXPECT_TRUE(approx(res.parameterization[2], 1/3.0)); - - p = Vector3d(0, 0, 1.5); - res = Project::projectTriangle(v1, v2, v3, p); + EXPECT_TRUE(approx(res.sqr_distance, (S)(4/3.0))); + EXPECT_TRUE(approx(res.parameterization[0], (S)(1/3.0))); + EXPECT_TRUE(approx(res.parameterization[1], (S)(1/3.0))); + EXPECT_TRUE(approx(res.parameterization[2], (S)(1/3.0))); + + p = Vector3(0, 0, 1.5); + res = Project::projectTriangle(v1, v2, v3, p); EXPECT_TRUE(res.encode == 1); - EXPECT_TRUE(approx(res.sqr_distance, 0.25)); - EXPECT_TRUE(approx(res.parameterization[0], 1)); - EXPECT_TRUE(approx(res.parameterization[1], 0)); - EXPECT_TRUE(approx(res.parameterization[2], 0)); + EXPECT_TRUE(approx(res.sqr_distance, (S)0.25)); + EXPECT_TRUE(approx(res.parameterization[0], (S)1)); + EXPECT_TRUE(approx(res.parameterization[1], (S)0)); + EXPECT_TRUE(approx(res.parameterization[2], (S)0)); - p = Vector3d(1.5, 0, 0); - res = Project::projectTriangle(v1, v2, v3, p); + p = Vector3(1.5, 0, 0); + res = Project::projectTriangle(v1, v2, v3, p); EXPECT_TRUE(res.encode == 4); - EXPECT_TRUE(approx(res.sqr_distance, 0.25)); - EXPECT_TRUE(approx(res.parameterization[0], 0)); - EXPECT_TRUE(approx(res.parameterization[1], 0)); - EXPECT_TRUE(approx(res.parameterization[2], 1)); + EXPECT_TRUE(approx(res.sqr_distance, (S)0.25)); + EXPECT_TRUE(approx(res.parameterization[0], (S)0)); + EXPECT_TRUE(approx(res.parameterization[1], (S)0)); + EXPECT_TRUE(approx(res.parameterization[2], (S)1)); - p = Vector3d(0, 1.5, 0); - res = Project::projectTriangle(v1, v2, v3, p); + p = Vector3(0, 1.5, 0); + res = Project::projectTriangle(v1, v2, v3, p); EXPECT_TRUE(res.encode == 2); - EXPECT_TRUE(approx(res.sqr_distance, 0.25)); - EXPECT_TRUE(approx(res.parameterization[0], 0)); - EXPECT_TRUE(approx(res.parameterization[1], 1)); - EXPECT_TRUE(approx(res.parameterization[2], 0)); + EXPECT_TRUE(approx(res.sqr_distance, (S)0.25)); + EXPECT_TRUE(approx(res.parameterization[0], (S)0)); + EXPECT_TRUE(approx(res.parameterization[1], (S)1)); + EXPECT_TRUE(approx(res.parameterization[2], (S)0)); - p = Vector3d(1, 1, 0); - res = Project::projectTriangle(v1, v2, v3, p); + p = Vector3(1, 1, 0); + res = Project::projectTriangle(v1, v2, v3, p); EXPECT_TRUE(res.encode == 6); - EXPECT_TRUE(approx(res.sqr_distance, 0.5)); - EXPECT_TRUE(approx(res.parameterization[0], 0)); - EXPECT_TRUE(approx(res.parameterization[1], 0.5)); - EXPECT_TRUE(approx(res.parameterization[2], 0.5)); + EXPECT_TRUE(approx(res.sqr_distance, (S)0.5)); + EXPECT_TRUE(approx(res.parameterization[0], (S)0)); + EXPECT_TRUE(approx(res.parameterization[1], (S)0.5)); + EXPECT_TRUE(approx(res.parameterization[2], (S)0.5)); - p = Vector3d(1, 0, 1); - res = Project::projectTriangle(v1, v2, v3, p); + p = Vector3(1, 0, 1); + res = Project::projectTriangle(v1, v2, v3, p); EXPECT_TRUE(res.encode == 5); - EXPECT_TRUE(approx(res.sqr_distance, 0.5)); - EXPECT_TRUE(approx(res.parameterization[0], 0.5)); - EXPECT_TRUE(approx(res.parameterization[1], 0)); - EXPECT_TRUE(approx(res.parameterization[2], 0.5)); + EXPECT_TRUE(approx(res.sqr_distance, (S)0.5)); + EXPECT_TRUE(approx(res.parameterization[0], (S)0.5)); + EXPECT_TRUE(approx(res.parameterization[1], (S)0)); + EXPECT_TRUE(approx(res.parameterization[2], (S)0.5)); - p = Vector3d(0, 1, 1); - res = Project::projectTriangle(v1, v2, v3, p); + p = Vector3(0, 1, 1); + res = Project::projectTriangle(v1, v2, v3, p); EXPECT_TRUE(res.encode == 3); - EXPECT_TRUE(approx(res.sqr_distance, 0.5)); - EXPECT_TRUE(approx(res.parameterization[0], 0.5)); - EXPECT_TRUE(approx(res.parameterization[1], 0.5)); - EXPECT_TRUE(approx(res.parameterization[2], 0)); + EXPECT_TRUE(approx(res.sqr_distance, (S)0.5)); + EXPECT_TRUE(approx(res.parameterization[0], (S)0.5)); + EXPECT_TRUE(approx(res.parameterization[1], (S)0.5)); + EXPECT_TRUE(approx(res.parameterization[2], (S)0)); } -GTEST_TEST(FCL_SIMPLE, projection_test_tetrahedron) +GTEST_TEST(FCL_SIMPLE, projection_test_triangle) { - Vector3d v1(0, 0, 1); - Vector3d v2(0, 1, 0); - Vector3d v3(1, 0, 0); - Vector3d v4(1, 1, 1); +// test_projection_test_triangle(); + test_projection_test_triangle(); +} + +template +void test_projection_test_tetrahedron() +{ + Vector3 v1(0, 0, 1); + Vector3 v2(0, 1, 0); + Vector3 v3(1, 0, 0); + Vector3 v4(1, 1, 1); - Vector3d p(0.5, 0.5, 0.5); - Project::ProjectResult res = Project::projectTetrahedra(v1, v2, v3, v4, p); + Vector3 p(0.5, 0.5, 0.5); + auto res = Project::projectTetrahedra(v1, v2, v3, v4, p); EXPECT_TRUE(res.encode == 15); - EXPECT_TRUE(approx(res.sqr_distance, 0)); - EXPECT_TRUE(approx(res.parameterization[0], 0.25)); - EXPECT_TRUE(approx(res.parameterization[1], 0.25)); - EXPECT_TRUE(approx(res.parameterization[2], 0.25)); - EXPECT_TRUE(approx(res.parameterization[3], 0.25)); - - p = Vector3d(0, 0, 0); - res = Project::projectTetrahedra(v1, v2, v3, v4, p); + EXPECT_TRUE(approx(res.sqr_distance, (S)0)); + EXPECT_TRUE(approx(res.parameterization[0], (S)0.25)); + EXPECT_TRUE(approx(res.parameterization[1], (S)0.25)); + EXPECT_TRUE(approx(res.parameterization[2], (S)0.25)); + EXPECT_TRUE(approx(res.parameterization[3], (S)0.25)); + + p = Vector3(0, 0, 0); + res = Project::projectTetrahedra(v1, v2, v3, v4, p); EXPECT_TRUE(res.encode == 7); - EXPECT_TRUE(approx(res.sqr_distance, 1/3.0)); - EXPECT_TRUE(approx(res.parameterization[0], 1/3.0)); - EXPECT_TRUE(approx(res.parameterization[1], 1/3.0)); - EXPECT_TRUE(approx(res.parameterization[2], 1/3.0)); - EXPECT_TRUE(approx(res.parameterization[3], 0)); - - p = Vector3d(0, 1, 1); - res = Project::projectTetrahedra(v1, v2, v3, v4, p); + EXPECT_TRUE(approx(res.sqr_distance, (S)(1/3.0))); + EXPECT_TRUE(approx(res.parameterization[0], (S)(1/3.0))); + EXPECT_TRUE(approx(res.parameterization[1], (S)(1/3.0))); + EXPECT_TRUE(approx(res.parameterization[2], (S)(1/3.0))); + EXPECT_TRUE(approx(res.parameterization[3], (S)0)); + + p = Vector3(0, 1, 1); + res = Project::projectTetrahedra(v1, v2, v3, v4, p); EXPECT_TRUE(res.encode == 11); - EXPECT_TRUE(approx(res.sqr_distance, 1/3.0)); - EXPECT_TRUE(approx(res.parameterization[0], 1/3.0)); - EXPECT_TRUE(approx(res.parameterization[1], 1/3.0)); - EXPECT_TRUE(approx(res.parameterization[2], 0)); - EXPECT_TRUE(approx(res.parameterization[3], 1/3.0)); - - p = Vector3d(1, 1, 0); - res = Project::projectTetrahedra(v1, v2, v3, v4, p); + EXPECT_TRUE(approx(res.sqr_distance, (S)(1/3.0))); + EXPECT_TRUE(approx(res.parameterization[0], (S)(1/3.0))); + EXPECT_TRUE(approx(res.parameterization[1], (S)(1/3.0))); + EXPECT_TRUE(approx(res.parameterization[2], (S)0)); + EXPECT_TRUE(approx(res.parameterization[3], (S)(1/3.0))); + + p = Vector3(1, 1, 0); + res = Project::projectTetrahedra(v1, v2, v3, v4, p); EXPECT_TRUE(res.encode == 14); - EXPECT_TRUE(approx(res.sqr_distance, 1/3.0)); - EXPECT_TRUE(approx(res.parameterization[0], 0)); - EXPECT_TRUE(approx(res.parameterization[1], 1/3.0)); - EXPECT_TRUE(approx(res.parameterization[2], 1/3.0)); - EXPECT_TRUE(approx(res.parameterization[3], 1/3.0)); - - p = Vector3d(1, 0, 1); - res = Project::projectTetrahedra(v1, v2, v3, v4, p); + EXPECT_TRUE(approx(res.sqr_distance, (S)(1/3.0))); + EXPECT_TRUE(approx(res.parameterization[0], (S)0)); + EXPECT_TRUE(approx(res.parameterization[1], (S)(1/3.0))); + EXPECT_TRUE(approx(res.parameterization[2], (S)(1/3.0))); + EXPECT_TRUE(approx(res.parameterization[3], (S)(1/3.0))); + + p = Vector3(1, 0, 1); + res = Project::projectTetrahedra(v1, v2, v3, v4, p); EXPECT_TRUE(res.encode == 13); - EXPECT_TRUE(approx(res.sqr_distance, 1/3.0)); - EXPECT_TRUE(approx(res.parameterization[0], 1/3.0)); - EXPECT_TRUE(approx(res.parameterization[1], 0)); - EXPECT_TRUE(approx(res.parameterization[2], 1/3.0)); - EXPECT_TRUE(approx(res.parameterization[3], 1/3.0)); - - p = Vector3d(1.5, 1.5, 1.5); - res = Project::projectTetrahedra(v1, v2, v3, v4, p); + EXPECT_TRUE(approx(res.sqr_distance, (S)(1/3.0))); + EXPECT_TRUE(approx(res.parameterization[0], (S)(1/3.0))); + EXPECT_TRUE(approx(res.parameterization[1], (S)0)); + EXPECT_TRUE(approx(res.parameterization[2], (S)(1/3.0))); + EXPECT_TRUE(approx(res.parameterization[3], (S)(1/3.0))); + + p = Vector3(1.5, 1.5, 1.5); + res = Project::projectTetrahedra(v1, v2, v3, v4, p); EXPECT_TRUE(res.encode == 8); - EXPECT_TRUE(approx(res.sqr_distance, 0.75)); - EXPECT_TRUE(approx(res.parameterization[0], 0)); - EXPECT_TRUE(approx(res.parameterization[1], 0)); - EXPECT_TRUE(approx(res.parameterization[2], 0)); - EXPECT_TRUE(approx(res.parameterization[3], 1)); - - p = Vector3d(1.5, -0.5, -0.5); - res = Project::projectTetrahedra(v1, v2, v3, v4, p); + EXPECT_TRUE(approx(res.sqr_distance, (S)0.75)); + EXPECT_TRUE(approx(res.parameterization[0], (S)0)); + EXPECT_TRUE(approx(res.parameterization[1], (S)0)); + EXPECT_TRUE(approx(res.parameterization[2], (S)0)); + EXPECT_TRUE(approx(res.parameterization[3], (S)1)); + + p = Vector3(1.5, -0.5, -0.5); + res = Project::projectTetrahedra(v1, v2, v3, v4, p); EXPECT_TRUE(res.encode == 4); - EXPECT_TRUE(approx(res.sqr_distance, 0.75)); - EXPECT_TRUE(approx(res.parameterization[0], 0)); - EXPECT_TRUE(approx(res.parameterization[1], 0)); - EXPECT_TRUE(approx(res.parameterization[2], 1)); - EXPECT_TRUE(approx(res.parameterization[3], 0)); - - p = Vector3d(-0.5, -0.5, 1.5); - res = Project::projectTetrahedra(v1, v2, v3, v4, p); + EXPECT_TRUE(approx(res.sqr_distance, (S)0.75)); + EXPECT_TRUE(approx(res.parameterization[0], (S)0)); + EXPECT_TRUE(approx(res.parameterization[1], (S)0)); + EXPECT_TRUE(approx(res.parameterization[2], (S)1)); + EXPECT_TRUE(approx(res.parameterization[3], (S)0)); + + p = Vector3(-0.5, -0.5, 1.5); + res = Project::projectTetrahedra(v1, v2, v3, v4, p); EXPECT_TRUE(res.encode == 1); - EXPECT_TRUE(approx(res.sqr_distance, 0.75)); - EXPECT_TRUE(approx(res.parameterization[0], 1)); - EXPECT_TRUE(approx(res.parameterization[1], 0)); - EXPECT_TRUE(approx(res.parameterization[2], 0)); - EXPECT_TRUE(approx(res.parameterization[3], 0)); - - p = Vector3d(-0.5, 1.5, -0.5); - res = Project::projectTetrahedra(v1, v2, v3, v4, p); + EXPECT_TRUE(approx(res.sqr_distance, (S)0.75)); + EXPECT_TRUE(approx(res.parameterization[0], (S)1)); + EXPECT_TRUE(approx(res.parameterization[1], (S)0)); + EXPECT_TRUE(approx(res.parameterization[2], (S)0)); + EXPECT_TRUE(approx(res.parameterization[3], (S)0)); + + p = Vector3(-0.5, 1.5, -0.5); + res = Project::projectTetrahedra(v1, v2, v3, v4, p); EXPECT_TRUE(res.encode == 2); - EXPECT_TRUE(approx(res.sqr_distance, 0.75)); - EXPECT_TRUE(approx(res.parameterization[0], 0)); - EXPECT_TRUE(approx(res.parameterization[1], 1)); - EXPECT_TRUE(approx(res.parameterization[2], 0)); - EXPECT_TRUE(approx(res.parameterization[3], 0)); - - p = Vector3d(0.5, -0.5, 0.5); - res = Project::projectTetrahedra(v1, v2, v3, v4, p); + EXPECT_TRUE(approx(res.sqr_distance, (S)0.75)); + EXPECT_TRUE(approx(res.parameterization[0], (S)0)); + EXPECT_TRUE(approx(res.parameterization[1], (S)1)); + EXPECT_TRUE(approx(res.parameterization[2], (S)0)); + EXPECT_TRUE(approx(res.parameterization[3], (S)0)); + + p = Vector3(0.5, -0.5, 0.5); + res = Project::projectTetrahedra(v1, v2, v3, v4, p); EXPECT_TRUE(res.encode == 5); - EXPECT_TRUE(approx(res.sqr_distance, 0.25)); - EXPECT_TRUE(approx(res.parameterization[0], 0.5)); - EXPECT_TRUE(approx(res.parameterization[1], 0)); - EXPECT_TRUE(approx(res.parameterization[2], 0.5)); - EXPECT_TRUE(approx(res.parameterization[3], 0)); - - p = Vector3d(0.5, 1.5, 0.5); - res = Project::projectTetrahedra(v1, v2, v3, v4, p); + EXPECT_TRUE(approx(res.sqr_distance, (S)0.25)); + EXPECT_TRUE(approx(res.parameterization[0], (S)0.5)); + EXPECT_TRUE(approx(res.parameterization[1], (S)0)); + EXPECT_TRUE(approx(res.parameterization[2], (S)0.5)); + EXPECT_TRUE(approx(res.parameterization[3], (S)0)); + + p = Vector3(0.5, 1.5, 0.5); + res = Project::projectTetrahedra(v1, v2, v3, v4, p); EXPECT_TRUE(res.encode == 10); - EXPECT_TRUE(approx(res.sqr_distance, 0.25)); - EXPECT_TRUE(approx(res.parameterization[0], 0)); - EXPECT_TRUE(approx(res.parameterization[1], 0.5)); - EXPECT_TRUE(approx(res.parameterization[2], 0)); - EXPECT_TRUE(approx(res.parameterization[3], 0.5)); - - p = Vector3d(1.5, 0.5, 0.5); - res = Project::projectTetrahedra(v1, v2, v3, v4, p); + EXPECT_TRUE(approx(res.sqr_distance, (S)0.25)); + EXPECT_TRUE(approx(res.parameterization[0], (S)0)); + EXPECT_TRUE(approx(res.parameterization[1], (S)0.5)); + EXPECT_TRUE(approx(res.parameterization[2], (S)0)); + EXPECT_TRUE(approx(res.parameterization[3], (S)0.5)); + + p = Vector3(1.5, 0.5, 0.5); + res = Project::projectTetrahedra(v1, v2, v3, v4, p); EXPECT_TRUE(res.encode == 12); - EXPECT_TRUE(approx(res.sqr_distance, 0.25)); - EXPECT_TRUE(approx(res.parameterization[0], 0)); - EXPECT_TRUE(approx(res.parameterization[1], 0)); - EXPECT_TRUE(approx(res.parameterization[2], 0.5)); - EXPECT_TRUE(approx(res.parameterization[3], 0.5)); - - p = Vector3d(-0.5, 0.5, 0.5); - res = Project::projectTetrahedra(v1, v2, v3, v4, p); + EXPECT_TRUE(approx(res.sqr_distance, (S)0.25)); + EXPECT_TRUE(approx(res.parameterization[0], (S)0)); + EXPECT_TRUE(approx(res.parameterization[1], (S)0)); + EXPECT_TRUE(approx(res.parameterization[2], (S)0.5)); + EXPECT_TRUE(approx(res.parameterization[3], (S)0.5)); + + p = Vector3(-0.5, 0.5, 0.5); + res = Project::projectTetrahedra(v1, v2, v3, v4, p); EXPECT_TRUE(res.encode == 3); - EXPECT_TRUE(approx(res.sqr_distance, 0.25)); - EXPECT_TRUE(approx(res.parameterization[0], 0.5)); - EXPECT_TRUE(approx(res.parameterization[1], 0.5)); - EXPECT_TRUE(approx(res.parameterization[2], 0)); - EXPECT_TRUE(approx(res.parameterization[3], 0)); - - p = Vector3d(0.5, 0.5, 1.5); - res = Project::projectTetrahedra(v1, v2, v3, v4, p); + EXPECT_TRUE(approx(res.sqr_distance, (S)0.25)); + EXPECT_TRUE(approx(res.parameterization[0], (S)0.5)); + EXPECT_TRUE(approx(res.parameterization[1], (S)0.5)); + EXPECT_TRUE(approx(res.parameterization[2], (S)0)); + EXPECT_TRUE(approx(res.parameterization[3], (S)0)); + + p = Vector3(0.5, 0.5, 1.5); + res = Project::projectTetrahedra(v1, v2, v3, v4, p); EXPECT_TRUE(res.encode == 9); - EXPECT_TRUE(approx(res.sqr_distance, 0.25)); - EXPECT_TRUE(approx(res.parameterization[0], 0.5)); - EXPECT_TRUE(approx(res.parameterization[1], 0)); - EXPECT_TRUE(approx(res.parameterization[2], 0)); - EXPECT_TRUE(approx(res.parameterization[3], 0.5)); - - p = Vector3d(0.5, 0.5, -0.5); - res = Project::projectTetrahedra(v1, v2, v3, v4, p); + EXPECT_TRUE(approx(res.sqr_distance, (S)0.25)); + EXPECT_TRUE(approx(res.parameterization[0], (S)0.5)); + EXPECT_TRUE(approx(res.parameterization[1], (S)0)); + EXPECT_TRUE(approx(res.parameterization[2], (S)0)); + EXPECT_TRUE(approx(res.parameterization[3], (S)0.5)); + + p = Vector3(0.5, 0.5, -0.5); + res = Project::projectTetrahedra(v1, v2, v3, v4, p); EXPECT_TRUE(res.encode == 6); - EXPECT_TRUE(approx(res.sqr_distance, 0.25)); - EXPECT_TRUE(approx(res.parameterization[0], 0)); - EXPECT_TRUE(approx(res.parameterization[1], 0.5)); - EXPECT_TRUE(approx(res.parameterization[2], 0.5)); - EXPECT_TRUE(approx(res.parameterization[3], 0)); + EXPECT_TRUE(approx(res.sqr_distance, (S)0.25)); + EXPECT_TRUE(approx(res.parameterization[0], (S)0)); + EXPECT_TRUE(approx(res.parameterization[1], (S)0.5)); + EXPECT_TRUE(approx(res.parameterization[2], (S)0.5)); + EXPECT_TRUE(approx(res.parameterization[3], (S)0)); } +GTEST_TEST(FCL_SIMPLE, projection_test_tetrahedron) +{ +// test_projection_test_tetrahedron(); + test_projection_test_tetrahedron(); +} + //============================================================================== int main(int argc, char* argv[]) { diff --git a/test/test_fcl_sphere_capsule.cpp b/test/test_fcl_sphere_capsule.cpp index 33bef8a1c..42aef6d36 100644 --- a/test/test_fcl_sphere_capsule.cpp +++ b/test/test_fcl_sphere_capsule.cpp @@ -40,162 +40,219 @@ #include "fcl/math/constants.h" #include "fcl/collision.h" #include "fcl/shape/geometric_shapes.h" -#include "fcl/narrowphase/narrowphase.h" +#include "fcl/narrowphase/gjk_solver_indep.h" +#include "fcl/narrowphase/gjk_solver_libccd.h" using namespace fcl; -GTEST_TEST(FCL_SPHERE_CAPSULE, Sphere_Capsule_Intersect_test_separated_z) +template +void test_Sphere_Capsule_Intersect_test_separated_z() { - GJKSolver_libccd solver; + GJKSolver_libccd solver; - Sphere sphere1 (50); - Transform3d sphere1_transform; - sphere1_transform.translation() = (Vector3d (0., 0., -50)); + Sphere sphere1 (50); + Transform3 sphere1_transform; + sphere1_transform.translation() = (Vector3 (0., 0., -50)); - Capsule capsule (50, 200.); - Transform3d capsule_transform(Eigen::Translation3d(Vector3d(0., 0., 200))); + Capsule capsule (50, 200.); + Transform3 capsule_transform(Translation3(Vector3(0., 0., 200))); - EXPECT_TRUE (!solver.shapeIntersect(sphere1, sphere1_transform, capsule, capsule_transform, NULL)); + EXPECT_TRUE (!solver.shapeIntersect(sphere1, sphere1_transform, capsule, capsule_transform, nullptr)); } -GTEST_TEST(FCL_SPHERE_CAPSULE, Sphere_Capsule_Intersect_test_separated_z_negative) +GTEST_TEST(FCL_SPHERE_CAPSULE, Sphere_Capsule_Intersect_test_separated_z) +{ +// test_Sphere_Capsule_Intersect_test_separated_z(); + test_Sphere_Capsule_Intersect_test_separated_z(); +} + +template +void test_Sphere_Capsule_Intersect_test_separated_z_negative() { - GJKSolver_libccd solver; + GJKSolver_libccd solver; - Sphere sphere1 (50); - Transform3d sphere1_transform; - sphere1_transform.translation() = (Vector3d (0., 0., 50)); + Sphere sphere1 (50); + Transform3 sphere1_transform; + sphere1_transform.translation() = (Vector3 (0., 0., 50)); - Capsule capsule (50, 200.); - Transform3d capsule_transform(Eigen::Translation3d(Vector3d(0., 0., -200))); + Capsule capsule (50, 200.); + Transform3 capsule_transform(Translation3(Vector3(0., 0., -200))); - EXPECT_TRUE (!solver.shapeIntersect(sphere1, sphere1_transform, capsule, capsule_transform, NULL)); + EXPECT_TRUE (!solver.shapeIntersect(sphere1, sphere1_transform, capsule, capsule_transform, nullptr)); } -GTEST_TEST(FCL_SPHERE_CAPSULE, Sphere_Capsule_Intersect_test_separated_x) +GTEST_TEST(FCL_SPHERE_CAPSULE, Sphere_Capsule_Intersect_test_separated_z_negative) +{ +// test_Sphere_Capsule_Intersect_test_separated_z_negative(); + test_Sphere_Capsule_Intersect_test_separated_z_negative(); +} + +template +void test_Sphere_Capsule_Intersect_test_separated_x() { - GJKSolver_libccd solver; + GJKSolver_libccd solver; - Sphere sphere1 (50); - Transform3d sphere1_transform; - sphere1_transform.translation() = (Vector3d (0., 0., -50)); + Sphere sphere1 (50); + Transform3 sphere1_transform; + sphere1_transform.translation() = (Vector3 (0., 0., -50)); - Capsule capsule (50, 200.); - Transform3d capsule_transform(Eigen::Translation3d(Vector3d(150., 0., 0.))); + Capsule capsule (50, 200.); + Transform3 capsule_transform(Translation3(Vector3(150., 0., 0.))); - EXPECT_TRUE (!solver.shapeIntersect(sphere1, sphere1_transform, capsule, capsule_transform, NULL)); + EXPECT_TRUE (!solver.shapeIntersect(sphere1, sphere1_transform, capsule, capsule_transform, nullptr)); } -GTEST_TEST(FCL_SPHERE_CAPSULE, Sphere_Capsule_Intersect_test_separated_capsule_rotated) +GTEST_TEST(FCL_SPHERE_CAPSULE, Sphere_Capsule_Intersect_test_separated_x) +{ +// test_Sphere_Capsule_Intersect_test_separated_x(); + test_Sphere_Capsule_Intersect_test_separated_x(); +} + +template +void test_Sphere_Capsule_Intersect_test_separated_capsule_rotated() { - GJKSolver_libccd solver; + GJKSolver_libccd solver; - Sphere sphere1 (50); - Transform3d sphere1_transform; - sphere1_transform.translation() = (Vector3d (0., 0., -50)); + Sphere sphere1 (50); + Transform3 sphere1_transform; + sphere1_transform.translation() = (Vector3 (0., 0., -50)); - Capsule capsule (50, 200.); - Matrix3d rotation( - Eigen::AngleAxisd(constants::pi * 0.5, Vector3d::UnitX()) - * Eigen::AngleAxisd(0.0, Vector3d::UnitY()) - * Eigen::AngleAxisd(0.0, Vector3d::UnitZ())); + Capsule capsule (50, 200.); + Matrix3 rotation( + AngleAxis(constants::pi() * 0.5, Vector3::UnitX()) + * AngleAxis(0.0, Vector3::UnitY()) + * AngleAxis(0.0, Vector3::UnitZ())); - Transform3d capsule_transform = Transform3d::Identity(); + Transform3 capsule_transform = Transform3::Identity(); capsule_transform.linear() = rotation; - capsule_transform.translation() = Vector3d(150., 0., 0.); + capsule_transform.translation() = Vector3(150., 0., 0.); - EXPECT_TRUE (!solver.shapeIntersect(sphere1, sphere1_transform, capsule, capsule_transform, NULL)); + EXPECT_TRUE (!solver.shapeIntersect(sphere1, sphere1_transform, capsule, capsule_transform, nullptr)); } -GTEST_TEST(FCL_SPHERE_CAPSULE, Sphere_Capsule_Intersect_test_penetration_z) +GTEST_TEST(FCL_SPHERE_CAPSULE, Sphere_Capsule_Intersect_test_separated_capsule_rotated) +{ +// test_Sphere_Capsule_Intersect_test_separated_capsule_rotated(); + test_Sphere_Capsule_Intersect_test_separated_capsule_rotated(); +} + +template +void test_Sphere_Capsule_Intersect_test_penetration_z() { - GJKSolver_libccd solver; + GJKSolver_libccd solver; - Sphere sphere1 (50); - Transform3d sphere1_transform(Eigen::Translation3d(Vector3d(0., 0., -50))); + Sphere sphere1 (50); + Transform3 sphere1_transform(Translation3(Vector3(0., 0., -50))); - Capsule capsule (50, 200.); - Transform3d capsule_transform(Eigen::Translation3d(Vector3d(0., 0., 125))); + Capsule capsule (50, 200.); + Transform3 capsule_transform(Translation3(Vector3(0., 0., 125))); - std::vector contacts; + std::vector> contacts; bool is_intersecting = solver.shapeIntersect(sphere1, sphere1_transform, capsule, capsule_transform, &contacts); - FCL_REAL penetration = contacts[0].penetration_depth; - Vector3d contact_point = contacts[0].pos; - Vector3d normal = contacts[0].normal; + S penetration = contacts[0].penetration_depth; + Vector3 contact_point = contacts[0].pos; + Vector3 normal = contacts[0].normal; EXPECT_TRUE (is_intersecting); EXPECT_TRUE (penetration == 25.); - EXPECT_TRUE (Vector3d (0., 0., 1.).isApprox(normal)); - EXPECT_TRUE (Vector3d (0., 0., 0.).isApprox(contact_point)); + EXPECT_TRUE (Vector3 (0., 0., 1.).isApprox(normal)); + EXPECT_TRUE (Vector3 (0., 0., 0.).isApprox(contact_point)); } -GTEST_TEST(FCL_SPHERE_CAPSULE, Sphere_Capsule_Intersect_test_penetration_z_rotated) +GTEST_TEST(FCL_SPHERE_CAPSULE, Sphere_Capsule_Intersect_test_penetration_z) +{ +// test_Sphere_Capsule_Intersect_test_penetration_z(); + test_Sphere_Capsule_Intersect_test_penetration_z(); +} + +template +void test_Sphere_Capsule_Intersect_test_penetration_z_rotated() { - GJKSolver_libccd solver; + GJKSolver_libccd solver; - Sphere sphere1 (50); - Transform3d sphere1_transform = Transform3d::Identity(); + Sphere sphere1 (50); + Transform3 sphere1_transform = Transform3::Identity(); - Capsule capsule (50, 200.); - Matrix3d rotation( - Eigen::AngleAxisd(constants::pi * 0.5, Vector3d::UnitX()) - * Eigen::AngleAxisd(0.0, Vector3d::UnitY()) - * Eigen::AngleAxisd(0.0, Vector3d::UnitZ())); - Transform3d capsule_transform = Transform3d::Identity(); + Capsule capsule (50, 200.); + Matrix3 rotation( + AngleAxis(constants::pi() * 0.5, Vector3::UnitX()) + * AngleAxis(0.0, Vector3::UnitY()) + * AngleAxis(0.0, Vector3::UnitZ())); + Transform3 capsule_transform = Transform3::Identity(); capsule_transform.linear() = rotation; - capsule_transform.translation() = Vector3d (0., 50., 75); + capsule_transform.translation() = Vector3 (0., 50., 75); - std::vector contacts; + std::vector> contacts; bool is_intersecting = solver.shapeIntersect(sphere1, sphere1_transform, capsule, capsule_transform, &contacts); - FCL_REAL penetration = contacts[0].penetration_depth; - Vector3d contact_point = contacts[0].pos; - Vector3d normal = contacts[0].normal; + S penetration = contacts[0].penetration_depth; + Vector3 contact_point = contacts[0].pos; + Vector3 normal = contacts[0].normal; EXPECT_TRUE (is_intersecting); EXPECT_NEAR (25, penetration, solver.collision_tolerance); - EXPECT_TRUE (Vector3d (0., 0., 1.).isApprox(normal)); - EXPECT_TRUE (Vector3d (0., 0., 50.).isApprox(contact_point, solver.collision_tolerance)); + EXPECT_TRUE (Vector3 (0., 0., 1.).isApprox(normal)); + EXPECT_TRUE (Vector3 (0., 0., 50.).isApprox(contact_point, solver.collision_tolerance)); } -GTEST_TEST(FCL_SPHERE_CAPSULE, Sphere_Capsule_Distance_test_collision) +GTEST_TEST(FCL_SPHERE_CAPSULE, Sphere_Capsule_Intersect_test_penetration_z_rotated) +{ +// test_Sphere_Capsule_Intersect_test_penetration_z_rotated(); + test_Sphere_Capsule_Intersect_test_penetration_z_rotated(); +} + +template +void test_Sphere_Capsule_Distance_test_collision() { - GJKSolver_libccd solver; + GJKSolver_libccd solver; - Sphere sphere1 (50); - Transform3d sphere1_transform(Eigen::Translation3d(Vector3d(0., 0., -50))); + Sphere sphere1 (50); + Transform3 sphere1_transform(Translation3(Vector3(0., 0., -50))); - Capsule capsule (50, 200.); - Transform3d capsule_transform(Eigen::Translation3d(Vector3d(0., 0., 100))); + Capsule capsule (50, 200.); + Transform3 capsule_transform(Translation3(Vector3(0., 0., 100))); - FCL_REAL distance; + S distance; EXPECT_TRUE (!solver.shapeDistance(sphere1, sphere1_transform, capsule, capsule_transform, &distance)); } -GTEST_TEST(FCL_SPHERE_CAPSULE, Sphere_Capsule_Distance_test_separated) +GTEST_TEST(FCL_SPHERE_CAPSULE, Sphere_Capsule_Distance_test_collision) +{ +// test_Sphere_Capsule_Distance_test_collision(); + test_Sphere_Capsule_Distance_test_collision(); +} + +template +void test_Sphere_Capsule_Distance_test_separated() { - GJKSolver_libccd solver; + GJKSolver_libccd solver; - Sphere sphere1 (50); - Transform3d sphere1_transform(Eigen::Translation3d(Vector3d(0., 0., -50))); + Sphere sphere1 (50); + Transform3 sphere1_transform(Translation3(Vector3(0., 0., -50))); - Capsule capsule (50, 200.); - Transform3d capsule_transform(Eigen::Translation3d(Vector3d(0., 0., 175))); + Capsule capsule (50, 200.); + Transform3 capsule_transform(Translation3(Vector3(0., 0., 175))); - FCL_REAL distance = 0.; - Vector3d p1; - Vector3d p2; - bool is_separated = solver.shapeDistance(sphere1, sphere1_transform, capsule, capsule_transform, &distance); + S distance = 0.; + Vector3 p1; + Vector3 p2; + bool is_separated = solver.shapeDistance(sphere1, sphere1_transform, capsule, capsule_transform, &distance); EXPECT_TRUE (is_separated); EXPECT_TRUE (distance == 25.); } +GTEST_TEST(FCL_SPHERE_CAPSULE, Sphere_Capsule_Distance_test_separated) +{ +// test_Sphere_Capsule_Distance_test_separated(); + test_Sphere_Capsule_Distance_test_separated(); +} + //============================================================================== int main(int argc, char* argv[]) { diff --git a/test/test_fcl_utility.cpp b/test/test_fcl_utility.cpp index 282622c04..1c51f1c31 100644 --- a/test/test_fcl_utility.cpp +++ b/test/test_fcl_utility.cpp @@ -41,8 +41,6 @@ #include "fcl/distance.h" #include #include -#include -#include namespace fcl { @@ -76,7 +74,7 @@ void Timer::start() #ifdef _WIN32 QueryPerformanceCounter(&startCount); #else - gettimeofday(&startCount, NULL); + gettimeofday(&startCount, nullptr); #endif } @@ -88,7 +86,7 @@ void Timer::stop() #ifdef _WIN32 QueryPerformanceCounter(&endCount); #else - gettimeofday(&endCount, NULL); + gettimeofday(&endCount, nullptr); #endif } @@ -103,7 +101,7 @@ double Timer::getElapsedTimeInMicroSec() endTimeInMicroSec = endCount.QuadPart * (1000000.0 / frequency.QuadPart); #else if(!stopped) - gettimeofday(&endCount, NULL); + gettimeofday(&endCount, nullptr); startTimeInMicroSec = (startCount.tv_sec * 1000000.0) + startCount.tv_usec; endTimeInMicroSec = (endCount.tv_sec * 1000000.0) + endCount.tv_usec; @@ -130,324 +128,6 @@ double Timer::getElapsedTime() return this->getElapsedTimeInMilliSec(); } - -FCL_REAL rand_interval(FCL_REAL rmin, FCL_REAL rmax) -{ - FCL_REAL t = rand() / ((FCL_REAL)RAND_MAX + 1); - return (t * (rmax - rmin) + rmin); -} - -void loadOBJFile(const char* filename, std::vector& points, std::vector& triangles) -{ - - FILE* file = fopen(filename, "rb"); - if(!file) - { - std::cerr << "file not exist" << std::endl; - return; - } - - bool has_normal = false; - bool has_texture = false; - char line_buffer[2000]; - while(fgets(line_buffer, 2000, file)) - { - char* first_token = strtok(line_buffer, "\r\n\t "); - if(!first_token || first_token[0] == '#' || first_token[0] == 0) - continue; - - switch(first_token[0]) - { - case 'v': - { - if(first_token[1] == 'n') - { - strtok(NULL, "\t "); - strtok(NULL, "\t "); - strtok(NULL, "\t "); - has_normal = true; - } - else if(first_token[1] == 't') - { - strtok(NULL, "\t "); - strtok(NULL, "\t "); - has_texture = true; - } - else - { - FCL_REAL x = (FCL_REAL)atof(strtok(NULL, "\t ")); - FCL_REAL y = (FCL_REAL)atof(strtok(NULL, "\t ")); - FCL_REAL z = (FCL_REAL)atof(strtok(NULL, "\t ")); - Vector3d p(x, y, z); - points.push_back(p); - } - } - break; - case 'f': - { - Triangle tri; - char* data[30]; - int n = 0; - while((data[n] = strtok(NULL, "\t \r\n")) != NULL) - { - if(strlen(data[n])) - n++; - } - - for(int t = 0; t < (n - 2); ++t) - { - if((!has_texture) && (!has_normal)) - { - tri[0] = atoi(data[0]) - 1; - tri[1] = atoi(data[1]) - 1; - tri[2] = atoi(data[2]) - 1; - } - else - { - const char *v1; - for(int i = 0; i < 3; i++) - { - // vertex ID - if(i == 0) - v1 = data[0]; - else - v1 = data[t + i]; - - tri[i] = atoi(v1) - 1; - } - } - triangles.push_back(tri); - } - } - } - } -} - - -void saveOBJFile(const char* filename, std::vector& points, std::vector& triangles) -{ - std::ofstream os(filename); - if(!os) - { - std::cerr << "file not exist" << std::endl; - return; - } - - for(std::size_t i = 0; i < points.size(); ++i) - { - os << "v " << points[i][0] << " " << points[i][1] << " " << points[i][2] << std::endl; - } - - for(std::size_t i = 0; i < triangles.size(); ++i) - { - os << "f " << triangles[i][0] + 1 << " " << triangles[i][1] + 1 << " " << triangles[i][2] + 1 << std::endl; - } - - os.close(); -} - - -void eulerToMatrix(FCL_REAL a, FCL_REAL b, FCL_REAL c, Matrix3d& R) -{ - FCL_REAL c1 = cos(a); - FCL_REAL c2 = cos(b); - FCL_REAL c3 = cos(c); - FCL_REAL s1 = sin(a); - FCL_REAL s2 = sin(b); - FCL_REAL s3 = sin(c); - - R << c1 * c2, - c2 * s1, s2, - c3 * s1 + c1 * s2 * s3, c1 * c3 - s1 * s2 * s3, - c2 * s3, - s1 * s3 - c1 * c3 * s2, c3 * s1 * s2 + c1 * s3, c2 * c3; -} - -void generateRandomTransform(FCL_REAL extents[6], Transform3d& transform) -{ - FCL_REAL x = rand_interval(extents[0], extents[3]); - FCL_REAL y = rand_interval(extents[1], extents[4]); - FCL_REAL z = rand_interval(extents[2], extents[5]); - - const FCL_REAL pi = 3.1415926; - FCL_REAL a = rand_interval(0, 2 * pi); - FCL_REAL b = rand_interval(0, 2 * pi); - FCL_REAL c = rand_interval(0, 2 * pi); - - Matrix3d R; - eulerToMatrix(a, b, c, R); - Vector3d T(x, y, z); - transform.linear() = R; - transform.translation() = T; -} - - -void generateRandomTransforms(FCL_REAL extents[6], std::vector& transforms, std::size_t n) -{ - transforms.resize(n); - for(std::size_t i = 0; i < n; ++i) - { - FCL_REAL x = rand_interval(extents[0], extents[3]); - FCL_REAL y = rand_interval(extents[1], extents[4]); - FCL_REAL z = rand_interval(extents[2], extents[5]); - - const FCL_REAL pi = 3.1415926; - FCL_REAL a = rand_interval(0, 2 * pi); - FCL_REAL b = rand_interval(0, 2 * pi); - FCL_REAL c = rand_interval(0, 2 * pi); - - { - Matrix3d R; - eulerToMatrix(a, b, c, R); - Vector3d T(x, y, z); - transforms[i].setIdentity(); - transforms[i].linear() = R; - transforms[i].translation() = T; - } - } -} - - -void generateRandomTransforms(FCL_REAL extents[6], FCL_REAL delta_trans[3], FCL_REAL delta_rot, std::vector& transforms, std::vector& transforms2, std::size_t n) -{ - transforms.resize(n); - transforms2.resize(n); - for(std::size_t i = 0; i < n; ++i) - { - FCL_REAL x = rand_interval(extents[0], extents[3]); - FCL_REAL y = rand_interval(extents[1], extents[4]); - FCL_REAL z = rand_interval(extents[2], extents[5]); - - const FCL_REAL pi = 3.1415926; - FCL_REAL a = rand_interval(0, 2 * pi); - FCL_REAL b = rand_interval(0, 2 * pi); - FCL_REAL c = rand_interval(0, 2 * pi); - - { - Matrix3d R; - eulerToMatrix(a, b, c, R); - Vector3d T(x, y, z); - transforms[i].setIdentity(); - transforms[i].linear() = R; - transforms[i].translation() = T; - } - - FCL_REAL deltax = rand_interval(-delta_trans[0], delta_trans[0]); - FCL_REAL deltay = rand_interval(-delta_trans[1], delta_trans[1]); - FCL_REAL deltaz = rand_interval(-delta_trans[2], delta_trans[2]); - - FCL_REAL deltaa = rand_interval(-delta_rot, delta_rot); - FCL_REAL deltab = rand_interval(-delta_rot, delta_rot); - FCL_REAL deltac = rand_interval(-delta_rot, delta_rot); - - { - Matrix3d R; - eulerToMatrix(a + deltaa, b + deltab, c + deltac, R); - Vector3d T(x + deltax, y + deltay, z + deltaz); - transforms2[i].setIdentity(); - transforms2[i].linear() = R; - transforms2[i].translation() = T; - } - } -} - -void generateRandomTransform_ccd(FCL_REAL extents[6], std::vector& transforms, std::vector& transforms2, FCL_REAL delta_trans[3], FCL_REAL delta_rot, std::size_t n, - const std::vector& vertices1, const std::vector& triangles1, - const std::vector& vertices2, const std::vector& triangles2) -{ - transforms.resize(n); - transforms2.resize(n); - - for(std::size_t i = 0; i < n;) - { - FCL_REAL x = rand_interval(extents[0], extents[3]); - FCL_REAL y = rand_interval(extents[1], extents[4]); - FCL_REAL z = rand_interval(extents[2], extents[5]); - - const FCL_REAL pi = 3.1415926; - FCL_REAL a = rand_interval(0, 2 * pi); - FCL_REAL b = rand_interval(0, 2 * pi); - FCL_REAL c = rand_interval(0, 2 * pi); - - - Matrix3d R; - eulerToMatrix(a, b, c, R); - Vector3d T(x, y, z); - Transform3d tf(Transform3d::Identity()); - tf.linear() = R; - tf.translation() = T; - - std::vector > results; - { - transforms[i] = tf; - - FCL_REAL deltax = rand_interval(-delta_trans[0], delta_trans[0]); - FCL_REAL deltay = rand_interval(-delta_trans[1], delta_trans[1]); - FCL_REAL deltaz = rand_interval(-delta_trans[2], delta_trans[2]); - - FCL_REAL deltaa = rand_interval(-delta_rot, delta_rot); - FCL_REAL deltab = rand_interval(-delta_rot, delta_rot); - FCL_REAL deltac = rand_interval(-delta_rot, delta_rot); - - Matrix3d R2; - eulerToMatrix(a + deltaa, b + deltab, c + deltac, R2); - Vector3d T2(x + deltax, y + deltay, z + deltaz); - transforms2[i].linear() = R2; - transforms2[i].translation() = T2; - ++i; - } - } -} - -bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2, void* cdata_) -{ - CollisionData* cdata = static_cast(cdata_); - const CollisionRequest& request = cdata->request; - CollisionResult& result = cdata->result; - - if(cdata->done) return true; - - collide(o1, o2, request, result); - - if(!request.enable_cost && (result.isCollision()) && (result.numContacts() >= request.num_max_contacts)) - cdata->done = true; - - return cdata->done; -} - -bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2, void* cdata_, FCL_REAL& dist) -{ - DistanceData* cdata = static_cast(cdata_); - const DistanceRequest& request = cdata->request; - DistanceResult& result = cdata->result; - - if(cdata->done) { dist = result.min_distance; return true; } - - distance(o1, o2, request, result); - - dist = result.min_distance; - - if(dist <= 0) return true; // in collision or in touch - - return cdata->done; -} - -bool defaultContinuousCollisionFunction(ContinuousCollisionObject* o1, ContinuousCollisionObject* o2, void* cdata_) -{ - ContinuousCollisionData* cdata = static_cast(cdata_); - const ContinuousCollisionRequest& request = cdata->request; - ContinuousCollisionResult& result = cdata->result; - - if(cdata->done) return true; - - collide(o1, o2, request, result); - - return cdata->done; -} - -bool defaultContinuousDistanceFunction(ContinuousCollisionObject* o1, ContinuousCollisionObject* o2, void* cdata_, FCL_REAL& dist) -{ - return true; -} - std::string getNodeTypeName(NODE_TYPE node_type) { if (node_type == BV_UNKNOWN) diff --git a/test/test_fcl_utility.h b/test/test_fcl_utility.h index a7ef1b846..80eaa31dc 100644 --- a/test/test_fcl_utility.h +++ b/test/test_fcl_utility.h @@ -38,9 +38,16 @@ #ifndef TEST_FCL_UTILITY_H #define TEST_FCL_UTILITY_H +#include +#include +#include +#include "fcl/math/constants.h" #include "fcl/math/triangle.h" +#include "fcl/collision.h" +#include "fcl/distance.h" #include "fcl/collision_data.h" #include "fcl/collision_object.h" +#include "fcl/continuous_collision_object.h" #ifdef _WIN32 #define NOMINMAX // required to avoid compilation errors with Visual Studio 2010 @@ -80,37 +87,71 @@ class Timer #endif }; +struct TStruct +{ + std::vector records; + double overall_time; + + TStruct() { overall_time = 0; } + + void push_back(double t) + { + records.push_back(t); + overall_time += t; + } +}; /// @brief Load an obj mesh file -void loadOBJFile(const char* filename, std::vector& points, std::vector& triangles); +template +void loadOBJFile(const char* filename, std::vector>& points, std::vector& triangles); + +template +void saveOBJFile(const char* filename, std::vector>& points, std::vector& triangles); -void saveOBJFile(const char* filename, std::vector& points, std::vector& triangles); +template +S rand_interval(S rmin, S rmax); + +template +void eulerToMatrix(S a, S b, S c, Matrix3& R); /// @brief Generate one random transform whose translation is constrained by extents and rotation without constraints. /// The translation is (x, y, z), and extents[0] <= x <= extents[3], extents[1] <= y <= extents[4], extents[2] <= z <= extents[5] -void generateRandomTransform(FCL_REAL extents[6], Transform3d& transform); +template +void generateRandomTransform(S extents[6], Transform3& transform); /// @brief Generate n random transforms whose translations are constrained by extents. -void generateRandomTransforms(FCL_REAL extents[6], std::vector& transforms, std::size_t n); +template +void generateRandomTransforms(S extents[6], Eigen::aligned_vector>& transforms, std::size_t n); /// @brief Generate n random transforms whose translations are constrained by extents. Also generate another transforms2 which have additional random translation & rotation to the transforms generated. -void generateRandomTransforms(FCL_REAL extents[6], FCL_REAL delta_trans[3], FCL_REAL delta_rot, std::vector& transforms, std::vector& transforms2, std::size_t n); +template +void generateRandomTransforms(S extents[6], S delta_trans[3], S delta_rot, Eigen::aligned_vector>& transforms, Eigen::aligned_vector>& transforms2, std::size_t n); /// @brief Generate n random tranforms and transform2 with addtional random translation/rotation. The transforms and transform2 are used as initial and goal configurations for the first mesh. The second mesh is in I. This is used for continuous collision detection checking. -void generateRandomTransforms_ccd(FCL_REAL extents[6], std::vector& transforms, std::vector& transforms2, FCL_REAL delta_trans[3], FCL_REAL delta_rot, std::size_t n, - const std::vector& vertices1, const std::vector& triangles1, - const std::vector& vertices2, const std::vector& triangles2); +template +void generateRandomTransforms_ccd(S extents[6], Eigen::aligned_vector>& transforms, Eigen::aligned_vector>& transforms2, S delta_trans[3], S delta_rot, std::size_t n, + const std::vector>& vertices1, const std::vector& triangles1, + const std::vector>& vertices2, const std::vector& triangles2); + +/// @brief Generate environment with 3 * n objects: n boxes, n spheres and n cylinders. +template +void generateEnvironments(std::vector*>& env, S env_scale, std::size_t n); +/// @brief Generate environment with 3 * n objects, but all in meshes. +template +void generateEnvironmentsMesh(std::vector*>& env, S env_scale, std::size_t n); -/// @ brief Structure for minimum distance between two meshes and the corresponding nearest point pair +/// @brief Structure for minimum distance between two meshes and the corresponding nearest point pair +template struct DistanceRes { - double distance; - Vector3d p1; - Vector3d p2; + S distance; + Vector3 p1; + Vector3 p2; }; /// @brief Collision data stores the collision request and the result given by collision algorithm. +template struct CollisionData { CollisionData() @@ -119,10 +160,10 @@ struct CollisionData } /// @brief Collision request - CollisionRequest request; + CollisionRequest request; /// @brief Collision result - CollisionResult result; + CollisionResult result; /// @brief Whether the collision iteration can stop bool done; @@ -130,6 +171,7 @@ struct CollisionData /// @brief Distance data stores the distance request and the result given by distance algorithm. +template struct DistanceData { DistanceData() @@ -138,10 +180,10 @@ struct DistanceData } /// @brief Distance request - DistanceRequest request; + DistanceRequest request; /// @brief Distance result - DistanceResult result; + DistanceResult result; /// @brief Whether the distance iteration can stop bool done; @@ -149,6 +191,7 @@ struct DistanceData }; /// @brief Continuous collision data stores the continuous collision request and result given the continuous collision algorithm. +template struct ContinuousCollisionData { ContinuousCollisionData() @@ -157,36 +200,543 @@ struct ContinuousCollisionData } /// @brief Continuous collision request - ContinuousCollisionRequest request; + ContinuousCollisionRequest request; /// @brief Continuous collision result - ContinuousCollisionResult result; + ContinuousCollisionResult result; /// @brief Whether the continuous collision iteration can stop bool done; }; - - /// @brief Default collision callback for two objects o1 and o2 in broad phase. return value means whether the broad phase can stop now. -bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2, void* cdata); +template +bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2, void* cdata_); /// @brief Default distance callback for two objects o1 and o2 in broad phase. return value means whether the broad phase can stop now. also return dist, i.e. the bmin distance till now -bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2, void* cdata, FCL_REAL& dist); +template +bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2, void* cdata_, S& dist); +template +bool defaultContinuousCollisionFunction(ContinuousCollisionObject* o1, ContinuousCollisionObject* o2, void* cdata_); +template +bool defaultContinuousDistanceFunction(ContinuousCollisionObject* o1, ContinuousCollisionObject* o2, void* cdata_, S& dist); +std::string getNodeTypeName(NODE_TYPE node_type); +std::string getGJKSolverName(GJKSolverType solver_type); -bool defaultContinuousCollisionFunction(ContinuousCollisionObject* o1, ContinuousCollisionObject* o2, void* cdata_); +#if FCL_HAVE_OCTOMAP -bool defaultContinuousDistanceFunction(ContinuousCollisionObject* o1, ContinuousCollisionObject* o2, void* cdata_, FCL_REAL& dist); +/// @brief Generate boxes from the octomap +template +void generateBoxesFromOctomap(std::vector*>& env, OcTree& tree); +/// @brief Generate boxes from the octomap +template +void generateBoxesFromOctomapMesh(std::vector*>& env, OcTree& tree); -std::string getNodeTypeName(NODE_TYPE node_type); +/// @brief Generate an octree +octomap::OcTree* generateOcTree(double resolution = 0.1); -std::string getGJKSolverName(GJKSolverType solver_type); +#endif +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +void loadOBJFile(const char* filename, std::vector>& points, std::vector& triangles) +{ + + FILE* file = fopen(filename, "rb"); + if(!file) + { + std::cerr << "file not exist" << std::endl; + return; + } + + bool has_normal = false; + bool has_texture = false; + char line_buffer[2000]; + while(fgets(line_buffer, 2000, file)) + { + char* first_token = strtok(line_buffer, "\r\n\t "); + if(!first_token || first_token[0] == '#' || first_token[0] == 0) + continue; + + switch(first_token[0]) + { + case 'v': + { + if(first_token[1] == 'n') + { + strtok(nullptr, "\t "); + strtok(nullptr, "\t "); + strtok(nullptr, "\t "); + has_normal = true; + } + else if(first_token[1] == 't') + { + strtok(nullptr, "\t "); + strtok(nullptr, "\t "); + has_texture = true; + } + else + { + S x = (S)atof(strtok(nullptr, "\t ")); + S y = (S)atof(strtok(nullptr, "\t ")); + S z = (S)atof(strtok(nullptr, "\t ")); + points.emplace_back(x, y, z); + } + } + break; + case 'f': + { + Triangle tri; + char* data[30]; + int n = 0; + while((data[n] = strtok(nullptr, "\t \r\n")) != nullptr) + { + if(strlen(data[n])) + n++; + } + + for(int t = 0; t < (n - 2); ++t) + { + if((!has_texture) && (!has_normal)) + { + tri[0] = atoi(data[0]) - 1; + tri[1] = atoi(data[1]) - 1; + tri[2] = atoi(data[2]) - 1; + } + else + { + const char *v1; + for(int i = 0; i < 3; i++) + { + // vertex ID + if(i == 0) + v1 = data[0]; + else + v1 = data[t + i]; + + tri[i] = atoi(v1) - 1; + } + } + triangles.push_back(tri); + } + } + } + } } +//============================================================================== +template +void saveOBJFile(const char* filename, std::vector>& points, std::vector& triangles) +{ + std::ofstream os(filename); + if(!os) + { + std::cerr << "file not exist" << std::endl; + return; + } + + for(std::size_t i = 0; i < points.size(); ++i) + { + os << "v " << points[i][0] << " " << points[i][1] << " " << points[i][2] << std::endl; + } + + for(std::size_t i = 0; i < triangles.size(); ++i) + { + os << "f " << triangles[i][0] + 1 << " " << triangles[i][1] + 1 << " " << triangles[i][2] + 1 << std::endl; + } + + os.close(); +} + +//============================================================================== +template +S rand_interval(S rmin, S rmax) +{ + S t = rand() / ((S)RAND_MAX + 1); + return (t * (rmax - rmin) + rmin); +} + +//============================================================================== +template +void eulerToMatrix(S a, S b, S c, Matrix3& R) +{ + auto c1 = std::cos(a); + auto c2 = std::cos(b); + auto c3 = std::cos(c); + auto s1 = std::sin(a); + auto s2 = std::sin(b); + auto s3 = std::sin(c); + + R << c1 * c2, - c2 * s1, s2, + c3 * s1 + c1 * s2 * s3, c1 * c3 - s1 * s2 * s3, - c2 * s3, + s1 * s3 - c1 * c3 * s2, c3 * s1 * s2 + c1 * s3, c2 * c3; +} + +//============================================================================== +template +void generateRandomTransform(const std::array& extents, Transform3& transform) +{ + auto x = rand_interval(extents[0], extents[3]); + auto y = rand_interval(extents[1], extents[4]); + auto z = rand_interval(extents[2], extents[5]); + + const auto pi = constants::pi(); + auto a = rand_interval((S)0, 2 * pi); + auto b = rand_interval((S)0, 2 * pi); + auto c = rand_interval((S)0, 2 * pi); + + Matrix3 R; + eulerToMatrix(a, b, c, R); + Vector3 T(x, y, z); + transform.linear() = R; + transform.translation() = T; +} + +//============================================================================== +template +void generateRandomTransforms(S extents[6], Eigen::aligned_vector>& transforms, std::size_t n) +{ + transforms.resize(n); + for(std::size_t i = 0; i < n; ++i) + { + auto x = rand_interval(extents[0], extents[3]); + auto y = rand_interval(extents[1], extents[4]); + auto z = rand_interval(extents[2], extents[5]); + + const auto pi = constants::pi(); + auto a = rand_interval((S)0, 2 * pi); + auto b = rand_interval((S)0, 2 * pi); + auto c = rand_interval((S)0, 2 * pi); + + { + Matrix3 R; + eulerToMatrix(a, b, c, R); + Vector3 T(x, y, z); + transforms[i].setIdentity(); + transforms[i].linear() = R; + transforms[i].translation() = T; + } + } +} + +//============================================================================== +template +void generateRandomTransforms(S extents[6], S delta_trans[3], S delta_rot, Eigen::aligned_vector>& transforms, Eigen::aligned_vector>& transforms2, std::size_t n) +{ + transforms.resize(n); + transforms2.resize(n); + for(std::size_t i = 0; i < n; ++i) + { + auto x = rand_interval(extents[0], extents[3]); + auto y = rand_interval(extents[1], extents[4]); + auto z = rand_interval(extents[2], extents[5]); + + const auto pi = constants::pi(); + auto a = rand_interval((S)0, 2 * pi); + auto b = rand_interval((S)0, 2 * pi); + auto c = rand_interval((S)0, 2 * pi); + + { + Matrix3 R; + eulerToMatrix(a, b, c, R); + Vector3 T(x, y, z); + transforms[i].setIdentity(); + transforms[i].linear() = R; + transforms[i].translation() = T; + } + + auto deltax = rand_interval(-delta_trans[0], delta_trans[0]); + auto deltay = rand_interval(-delta_trans[1], delta_trans[1]); + auto deltaz = rand_interval(-delta_trans[2], delta_trans[2]); + + auto deltaa = rand_interval(-delta_rot, delta_rot); + auto deltab = rand_interval(-delta_rot, delta_rot); + auto deltac = rand_interval(-delta_rot, delta_rot); + + { + Matrix3 R; + eulerToMatrix(a + deltaa, b + deltab, c + deltac, R); + Vector3 T(x + deltax, y + deltay, z + deltaz); + transforms2[i].setIdentity(); + transforms2[i].linear() = R; + transforms2[i].translation() = T; + } + } +} + +//============================================================================== +template +void generateRandomTransforms_ccd(S extents[6], Eigen::aligned_vector>& transforms, Eigen::aligned_vector>& transforms2, S delta_trans[3], S delta_rot, std::size_t n, + const std::vector>& vertices1, const std::vector& triangles1, + const std::vector>& vertices2, const std::vector& triangles2) +{ + transforms.resize(n); + transforms2.resize(n); + + for(std::size_t i = 0; i < n;) + { + auto x = rand_interval(extents[0], extents[3]); + auto y = rand_interval(extents[1], extents[4]); + auto z = rand_interval(extents[2], extents[5]); + + const auto pi = constants::pi(); + auto a = rand_interval(0, 2 * pi); + auto b = rand_interval(0, 2 * pi); + auto c = rand_interval(0, 2 * pi); + + + Matrix3 R; + eulerToMatrix(a, b, c, R); + Vector3 T(x, y, z); + Transform3 tf(Transform3::Identity()); + tf.linear() = R; + tf.translation() = T; + + std::vector > results; + { + transforms[i] = tf; + + auto deltax = rand_interval(-delta_trans[0], delta_trans[0]); + auto deltay = rand_interval(-delta_trans[1], delta_trans[1]); + auto deltaz = rand_interval(-delta_trans[2], delta_trans[2]); + + auto deltaa = rand_interval(-delta_rot, delta_rot); + auto deltab = rand_interval(-delta_rot, delta_rot); + auto deltac = rand_interval(-delta_rot, delta_rot); + + Matrix3 R2; + eulerToMatrix(a + deltaa, b + deltab, c + deltac, R2); + Vector3 T2(x + deltax, y + deltay, z + deltaz); + transforms2[i].linear() = R2; + transforms2[i].translation() = T2; + ++i; + } + } +} + +//============================================================================== +template +void generateEnvironments(std::vector*>& env, S env_scale, std::size_t n) +{ + S extents[] = {-env_scale, env_scale, -env_scale, env_scale, -env_scale, env_scale}; + Eigen::aligned_vector> transforms; + + generateRandomTransforms(extents, transforms, n); + for(std::size_t i = 0; i < n; ++i) + { + Box* box = new Box(5, 10, 20); + env.push_back(new CollisionObject(std::shared_ptr>(box), transforms[i])); + } + + generateRandomTransforms(extents, transforms, n); + for(std::size_t i = 0; i < n; ++i) + { + Sphere* sphere = new Sphere(30); + env.push_back(new CollisionObject(std::shared_ptr>(sphere), transforms[i])); + } + + generateRandomTransforms(extents, transforms, n); + for(std::size_t i = 0; i < n; ++i) + { + Cylinder* cylinder = new Cylinder(10, 40); + env.push_back(new CollisionObject(std::shared_ptr>(cylinder), transforms[i])); + } +} + +//============================================================================== +template +void generateEnvironmentsMesh(std::vector*>& env, S env_scale, std::size_t n) +{ + S extents[] = {-env_scale, env_scale, -env_scale, env_scale, -env_scale, env_scale}; + Eigen::aligned_vector> transforms; + + generateRandomTransforms(extents, transforms, n); + Box box(5, 10, 20); + for(std::size_t i = 0; i < n; ++i) + { + BVHModel>* model = new BVHModel>(); + generateBVHModel(*model, box, Transform3::Identity()); + env.push_back(new CollisionObject(std::shared_ptr>(model), transforms[i])); + } + + generateRandomTransforms(extents, transforms, n); + Sphere sphere(30); + for(std::size_t i = 0; i < n; ++i) + { + BVHModel>* model = new BVHModel>(); + generateBVHModel(*model, sphere, Transform3::Identity(), 16, 16); + env.push_back(new CollisionObject(std::shared_ptr>(model), transforms[i])); + } + + generateRandomTransforms(extents, transforms, n); + Cylinder cylinder(10, 40); + for(std::size_t i = 0; i < n; ++i) + { + BVHModel>* model = new BVHModel>(); + generateBVHModel(*model, cylinder, Transform3::Identity(), 16, 16); + env.push_back(new CollisionObject(std::shared_ptr>(model), transforms[i])); + } +} + +//============================================================================== +template +bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2, void* cdata_) +{ + auto* cdata = static_cast*>(cdata_); + const auto& request = cdata->request; + auto& result = cdata->result; + + if(cdata->done) return true; + + collide(o1, o2, request, result); + + if(!request.enable_cost && (result.isCollision()) && (result.numContacts() >= request.num_max_contacts)) + cdata->done = true; + + return cdata->done; +} + +//============================================================================== +template +bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2, void* cdata_, S& dist) +{ + auto* cdata = static_cast*>(cdata_); + const DistanceRequest& request = cdata->request; + DistanceResult& result = cdata->result; + + if(cdata->done) { dist = result.min_distance; return true; } + + distance(o1, o2, request, result); + + dist = result.min_distance; + + if(dist <= 0) return true; // in collision or in touch + + return cdata->done; +} + +//============================================================================== +template +bool defaultContinuousCollisionFunction(ContinuousCollisionObject* o1, ContinuousCollisionObject* o2, void* cdata_) +{ + auto* cdata = static_cast*>(cdata_); + const ContinuousCollisionRequest& request = cdata->request; + ContinuousCollisionResult& result = cdata->result; + + if(cdata->done) return true; + + collide(o1, o2, request, result); + + return cdata->done; +} + +//============================================================================== +template +bool defaultContinuousDistanceFunction(ContinuousCollisionObject* o1, ContinuousCollisionObject* o2, void* cdata_, S& dist) +{ + return true; +} + +#if FCL_HAVE_OCTOMAP + +//============================================================================== +template +void generateBoxesFromOctomap(std::vector*>& boxes, OcTree& tree) +{ + std::vector > boxes_ = tree.toBoxes(); + + for(std::size_t i = 0; i < boxes_.size(); ++i) + { + S x = boxes_[i][0]; + S y = boxes_[i][1]; + S z = boxes_[i][2]; + S size = boxes_[i][3]; + S cost = boxes_[i][4]; + S threshold = boxes_[i][5]; + + Box* box = new Box(size, size, size); + box->cost_density = cost; + box->threshold_occupied = threshold; + CollisionObject* obj = new CollisionObject(std::shared_ptr>(box), Transform3(Translation3(Vector3(x, y, z)))); + boxes.push_back(obj); + } + + std::cout << "boxes size: " << boxes.size() << std::endl; + +} + +//============================================================================== +template +void generateBoxesFromOctomapMesh(std::vector*>& boxes, OcTree& tree) +{ + std::vector > boxes_ = tree.toBoxes(); + + for(std::size_t i = 0; i < boxes_.size(); ++i) + { + S x = boxes_[i][0]; + S y = boxes_[i][1]; + S z = boxes_[i][2]; + S size = boxes_[i][3]; + S cost = boxes_[i][4]; + S threshold = boxes_[i][5]; + + Box box(size, size, size); + BVHModel>* model = new BVHModel>(); + generateBVHModel(*model, box, Transform3::Identity()); + model->cost_density = cost; + model->threshold_occupied = threshold; + CollisionObject* obj = new CollisionObject(std::shared_ptr>(model), Transform3(Translation3(Vector3(x, y, z)))); + boxes.push_back(obj); + } + + std::cout << "boxes size: " << boxes.size() << std::endl; +} + +//============================================================================== +inline octomap::OcTree* generateOcTree(double resolution) +{ + octomap::OcTree* tree = new octomap::OcTree(resolution); + + // insert some measurements of occupied cells + for(int x = -20; x < 20; x++) + { + for(int y = -20; y < 20; y++) + { + for(int z = -20; z < 20; z++) + { + tree->updateNode(octomap::point3d(x * 0.05, y * 0.05, z * 0.05), true); + } + } + } + + // insert some measurements of free cells + for(int x = -30; x < 30; x++) + { + for(int y = -30; y < 30; y++) + { + for(int z = -30; z < 30; z++) + { + tree->updateNode(octomap::point3d(x*0.02 -1.0, y*0.02-1.0, z*0.02-1.0), false); + } + } + } + + return tree; +} + +#endif // FCL_HAVE_OCTOMAP + +} // namespace fcl + #endif