Contact patch: add bvh and hfield

include/hpp/fcl/collision_data.h

@@ -894,6 +894,20 @@ struct HPP_FCL_DLLAPI ContactPatchResult {
     return this->m_contact_patches.back();
   }
 
+  /// @brief Getter for the i-th contact patch of the result.
+  ContactPatch& contactPatch(const size_t i) {
+    if (this->m_contact_patches.empty()) {
+      HPP_FCL_THROW_PRETTY(
+          "The number of contact patches is zero. No ContactPatch can be "
+          "returned.",
+          std::invalid_argument);
+    }
+    if (i < this->m_contact_patches.size()) {
+      return this->m_contact_patches[i];
+    }
+    return this->m_contact_patches.back();
+  }
+
   /// @brief Clears the contact patch result.
   void clear() {
     this->m_contact_patches.clear();
@@ -948,6 +962,23 @@ struct HPP_FCL_DLLAPI ContactPatchResult {
 
     return true;
   }
+
+  /// @brief Repositions the ContactPatch when they get inverted during their
+  /// construction.
+  void swapObjects() {
+    // Create new transform: it's the reflection of `tf` along the z-axis.
+    // This corresponds to doing a PI rotation along the y-axis, i.e. the x-axis
+    // becomes -x-axis, y-axis stays the same, z-axis becomes -z-axis.
+    for (size_t i = 0; i < this->numContactPatches(); ++i) {
+      ContactPatch& patch = this->contactPatch(i);
+      patch.tf.rotation().col(0) *= -1.0;
+      patch.tf.rotation().col(2) *= -1.0;
+
+      for (size_t j = 0; j < patch.size(); ++j) {
+        patch.point(i)(0) *= -1.0;  // only invert the x-axis
+      }
+    }
+  }
 };
 
 struct DistanceResult;

include/hpp/fcl/contact_patch.h

@@ -107,6 +107,7 @@ class HPP_FCL_DLLAPI ComputeContactPatch {
   mutable ContactPatchSolver csolver;
 
   ContactPatchFunctionMatrix::ContactPatchFunc func;
+  bool swap_geoms;
 
   virtual void run(const Transform3f& tf1, const Transform3f& tf2,
                    const CollisionResult& collision_result,

src/contact_patch.cpp

@@ -55,26 +55,34 @@ void computeContactPatch(const CollisionGeometry* o1, const Transform3f& tf1,
     return;
   }
 
+  // Before doing any computation, we initialize and clear the input result.
+  result.set(request);
+  ContactPatchSolver csolver(request);
+
   OBJECT_TYPE object_type1 = o1->getObjectType();
   OBJECT_TYPE object_type2 = o2->getObjectType();
   NODE_TYPE node_type1 = o1->getNodeType();
   NODE_TYPE node_type2 = o2->getNodeType();
-  // TODO(louis): add support for BVH (leaf is a triangle) and Hfield (leaf is a
-  // convex)
-  if (object_type1 != OBJECT_TYPE::OT_GEOM ||
-      object_type2 != OBJECT_TYPE::OT_GEOM) {
-    HPP_FCL_THROW_PRETTY("Computing contact patches between node type "
-                             << std::string(get_node_type_name(node_type1))
-                             << " and node type "
-                             << std::string(get_node_type_name(node_type2))
-                             << " is not yet supported. Only primitive shapes "
-                                "are supported for now.",
-                         std::invalid_argument);
-    return;
-  }
 
   const ContactPatchFunctionMatrix& looktable =
       getContactPatchFunctionLookTable();
+
+  if (object_type1 == OT_GEOM &&
+      (object_type2 == OT_BVH || object_type2 == OT_HFIELD)) {
+    if (!looktable.contact_patch_matrix[node_type2][node_type1]) {
+      HPP_FCL_THROW_PRETTY("Computing contact patches between node type "
+                               << std::string(get_node_type_name(node_type1))
+                               << " and node type "
+                               << std::string(get_node_type_name(node_type2))
+                               << " is not yet supported.",
+                           std::invalid_argument);
+    }
+    looktable.contact_patch_matrix[node_type2][node_type1](
+        o2, tf2, o1, tf1, collision_result, &csolver, request, result);
+    result.swapObjects();
+    return;
+  }
+
   if (!looktable.contact_patch_matrix[node_type1][node_type2]) {
     HPP_FCL_THROW_PRETTY("Contact patch computation between node type "
                              << std::string(get_node_type_name(node_type1))
@@ -84,9 +92,6 @@ void computeContactPatch(const CollisionGeometry* o1, const Transform3f& tf1,
                          std::invalid_argument);
   }
 
-  // Before doing any computation, we initialize and clear the input result.
-  result.set(request);
-  ContactPatchSolver csolver(request);
   return looktable.contact_patch_matrix[node_type1][node_type2](
       o1, tf1, o2, tf2, collision_result, &csolver, request, result);
 }
@@ -111,27 +116,25 @@ ComputeContactPatch::ComputeContactPatch(const CollisionGeometry* o1,
   OBJECT_TYPE object_type2 = this->o2->getObjectType();
   NODE_TYPE node_type2 = this->o2->getNodeType();
 
-  if (object_type1 != OBJECT_TYPE::OT_GEOM ||
-      object_type2 != OBJECT_TYPE::OT_GEOM) {
-    HPP_FCL_THROW_PRETTY("Computing contact patches between node type "
-                             << std::string(get_node_type_name(node_type1))
-                             << " and node type "
-                             << std::string(get_node_type_name(node_type2))
-                             << " is not yet supported. Only primitive shapes "
-                                "are supported for now.",
-                         std::invalid_argument);
-  }
-
-  if (!looktable.contact_patch_matrix[node_type1][node_type2]) {
-    HPP_FCL_THROW_PRETTY("Contact patch computation between node type "
+  this->swap_geoms = object_type1 == OT_GEOM &&
+                     (object_type2 == OT_BVH || object_type2 == OT_HFIELD);
+  if ((this->swap_geoms &&
+       !looktable.contact_patch_matrix[node_type2][node_type1]) ||
+      (!this->swap_geoms &&
+       !looktable.contact_patch_matrix[node_type1][node_type2])) {
+    HPP_FCL_THROW_PRETTY("Collision function between node type "
                              << std::string(get_node_type_name(node_type1))
                              << " and node type "
                              << std::string(get_node_type_name(node_type2))
                              << " is not yet supported.",
                          std::invalid_argument);
   }
 
-  this->func = looktable.contact_patch_matrix[node_type1][node_type2];
+  if (this->swap_geoms) {
+    this->func = looktable.contact_patch_matrix[node_type2][node_type1];
+  } else {
+    this->func = looktable.contact_patch_matrix[node_type1][node_type2];
+  }
 }
 
 void ComputeContactPatch::run(const Transform3f& tf1, const Transform3f& tf2,
@@ -145,8 +148,14 @@ void ComputeContactPatch::run(const Transform3f& tf1, const Transform3f& tf2,
 
   // Before doing any computation, we initialize and clear the input result.
   result.set(request);
-  this->func(this->o1, tf1, this->o2, tf2, collision_result, &(this->csolver),
-             request, result);
+  if (this->swap_geoms) {
+    this->func(this->o2, tf2, this->o1, tf1, collision_result, &(this->csolver),
+               request, result);
+    result.swapObjects();
+  } else {
+    this->func(this->o1, tf1, this->o2, tf2, collision_result, &(this->csolver),
+               request, result);
+  }
 }
 
 void ComputeContactPatch::operator()(const Transform3f& tf1,