diff --git a/include/fcl/CMakeLists.txt b/include/fcl/CMakeLists.txt
index eeba55a1abac7002f8e4686d74496ee7afe4a9f7..723bd6d0089847e36bec320cead75eb30b992324 100644
--- a/include/fcl/CMakeLists.txt
+++ b/include/fcl/CMakeLists.txt
@@ -1,3 +1,7 @@
+file(GLOB_RECURSE HEADERS ${CMAKE_CURRENT_SOURCE_DIR}/*.h)
+file(GLOB_RECURSE CONFIGURED_HEADERS ${CMAKE_CURRENT_BINARY_DIR}/*.h)
+set(FCL_HEADERS ${HEADERS} ${CONFIGURED_HEADERS} PARENT_SCOPE)
+
file(TO_NATIVE_PATH "${CMAKE_CURRENT_SOURCE_DIR}" FCL_CONFIG_IN_DIR)
file(TO_NATIVE_PATH "${CMAKE_CURRENT_BINARY_DIR}" FCL_CONFIG_OUT_DIR)
configure_file("${FCL_CONFIG_IN_DIR}/config.h.in" "${FCL_CONFIG_OUT_DIR}/config.h")
diff --git a/include/fcl/narrowphase/narrowphase.h b/include/fcl/narrowphase/narrowphase.h
index 5c8132685972117fd442c14c2fd94dfaebb47ab2..532abf711020f1815e56f73cccc022570f49719a 100644
--- a/include/fcl/narrowphase/narrowphase.h
+++ b/include/fcl/narrowphase/narrowphase.h
@@ -249,6 +249,11 @@ bool GJKSolver_libccd::shapeIntersect<Sphere, Capsule>(const Sphere& s1, const T
const Capsule& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+template<>
+bool GJKSolver_libccd::shapeIntersect<Capsule, Sphere>(const Capsule &s1, const Transform3f& tf1,
+ const Sphere &s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
/// @brief Fast implementation for sphere-sphere collision
template<>
bool GJKSolver_libccd::shapeIntersect<Sphere, Sphere>(const Sphere& s1, const Transform3f& tf1,
@@ -266,26 +271,51 @@ bool GJKSolver_libccd::shapeIntersect<Sphere, Halfspace>(const Sphere& s1, const
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+template<>
+bool GJKSolver_libccd::shapeIntersect<Halfspace, Sphere>(const Halfspace& s1, const Transform3f& tf1,
+ const Sphere& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
template<>
bool GJKSolver_libccd::shapeIntersect<Box, Halfspace>(const Box& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+template<>
+bool GJKSolver_libccd::shapeIntersect<Halfspace, Box>(const Halfspace& s1, const Transform3f& tf1,
+ const Box& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
template<>
bool GJKSolver_libccd::shapeIntersect<Capsule, Halfspace>(const Capsule& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+template<>
+bool GJKSolver_libccd::shapeIntersect<Halfspace, Capsule>(const Halfspace& s1, const Transform3f& tf1,
+ const Capsule& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
template<>
bool GJKSolver_libccd::shapeIntersect<Cylinder, Halfspace>(const Cylinder& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+template<>
+bool GJKSolver_libccd::shapeIntersect<Halfspace, Cylinder>(const Halfspace& s1, const Transform3f& tf1,
+ const Cylinder& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
template<>
bool GJKSolver_libccd::shapeIntersect<Cone, Halfspace>(const Cone& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+template<>
+bool GJKSolver_libccd::shapeIntersect<Halfspace, Cone>(const Halfspace& s1, const Transform3f& tf1,
+ const Cone& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
template<>
bool GJKSolver_libccd::shapeIntersect<Halfspace, Halfspace>(const Halfspace& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
@@ -296,35 +326,60 @@ bool GJKSolver_libccd::shapeIntersect<Plane, Halfspace>(const Plane& s1, const T
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+template<>
+bool GJKSolver_libccd::shapeIntersect<Halfspace, Plane>(const Halfspace& s1, const Transform3f& tf1,
+ const Plane& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
template<>
bool GJKSolver_libccd::shapeIntersect<Sphere, Plane>(const Sphere& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+template<>
+bool GJKSolver_libccd::shapeIntersect<Plane, Sphere>(const Plane& s1, const Transform3f& tf1,
+ const Sphere& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
template<>
bool GJKSolver_libccd::shapeIntersect<Box, Plane>(const Box& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+template<>
+bool GJKSolver_libccd::shapeIntersect<Plane, Box>(const Plane& s1, const Transform3f& tf1,
+ const Box& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
template<>
bool GJKSolver_libccd::shapeIntersect<Capsule, Plane>(const Capsule& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+template<>
+bool GJKSolver_libccd::shapeIntersect<Plane, Capsule>(const Plane& s1, const Transform3f& tf1,
+ const Capsule& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
template<>
bool GJKSolver_libccd::shapeIntersect<Cylinder, Plane>(const Cylinder& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+template<>
+bool GJKSolver_libccd::shapeIntersect<Plane, Cylinder>(const Plane& s1, const Transform3f& tf1,
+ const Cylinder& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
template<>
bool GJKSolver_libccd::shapeIntersect<Cone, Plane>(const Cone& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
-bool GJKSolver_libccd::shapeIntersect<Halfspace, Plane>(const Halfspace& s1, const Transform3f& tf1,
- const Plane& s2, const Transform3f& tf2,
- Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+bool GJKSolver_libccd::shapeIntersect<Plane, Cone>(const Plane& s1, const Transform3f& tf1,
+ const Cone& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_libccd::shapeIntersect<Plane, Plane>(const Plane& s1, const Transform3f& tf1,
@@ -356,12 +411,23 @@ bool GJKSolver_libccd::shapeDistance<Sphere, Capsule>(const Sphere& s1, const Tr
const Capsule& s2, const Transform3f& tf2,
FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
+template<>
+bool GJKSolver_libccd::shapeDistance<Capsule, Sphere>(const Capsule& s1, const Transform3f& tf1,
+ const Sphere& s2, const Transform3f& tf2,
+ FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
+
/// @brief Fast implementation for sphere-sphere distance
template<>
bool GJKSolver_libccd::shapeDistance<Sphere, Sphere>(const Sphere& s1, const Transform3f& tf1,
const Sphere& s2, const Transform3f& tf2,
FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
+// @brief Computation of the distance result for capsule capsule. Closest points are based on two line-segments.
+template<>
+bool GJKSolver_libccd::shapeDistance<Capsule, Capsule>(const Capsule& s1, const Transform3f& tf1,
+ const Capsule& s2, const Transform3f& tf2,
+ FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
+
/// @brief Fast implementation for sphere-triangle distance
template<>
bool GJKSolver_libccd::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf,
@@ -373,12 +439,6 @@ template<>
bool GJKSolver_libccd::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf1,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
-// @brief Computation of the distance result for capsule capsule. Closest points are based on two line-segments.
-template<>
-bool GJKSolver_libccd::shapeDistance<Capsule, Capsule>(const Capsule& s1, const Transform3f& tf1,
- const Capsule& s2, const Transform3f& tf2,
- FCL_REAL* dist, Vec3f* p1, Vec3f* 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
@@ -732,18 +792,24 @@ struct GJKSolver_indep
mutable Vec3f cached_guess;
};
+/// @brief Fast implementation for sphere-capsule collision
template<>
-bool GJKSolver_indep::shapeIntersect<Sphere, Capsule>(const Sphere &s1, const Transform3f& tf1,
- const Capsule &s2, const Transform3f& tf2,
+bool GJKSolver_indep::shapeIntersect<Sphere, Capsule>(const Sphere& s1, const Transform3f& tf1,
+ const Capsule& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
-/// @brief Fast implementation for sphere-sphere collision
+template<>
+bool GJKSolver_indep::shapeIntersect<Capsule, Sphere>(const Capsule &s1, const Transform3f& tf1,
+ const Sphere &s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
+/// @brief Fast implementation for sphere-sphere collision
template<>
bool GJKSolver_indep::shapeIntersect<Sphere, Sphere>(const Sphere& s1, const Transform3f& tf1,
const Sphere& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
-/// @brief Fast implementation for box-box collision
+/// @brief Fast implementation for box-box collision
template<>
bool GJKSolver_indep::shapeIntersect<Box, Box>(const Box& s1, const Transform3f& tf1,
const Box& s2, const Transform3f& tf2,
@@ -754,26 +820,51 @@ bool GJKSolver_indep::shapeIntersect<Sphere, Halfspace>(const Sphere& s1, const
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+template<>
+bool GJKSolver_indep::shapeIntersect<Halfspace, Sphere>(const Halfspace& s1, const Transform3f& tf1,
+ const Sphere& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
template<>
bool GJKSolver_indep::shapeIntersect<Box, Halfspace>(const Box& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+template<>
+bool GJKSolver_indep::shapeIntersect<Halfspace, Box>(const Halfspace& s1, const Transform3f& tf1,
+ const Box& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
template<>
bool GJKSolver_indep::shapeIntersect<Capsule, Halfspace>(const Capsule& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+template<>
+bool GJKSolver_indep::shapeIntersect<Halfspace, Capsule>(const Halfspace& s1, const Transform3f& tf1,
+ const Capsule& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
template<>
bool GJKSolver_indep::shapeIntersect<Cylinder, Halfspace>(const Cylinder& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+template<>
+bool GJKSolver_indep::shapeIntersect<Halfspace, Cylinder>(const Halfspace& s1, const Transform3f& tf1,
+ const Cylinder& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
template<>
bool GJKSolver_indep::shapeIntersect<Cone, Halfspace>(const Cone& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+template<>
+bool GJKSolver_indep::shapeIntersect<Halfspace, Cone>(const Halfspace& s1, const Transform3f& tf1,
+ const Cone& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
template<>
bool GJKSolver_indep::shapeIntersect<Halfspace, Halfspace>(const Halfspace& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
@@ -784,35 +875,60 @@ bool GJKSolver_indep::shapeIntersect<Plane, Halfspace>(const Plane& s1, const Tr
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+template<>
+bool GJKSolver_indep::shapeIntersect<Halfspace, Plane>(const Halfspace& s1, const Transform3f& tf1,
+ const Plane& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
template<>
bool GJKSolver_indep::shapeIntersect<Sphere, Plane>(const Sphere& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+template<>
+bool GJKSolver_indep::shapeIntersect<Plane, Sphere>(const Plane& s1, const Transform3f& tf1,
+ const Sphere& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
template<>
bool GJKSolver_indep::shapeIntersect<Box, Plane>(const Box& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+template<>
+bool GJKSolver_indep::shapeIntersect<Plane, Box>(const Plane& s1, const Transform3f& tf1,
+ const Box& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
template<>
bool GJKSolver_indep::shapeIntersect<Capsule, Plane>(const Capsule& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+template<>
+bool GJKSolver_indep::shapeIntersect<Plane, Capsule>(const Plane& s1, const Transform3f& tf1,
+ const Capsule& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
template<>
bool GJKSolver_indep::shapeIntersect<Cylinder, Plane>(const Cylinder& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+template<>
+bool GJKSolver_indep::shapeIntersect<Plane, Cylinder>(const Plane& s1, const Transform3f& tf1,
+ const Cylinder& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
template<>
bool GJKSolver_indep::shapeIntersect<Cone, Plane>(const Cone& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
-bool GJKSolver_indep::shapeIntersect<Halfspace, Plane>(const Halfspace& s1, const Transform3f& tf1,
- const Plane& s2, const Transform3f& tf2,
- Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+bool GJKSolver_indep::shapeIntersect<Plane, Cone>(const Plane& s1, const Transform3f& tf1,
+ const Cone& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_indep::shapeIntersect<Plane, Plane>(const Plane& s1, const Transform3f& tf1,
@@ -820,15 +936,16 @@ bool GJKSolver_indep::shapeIntersect<Plane, Plane>(const Plane& s1, const Transf
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
/// @brief Fast implementation for sphere-triangle collision
-template<>
+template<>
bool GJKSolver_indep::shapeTriangleIntersect(const Sphere& s, const Transform3f& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
/// @brief Fast implementation for sphere-triangle collision
-template<>
+template<>
bool GJKSolver_indep::shapeTriangleIntersect(const Sphere& s, const Transform3f& tf1,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+
template<>
bool GJKSolver_indep::shapeTriangleIntersect(const Halfspace& s, const Transform3f& tf1,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
@@ -837,18 +954,16 @@ template<>
bool GJKSolver_indep::shapeTriangleIntersect(const Plane& s, const Transform3f& tf1,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
-
+/// @brief Fast implementation for sphere-capsule distance
template<>
bool GJKSolver_indep::shapeDistance<Sphere, Capsule>(const Sphere& s1, const Transform3f& tf1,
const Capsule& s2, const Transform3f& tf2,
FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
-// @brief Computation of the distance result for capsule capsule. Closest points are based on two line-segments.
- template<>
- bool GJKSolver_indep::shapeDistance<Capsule, Capsule>(const Capsule& s1, const Transform3f& tf1,
- const Capsule& s2, const Transform3f& tf2,
- FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
-
+template<>
+bool GJKSolver_indep::shapeDistance<Capsule, Sphere>(const Capsule& s1, const Transform3f& tf1,
+ const Sphere& s2, const Transform3f& tf2,
+ FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
/// @brief Fast implementation for sphere-sphere distance
template<>
@@ -856,19 +971,24 @@ bool GJKSolver_indep::shapeDistance<Sphere, Sphere>(const Sphere& s1, const Tran
const Sphere& s2, const Transform3f& tf2,
FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
+// @brief Computation of the distance result for capsule capsule. Closest points are based on two line-segments.
+template<>
+bool GJKSolver_indep::shapeDistance<Capsule, Capsule>(const Capsule& s1, const Transform3f& tf1,
+ const Capsule& s2, const Transform3f& tf2,
+ FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
+
/// @brief Fast implementation for sphere-triangle distance
template<>
bool GJKSolver_indep::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf,
- const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
+ const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
/// @brief Fast implementation for sphere-triangle distance
-template<>
-bool GJKSolver_indep::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf1,
+template<>
+bool GJKSolver_indep::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf1,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
-
}
#endif
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 33cb7fa1e56615cc1ffef094790c5e26f6e17015..05a002014e39cae8df367d55a3957e2604664358 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -1,8 +1,8 @@
file(GLOB_RECURSE FCL_SOURCE_CODE ${CMAKE_CURRENT_SOURCE_DIR}/*.cpp)
if(FCL_STATIC_LIBRARY)
- add_library(${PROJECT_NAME} STATIC ${FCL_SOURCE_CODE})
+ add_library(${PROJECT_NAME} STATIC ${FCL_HEADERS} ${FCL_SOURCE_CODE})
else()
- add_library(${PROJECT_NAME} SHARED ${FCL_SOURCE_CODE})
+ add_library(${PROJECT_NAME} SHARED ${FCL_HEADERS} ${FCL_SOURCE_CODE})
endif()
target_link_libraries(${PROJECT_NAME} ${CCD_LIBRARIES} ${OCTOMAP_LIBRARIES} ${Boost_LIBRARIES})
diff --git a/src/narrowphase/narrowphase.cpp b/src/narrowphase/narrowphase.cpp
index 61be491b015c284e935462b49f63ad80133b4737..2801cc666f0cbe7580411ad4293beb655efb2aca 100644
--- a/src/narrowphase/narrowphase.cpp
+++ b/src/narrowphase/narrowphase.cpp
@@ -2555,12 +2555,44 @@ bool planeIntersect(const Plane& s1, const Transform3f& tf1,
} // details
+// Shape intersect algorithms not using libccd
+//
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// | | box | sphere | capsule | cone | cylinder | plane | half-space | triangle |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// | box | O | | | | | O | O | |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// | sphere |/////| O | O | | | O | O | O |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// | capsule |/////|////////| | | | O | O | |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// | cone |/////|////////|/////////| | | O | O | |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// | cylinder |/////|////////|/////////|//////| | O | O | |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// | plane |/////|////////|/////////|//////|//////////| O | O | O |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// | half-space |/////|////////|/////////|//////|//////////|///////| O | O |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// | triangle |/////|////////|/////////|//////|//////////|///////|////////////| |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+
template<>
bool GJKSolver_libccd::shapeIntersect<Sphere, Capsule>(const Sphere &s1, const Transform3f& tf1,
const Capsule &s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
- return details::sphereCapsuleIntersect (s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
+ return details::sphereCapsuleIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
+}
+
+template<>
+bool GJKSolver_libccd::shapeIntersect<Capsule, Sphere>(const Capsule &s1, const Transform3f& tf1,
+ const Sphere &s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+{
+ const bool res = details::sphereCapsuleIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
+ if (normal) (*normal) *= -1.0;
+ return res;
}
template<>
@@ -2587,6 +2619,16 @@ bool GJKSolver_libccd::shapeIntersect<Sphere, Halfspace>(const Sphere& s1, const
return details::sphereHalfspaceIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
}
+template<>
+bool GJKSolver_libccd::shapeIntersect<Halfspace, Sphere>(const Halfspace& s1, const Transform3f& tf1,
+ const Sphere& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+{
+ const bool res = details::sphereHalfspaceIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
+ if (normal) (*normal) *= -1.0;
+ return res;
+}
+
template<>
bool GJKSolver_libccd::shapeIntersect<Box, Halfspace>(const Box& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
@@ -2595,6 +2637,16 @@ bool GJKSolver_libccd::shapeIntersect<Box, Halfspace>(const Box& s1, const Trans
return details::boxHalfspaceIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
}
+template<>
+bool GJKSolver_libccd::shapeIntersect<Halfspace, Box>(const Halfspace& s1, const Transform3f& tf1,
+ const Box& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+{
+ const bool res = details::boxHalfspaceIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
+ if (normal) (*normal) *= -1.0;
+ return res;
+}
+
template<>
bool GJKSolver_libccd::shapeIntersect<Capsule, Halfspace>(const Capsule& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
@@ -2603,6 +2655,16 @@ bool GJKSolver_libccd::shapeIntersect<Capsule, Halfspace>(const Capsule& s1, con
return details::capsuleHalfspaceIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
}
+template<>
+bool GJKSolver_libccd::shapeIntersect<Halfspace, Capsule>(const Halfspace& s1, const Transform3f& tf1,
+ const Capsule& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+{
+ const bool res = details::capsuleHalfspaceIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
+ if (normal) (*normal) *= -1.0;
+ return res;
+}
+
template<>
bool GJKSolver_libccd::shapeIntersect<Cylinder, Halfspace>(const Cylinder& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
@@ -2611,6 +2673,16 @@ bool GJKSolver_libccd::shapeIntersect<Cylinder, Halfspace>(const Cylinder& s1, c
return details::cylinderHalfspaceIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
}
+template<>
+bool GJKSolver_libccd::shapeIntersect<Halfspace, Cylinder>(const Halfspace& s1, const Transform3f& tf1,
+ const Cylinder& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+{
+ const bool res = details::cylinderHalfspaceIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
+ if (normal) (*normal) *= -1.0;
+ return res;
+}
+
template<>
bool GJKSolver_libccd::shapeIntersect<Cone, Halfspace>(const Cone& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
@@ -2619,6 +2691,16 @@ bool GJKSolver_libccd::shapeIntersect<Cone, Halfspace>(const Cone& s1, const Tra
return details::coneHalfspaceIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
}
+template<>
+bool GJKSolver_libccd::shapeIntersect<Halfspace, Cone>(const Halfspace& s1, const Transform3f& tf1,
+ const Cone& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+{
+ const bool res = details::coneHalfspaceIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
+ if (normal) (*normal) *= -1.0;
+ return res;
+}
+
template<>
bool GJKSolver_libccd::shapeIntersect<Halfspace, Halfspace>(const Halfspace& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
@@ -2643,6 +2725,18 @@ bool GJKSolver_libccd::shapeIntersect<Plane, Halfspace>(const Plane& s1, const T
return details::planeHalfspaceIntersect(s1, tf1, s2, tf2, pl, p, d, depth, ret);
}
+template<>
+bool GJKSolver_libccd::shapeIntersect<Halfspace, Plane>(const Halfspace& s1, const Transform3f& tf1,
+ const Plane& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+{
+ Plane pl;
+ Vec3f p, d;
+ FCL_REAL depth;
+ int ret;
+ return details::halfspacePlaneIntersect(s1, tf1, s2, tf2, pl, p, d, depth, ret);
+}
+
template<>
bool GJKSolver_libccd::shapeIntersect<Sphere, Plane>(const Sphere& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
@@ -2651,6 +2745,16 @@ bool GJKSolver_libccd::shapeIntersect<Sphere, Plane>(const Sphere& s1, const Tra
return details::spherePlaneIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
}
+template<>
+bool GJKSolver_libccd::shapeIntersect<Plane, Sphere>(const Plane& s1, const Transform3f& tf1,
+ const Sphere& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+{
+ const bool res = details::spherePlaneIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
+ if (normal) (*normal) *= -1.0;
+ return res;
+}
+
template<>
bool GJKSolver_libccd::shapeIntersect<Box, Plane>(const Box& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
@@ -2659,6 +2763,16 @@ bool GJKSolver_libccd::shapeIntersect<Box, Plane>(const Box& s1, const Transform
return details::boxPlaneIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
}
+template<>
+bool GJKSolver_libccd::shapeIntersect<Plane, Box>(const Plane& s1, const Transform3f& tf1,
+ const Box& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+{
+ const bool res = details::boxPlaneIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
+ if (normal) (*normal) *= -1.0;
+ return res;
+}
+
template<>
bool GJKSolver_libccd::shapeIntersect<Capsule, Plane>(const Capsule& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
@@ -2667,6 +2781,16 @@ bool GJKSolver_libccd::shapeIntersect<Capsule, Plane>(const Capsule& s1, const T
return details::capsulePlaneIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
}
+template<>
+bool GJKSolver_libccd::shapeIntersect<Plane, Capsule>(const Plane& s1, const Transform3f& tf1,
+ const Capsule& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+{
+ const bool res = details::capsulePlaneIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
+ if (normal) (*normal) *= -1.0;
+ return res;
+}
+
template<>
bool GJKSolver_libccd::shapeIntersect<Cylinder, Plane>(const Cylinder& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
@@ -2675,6 +2799,16 @@ bool GJKSolver_libccd::shapeIntersect<Cylinder, Plane>(const Cylinder& s1, const
return details::cylinderPlaneIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
}
+template<>
+bool GJKSolver_libccd::shapeIntersect<Plane, Cylinder>(const Plane& s1, const Transform3f& tf1,
+ const Cylinder& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+{
+ const bool res = details::cylinderPlaneIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
+ if (normal) (*normal) *= -1.0;
+ return res;
+}
+
template<>
bool GJKSolver_libccd::shapeIntersect<Cone, Plane>(const Cone& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
@@ -2684,15 +2818,13 @@ bool GJKSolver_libccd::shapeIntersect<Cone, Plane>(const Cone& s1, const Transfo
}
template<>
-bool GJKSolver_libccd::shapeIntersect<Halfspace, Plane>(const Halfspace& s1, const Transform3f& tf1,
- const Plane& s2, const Transform3f& tf2,
- Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+bool GJKSolver_libccd::shapeIntersect<Plane, Cone>(const Plane& s1, const Transform3f& tf1,
+ const Cone& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
- Plane pl;
- Vec3f p, d;
- FCL_REAL depth;
- int ret;
- return details::halfspacePlaneIntersect(s1, tf1, s2, tf2, pl, p, d, depth, ret);
+ const bool res = details::conePlaneIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
+ if (normal) (*normal) *= -1.0;
+ return res;
}
template<>
@@ -2704,6 +2836,8 @@ bool GJKSolver_libccd::shapeIntersect<Plane, Plane>(const Plane& s1, const Trans
}
+
+
template<>
bool GJKSolver_libccd::shapeTriangleIntersect(const Sphere& s, const Transform3f& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
@@ -2718,7 +2852,6 @@ bool GJKSolver_libccd::shapeTriangleIntersect(const Sphere& s, const Transform3f
return details::sphereTriangleIntersect(s, tf1, tf2.transform(P1), tf2.transform(P2), tf2.transform(P3), contact_points, penetration_depth, normal);
}
-
template<>
bool GJKSolver_libccd::shapeTriangleIntersect(const Halfspace& s, const Transform3f& tf1,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
@@ -2733,8 +2866,27 @@ bool GJKSolver_libccd::shapeTriangleIntersect(const Plane& s, const Transform3f&
return details::planeTriangleIntersect(s, tf1, P1, P2, P3, tf2, contact_points, penetration_depth, normal);
}
-
-
+// Shape distance algorithms not using libccd
+//
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// | | box | sphere | capsule | cone | cylinder | plane | half-space | triangle |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// | box | | | | | | | | |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// | sphere |/////| O | O | | | | | O |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// | capsule |/////|////////| O | | | | | |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// | cone |/////|////////|/////////| | | | | |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// | cylinder |/////|////////|/////////|//////| | | | |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// | plane |/////|////////|/////////|//////|//////////| | | |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// | half-space |/////|////////|/////////|//////|//////////|///////| | |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// | triangle |/////|////////|/////////|//////|//////////|///////|////////////| |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
template<>
bool GJKSolver_libccd::shapeDistance<Sphere, Capsule>(const Sphere& s1, const Transform3f& tf1,
@@ -2744,6 +2896,14 @@ bool GJKSolver_libccd::shapeDistance<Sphere, Capsule>(const Sphere& s1, const Tr
return details::sphereCapsuleDistance(s1, tf1, s2, tf2, dist, p1, p2);
}
+template<>
+bool GJKSolver_libccd::shapeDistance<Capsule, Sphere>(const Capsule& s1, const Transform3f& tf1,
+ const Sphere& s2, const Transform3f& tf2,
+ FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const
+{
+ return details::sphereCapsuleDistance(s2, tf2, s1, tf1, dist, p2, p1);
+}
+
template<>
bool GJKSolver_libccd::shapeDistance<Sphere, Sphere>(const Sphere& s1, const Transform3f& tf1,
const Sphere& s2, const Transform3f& tf2,
@@ -2752,32 +2912,71 @@ bool GJKSolver_libccd::shapeDistance<Sphere, Sphere>(const Sphere& s1, const Tra
return details::sphereSphereDistance(s1, tf1, s2, tf2, dist, p1, p2);
}
+template<>
+bool GJKSolver_libccd::shapeDistance<Capsule, Capsule>(const Capsule& s1, const Transform3f& tf1,
+ const Capsule& s2, const Transform3f& tf2,
+ FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const
+{
+ return details::capsuleCapsuleDistance(s1, tf1, s2, tf2, dist, p1, p2);
+}
+
+
+
+
template<>
bool GJKSolver_libccd::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf,
- const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
+ const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const
{
return details::sphereTriangleDistance(s, tf, P1, P2, P3, dist, p1, p2);
}
-template<>
-bool GJKSolver_libccd::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf1,
+template<>
+bool GJKSolver_libccd::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf1,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
FCL_REAL* dist, Vec3f* p1, Vec3f* 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 | capsule | cone | cylinder | plane | half-space | triangle |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// | box | O | | | | | O | O | |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// | sphere |/////| O | O | | | O | 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<Sphere, Capsule>(const Sphere &s1, const Transform3f& tf1,
const Capsule &s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
- return details::sphereCapsuleIntersect (s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
+ return details::sphereCapsuleIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
+}
+
+template<>
+bool GJKSolver_indep::shapeIntersect<Capsule, Sphere>(const Capsule &s1, const Transform3f& tf1,
+ const Sphere &s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+{
+ const bool res = details::sphereCapsuleIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
+ if (normal) (*normal) *= -1.0;
+ return res;
}
template<>
@@ -2804,6 +3003,16 @@ bool GJKSolver_indep::shapeIntersect<Sphere, Halfspace>(const Sphere& s1, const
return details::sphereHalfspaceIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
}
+template<>
+bool GJKSolver_indep::shapeIntersect<Halfspace, Sphere>(const Halfspace& s1, const Transform3f& tf1,
+ const Sphere& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+{
+ const bool res = details::sphereHalfspaceIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
+ if (normal) (*normal) *= -1.0;
+ return res;
+}
+
template<>
bool GJKSolver_indep::shapeIntersect<Box, Halfspace>(const Box& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
@@ -2812,6 +3021,16 @@ bool GJKSolver_indep::shapeIntersect<Box, Halfspace>(const Box& s1, const Transf
return details::boxHalfspaceIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
}
+template<>
+bool GJKSolver_indep::shapeIntersect<Halfspace, Box>(const Halfspace& s1, const Transform3f& tf1,
+ const Box& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+{
+ const bool res = details::boxHalfspaceIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
+ if (normal) (*normal) *= -1.0;
+ return res;
+}
+
template<>
bool GJKSolver_indep::shapeIntersect<Capsule, Halfspace>(const Capsule& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
@@ -2820,6 +3039,16 @@ bool GJKSolver_indep::shapeIntersect<Capsule, Halfspace>(const Capsule& s1, cons
return details::capsuleHalfspaceIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
}
+template<>
+bool GJKSolver_indep::shapeIntersect<Halfspace, Capsule>(const Halfspace& s1, const Transform3f& tf1,
+ const Capsule& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+{
+ const bool res = details::capsuleHalfspaceIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
+ if (normal) (*normal) *= -1.0;
+ return res;
+}
+
template<>
bool GJKSolver_indep::shapeIntersect<Cylinder, Halfspace>(const Cylinder& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
@@ -2828,6 +3057,16 @@ bool GJKSolver_indep::shapeIntersect<Cylinder, Halfspace>(const Cylinder& s1, co
return details::cylinderHalfspaceIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
}
+template<>
+bool GJKSolver_indep::shapeIntersect<Halfspace, Cylinder>(const Halfspace& s1, const Transform3f& tf1,
+ const Cylinder& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+{
+ const bool res = details::cylinderHalfspaceIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
+ if (normal) (*normal) *= -1.0;
+ return res;
+}
+
template<>
bool GJKSolver_indep::shapeIntersect<Cone, Halfspace>(const Cone& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
@@ -2836,6 +3075,16 @@ bool GJKSolver_indep::shapeIntersect<Cone, Halfspace>(const Cone& s1, const Tran
return details::coneHalfspaceIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
}
+template<>
+bool GJKSolver_indep::shapeIntersect<Halfspace, Cone>(const Halfspace& s1, const Transform3f& tf1,
+ const Cone& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+{
+ const bool res = details::coneHalfspaceIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
+ if (normal) (*normal) *= -1.0;
+ return res;
+}
+
template<>
bool GJKSolver_indep::shapeIntersect<Halfspace, Halfspace>(const Halfspace& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
@@ -2845,7 +3094,6 @@ bool GJKSolver_indep::shapeIntersect<Halfspace, Halfspace>(const Halfspace& s1,
Vec3f p, d;
FCL_REAL depth;
int ret;
-
return details::halfspaceIntersect(s1, tf1, s2, tf2, p, d, s, depth, ret);
}
@@ -2861,6 +3109,18 @@ bool GJKSolver_indep::shapeIntersect<Plane, Halfspace>(const Plane& s1, const Tr
return details::planeHalfspaceIntersect(s1, tf1, s2, tf2, pl, p, d, depth, ret);
}
+template<>
+bool GJKSolver_indep::shapeIntersect<Halfspace, Plane>(const Halfspace& s1, const Transform3f& tf1,
+ const Plane& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+{
+ Plane pl;
+ Vec3f p, d;
+ FCL_REAL depth;
+ int ret;
+ return details::halfspacePlaneIntersect(s1, tf1, s2, tf2, pl, p, d, depth, ret);
+}
+
template<>
bool GJKSolver_indep::shapeIntersect<Sphere, Plane>(const Sphere& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
@@ -2869,6 +3129,16 @@ bool GJKSolver_indep::shapeIntersect<Sphere, Plane>(const Sphere& s1, const Tran
return details::spherePlaneIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
}
+template<>
+bool GJKSolver_indep::shapeIntersect<Plane, Sphere>(const Plane& s1, const Transform3f& tf1,
+ const Sphere& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+{
+ const bool res = details::spherePlaneIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
+ if (normal) (*normal) *= -1.0;
+ return res;
+}
+
template<>
bool GJKSolver_indep::shapeIntersect<Box, Plane>(const Box& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
@@ -2877,6 +3147,16 @@ bool GJKSolver_indep::shapeIntersect<Box, Plane>(const Box& s1, const Transform3
return details::boxPlaneIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
}
+template<>
+bool GJKSolver_indep::shapeIntersect<Plane, Box>(const Plane& s1, const Transform3f& tf1,
+ const Box& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+{
+ const bool res = details::boxPlaneIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
+ if (normal) (*normal) *= -1.0;
+ return res;
+}
+
template<>
bool GJKSolver_indep::shapeIntersect<Capsule, Plane>(const Capsule& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
@@ -2885,6 +3165,16 @@ bool GJKSolver_indep::shapeIntersect<Capsule, Plane>(const Capsule& s1, const Tr
return details::capsulePlaneIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
}
+template<>
+bool GJKSolver_indep::shapeIntersect<Plane, Capsule>(const Plane& s1, const Transform3f& tf1,
+ const Capsule& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+{
+ const bool res = details::capsulePlaneIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
+ if (normal) (*normal) *= -1.0;
+ return res;
+}
+
template<>
bool GJKSolver_indep::shapeIntersect<Cylinder, Plane>(const Cylinder& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
@@ -2893,6 +3183,16 @@ bool GJKSolver_indep::shapeIntersect<Cylinder, Plane>(const Cylinder& s1, const
return details::cylinderPlaneIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
}
+template<>
+bool GJKSolver_indep::shapeIntersect<Plane, Cylinder>(const Plane& s1, const Transform3f& tf1,
+ const Cylinder& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+{
+ const bool res = details::cylinderPlaneIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
+ if (normal) (*normal) *= -1.0;
+ return res;
+}
+
template<>
bool GJKSolver_indep::shapeIntersect<Cone, Plane>(const Cone& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
@@ -2902,15 +3202,13 @@ bool GJKSolver_indep::shapeIntersect<Cone, Plane>(const Cone& s1, const Transfor
}
template<>
-bool GJKSolver_indep::shapeIntersect<Halfspace, Plane>(const Halfspace& s1, const Transform3f& tf1,
- const Plane& s2, const Transform3f& tf2,
- Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+bool GJKSolver_indep::shapeIntersect<Plane, Cone>(const Plane& s1, const Transform3f& tf1,
+ const Cone& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
- Plane pl;
- Vec3f p, d;
- FCL_REAL depth;
- int ret;
- return details::halfspacePlaneIntersect(s1, tf1, s2, tf2, pl, p, d, depth, ret);
+ const bool res = details::conePlaneIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
+ if (normal) (*normal) *= -1.0;
+ return res;
}
template<>
@@ -2922,14 +3220,16 @@ bool GJKSolver_indep::shapeIntersect<Plane, Plane>(const Plane& s1, const Transf
}
-template<>
+
+
+template<>
bool GJKSolver_indep::shapeTriangleIntersect(const Sphere& s, const Transform3f& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
return details::sphereTriangleIntersect(s, tf, P1, P2, P3, contact_points, penetration_depth, normal);
}
-template<>
+template<>
bool GJKSolver_indep::shapeTriangleIntersect(const Sphere& s, const Transform3f& tf1,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
@@ -2950,6 +3250,27 @@ bool GJKSolver_indep::shapeTriangleIntersect(const Plane& s, const Transform3f&
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 | capsule | cone | cylinder | plane | half-space | triangle |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// | box | | | | | | | | |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// | sphere |/////| O | O | | | | | O |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// | capsule |/////|////////| O | | | | | |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// | cone |/////|////////|/////////| | | | | |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// | cylinder |/////|////////|/////////|//////| | | | |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// | plane |/////|////////|/////////|//////|//////////| | | |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// | half-space |/////|////////|/////////|//////|//////////|///////| | |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// | triangle |/////|////////|/////////|//////|//////////|///////|////////////| |
+// +------------+-----+--------+---------+------+----------+-------+------------+----------+
template<>
bool GJKSolver_indep::shapeDistance<Sphere, Capsule>(const Sphere& s1, const Transform3f& tf1,
@@ -2959,6 +3280,14 @@ bool GJKSolver_indep::shapeDistance<Sphere, Capsule>(const Sphere& s1, const Tra
return details::sphereCapsuleDistance(s1, tf1, s2, tf2, dist, p1, p2);
}
+template<>
+bool GJKSolver_indep::shapeDistance<Capsule, Sphere>(const Capsule& s1, const Transform3f& tf1,
+ const Sphere& s2, const Transform3f& tf2,
+ FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const
+{
+ return details::sphereCapsuleDistance(s2, tf2, s1, tf1, dist, p2, p1);
+}
+
template<>
bool GJKSolver_indep::shapeDistance<Sphere, Sphere>(const Sphere& s1, const Transform3f& tf1,
const Sphere& s2, const Transform3f& tf2,
@@ -2968,36 +3297,30 @@ bool GJKSolver_indep::shapeDistance<Sphere, Sphere>(const Sphere& s1, const Tran
}
template<>
-bool GJKSolver_indep::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf,
- const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
- FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const
+bool GJKSolver_indep::shapeDistance<Capsule, Capsule>(const Capsule& s1, const Transform3f& tf1,
+ const Capsule& s2, const Transform3f& tf2,
+ FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const
{
- return details::sphereTriangleDistance(s, tf, P1, P2, P3, dist, p1, p2);
+ return details::capsuleCapsuleDistance(s1, tf1, s2, tf2, dist, p1, p2);
}
-template<>
-bool GJKSolver_indep::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf1,
- const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
- FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const
-{
- return details::sphereTriangleDistance(s, tf1, P1, P2, P3, tf2, dist, p1, p2);
-}
+
template<>
-bool GJKSolver_indep::shapeDistance<Capsule, Capsule>(const Capsule& s1, const Transform3f& tf1,
- const Capsule& s2, const Transform3f& tf2,
- FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const
+bool GJKSolver_indep::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf,
+ const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
+ FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const
{
- return details::capsuleCapsuleDistance(s1, tf1, s2, tf2, dist, p1, p2);
+ return details::sphereTriangleDistance(s, tf, P1, P2, P3, dist, p1, p2);
}
template<>
-bool GJKSolver_libccd::shapeDistance<Capsule, Capsule>(const Capsule& s1, const Transform3f& tf1,
- const Capsule& s2, const Transform3f& tf2,
- FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const
+bool GJKSolver_indep::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf1,
+ const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
+ FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const
{
- return details::capsuleCapsuleDistance(s1, tf1, s2, tf2, dist, p1, p2);
+ return details::sphereTriangleDistance(s, tf1, P1, P2, P3, tf2, dist, p1, p2);
}
} // fcl
diff --git a/test/test_fcl_geometric_shapes.cpp b/test/test_fcl_geometric_shapes.cpp
index 9e482e2cf2ca0da6292fdd1b4a6497558bc677a4..ea27389dd679e4334ae7a1ea4f7d90617fb74418 100644
--- a/test/test_fcl_geometric_shapes.cpp
+++ b/test/test_fcl_geometric_shapes.cpp
@@ -2752,3 +2752,177 @@ BOOST_AUTO_TEST_CASE(conecone)
+template<typename S1, typename S2>
+void testReversibleShapeIntersection(const S1& s1, const S2& s2, FCL_REAL distance)
+{
+ Transform3f tf1(Vec3f(-0.5 * distance, 0.0, 0.0));
+ Transform3f tf2(Vec3f(+0.5 * distance, 0.0, 0.0));
+
+ Vec3f contactA;
+ Vec3f contactB;
+ FCL_REAL depthA;
+ FCL_REAL depthB;
+ Vec3f normalA;
+ Vec3f normalB;
+
+ bool resA;
+ bool resB;
+
+ const double tol = 1e-6;
+
+ resA = solver1.shapeIntersect(s1, tf1, s2, tf2, &contactA, &depthA, &normalA);
+ resB = solver1.shapeIntersect(s2, tf2, s1, tf1, &contactB, &depthB, &normalB);
+
+ BOOST_CHECK(resA);
+ BOOST_CHECK(resB);
+ BOOST_CHECK(contactA.equal(contactB, tol)); // contact point should be same
+ BOOST_CHECK(normalA.equal(-normalB, tol)); // normal should be opposite
+ BOOST_CHECK_CLOSE(depthA, depthB, tol); // penetration depth should be same
+
+ resA = solver2.shapeIntersect(s1, tf1, s2, tf2, &contactA, &depthA, &normalA);
+ resB = solver2.shapeIntersect(s2, tf2, s1, tf1, &contactB, &depthB, &normalB);
+
+ BOOST_CHECK(resA);
+ BOOST_CHECK(resB);
+ BOOST_CHECK(contactA.equal(contactB, tol));
+ BOOST_CHECK(normalA.equal(-normalB, tol));
+ BOOST_CHECK_CLOSE(depthA, depthB, tol);
+}
+
+BOOST_AUTO_TEST_CASE(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 (7) -- box, sphere, capsule, cone, cylinder, plane, halfspace
+ Box box(10, 10, 10);
+ Sphere sphere(5);
+ Capsule capsule(5, 10);
+ Cone cone(5, 10);
+ Cylinder cylinder(5, 10);
+ Plane plane(Vec3f(), 0.0);
+ Halfspace halfspace(Vec3f(), 0.0);
+
+ // Use sufficiently short distance so that all the primitive shapes can intersect
+ FCL_REAL distance = 5.0;
+
+ // If new shape intersection algorithm is added for two distinct primitive
+ // shapes, uncomment associated lines. For example, box-sphere intersection
+ // algorithm is added, then uncomment box-sphere.
+
+// testReversibleShapeIntersection(box, sphere, distance);
+// testReversibleShapeIntersection(box, capsule, distance);
+// testReversibleShapeIntersection(box, cone, distance);
+// testReversibleShapeIntersection(box, cylinder, distance);
+ testReversibleShapeIntersection(box, plane, distance);
+ testReversibleShapeIntersection(box, halfspace, distance);
+
+ testReversibleShapeIntersection(sphere, capsule, distance);
+// testReversibleShapeIntersection(sphere, cone, distance);
+// testReversibleShapeIntersection(sphere, cylinder, distance);
+ testReversibleShapeIntersection(sphere, plane, distance);
+ testReversibleShapeIntersection(sphere, halfspace, distance);
+
+// testReversibleShapeIntersection(capsule, cone, distance);
+// testReversibleShapeIntersection(capsule, cylinder, distance);
+ testReversibleShapeIntersection(capsule, plane, distance);
+ testReversibleShapeIntersection(capsule, halfspace, distance);
+
+// testReversibleShapeIntersection(cone, cylinder, distance);
+ testReversibleShapeIntersection(cone, plane, distance);
+ testReversibleShapeIntersection(cone, halfspace, distance);
+
+ testReversibleShapeIntersection(cylinder, plane, distance);
+ testReversibleShapeIntersection(cylinder, halfspace, distance);
+
+ testReversibleShapeIntersection(plane, halfspace, distance);
+}
+
+template<typename S1, typename S2>
+void testReversibleShapeDistance(const S1& s1, const S2& s2, FCL_REAL distance)
+{
+ Transform3f tf1(Vec3f(-0.5 * distance, 0.0, 0.0));
+ Transform3f tf2(Vec3f(+0.5 * distance, 0.0, 0.0));
+
+ FCL_REAL distA;
+ FCL_REAL distB;
+ Vec3f p1A;
+ Vec3f p1B;
+ Vec3f p2A;
+ Vec3f p2B;
+
+ 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);
+
+ BOOST_CHECK(resA);
+ BOOST_CHECK(resB);
+ BOOST_CHECK_CLOSE(distA, distB, tol); // distances should be same
+ BOOST_CHECK(p1A.equal(p2B, tol)); // closest points should in reverse order
+ BOOST_CHECK(p2A.equal(p1B, tol));
+
+ resA = solver2.shapeDistance(s1, tf1, s2, tf2, &distA, &p1A, &p2A);
+ resB = solver2.shapeDistance(s2, tf2, s1, tf1, &distB, &p1B, &p2B);
+
+ BOOST_CHECK(resA);
+ BOOST_CHECK(resB);
+ BOOST_CHECK_CLOSE(distA, distB, tol);
+ BOOST_CHECK(p1A.equal(p2B, tol));
+ BOOST_CHECK(p2A.equal(p1B, tol));
+}
+
+BOOST_AUTO_TEST_CASE(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 (7) -- box, sphere, capsule, cone, cylinder, plane, halfspace
+ Box box(10, 10, 10);
+ Sphere sphere(5);
+ Capsule capsule(5, 10);
+ Cone cone(5, 10);
+ Cylinder cylinder(5, 10);
+ Plane plane(Vec3f(), 0.0);
+ Halfspace halfspace(Vec3f(), 0.0);
+
+ // Use sufficiently long distance so that all the primitive shapes CANNOT intersect
+ FCL_REAL distance = 15.0;
+
+ // If new shape distance algorithm is added for two distinct primitive
+ // shapes, uncomment associated lines. For example, box-sphere intersection
+ // algorithm is added, then uncomment box-sphere.
+
+// testReversibleShapeDistance(box, sphere, distance);
+// testReversibleShapeDistance(box, capsule, distance);
+// testReversibleShapeDistance(box, cone, distance);
+// testReversibleShapeDistance(box, cylinder, distance);
+// testReversibleShapeDistance(box, plane, distance);
+// testReversibleShapeDistance(box, halfspace, distance);
+
+ testReversibleShapeDistance(sphere, capsule, distance);
+// testReversibleShapeDistance(sphere, cone, distance);
+// testReversibleShapeDistance(sphere, cylinder, distance);
+// testReversibleShapeDistance(sphere, plane, distance);
+// testReversibleShapeDistance(sphere, halfspace, distance);
+
+// testReversibleShapeDistance(capsule, cone, distance);
+// testReversibleShapeDistance(capsule, cylinder, distance);
+// testReversibleShapeDistance(capsule, plane, distance);
+// testReversibleShapeDistance(capsule, halfspace, distance);
+
+// testReversibleShapeDistance(cone, cylinder, distance);
+// testReversibleShapeDistance(cone, plane, distance);
+// testReversibleShapeDistance(cone, halfspace, distance);
+
+// testReversibleShapeDistance(cylinder, plane, distance);
+// testReversibleShapeDistance(cylinder, halfspace, distance);
+
+// testReversibleShapeDistance(plane, halfspace, distance);
+}
+