Merge pull request #592 from jcarpent/topic/devel

Add contact_patch_function_not_implemented

Author: jcarpent

CHANGELOG.md

@@ -5,6 +5,8 @@ All notable changes to this project will be documented in this file.
 The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/).
 
 ## [Unreleased]
+
+### Added
 - Added `Transform3f::Random` and `Transform3f::setRandom` ([#584](https://github.com/humanoid-path-planner/hpp-fcl/pull/584))
 - New feature: computation of contact surfaces for any pair of primitive shapes (triangle, sphere, ellipsoid, plane, halfspace, cone, capsule, cylinder, convex) ([#574](https://github.com/humanoid-path-planner/hpp-fcl/pull/574)).
 - Enhance Broadphase DynamicAABBTree to better handle planes and halfspace ([#570](https://github.com/humanoid-path-planner/hpp-fcl/pull/570))
@@ -17,12 +19,9 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/).
 - Fixed too low tolerance in GJK/EPA asserts ([#554](https://github.com/humanoid-path-planner/hpp-fcl/pull/554))
 - Fixed `normal_and_nearest_points` test (no need to have Eigen 3.4) ([#553](https://github.com/humanoid-path-planner/hpp-fcl/pull/553))
 - [#549](https://github.com/humanoid-path-planner/hpp-fcl/pull/549)
-  - Optimize EPA: ignore useless faces in EPA's polytope; warm-start support computation for `Convex`; fix edge-cases witness points computation.
-  - Add `Serializable` trait to transform, collision data, collision geometries, bounding volumes, bvh models, hfields. Collision problems can now be serialized from C++ and sent to python and vice versa.
+- Optimize EPA: ignore useless faces in EPA's polytope; warm-start support computation for `Convex`; fix edge-cases witness points computation.
+- Add `Serializable` trait to transform, collision data, collision geometries, bounding volumes, bvh models, hfields. Collision problems can now be serialized from C++ and sent to python and vice versa.
 - CMake: allow use of installed jrl-cmakemodules ([#564](https://github.com/humanoid-path-planner/hpp-fcl/pull/564))
-- Fix compilation with earlier Eigen version
-- Fix compilation warning message
-- Fix issue in Octomap.computeLocalAABB
 
 ### Fixed
 
@@ -43,6 +42,10 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/).
 - Account for lateral borders in Height Fields model.
 - Fix compilation error on recent APPLE compilers ([#539](https://github.com/humanoid-path-planner/hpp-fcl/pull/539)).
 - Fix printing of deprecated message ([#540](https://github.com/humanoid-path-planner/hpp-fcl/pull/540)).
+- Fix compilation with earlier Eigen version
+- Fix compilation warning message
+- Fix issue in Octomap.computeLocalAABB
+- Fix unsupported function for contact_patch_matrix
 
 ## [2.4.4] - 2024-03-06
 

src/contact_patch_func_matrix.cpp

@@ -118,6 +118,23 @@ struct BVHComputeContactPatch {
   }
 };
 
+HPP_FCL_LOCAL void contact_patch_function_not_implemented(
+    const CollisionGeometry* o1, const Transform3f& /*tf1*/,
+    const CollisionGeometry* o2, const Transform3f& /*tf2*/,
+    const CollisionResult& /*collision_result*/,
+    const ContactPatchSolver* /*csolver*/,
+    const ContactPatchRequest& /*request*/, ContactPatchResult& /*result*/) {
+  NODE_TYPE node_type1 = o1->getNodeType();
+  NODE_TYPE node_type2 = o2->getNodeType();
+
+  HPP_FCL_THROW_PRETTY("Contact patch function between node type "
+                           << std::string(get_node_type_name(node_type1))
+                           << " and node type "
+                           << std::string(get_node_type_name(node_type2))
+                           << " is not yet supported.",
+                       std::invalid_argument);
+}
+
 ContactPatchFunctionMatrix::ContactPatchFunctionMatrix() {
   for (int i = 0; i < NODE_COUNT; ++i) {
     for (int j = 0; j < NODE_COUNT; ++j) contact_patch_matrix[i][j] = nullptr;
@@ -347,6 +364,46 @@ ContactPatchFunctionMatrix::ContactPatchFunctionMatrix() {
   contact_patch_matrix[BV_OBBRSS][BV_OBBRSS]      = &BVHComputeContactPatch<OBBRSS>::run;
 
   // TODO(louis): octrees
+#ifdef HPP_FCL_HAS_OCTOMAP
+  contact_patch_matrix[GEOM_OCTREE][GEOM_OCTREE] = &contact_patch_function_not_implemented;
+  contact_patch_matrix[GEOM_OCTREE][GEOM_BOX] = &contact_patch_function_not_implemented;
+  contact_patch_matrix[GEOM_OCTREE][GEOM_SPHERE] = &contact_patch_function_not_implemented;
+  contact_patch_matrix[GEOM_OCTREE][GEOM_CAPSULE] = &contact_patch_function_not_implemented;
+  contact_patch_matrix[GEOM_OCTREE][GEOM_CONE] = &contact_patch_function_not_implemented;
+  contact_patch_matrix[GEOM_OCTREE][GEOM_CYLINDER] = &contact_patch_function_not_implemented;
+  contact_patch_matrix[GEOM_OCTREE][GEOM_CONVEX] = &contact_patch_function_not_implemented;
+  contact_patch_matrix[GEOM_OCTREE][GEOM_PLANE] = &contact_patch_function_not_implemented;
+  contact_patch_matrix[GEOM_OCTREE][GEOM_HALFSPACE] = &contact_patch_function_not_implemented;
+  contact_patch_matrix[GEOM_OCTREE][GEOM_ELLIPSOID] = &contact_patch_function_not_implemented;
+  contact_patch_matrix[GEOM_OCTREE][GEOM_TRIANGLE] = &contact_patch_function_not_implemented;
+  contact_patch_matrix[GEOM_OCTREE][BV_AABB] = &contact_patch_function_not_implemented;
+  contact_patch_matrix[GEOM_OCTREE][BV_OBB] = &contact_patch_function_not_implemented;
+  contact_patch_matrix[GEOM_OCTREE][BV_RSS] = &contact_patch_function_not_implemented;
+  contact_patch_matrix[GEOM_OCTREE][BV_OBBRSS] = &contact_patch_function_not_implemented;
+  contact_patch_matrix[GEOM_OCTREE][BV_kIOS] = &contact_patch_function_not_implemented;
+  contact_patch_matrix[GEOM_OCTREE][BV_KDOP16] = &contact_patch_function_not_implemented;
+  contact_patch_matrix[GEOM_OCTREE][BV_KDOP18] = &contact_patch_function_not_implemented;
+  contact_patch_matrix[GEOM_OCTREE][BV_KDOP24] = &contact_patch_function_not_implemented;
+
+  contact_patch_matrix[GEOM_BOX][GEOM_OCTREE]  = &contact_patch_function_not_implemented;
+  contact_patch_matrix[GEOM_SPHERE][GEOM_OCTREE]  = &contact_patch_function_not_implemented;
+  contact_patch_matrix[GEOM_CAPSULE][GEOM_OCTREE]  = &contact_patch_function_not_implemented;
+  contact_patch_matrix[GEOM_CONE][GEOM_OCTREE]  = &contact_patch_function_not_implemented;
+  contact_patch_matrix[GEOM_CYLINDER][GEOM_OCTREE]  = &contact_patch_function_not_implemented;
+  contact_patch_matrix[GEOM_CONVEX][GEOM_OCTREE]  = &contact_patch_function_not_implemented;
+  contact_patch_matrix[GEOM_PLANE][GEOM_OCTREE]  = &contact_patch_function_not_implemented;
+  contact_patch_matrix[GEOM_HALFSPACE][GEOM_OCTREE]  = &contact_patch_function_not_implemented;
+  contact_patch_matrix[GEOM_ELLIPSOID][GEOM_OCTREE]  = &contact_patch_function_not_implemented;
+  contact_patch_matrix[GEOM_TRIANGLE][GEOM_OCTREE]  = &contact_patch_function_not_implemented;
+  contact_patch_matrix[BV_AABB][GEOM_OCTREE]  = &contact_patch_function_not_implemented;
+  contact_patch_matrix[BV_OBB][GEOM_OCTREE]  = &contact_patch_function_not_implemented;
+  contact_patch_matrix[BV_RSS][GEOM_OCTREE]  = &contact_patch_function_not_implemented;
+  contact_patch_matrix[BV_OBBRSS][GEOM_OCTREE]  = &contact_patch_function_not_implemented;
+  contact_patch_matrix[BV_kIOS][GEOM_OCTREE]  = &contact_patch_function_not_implemented;
+  contact_patch_matrix[BV_KDOP16][GEOM_OCTREE]  = &contact_patch_function_not_implemented;
+  contact_patch_matrix[BV_KDOP18][GEOM_OCTREE]  = &contact_patch_function_not_implemented;
+  contact_patch_matrix[BV_KDOP24][GEOM_OCTREE]  = &contact_patch_function_not_implemented;
+#endif
   // clang-format on
 }