Add CollisionResult::nearest_points

.github/workflows/ros_ci.yml

@@ -19,7 +19,7 @@ jobs:
           - {ROS_DISTRO: foxy, PRERELEASE: true}
           #- {ROS_DISTRO: galactic, PRERELEASE: true} ## Activate once eigenpy released to galactic
     env:
-      AFTER_RUN_TARGET_TEST: 'cd /root/target_ws/build/hpp-fcl && make test'
+      AFTER_RUN_TARGET_TEST: 'cd /root/target_ws/build/hpp-fcl && make build_tests && ctest --output-on-failure'
       CCACHE_DIR: /github/home/.ccache # Enable ccache
     runs-on: ubuntu-latest
     steps:

include/hpp/fcl/collision_data.h

@@ -149,6 +149,9 @@ struct HPP_FCL_DLLAPI QueryRequest {
   /// @brief enable timings when performing collision/distance request
   bool enable_timings;
 
+  /// @brief threshold below which a collision is considered.
+  FCL_REAL collision_distance_threshold;
+
   HPP_FCL_COMPILER_DIAGNOSTIC_PUSH
   HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
   /// @brief Default constructor.
@@ -162,7 +165,9 @@ struct HPP_FCL_DLLAPI QueryRequest {
         gjk_max_iterations(128),
         cached_gjk_guess(1, 0, 0),
         cached_support_func_guess(support_func_guess_t::Zero()),
-        enable_timings(false) {}
+        enable_timings(false),
+        collision_distance_threshold(
+            Eigen::NumTraits<FCL_REAL>::dummy_precision()) {}
 
   /// @brief Copy  constructor.
   QueryRequest(const QueryRequest& other) = default;
@@ -306,6 +311,11 @@ struct HPP_FCL_DLLAPI CollisionResult : QueryResult {
   /// overhead).
   FCL_REAL distance_lower_bound;
 
+  /// @brief nearest points
+  /// available only when distance_lower_bound is inferior to
+  /// CollisionRequest::break_distance.
+  Vec3f nearest_points[2];
+
  public:
   CollisionResult()
       : distance_lower_bound((std::numeric_limits<FCL_REAL>::max)()) {}
@@ -343,6 +353,18 @@ struct HPP_FCL_DLLAPI CollisionResult : QueryResult {
       return contacts.back();
   }
 
+  /// @brief set the i-th contact calculated
+  void setContact(size_t i, const Contact& c) {
+    if (contacts.size() == 0)
+      throw std::invalid_argument(
+          "The number of contacts is zero. No Contact can be returned.");
+
+    if (i < contacts.size())
+      contacts[i] = c;
+    else
+      contacts.back() = c;
+  }
+
   /// @brief get all the contacts
   void getContacts(std::vector<Contact>& contacts_) const {
     contacts_.resize(contacts.size());

include/hpp/fcl/internal/traversal_node_base.h

@@ -167,6 +167,28 @@ 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

@@ -170,16 +170,19 @@ class MeshShapeCollisionTraversalNode
   bool BVDisjoints(unsigned int b1, unsigned int /*b2*/,
                    FCL_REAL& sqrDistLowerBound) const {
     if (this->enable_statistics) this->num_bv_tests++;
-    bool res;
+    bool disjoint;
     if (RTIsIdentity)
-      res = !this->model1->getBV(b1).bv.overlap(this->model2_bv, this->request,
-                                                sqrDistLowerBound);
+      disjoint = !this->model1->getBV(b1).bv.overlap(
+          this->model2_bv, this->request, sqrDistLowerBound);
     else
-      res = !overlap(this->tf1.getRotation(), this->tf1.getTranslation(),
-                     this->model2_bv, this->model1->getBV(b1).bv, this->request,
-                     sqrDistLowerBound);
-    assert(!res || sqrDistLowerBound > 0);
-    return res;
+      disjoint = !overlap(this->tf1.getRotation(), this->tf1.getTranslation(),
+                          this->model2_bv, this->model1->getBV(b1).bv,
+                          this->request, sqrDistLowerBound);
+    if (disjoint)
+      internal::updateDistanceLowerBoundFromBV(this->request, *this->result,
+                                               sqrDistLowerBound);
+    assert(!disjoint || sqrDistLowerBound > 0);
+    return disjoint;
   }
 
   /// @brief Intersection testing between leaves (one triangle and one shape)
@@ -211,24 +214,32 @@ class MeshShapeCollisionTraversalNode
                                                     distance, c2, c1, normal);
     }
 
+    FCL_REAL distToCollision = distance - this->request.security_margin;
     if (collision) {
+      sqrDistLowerBound = 0;
       if (this->request.num_max_contacts > this->result->numContacts()) {
         this->result->addContact(Contact(this->model1, this->model2,
                                          primitive_id, Contact::NONE, c1,
                                          -normal, -distance));
         assert(this->result->isCollision());
-        return;
       }
-    }
-    sqrDistLowerBound = distance * distance;
-    assert(distance > 0);
-    if (this->request.security_margin > 0 &&
-        distance <= this->request.security_margin) {
-      this->result->addContact(Contact(this->model1, this->model2, primitive_id,
-                                       Contact::NONE, .5 * (c1 + c2),
-                                       (c2 - c1).normalized(), -distance));
-    }
-    assert(!this->result->isCollision() || sqrDistLowerBound > 0);
+    } else if (distToCollision <= this->request.collision_distance_threshold) {
+      sqrDistLowerBound = 0;
+      if (this->request.num_max_contacts > this->result->numContacts()) {
+        this->result->addContact(
+            Contact(this->model1, this->model2, primitive_id, Contact::NONE,
+                    .5 * (c1 + c2), (c2 - c1).normalized(), -distance));
+      }
+    } else
+      sqrDistLowerBound =
+          distToCollision *
+          distToCollision;  // TODO(jcarpent): should it be distToCollision *
+                            // distToCollision?
+
+    internal::updateDistanceLowerBoundFromLeaf(this->request, *this->result,
+                                               distToCollision, c1, c2);
+
+    assert(this->result->isCollision() || sqrDistLowerBound > 0);
   }
 
   Vec3f* vertices;
@@ -273,16 +284,19 @@ class ShapeMeshCollisionTraversalNode
   bool BVDisjoints(unsigned int /*b1*/, unsigned int b2,
                    FCL_REAL& sqrDistLowerBound) const {
     if (this->enable_statistics) this->num_bv_tests++;
-    bool res;
+    bool disjoint;
     if (RTIsIdentity)
-      res = !this->model2->getBV(b2).bv.overlap(this->model1_bv,
-                                                sqrDistLowerBound);
+      disjoint = !this->model2->getBV(b2).bv.overlap(this->model1_bv,
+                                                     sqrDistLowerBound);
     else
-      res = !overlap(this->tf2.getRotation(), this->tf2.getTranslation(),
-                     this->model1_bv, this->model2->getBV(b2).bv,
-                     sqrDistLowerBound);
-    assert(!res || sqrDistLowerBound > 0);
-    return res;
+      disjoint = !overlap(this->tf2.getRotation(), this->tf2.getTranslation(),
+                          this->model1_bv, this->model2->getBV(b2).bv,
+                          sqrDistLowerBound);
+    if (disjoint)
+      internal::updateDistanceLowerBoundFromBV(this->request, *this->result,
+                                               sqrDistLowerBound);
+    assert(!disjoint || sqrDistLowerBound > 0);
+    return disjoint;
   }
 
   /// @brief Intersection testing between leaves (one shape and one triangle)
@@ -314,24 +328,29 @@ class ShapeMeshCollisionTraversalNode
                                                     c2, distance, normal);
     }
 
+    FCL_REAL distToCollision = distance - this->request.security_margin;
     if (collision) {
+      sqrDistLowerBound = 0;
       if (this->request.num_max_contacts > this->result->numContacts()) {
         this->result->addContact(Contact(this->model1, this->model2,
                                          Contact::NONE, primitive_id, c1,
                                          normal, -distance));
         assert(this->result->isCollision());
-        return;
       }
-    }
-    sqrDistLowerBound = distance * distance;
-    assert(distance > 0);
-    if (this->request.security_margin == 0 &&
-        distance <= this->request.security_margin) {
-      this->result.addContact(Contact(this->model1, this->model2, Contact::NONE,
-                                      primitive_id, .5 * (c1 + c2),
-                                      (c2 - c1).normalized(), -distance));
-    }
-    assert(!this->result->isCollision() || sqrDistLowerBound > 0);
+    } else if (distToCollision <= this->request.collision_distance_threshold) {
+      sqrDistLowerBound = 0;
+      if (this->request.num_max_contacts > this->result->numContacts()) {
+        this->result->addContact(
+            Contact(this->model1, this->model2, Contact::NONE, primitive_id,
+                    .5 * (c1 + c2), (c2 - c1).normalized(), -distance));
+      }
+    } else
+      sqrDistLowerBound = distToCollision * distToCollision;
+
+    internal::updateDistanceLowerBoundFromLeaf(this->request, *this->result,
+                                               distToCollision, c1, c2);
+
+    assert(this->result->isCollision() || sqrDistLowerBound > 0);
   }
 
   Vec3f* vertices;

include/hpp/fcl/internal/traversal_node_bvhs.h

@@ -163,16 +163,19 @@ class MeshCollisionTraversalNode : public BVHCollisionTraversalNode<BV> {
   bool BVDisjoints(unsigned int b1, unsigned int b2,
                    FCL_REAL& sqrDistLowerBound) const {
     if (this->enable_statistics) this->num_bv_tests++;
+    bool disjoint;
     if (RTIsIdentity)
-      return !this->model1->getBV(b1).overlap(this->model2->getBV(b2),
-                                              this->request, sqrDistLowerBound);
+      disjoint = !this->model1->getBV(b1).overlap(
+          this->model2->getBV(b2), this->request, sqrDistLowerBound);
     else {
-      bool res = !overlap(RT._R(), RT._T(), this->model1->getBV(b1).bv,
+      disjoint = !overlap(RT._R(), RT._T(), this->model1->getBV(b1).bv,
                           this->model2->getBV(b2).bv, this->request,
                           sqrDistLowerBound);
-      assert(!res || sqrDistLowerBound > 0);
-      return res;
     }
+    if (disjoint)
+      internal::updateDistanceLowerBoundFromBV(this->request, *this->result,
+                                               sqrDistLowerBound);
+    return disjoint;
   }
 
   /// Intersection testing between leaves (two triangles)
@@ -218,24 +221,28 @@ class MeshCollisionTraversalNode : public BVHCollisionTraversalNode<BV> {
     FCL_REAL distance;
     solver.shapeDistance(tri1, this->tf1, tri2, this->tf2, distance, p1, p2,
                          normal);
-    FCL_REAL distToCollision = distance - this->request.security_margin;
-    sqrDistLowerBound = distance * distance;
-    if (distToCollision <= 0) {  // collision
-      Vec3f p(p1);               // contact point
-      FCL_REAL penetrationDepth(0);
+
+    const FCL_REAL distToCollision = distance - this->request.security_margin;
+    if (distToCollision <=
+        this->request.collision_distance_threshold) {  // collision
+      sqrDistLowerBound = 0;
+      Vec3f p(p1);  // contact point
       if (this->result->numContacts() < this->request.num_max_contacts) {
         // How much (Q1, Q2, Q3) should be moved so that all vertices are
         // above (P1, P2, P3).
-        penetrationDepth = -distance;
         if (distance > 0) {
           normal = (p2 - p1).normalized();
           p = .5 * (p1 + p2);
         }
         this->result->addContact(Contact(this->model1, this->model2,
                                          primitive_id1, primitive_id2, p,
-                                         normal, penetrationDepth));
+                                         normal, -distance));
       }
-    }
+    } else
+      sqrDistLowerBound = distToCollision * distToCollision;
+
+    internal::updateDistanceLowerBoundFromLeaf(this->request, *this->result,
+                                               distToCollision, p1, p2);
   }
 
   Vec3f* vertices1;