Merge pull request #329 from lmontaut/normal_and_nearest_points

Normal and nearest points uniformization

include/hpp/fcl/collision_data.h

@@ -39,6 +39,7 @@
 #define HPP_FCL_COLLISION_DATA_H
 
 #include <vector>
+#include <array>
 #include <set>
 #include <limits>
 
@@ -70,9 +71,14 @@ struct HPP_FCL_DLLAPI Contact {
   /// if object 2 is octree, it is the id of the cell
   int b2;
 
-  /// @brief contact normal, pointing from o1 to o2
+  /// @brief contact normal, pointing from o1 to o2.
+  /// See DistanceResult::normal for a complete definition of the normal.
   Vec3f normal;
 
+  /// @brief nearest points associated to this contact.
+  /// See \ref CollisionResult::nearest_points.
+  std::array<Vec3f, 2> nearest_points;
+
   /// @brief contact position, in world space
   Vec3f pos;
 
@@ -97,7 +103,24 @@ struct HPP_FCL_DLLAPI Contact {
         b2(b2_),
         normal(normal_),
         pos(pos_),
-        penetration_depth(depth_) {}
+        penetration_depth(depth_) {
+    nearest_points[0] = pos - 0.5 * depth_ * normal_;
+    nearest_points[1] = pos + 0.5 * depth_ * normal_;
+  }
+
+  Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_,
+          int b2_, const Vec3f& p1, const Vec3f& p2, const Vec3f& normal_,
+          FCL_REAL depth_)
+      : o1(o1_),
+        o2(o2_),
+        b1(b1_),
+        b2(b2_),
+        normal(normal_),
+        penetration_depth(depth_) {
+    nearest_points[0] = p1;
+    nearest_points[1] = p2;
+    pos = (p1 + p2) / 2;
+  }
 
   bool operator<(const Contact& other) const {
     if (b1 == other.b1) return b2 < other.b2;
@@ -117,7 +140,7 @@ struct QueryResult;
 
 /// @brief base class for all query requests
 struct HPP_FCL_DLLAPI QueryRequest {
-  // @briefInitial guess to use for the GJK algorithm
+  // @brief Initial guess to use for the GJK algorithm
   GJKInitialGuess gjk_initial_guess;
 
   /// @brief whether enable gjk initial guess
@@ -233,11 +256,13 @@ enum CollisionRequestFlag {
 
 /// @brief request to the collision algorithm
 struct HPP_FCL_DLLAPI CollisionRequest : QueryRequest {
-  /// @brief The maximum number of contacts will return
+  /// @brief The maximum number of contacts that can be returned
   size_t num_max_contacts;
 
   /// @brief whether the contact information (normal, penetration depth and
   /// contact position) will return
+  /// @note Only effective if the collision pair involves an Octree.
+  /// Otherwise, it is always true.
   bool enable_contact;
 
   /// Whether a lower bound on distance is returned when objects are disjoint
@@ -307,13 +332,21 @@ struct HPP_FCL_DLLAPI CollisionResult : QueryResult {
  public:
   /// Lower bound on distance between objects if they are disjoint.
   /// See \ref hpp_fcl_collision_and_distance_lower_bound_computation
-  /// @note computed only on request (or if it does not add any computational
-  /// overhead).
+  /// @note Always computed. If \ref CollisionRequest::distance_upper_bound is
+  /// set to infinity, distance_lower_bound is the actual distance between the
+  /// shapes.
   FCL_REAL distance_lower_bound;
 
   /// @brief nearest points
   /// available only when distance_lower_bound is inferior to
   /// CollisionRequest::break_distance.
+  /// @note Also referred as "witness points" in other collision libraries.
+  /// The points p1 = nearest_points[0] and p2 = nearest_points[1] verify the
+  /// property that dist(o1, o2) * (p1 - p2) is the separation vector between o1
+  /// and o2, with dist(o1, o2) being the **signed** distance separating o1 from
+  /// o2. See \ref DistanceResult::normal for the definition of the separation
+  /// vector. If o1 and o2 have multiple contacts, the nearest_points are
+  /// associated with the contact which has the greatest penetration depth.
   Vec3f nearest_points[2];
 
  public:
@@ -419,14 +452,24 @@ struct HPP_FCL_DLLAPI DistanceRequest : QueryRequest {
 /// @brief distance result
 struct HPP_FCL_DLLAPI DistanceResult : QueryResult {
  public:
-  /// @brief minimum distance between two objects. if two objects are in
+  /// @brief minimum distance between two objects. If two objects are in
   /// collision, min_distance <= 0.
   FCL_REAL min_distance;
 
-  /// @brief nearest points
-  Vec3f nearest_points[2];
-
-  /// In case both objects are in collision, store the normal
+  /// @brief nearest points.
+  /// See CollisionResult::nearest_points.
+  std::array<Vec3f, 2> nearest_points;
+
+  /// Stores the normal, defined as the normalized separation vector:
+  /// normal = (p2 - p1) / dist(o1, o2), where p1 = nearest_points[0]
+  /// belongs to o1 and p2 = nearest_points[1] belongs to o2 and dist(o1, o2) is
+  /// the **signed** distance between o1 and o2. The normal always points from
+  /// o1 to o2.
+  /// @note The separation vector is the smallest vector such that if o1 is
+  /// translated by it, o1 and o2 are in touching contact (they share at least
+  /// one contact point but have a zero intersection volume). If the shapes
+  /// overlap, dist(o1, o2) = -((p2-p1).norm()). Otherwise, dist(o1, o2) =
+  /// (p2-p1).norm().
   Vec3f normal;
 
   /// @brief collision object 1

include/hpp/fcl/internal/shape_shape_func.h

@@ -66,6 +66,7 @@ struct ShapeShapeCollider {
     if (request.isSatisfied(result)) return result.numContacts();
 
     DistanceResult distanceResult;
+    // (louis) enable_contact not used yet but may be in the future
     DistanceRequest distanceRequest(request.enable_contact);
     FCL_REAL distance = ShapeShapeDistance<T_SH1, T_SH2>(
         o1, tf1, o2, tf2, nsolver, distanceRequest, distanceResult);
@@ -83,10 +84,8 @@ struct ShapeShapeCollider {
         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);
+        Contact contact(o1, o2, distanceResult.b1, distanceResult.b2, p1, p2,
+                        distanceResult.normal, distance);
 
         result.addContact(contact);
       }
@@ -136,6 +135,11 @@ SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Sphere, Halfspace);
 SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Sphere, Plane);
 SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Sphere, Sphere);
 SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Sphere, Cylinder);
+// TODO
+// SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Sphere, Capsule);
+// SHAPE_SHAPE_DISTANCE_SPECIALIZATION(TriangleP, TriangleP);
+// SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Ellispoids, Plane);
+// SHAPE_SHAPE_DISTANCE_SPECIALIZATION(Ellispoids, Halfspace);
 
 SHAPE_SHAPE_DISTANCE_SPECIALIZATION(ConvexBase, Halfspace);
 SHAPE_SHAPE_DISTANCE_SPECIALIZATION(TriangleP, Halfspace);

include/hpp/fcl/narrowphase/narrowphase.h

@@ -132,7 +132,7 @@ struct HPP_FCL_DLLAPI GJKSolver {
           gjk.getClosestPoints(shape, w0, w1);
           distance_lower_bound = gjk.distance;
           if (normal)
-            (*normal).noalias() = tf1.getRotation() * (w0 - w1).normalized();
+            (*normal).noalias() = tf1.getRotation() * (w1 - w0).normalized();
           if (contact_points) *contact_points = tf1.transform((w0 + w1) / 2);
           return true;
         } else {
@@ -206,7 +206,7 @@ struct HPP_FCL_DLLAPI GJKSolver {
         if (gjk.hasPenetrationInformation(shape)) {
           gjk.getClosestPoints(shape, w0, w1);
           distance = gjk.distance;
-          normal.noalias() = tf1.getRotation() * (w0 - w1).normalized();
+          normal.noalias() = tf1.getRotation() * (w1 - w0).normalized();
           p1 = p2 = tf1.transform((w0 + w1) / 2);
         } else {
           details::EPA epa(epa_max_face_num, epa_max_vertex_num,
@@ -289,7 +289,7 @@ struct HPP_FCL_DLLAPI GJKSolver {
       // assert (distance == (w0 - w1).norm());
       distance = gjk.distance;
 
-      normal.noalias() = tf1.getRotation() * gjk.ray;
+      normal.noalias() = -tf1.getRotation() * gjk.ray;
       normal.normalize();
       p1 = tf1.transform(p1);
       p2 = tf1.transform(p2);
@@ -302,7 +302,7 @@ struct HPP_FCL_DLLAPI GJKSolver {
         // Return contact points in case of collision
         // p1 = tf1.transform (p1);
         // p2 = tf1.transform (p2);
-        normal.noalias() = tf1.getRotation() * (p1 - p2);
+        normal.noalias() = tf1.getRotation() * (p2 - p1);
         normal.normalize();
         p1 = tf1.transform(p1);
         p2 = tf1.transform(p2);

include/hpp/fcl/serialization/collision_data.h

@@ -17,6 +17,7 @@ void save(Archive& ar, const hpp::fcl::Contact& contact,
   ar& make_nvp("b1", contact.b1);
   ar& make_nvp("b2", contact.b2);
   ar& make_nvp("normal", contact.normal);
+  ar& make_nvp("nearest_points", contact.nearest_points);
   ar& make_nvp("pos", contact.pos);
   ar& make_nvp("penetration_depth", contact.penetration_depth);
 }
@@ -27,6 +28,10 @@ void load(Archive& ar, hpp::fcl::Contact& contact,
   ar >> make_nvp("b1", contact.b1);
   ar >> make_nvp("b2", contact.b2);
   ar >> make_nvp("normal", contact.normal);
+  std::array<hpp::fcl::Vec3f, 2> nearest_points;
+  ar >> make_nvp("nearest_points", nearest_points);
+  contact.nearest_points[0] = nearest_points[0];
+  contact.nearest_points[1] = nearest_points[1];
   ar >> make_nvp("pos", contact.pos);
   ar >> make_nvp("penetration_depth", contact.penetration_depth);
   contact.o1 = NULL;
@@ -115,7 +120,7 @@ void save(Archive& ar, const hpp::fcl::DistanceResult& distance_result,
   ar& make_nvp("base", boost::serialization::base_object<hpp::fcl::QueryResult>(
                            distance_result));
   ar& make_nvp("min_distance", distance_result.min_distance);
-  ar& make_nvp("nearest_points", make_array(distance_result.nearest_points, 2));
+  ar& make_nvp("nearest_points", distance_result.nearest_points);
   ar& make_nvp("normal", distance_result.normal);
   ar& make_nvp("b1", distance_result.b1);
   ar& make_nvp("b2", distance_result.b2);
@@ -128,8 +133,10 @@ void load(Archive& ar, hpp::fcl::DistanceResult& distance_result,
       make_nvp("base", boost::serialization::base_object<hpp::fcl::QueryResult>(
                            distance_result));
   ar >> make_nvp("min_distance", distance_result.min_distance);
-  ar >>
-      make_nvp("nearest_points", make_array(distance_result.nearest_points, 2));
+  std::array<hpp::fcl::Vec3f, 2> nearest_points;
+  ar >> make_nvp("nearest_points", nearest_points);
+  distance_result.nearest_points[0] = nearest_points[0];
+  distance_result.nearest_points[1] = nearest_points[1];
   ar >> make_nvp("normal", distance_result.normal);
   ar >> make_nvp("b1", distance_result.b1);
   ar >> make_nvp("b2", distance_result.b2);

python/collision.cc

@@ -59,6 +59,15 @@ const CollisionGeometry* geto(const Contact& c) {
   return index == 1 ? c.o1 : c.o2;
 }
 
+struct ContactWrapper {
+  static Vec3f getNearestPoint1(const Contact& contact) {
+    return contact.nearest_points[0];
+  }
+  static Vec3f getNearestPoint2(const Contact& contact) {
+    return contact.nearest_points[1];
+  }
+};
+
 void exposeCollisionAPI() {
   if (!eigenpy::register_symbolic_link_to_registered_type<
           CollisionRequestFlag>()) {
@@ -152,9 +161,14 @@ void exposeCollisionAPI() {
             make_function(&geto<2>,
                           return_value_policy<reference_existing_object>()),
             doxygen::class_attrib_doc<Contact>("o2"))
+        .def("getNearestPoint1", &ContactWrapper::getNearestPoint1,
+             doxygen::class_attrib_doc<Contact>("nearest_points"))
+        .def("getNearestPoint2", &ContactWrapper::getNearestPoint2,
+             doxygen::class_attrib_doc<Contact>("nearest_points"))
         .DEF_RW_CLASS_ATTRIB(Contact, b1)
         .DEF_RW_CLASS_ATTRIB(Contact, b2)
         .DEF_RW_CLASS_ATTRIB(Contact, normal)
+        .DEF_RW_CLASS_ATTRIB(Contact, nearest_points)
         .DEF_RW_CLASS_ATTRIB(Contact, pos)
         .DEF_RW_CLASS_ATTRIB(Contact, penetration_depth)
         .def(self == self)

python/distance.cc

@@ -88,6 +88,7 @@ void exposeDistanceAPI() {
              doxygen::class_attrib_doc<DistanceResult>("nearest_points"))
         .def("getNearestPoint2", &DistanceRequestWrapper::getNearestPoint2,
              doxygen::class_attrib_doc<DistanceResult>("nearest_points"))
+        .DEF_RO_CLASS_ATTRIB(DistanceResult, nearest_points)
         .DEF_RO_CLASS_ATTRIB(DistanceResult, o1)
         .DEF_RO_CLASS_ATTRIB(DistanceResult, o2)
         .DEF_RW_CLASS_ATTRIB(DistanceResult, b1)

src/collision.cpp

@@ -70,7 +70,7 @@ std::size_t collide(const CollisionObject* o1, const CollisionObject* o2,
 std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1,
                     const CollisionGeometry* o2, const Transform3f& tf2,
                     const CollisionRequest& request, CollisionResult& result) {
-  // If securit margin is set to -infinity, return that there is no collision
+  // If security margin is set to -infinity, return that there is no collision
   if (request.security_margin == -std::numeric_limits<FCL_REAL>::infinity()) {
     result.clear();
     return false;

src/distance/capsule_capsule.cpp

@@ -80,7 +80,7 @@ template <>
 FCL_REAL ShapeShapeDistance<Capsule, Capsule>(
     const CollisionGeometry* o1, const Transform3f& tf1,
     const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*,
-    const DistanceRequest& request, DistanceResult& result) {
+    const DistanceRequest& /*request*/, DistanceResult& result) {
   const Capsule* capsule1 = static_cast<const Capsule*>(o1);
   const Capsule* capsule2 = static_cast<const Capsule*>(o2);
 
@@ -153,18 +153,15 @@ FCL_REAL ShapeShapeDistance<Capsule, Capsule>(
 
   // witness points achieving the distance between the two segments
   FCL_REAL distance = (w1 - w2).norm();
-  const Vec3f normal = (w1 - w2) / distance;
-  result.normal = normal;
 
   // capsule spcecific distance computation
   distance = distance - (radius1 + radius2);
   result.min_distance = distance;
 
-  // witness points for the capsules
-  if (request.enable_nearest_points) {
-    result.nearest_points[0] = w1 - radius1 * normal;
-    result.nearest_points[1] = w2 + radius2 * normal;
-  }
+  // Normal points from o1 to o2
+  result.normal = (w2 - w1).normalized();
+  result.nearest_points[0] = w1 + radius1 * result.normal;
+  result.nearest_points[1] = w2 - radius2 * result.normal;
 
   return distance;
 }

src/distance/sphere_sphere.cpp

@@ -74,22 +74,10 @@ FCL_REAL ShapeShapeDistance<Sphere, Sphere>(
   FCL_REAL dist = c1c2.norm();
   Vec3f unit(0, 0, 0);
   if (dist > epsilon) unit = c1c2 / dist;
-  FCL_REAL penetrationDepth;
-  penetrationDepth = r1 + r2 - dist;
-  bool collision = (penetrationDepth >= 0);
-  result.min_distance = -penetrationDepth;
-  if (collision) {
-    // Take contact point at the middle of intersection between each sphere
-    // and segment [c1 c2].
-    FCL_REAL abscissa = .5 * r1 + .5 * (dist - r2);
-    Vec3f contact = center1 + abscissa * unit;
-    result.nearest_points[0] = result.nearest_points[1] = contact;
-    return result.min_distance;
-  } else {
-    FCL_REAL abs1(r1), abs2(dist - r2);
-    result.nearest_points[0] = center1 + abs1 * unit;
-    result.nearest_points[1] = center1 + abs2 * unit;
-  }
+  result.min_distance = dist - (r1 + r2);
+  result.normal = unit;
+  result.nearest_points[0] = center1 + r1 * unit;
+  result.nearest_points[1] = center2 - r2 * unit;
   return result.min_distance;
 }
 
@@ -101,7 +89,7 @@ std::size_t ShapeShapeCollider<Sphere, Sphere>::run(
   const Sphere* s1 = static_cast<const Sphere*>(o1);
   const Sphere* s2 = static_cast<const Sphere*>(o2);
 
-  // We assume that capsules are centered at the origin.
+  // We assume that spheres are centered at the origin.
   const fcl::Vec3f& center1 = tf1.getTranslation();
   const fcl::Vec3f& center2 = tf2.getTranslation();
   FCL_REAL r1 = s1->radius;
@@ -119,12 +107,8 @@ std::size_t ShapeShapeCollider<Sphere, Sphere>::run(
                                              center1 + unit * r1,
                                              center2 - unit * r2);
   if (distToCollision <= request.collision_distance_threshold) {
-    // Take contact point at the middle of intersection between each sphere
-    // and segment [c1 c2].
-    FCL_REAL abscissa = .5 * r1 + .5 * (dist - r2);
-    Vec3f contactPoint = center1 + abscissa * unit;
-    Contact contact(o1, o2, -1, -1, contactPoint, unit,
-                    -(distToCollision + margin));
+    Contact contact(o1, o2, -1, -1, center1 + unit * r1, center2 - unit * r2,
+                    unit, distToCollision + margin);
     result.addContact(contact);
     return 1;
   }