Handle negative security margin

CMakeLists.txt

@@ -209,6 +209,7 @@ SET(${PROJECT_NAME}_HEADERS
   include/hpp/fcl/shape/details/convex.hxx
   include/hpp/fcl/shape/geometric_shape_to_BVH_model.h
   include/hpp/fcl/shape/geometric_shapes.h
+  include/hpp/fcl/shape/geometric_shapes_traits.h
   include/hpp/fcl/shape/geometric_shapes_utility.h
   include/hpp/fcl/distance_func_matrix.h
   include/hpp/fcl/collision.h
@@ -232,6 +233,7 @@ SET(${PROJECT_NAME}_HEADERS
   include/hpp/fcl/mesh_loader/loader.h
   include/hpp/fcl/internal/BV_fitter.h
   include/hpp/fcl/internal/BV_splitter.h
+  include/hpp/fcl/internal/shape_shape_func.h
   include/hpp/fcl/internal/intersect.h
   include/hpp/fcl/internal/tools.h
   include/hpp/fcl/internal/traversal_node_base.h

include/hpp/fcl/BV/kDOP.h

@@ -38,7 +38,7 @@
 #ifndef HPP_FCL_KDOP_H
 #define HPP_FCL_KDOP_H
 
-#include <stdexcept>
+#include <hpp/fcl/fwd.hh>
 #include <hpp/fcl/data_types.h>
 
 namespace hpp {
@@ -175,14 +175,14 @@ class HPP_FCL_DLLAPI KDOP {
 template <short N>
 bool overlap(const Matrix3f& /*R0*/, const Vec3f& /*T0*/, const KDOP<N>& /*b1*/,
              const KDOP<N>& /*b2*/) {
-  throw std::logic_error("not implemented");
+  HPP_FCL_THROW_PRETTY("not implemented", std::logic_error);
 }
 
 template <short N>
 bool overlap(const Matrix3f& /*R0*/, const Vec3f& /*T0*/, const KDOP<N>& /*b1*/,
              const KDOP<N>& /*b2*/, const CollisionRequest& /*request*/,
              FCL_REAL& /*sqrDistLowerBound*/) {
-  throw std::logic_error("not implemented");
+  HPP_FCL_THROW_PRETTY("not implemented", std::logic_error);
 }
 
 /// @brief translate the KDOP BV

include/hpp/fcl/broadphase/detail/simple_hash_table-inl.h

@@ -56,7 +56,8 @@ SimpleHashTable<Key, Data, HashFnc>::SimpleHashTable(const HashFnc& h) : h_(h) {
 template <typename Key, typename Data, typename HashFnc>
 void SimpleHashTable<Key, Data, HashFnc>::init(size_t size) {
   if (size == 0) {
-    throw std::logic_error("SimpleHashTable must have non-zero size.");
+    HPP_FCL_THROW_PRETTY("SimpleHashTable must have non-zero size.",
+                         std::logic_error);
   }
 
   table_.resize(size);

include/hpp/fcl/broadphase/detail/simple_hash_table.h

@@ -38,10 +38,10 @@
 #ifndef HPP_FCL_BROADPHASE_SIMPLEHASHTABLE_H
 #define HPP_FCL_BROADPHASE_SIMPLEHASHTABLE_H
 
-#include <stdexcept>
 #include <set>
 #include <vector>
 #include <list>
+
 namespace hpp {
 namespace fcl {
 

include/hpp/fcl/broadphase/detail/sparse_hash_table.h

@@ -38,7 +38,6 @@
 #ifndef HPP_FCL_BROADPHASE_SPARSEHASHTABLE_H
 #define HPP_FCL_BROADPHASE_SPARSEHASHTABLE_H
 
-#include <stdexcept>
 #include <set>
 #include <vector>
 #include <list>

include/hpp/fcl/collision_data.h

@@ -534,6 +534,28 @@ struct HPP_FCL_DLLAPI DistanceResult : QueryResult {
   }
 };
 
+namespace internal {
+inline void updateDistanceLowerBoundFromBV(const CollisionRequest& /*req*/,
+                                           CollisionResult& res,
+                                           const FCL_REAL& sqrDistLowerBound) {
+  // BV cannot find negative distance.
+  if (res.distance_lower_bound <= 0) return;
+  FCL_REAL new_dlb = std::sqrt(sqrDistLowerBound);  // - req.security_margin;
+  if (new_dlb < res.distance_lower_bound) res.distance_lower_bound = new_dlb;
+}
+
+inline void updateDistanceLowerBoundFromLeaf(const CollisionRequest&,
+                                             CollisionResult& res,
+                                             const FCL_REAL& distance,
+                                             const Vec3f& p0, const Vec3f& p1) {
+  if (distance < res.distance_lower_bound) {
+    res.distance_lower_bound = distance;
+    res.nearest_points[0] = p0;
+    res.nearest_points[1] = p1;
+  }
+}
+}  // namespace internal
+
 inline CollisionRequestFlag operator~(CollisionRequestFlag a) {
   return static_cast<CollisionRequestFlag>(~static_cast<int>(a));
 }

include/hpp/fcl/fwd.hh

@@ -40,6 +40,7 @@
 
 #include <memory>
 #include <sstream>
+#include <stdexcept>
 
 #include <hpp/fcl/config.hh>
 #include <hpp/fcl/deprecated.hh>

include/hpp/fcl/hfield.h

@@ -61,14 +61,22 @@ struct HPP_FCL_DLLAPI HFNodeBase {
   Eigen::DenseIndex x_id, x_size;
   Eigen::DenseIndex y_id, y_size;
 
+  FCL_REAL max_height;
+
   /// @brief Default constructor
-  HFNodeBase() : first_child(0), x_id(-1), x_size(0), y_id(-1), y_size(0) {}
+  HFNodeBase()
+      : first_child(0),
+        x_id(-1),
+        x_size(0),
+        y_id(-1),
+        y_size(0),
+        max_height(std::numeric_limits<FCL_REAL>::lowest()) {}
 
   /// @brief Comparison operator
   bool operator==(const HFNodeBase& other) const {
     return first_child == other.first_child && x_id == other.x_id &&
            x_size == other.x_size && y_id == other.y_id &&
-           y_size == other.y_size;
+           y_size == other.y_size && max_height == other.max_height;
   }
 
   /// @brief Difference operator
@@ -353,6 +361,8 @@ class HPP_FCL_DLLAPI HeightField : public CollisionGeometry {
       max_height = (std::max)(max_left_height, max_right_height);
     }
 
+    bv_node.max_height = max_height;
+
     const Vec3f pointA(x_grid[bv_node.x_id], y_grid[bv_node.y_id], min_height);
     const Vec3f pointB(x_grid[bv_node.x_id + bv_node.x_size],
                        y_grid[bv_node.y_id + bv_node.y_size], max_height);
@@ -372,42 +382,43 @@ class HPP_FCL_DLLAPI HeightField : public CollisionGeometry {
            "x_size or y_size are not of correct value");
     assert(bv_id < bvs.size() && "bv_id exceeds the vector dimension");
 
-    HFNode<BV>& bvnode = bvs[bv_id];
+    HFNode<BV>& bv_node = bvs[bv_id];
     FCL_REAL max_height;
     if (x_size == 1 &&
         y_size == 1)  // don't build any BV for the current child node
     {
       max_height = heights.block<2, 2>(y_id, x_id).maxCoeff();
     } else {
-      bvnode.first_child = num_bvs;
+      bv_node.first_child = num_bvs;
       num_bvs += 2;
 
       FCL_REAL max_left_height = min_height, max_right_height = min_height;
       if (x_size >= y_size)  // splitting along the X axis
       {
         Eigen::DenseIndex x_size_half = x_size / 2;
         if (x_size == 1) x_size_half = 1;
-        max_left_height = recursiveBuildTree(bvnode.leftChild(), x_id,
+        max_left_height = recursiveBuildTree(bv_node.leftChild(), x_id,
                                              x_size_half, y_id, y_size);
 
         max_right_height =
-            recursiveBuildTree(bvnode.rightChild(), x_id + x_size_half,
+            recursiveBuildTree(bv_node.rightChild(), x_id + x_size_half,
                                x_size - x_size_half, y_id, y_size);
       } else  // splitting along the Y axis
       {
         Eigen::DenseIndex y_size_half = y_size / 2;
         if (y_size == 1) y_size_half = 1;
-        max_left_height = recursiveBuildTree(bvnode.leftChild(), x_id, x_size,
+        max_left_height = recursiveBuildTree(bv_node.leftChild(), x_id, x_size,
                                              y_id, y_size_half);
 
         max_right_height =
-            recursiveBuildTree(bvnode.rightChild(), x_id, x_size,
+            recursiveBuildTree(bv_node.rightChild(), x_id, x_size,
                                y_id + y_size_half, y_size - y_size_half);
       }
 
       max_height = (std::max)(max_left_height, max_right_height);
     }
 
+    bv_node.max_height = max_height;
     //    max_height = std::max(max_height,min_height);
 
     const Vec3f pointA(x_grid[x_id], y_grid[y_id], min_height);
@@ -416,11 +427,11 @@ class HPP_FCL_DLLAPI HeightField : public CollisionGeometry {
     const Vec3f pointB(x_grid[x_id + x_size], y_grid[y_id + y_size],
                        max_height);
 
-    details::UpdateBoundingVolume<BV>::run(pointA, pointB, bvnode.bv);
-    bvnode.x_id = x_id;
-    bvnode.y_id = y_id;
-    bvnode.x_size = x_size;
-    bvnode.y_size = y_size;
+    details::UpdateBoundingVolume<BV>::run(pointA, pointB, bv_node.bv);
+    bv_node.x_id = x_id;
+    bv_node.y_id = y_id;
+    bv_node.x_size = x_size;
+    bv_node.y_size = y_size;
 
     return max_height;
   }

include/hpp/fcl/internal/shape_shape_func.h

@@ -40,10 +40,13 @@
 /// @cond INTERNAL
 
 #include <hpp/fcl/collision_data.h>
+#include <hpp/fcl/collision_utility.h>
 #include <hpp/fcl/narrowphase/narrowphase.h>
+#include <hpp/fcl/shape/geometric_shapes_traits.h>
 
 namespace hpp {
 namespace fcl {
+
 template <typename T_SH1, typename T_SH2>
 HPP_FCL_DLLAPI FCL_REAL ShapeShapeDistance(const CollisionGeometry* o1,
                                            const Transform3f& tf1,
@@ -54,13 +57,56 @@ HPP_FCL_DLLAPI FCL_REAL ShapeShapeDistance(const CollisionGeometry* o1,
                                            DistanceResult& result);
 
 template <typename T_SH1, typename T_SH2>
-HPP_FCL_DLLAPI std::size_t ShapeShapeCollide(const CollisionGeometry* o1,
-                                             const Transform3f& tf1,
-                                             const CollisionGeometry* o2,
-                                             const Transform3f& tf2,
-                                             const GJKSolver* nsolver,
-                                             const CollisionRequest& request,
-                                             CollisionResult& result);
+struct ShapeShapeCollider {
+  static std::size_t run(const CollisionGeometry* o1, const Transform3f& tf1,
+                         const CollisionGeometry* o2, const Transform3f& tf2,
+                         const GJKSolver* nsolver,
+                         const CollisionRequest& request,
+                         CollisionResult& result) {
+    if (request.isSatisfied(result)) return result.numContacts();
+
+    DistanceResult distanceResult;
+    DistanceRequest distanceRequest(request.enable_contact);
+    FCL_REAL distance = ShapeShapeDistance<T_SH1, T_SH2>(
+        o1, tf1, o2, tf2, nsolver, distanceRequest, distanceResult);
+
+    size_t num_contacts = 0;
+    const Vec3f& p1 = distanceResult.nearest_points[0];
+    const Vec3f& p2 = distanceResult.nearest_points[1];
+    FCL_REAL distToCollision = distance - request.security_margin;
+
+    internal::updateDistanceLowerBoundFromLeaf(request, result, distToCollision,
+                                               p1, p2);
+    if (distToCollision <= request.collision_distance_threshold &&
+        result.numContacts() < request.num_max_contacts) {
+      if (result.numContacts() < request.num_max_contacts) {
+        const Vec3f& p1 = distanceResult.nearest_points[0];
+        const Vec3f& p2 = distanceResult.nearest_points[1];
+
+        Contact contact(
+            o1, o2, distanceResult.b1, distanceResult.b2, (p1 + p2) / 2,
+            (distance <= 0 ? distanceResult.normal : (p2 - p1).normalized()),
+            -distance);
+
+        result.addContact(contact);
+      }
+      num_contacts = result.numContacts();
+    }
+
+    return num_contacts;
+  }
+};
+
+template <typename ShapeType1, typename ShapeType2>
+std::size_t ShapeShapeCollide(const CollisionGeometry* o1,
+                              const Transform3f& tf1,
+                              const CollisionGeometry* o2,
+                              const Transform3f& tf2, const GJKSolver* nsolver,
+                              const CollisionRequest& request,
+                              CollisionResult& result) {
+  return ShapeShapeCollider<ShapeType1, ShapeType2>::run(
+      o1, tf1, o2, tf2, nsolver, request, result);
+}
 
 #define SHAPE_SHAPE_DISTANCE_SPECIALIZATION(T1, T2)             \
   template <>                                                   \
@@ -96,13 +142,17 @@ SHAPE_SHAPE_DISTANCE_SPECIALIZATION(TriangleP, Halfspace);
 
 #undef SHAPE_SHAPE_DISTANCE_SPECIALIZATION
 
-#define SHAPE_SHAPE_COLLIDE_SPECIALIZATION(T1, T2)               \
-  template <>                                                    \
-  HPP_FCL_DLLAPI std::size_t ShapeShapeCollide<T1, T2>(          \
-      const CollisionGeometry* o1, const Transform3f& tf1,       \
-      const CollisionGeometry* o2, const Transform3f& tf2,       \
-      const GJKSolver* nsolver, const CollisionRequest& request, \
-      CollisionResult& result)
+#define SHAPE_SHAPE_COLLIDE_SPECIALIZATION(T1, T2)                         \
+  template <>                                                              \
+  struct ShapeShapeCollider<T1, T2> {                                      \
+    static HPP_FCL_DLLAPI std::size_t run(const CollisionGeometry* o1,     \
+                                          const Transform3f& tf1,          \
+                                          const CollisionGeometry* o2,     \
+                                          const Transform3f& tf2,          \
+                                          const GJKSolver* nsolver,        \
+                                          const CollisionRequest& request, \
+                                          CollisionResult& result);        \
+  }
 
 SHAPE_SHAPE_COLLIDE_SPECIALIZATION(Sphere, Sphere);
 

include/hpp/fcl/internal/traversal.h

@@ -59,10 +59,10 @@ struct HPP_FCL_DLLAPI RelativeTransformation {
 template <>
 struct HPP_FCL_DLLAPI RelativeTransformation<false> {
   static const Matrix3f& _R() {
-    throw std::logic_error("should never reach this point");
+    HPP_FCL_THROW_PRETTY("should never reach this point", std::logic_error);
   }
   static const Vec3f& _T() {
-    throw std::logic_error("should never reach this point");
+    HPP_FCL_THROW_PRETTY("should never reach this point", std::logic_error);
   }
 };
 }  // namespace details

include/hpp/fcl/internal/traversal_node_base.h

@@ -108,9 +108,6 @@ class CollisionTraversalNodeBase : public TraversalNodeBase {
 
   virtual ~CollisionTraversalNodeBase() {}
 
-  /// @brief BV test between b1 and b2
-  virtual bool BVDisjoints(unsigned int b1, unsigned int b2) const = 0;
-
   /// BV test between b1 and b2
   /// @param b1, b2 Bounding volumes to test,
   /// @retval sqrDistLowerBound square of a lower bound of the minimal
@@ -167,28 +164,6 @@ class DistanceTraversalNodeBase : public TraversalNodeBase {
   DistanceResult* result;
 };
 
-namespace internal {
-inline void updateDistanceLowerBoundFromBV(const CollisionRequest& req,
-                                           CollisionResult& res,
-                                           const FCL_REAL& sqrDistLowerBound) {
-  // BV cannot find negative distance.
-  if (res.distance_lower_bound <= 0) return;
-  FCL_REAL new_dlb = std::sqrt(sqrDistLowerBound) - req.security_margin;
-  if (new_dlb < res.distance_lower_bound) res.distance_lower_bound = new_dlb;
-}
-
-inline void updateDistanceLowerBoundFromLeaf(const CollisionRequest&,
-                                             CollisionResult& res,
-                                             const FCL_REAL& distance,
-                                             const Vec3f& p0, const Vec3f& p1) {
-  if (distance < res.distance_lower_bound) {
-    res.distance_lower_bound = distance;
-    res.nearest_points[0] = p0;
-    res.nearest_points[1] = p1;
-  }
-}
-}  // namespace internal
-
 ///@}
 
 }  // namespace fcl

include/hpp/fcl/internal/traversal_node_bvh_shape.h

@@ -152,16 +152,6 @@ class MeshShapeCollisionTraversalNode
     nsolver = NULL;
   }
 
-  /// @brief BV culling test in one BVTT node
-  bool BVDisjoints(unsigned int b1, unsigned int /*b2*/) const {
-    if (this->enable_statistics) this->num_bv_tests++;
-    if (RTIsIdentity)
-      return !this->model1->getBV(b1).bv.overlap(this->model2_bv);
-    else
-      return !overlap(this->tf1.getRotation(), this->tf1.getTranslation(),
-                      this->model2_bv, this->model1->getBV(b1).bv);
-  }
-
   /// test between BV b1 and shape
   /// @param b1 BV to test,
   /// @retval sqrDistLowerBound square of a lower bound of the minimal
@@ -263,17 +253,6 @@ class ShapeMeshCollisionTraversalNode
     nsolver = NULL;
   }
 
-  /// BV test between b1 and b2
-  /// @param b2 Bounding volumes to test,
-  bool BVDisjoints(unsigned int /*b1*/, unsigned int b2) const {
-    if (this->enable_statistics) this->num_bv_tests++;
-    if (RTIsIdentity)
-      return !this->model2->getBV(b2).bv.overlap(this->model1_bv);
-    else
-      return !overlap(this->tf2.getRotation(), this->tf2.getTranslation(),
-                      this->model1_bv, this->model2->getBV(b2).bv);
-  }
-
   /// BV test between b1 and b2
   /// @param b2 Bounding volumes to test,
   /// @retval sqrDistLowerBound square of a lower bound of the minimal