Merge pull request #149 from jmirabel/devel

Bug fix + prepare optimization of collision using GJK / EPA

include/hpp/fcl/collision_data.h

@@ -223,9 +223,16 @@ struct CollisionResult
 
 public:
   CollisionResult()
+    : distance_lower_bound (std::numeric_limits<FCL_REAL>::max())
   {
   }
 
+  /// @brief Update the lower bound only if the distance in inferior.
+  inline void updateDistanceLowerBound (const FCL_REAL& distance_lower_bound_)
+  {
+    if (distance_lower_bound_ < distance_lower_bound)
+      distance_lower_bound = distance_lower_bound_;
+  }
 
   /// @brief add one contact into result structure
   inline void addContact(const Contact& c) 

include/hpp/fcl/internal/traversal_node_octree.h

@@ -304,7 +304,8 @@ class OcTreeSolver
         Transform3f box_tf;
         constructBox(bv1, tf1, box, box_tf);
 
-        if(solver->shapeIntersect(box, box_tf, s, tf2, NULL, NULL, NULL))
+        FCL_REAL distance;
+        if(solver->shapeIntersect(box, box_tf, s, tf2, distance, false, NULL, NULL))
         {
           AABB overlap_part;
           AABB aabb1, aabb2;
@@ -328,9 +329,10 @@ class OcTreeSolver
           Transform3f box_tf;
           constructBox(bv1, tf1, box, box_tf);
 
+          FCL_REAL distance;
           if(!crequest->enable_contact)
           {
-            if(solver->shapeIntersect(box, box_tf, s, tf2, NULL, NULL, NULL))
+            if(solver->shapeIntersect(box, box_tf, s, tf2, distance, false, NULL, NULL))
             {
               if(cresult->numContacts() < crequest->num_max_contacts)
                 cresult->addContact(Contact(tree1, &s, static_cast<int>(root1 - tree1->getRoot()), Contact::NONE));
@@ -339,13 +341,12 @@ class OcTreeSolver
           else
           {
             Vec3f contact;
-            FCL_REAL depth;
             Vec3f normal;
 
-            if(solver->shapeIntersect(box, box_tf, s, tf2, &contact, &depth, &normal))
+            if(solver->shapeIntersect(box, box_tf, s, tf2, distance, true, &contact, &normal))
             {
               if(cresult->numContacts() < crequest->num_max_contacts)
-                cresult->addContact(Contact(tree1, &s, static_cast<int>(root1 - tree1->getRoot()), Contact::NONE, contact, normal, depth));
+                cresult->addContact(Contact(tree1, &s, static_cast<int>(root1 - tree1->getRoot()), Contact::NONE, contact, normal, distance));
             }
           }
 
@@ -819,12 +820,12 @@ class OcTreeSolver
           constructBox(bv2, tf2, box2, box2_tf);
 
           Vec3f contact;
-          FCL_REAL depth;
+          FCL_REAL distance;
           Vec3f normal;
-          if(solver->shapeIntersect(box1, box1_tf, box2, box2_tf, &contact, &depth, &normal))
+          if(solver->shapeIntersect(box1, box1_tf, box2, box2_tf, distance, true, &contact, &normal))
           {
             if(cresult->numContacts() < crequest->num_max_contacts)
-              cresult->addContact(Contact(tree1, tree2, static_cast<int>(root1 - tree1->getRoot()), static_cast<int>(root2 - tree2->getRoot()), contact, normal, depth));
+              cresult->addContact(Contact(tree1, tree2, static_cast<int>(root1 - tree1->getRoot()), static_cast<int>(root2 - tree2->getRoot()), contact, normal, distance));
           }
         }
 

include/hpp/fcl/internal/traversal_node_shapes.h

@@ -84,23 +84,26 @@ class ShapeCollisionTraversalNode : public CollisionTraversalNodeBase
   void leafCollides(int, int, FCL_REAL&) const
   {
     bool is_collision = false;
-    if(request.enable_contact)
+    FCL_REAL distance;
+    if(request.enable_contact && request.num_max_contacts > result->numContacts())
     {
       Vec3f contact_point, normal;
-      FCL_REAL penetration_depth;
-      if(nsolver->shapeIntersect(*model1, tf1, *model2, tf2, &contact_point,
-                                 &penetration_depth, &normal))
+      if(nsolver->shapeIntersect(*model1, tf1, *model2, tf2, distance, true,
+                                 &contact_point, &normal))
       {
         is_collision = true;
-        if(request.num_max_contacts > result->numContacts())
-          result->addContact(Contact(model1, model2, Contact::NONE,
-                                     Contact::NONE, contact_point,
-                                     normal, penetration_depth));
+        result->addContact(Contact(model1, model2, Contact::NONE,
+                                   Contact::NONE, contact_point,
+                                   normal, distance));
       }
     }
     else
     {
-      if(nsolver->shapeIntersect(*model1, tf1, *model2, tf2, NULL, NULL, NULL))
+      bool res = nsolver->shapeIntersect(*model1, tf1, *model2, tf2, distance,
+          request.enable_distance_lower_bound, NULL, NULL);
+      if (request.enable_distance_lower_bound)
+        result->updateDistanceLowerBound (distance);
+      if(res)
       {
         is_collision = true;
         if(request.num_max_contacts > result->numContacts())

include/hpp/fcl/narrowphase/narrowphase.h

@@ -56,7 +56,9 @@ namespace fcl
     template<typename S1, typename S2>
       bool shapeIntersect(const S1& s1, const Transform3f& tf1,
                           const S2& s2, const Transform3f& tf2,
-                          Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+                          FCL_REAL& distance_lower_bound,
+                          bool enable_penetration,
+                          Vec3f* contact_points, Vec3f* normal) const
     {
       Vec3f guess(1, 0, 0);
       if(enable_cached_guess) guess = cached_guess;
@@ -71,29 +73,36 @@ namespace fcl
       Vec3f w0, w1;
       switch(gjk_status) {
         case details::GJK::Inside:
+          if (!enable_penetration && contact_points == NULL && normal == NULL)
+            return true;
           if (gjk.hasPenetrationInformation(shape)) {
             gjk.getClosestPoints (shape, w0, w1);
-            if(penetration_depth) *penetration_depth = gjk.distance;
+            distance_lower_bound = gjk.distance;
             if(normal) *normal = tf1.getRotation() * (w0 - w1).normalized();
             if(contact_points) *contact_points = tf1.transform((w0 + w1) / 2);
             return true;
           } else {
             details::EPA epa(epa_max_face_num, epa_max_vertex_num, epa_max_iterations, epa_tolerance);
             details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
-            if(epa_status & details::EPA::Failed)
+            if(epa_status & details::EPA::Valid
+                || epa_status == details::EPA::OutOfFaces    // Warnings
+                || epa_status == details::EPA::OutOfVertices // Warnings
+                )
             {
               epa.getClosestPoints (shape, w0, w1);
-              if(penetration_depth) *penetration_depth = -epa.depth;
-              // TODO The normal computed by GJK in the s1 frame so this should be
-              // if(normal) *normal = tf1.getRotation() * epa.normal;
-              if(normal) *normal = tf2.getRotation() * epa.normal;
+              distance_lower_bound = -epa.depth;
+              if(normal) *normal = tf1.getRotation() * epa.normal;
               if(contact_points) *contact_points = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
               return true;
             }
+            distance_lower_bound = -std::numeric_limits<FCL_REAL>::max();
             // EPA failed but we know there is a collision so we should
             return true;
           }
           break;
+        case details::GJK::Valid:
+          distance_lower_bound = gjk.distance;
+          break;
         default:
           ;
       }
@@ -139,13 +148,21 @@ namespace fcl
           } else {
             details::EPA epa(epa_max_face_num, epa_max_vertex_num, epa_max_iterations, epa_tolerance);
             details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
-            assert (epa_status & details::EPA::Valid); (void) epa_status;
-
-            epa.getClosestPoints (shape, w0, w1);
-            distance = -epa.depth;
-            normal = -epa.normal;
-            p1 = p2 = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
-            assert (distance <= 1e-6);
+            if(epa_status & details::EPA::Valid
+                || epa_status == details::EPA::OutOfFaces    // Warnings
+                || epa_status == details::EPA::OutOfVertices // Warnings
+                )
+            {
+              epa.getClosestPoints (shape, w0, w1);
+              distance = -epa.depth;
+              normal = -epa.normal;
+              p1 = p2 = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
+              assert (distance <= 1e-6);
+            } else {
+              distance = -std::numeric_limits<FCL_REAL>::max();
+              gjk.getClosestPoints (shape, w0, w1);
+              p1 = p2 = tf1.transform (w0);
+            }
           }
           break;
         case details::GJK::Valid:
@@ -240,7 +257,11 @@ namespace fcl
               // normal = tf1.getRotation() * epa.normal;
               normal = tf2.getRotation() * epa.normal;
               p1 = p2 = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
+              return false;
             }
+            distance = -std::numeric_limits<FCL_REAL>::max();
+            gjk.getClosestPoints (shape, p1, p2);
+            p1 = p2 = tf1.transform (p1);
           }
           return false;
         }
@@ -305,15 +326,16 @@ namespace fcl
   /// \{
 
 // param doc is the doxygen detailled description (should be enclosed in /** */
-// and contain no space.
+// and contain no dot for some obscure reasons).
 #define HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape1,Shape2,doc)                     \
   /** @brief Fast implementation for Shape1-Shape2 collision. */               \
   doc                                                                          \
   template<>                                                                   \
     bool GJKSolver::shapeIntersect<Shape1, Shape2>                             \
     (const Shape1& s1, const Transform3f& tf1,                                 \
      const Shape2& s2, const Transform3f& tf2,                                 \
-     Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+     FCL_REAL& distance_lower_bound, bool enable_penetration,                  \
+     Vec3f* contact_points, Vec3f* normal) const
 #define HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Shape,doc)                        \
   HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape,Shape,doc)
 #define HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Shape1,Shape2,doc)                \
@@ -325,7 +347,17 @@ namespace fcl
   HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Sphere, Halfspace,);
   HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Sphere, Plane,);
 
-  HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Box,);
+#ifdef IS_DOXYGEN // for doxygen only
+  /** \todo currently disabled and to re-enable it, API of function
+   *  \ref obbDisjointAndLowerBoundDistance should be modified.
+   *  */
+  template<> bool GJKSolver::shapeIntersect<Box, Box>
+    (const Box& s1, const Transform3f& tf1,
+     const Box& s2, const Transform3f& tf2,
+     FCL_REAL& distance_lower_bound, bool enable_penetration,
+     Vec3f* contact_points, Vec3f* normal) const;
+#endif
+  //HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Box,);
   HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Box, Halfspace,);
   HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Box, Plane,);
 
@@ -352,6 +384,8 @@ namespace fcl
   /// \name Shape triangle interaction specializations
   /// \{
 
+// param doc is the doxygen detailled description (should be enclosed in /** */
+// and contain no dot for some obscure reasons).
 #define HPP_FCL_DECLARE_SHAPE_TRIANGLE(Shape,doc)                              \
   /** @brief Fast implementation for Shape-Triangle interaction. */            \
   doc                                                                          \
@@ -372,7 +406,7 @@ namespace fcl
   /// \{
 
 // param doc is the doxygen detailled description (should be enclosed in /** */
-// and contain no space.
+// and contain no dot for some obscure reasons).
 #define HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape1,Shape2,doc)                      \
   /** @brief Fast implementation for Shape1-Shape2 distance. */                \
   doc                                                                          \

src/collision.cpp

@@ -89,7 +89,6 @@ std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1,
     solver.cached_guess = request.cached_gjk_guess;
 
   const CollisionFunctionMatrix& looktable = getCollisionFunctionLookTable();
-  result.distance_lower_bound = -1;
   std::size_t res;
   if(request.num_max_contacts == 0)
   {

src/collision_func_matrix.cpp

@@ -109,7 +109,7 @@ std::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf
     }
     return 1;
   }
-  result.distance_lower_bound = distance;
+  result.updateDistanceLowerBound (distance);
   return 0;
 }
 

src/collision_node.cpp

@@ -60,7 +60,7 @@ void collide(CollisionTraversalNodeBase* node,
       collisionRecurse(node, 0, 0, front_list, sqrDistLowerBound);
     else
       collisionNonRecurse(node, front_list, sqrDistLowerBound);
-    result.distance_lower_bound = sqrt (sqrDistLowerBound);
+    result.updateDistanceLowerBound (sqrt (sqrDistLowerBound));
   }
 }
 

src/distance_sphere_sphere.cpp

@@ -132,7 +132,7 @@ namespace fcl {
       result.addContact (contact);
       return 1;
     }
-    result.distance_lower_bound = -penetrationDepth;
+    result.updateDistanceLowerBound (-penetrationDepth);
     return 0;
   }
 } // namespace fcl