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 @@ public:
 
 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 @@ private:
         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 @@ private:
           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 @@ private:
           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 @@ private:
           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 @@ public:
   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,7 +326,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_INTERSECT(Shape1,Shape2,doc)                     \
   /** @brief Fast implementation for Shape1-Shape2 collision. */               \
   doc                                                                          \
@@ -313,7 +334,8 @@ namespace fcl
     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

src/narrowphase/details.h

@@ -71,7 +71,7 @@ namespace fcl {
     inline bool sphereCapsuleIntersect
       (const Sphere& s1, const Transform3f& tf1,
        const Capsule& s2, const Transform3f& tf2,
-       Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal_)
+       FCL_REAL& distance, Vec3f* contact_points, Vec3f* normal_)
     {
       Vec3f pos1 (tf2.transform (Vec3f (0., 0.,  s2.halfLength))); // from distance function
       Vec3f pos2 (tf2.transform (Vec3f (0., 0., -s2.halfLength)));
@@ -83,16 +83,13 @@ namespace fcl {
       Vec3f diff = s_c - segment_point;
 
       FCL_REAL diffN = diff.norm();
-      FCL_REAL distance = diffN - s1.radius - s2.radius;
+      distance = diffN - s1.radius - s2.radius;
 
       if (distance > 0)
         return false;
 
       // Vec3f normal (-diff.normalized());
 
-      if (distance < 0 && penetration_depth)
-        *penetration_depth = -distance;
-
       if (normal_)
         *normal_ = -diff / diffN;
 
@@ -229,16 +226,14 @@ namespace fcl {
     inline bool sphereSphereIntersect
       (const Sphere& s1, const Transform3f& tf1,
        const Sphere& s2, const Transform3f& tf2,
-       Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal)
+       FCL_REAL& distance, Vec3f* contact_points, Vec3f* normal)
     {
       const Vec3f diff = tf2.getTranslation() - tf1.getTranslation();
       FCL_REAL len = diff.norm();
-      if(len > s1.radius + s2.radius)
+      distance = len - s1.radius - s2.radius;
+      if(distance > 0)
         return false;
 
-      if(penetration_depth)
-        *penetration_depth = s1.radius + s2.radius - len;
-
       // If the centers of two sphere are at the same position, the normal is (0, 0, 0).
       // Otherwise, normal is pointing from center of object 1 to center of object 2
       if(normal)
@@ -1843,29 +1838,30 @@ namespace fcl {
 
       ret = 0;
 
+      penetration_depth = std::numeric_limits<FCL_REAL>::max();
       Vec3f dir = (new_s1.n).cross(new_s2.n);
       FCL_REAL dir_norm = dir.squaredNorm();
       if(dir_norm < std::numeric_limits<FCL_REAL>::epsilon()) // parallel
         {
           if((new_s1.n).dot(new_s2.n) > 0)
             {
-              if(new_s1.d < new_s2.d)
+              penetration_depth = new_s2.d - new_s1.d;
+              if(penetration_depth < 0)
+                return false;
+              else
                 {
-                  penetration_depth = new_s2.d - new_s1.d;
                   ret = 1;
                   pl = new_s1;
                   return true;
                 }
-              else
-                return false;
             }
           else
             {
-              if(new_s1.d + new_s2.d > 0)
+              penetration_depth = -(new_s1.d + new_s2.d);
+              if(penetration_depth < 0)
                 return false;
               else
                 {
-                  penetration_depth = -(new_s1.d + new_s2.d);
                   ret = 2;
                   pl = new_s1;
                   return true;
@@ -1880,7 +1876,6 @@ namespace fcl {
       p = origin;
       d = dir;
       ret = 3;
-      penetration_depth = std::numeric_limits<FCL_REAL>::max();
 
       return true;
     }
@@ -1904,6 +1899,7 @@ namespace fcl {
 
       ret = 0;
 
+      penetration_depth = std::numeric_limits<FCL_REAL>::max();
       Vec3f dir = (new_s1.n).cross(new_s2.n);
       FCL_REAL dir_norm = dir.squaredNorm();
       if(dir_norm < std::numeric_limits<FCL_REAL>::epsilon()) // parallel
@@ -1913,25 +1909,23 @@ namespace fcl {
               if(new_s1.d < new_s2.d) // s1 is inside s2
                 {
                   ret = 1;
-                  penetration_depth = std::numeric_limits<FCL_REAL>::max();
                   s = new_s1;
                 }
               else // s2 is inside s1
                 {
                   ret = 2;
-                  penetration_depth = std::numeric_limits<FCL_REAL>::max();
                   s = new_s2;
                 }
               return true;
             }
           else
             {
-              if(new_s1.d + new_s2.d > 0) // not collision
+              penetration_depth = -(new_s1.d + new_s2.d);
+              if(penetration_depth < 0) // not collision
                 return false;
               else // in each other
                 {
                   ret = 3;
-                  penetration_depth = -(new_s1.d + new_s2.d);
                   return true;
                 }
             }
@@ -1944,7 +1938,6 @@ namespace fcl {
       p = origin;
       d = dir;
       ret = 4;
-      penetration_depth = std::numeric_limits<FCL_REAL>::max();
 
       return true;
     }

src/narrowphase/gjk.cpp

@@ -120,7 +120,8 @@ void getShapeSupport(const TriangleP* triangle, const Vec3f& dir, Vec3f& support
 
 inline void getShapeSupport(const Box* box, const Vec3f& dir, Vec3f& support)
 {
-  support.noalias() = (dir.array() > 0).select(box->halfSide, -box->halfSide);
+  const FCL_REAL inflate = (dir.array() == 0).any() ? 1.00000001 : 1.;
+  support.noalias() = (dir.array() > 0).select(inflate * box->halfSide, -inflate * box->halfSide);
 }
 
 inline void getShapeSupport(const Sphere*, const Vec3f& /*dir*/, Vec3f& support)

src/traversal/traversal_recurse.cpp

@@ -399,7 +399,7 @@ void propagateBVHFrontListCollisionRecurse
         }
       }
     }
-    result.distance_lower_bound = sqrt (sqrDistLowerBound);
+    result.updateDistanceLowerBound (sqrt (sqrDistLowerBound));
   }
 
 

test/box_box_distance.cpp

@@ -217,3 +217,42 @@ BOOST_AUTO_TEST_CASE(distance_box_box_3)
   BOOST_CHECK_CLOSE (p2 [2], p2Moved [2], 1e-4);
   
 }
+
+BOOST_AUTO_TEST_CASE(distance_box_box_4)
+{
+  hpp::fcl::Box s1 (1, 1, 1);
+  hpp::fcl::Box s2 (1, 1, 1);
+
+  // Enable computation of nearest points
+  DistanceRequest distanceRequest (true, 0, 0);
+  DistanceResult distanceResult;
+  double distance;
+
+  Transform3f tf1 (Vec3f (2, 0, 0));
+  Transform3f tf2;
+  hpp::fcl::distance (&s1, tf1, &s2, tf2, distanceRequest, distanceResult);
+
+  distance = 1.;
+  BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 1e-4);
+
+  tf1.setTranslation(Vec3f (1.01, 0, 0));
+  distanceResult.clear();
+  hpp::fcl::distance (&s1, tf1, &s2, tf2, distanceRequest, distanceResult);
+
+  distance = 0.01;
+  BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 2e-3);
+
+  tf1.setTranslation(Vec3f (0.99, 0, 0));
+  distanceResult.clear();
+  hpp::fcl::distance (&s1, tf1, &s2, tf2, distanceRequest, distanceResult);
+
+  distance = -0.01;
+  BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 2e-3);
+
+  tf1.setTranslation(Vec3f (0, 0, 0));
+  distanceResult.clear();
+  hpp::fcl::distance (&s1, tf1, &s2, tf2, distanceRequest, distanceResult);
+
+  distance = -1;
+  BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 2e-3);
+}

test/collision.cpp

@@ -104,8 +104,9 @@ BOOST_AUTO_TEST_CASE(OBB_Box_test)
 
     GJKSolver solver;
 
+    FCL_REAL distance;
     bool overlap_obb = obb1.overlap(obb2);
-    bool overlap_box = solver.shapeIntersect(box1, box1_tf, box2, box2_tf, NULL, NULL, NULL);
+    bool overlap_box = solver.shapeIntersect(box1, box1_tf, box2, box2_tf, distance, false, NULL, NULL);
     
     BOOST_CHECK(overlap_obb == overlap_box);
   }
@@ -138,13 +139,14 @@ BOOST_AUTO_TEST_CASE(OBB_shape_test)
     FCL_REAL len = (aabb1.max_[0] - aabb1.min_[0]) * 0.5;
     OBB obb2;
     GJKSolver solver;
+    FCL_REAL distance;
  
     {  
       Sphere sphere(len);
       computeBV(sphere, transforms[i], obb2);
  
       bool overlap_obb = obb1.overlap(obb2);
-      bool overlap_sphere = solver.shapeIntersect(box1, box1_tf, sphere, transforms[i], NULL, NULL, NULL);
+      bool overlap_sphere = solver.shapeIntersect(box1, box1_tf, sphere, transforms[i], distance, false, NULL, NULL);
       BOOST_CHECK(overlap_obb >= overlap_sphere);
     }
 
@@ -153,7 +155,7 @@ BOOST_AUTO_TEST_CASE(OBB_shape_test)
       computeBV(capsule, transforms[i], obb2);
       
       bool overlap_obb = obb1.overlap(obb2);
-      bool overlap_capsule = solver.shapeIntersect(box1, box1_tf, capsule, transforms[i], NULL, NULL, NULL);
+      bool overlap_capsule = solver.shapeIntersect(box1, box1_tf, capsule, transforms[i], distance, false, NULL, NULL);
       BOOST_CHECK(overlap_obb >= overlap_capsule);
     }
 
@@ -162,7 +164,7 @@ BOOST_AUTO_TEST_CASE(OBB_shape_test)
       computeBV(cone, transforms[i], obb2);
       
       bool overlap_obb = obb1.overlap(obb2);
-      bool overlap_cone = solver.shapeIntersect(box1, box1_tf, cone, transforms[i], NULL, NULL, NULL);
+      bool overlap_cone = solver.shapeIntersect(box1, box1_tf, cone, transforms[i], distance, false, NULL, NULL);
       BOOST_CHECK(overlap_obb >= overlap_cone);
     }
 
@@ -171,7 +173,7 @@ BOOST_AUTO_TEST_CASE(OBB_shape_test)
       computeBV(cylinder, transforms[i], obb2);
       
       bool overlap_obb = obb1.overlap(obb2);
-      bool overlap_cylinder = solver.shapeIntersect(box1, box1_tf, cylinder, transforms[i], NULL, NULL, NULL);
+      bool overlap_cylinder = solver.shapeIntersect(box1, box1_tf, cylinder, transforms[i], distance, false, NULL, NULL);
       BOOST_CHECK(overlap_obb >= overlap_cylinder);
     }
   }

test/convex.cpp

@@ -217,7 +217,7 @@ template <typename Sa, typename Sb> void compareShapeDistance (
 BOOST_AUTO_TEST_CASE(compare_convex_box)
 {
   FCL_REAL extents [6] = {0, 0, 0, 10, 10, 10};
-  FCL_REAL l = 1, w = 1, d = 1;
+  FCL_REAL l = 1, w = 1, d = 1, eps = 1e-4;
   Box box(l*2, w*2, d*2);
   Convex<Quadrilateral> convex_box (buildBox (l, w, d));
 
@@ -225,16 +225,16 @@ BOOST_AUTO_TEST_CASE(compare_convex_box)
   Transform3f tf2;
 
   tf2.setTranslation (Vec3f (3, 0, 0));
-  compareShapeIntersection(box, convex_box, tf1, tf2);
-  compareShapeDistance    (box, convex_box, tf1, tf2);
+  compareShapeIntersection(box, convex_box, tf1, tf2, eps);
+  compareShapeDistance    (box, convex_box, tf1, tf2, eps);
 
   tf2.setTranslation (Vec3f (0, 0, 0));
-  compareShapeIntersection(box, convex_box, tf1, tf2);
-  compareShapeDistance    (box, convex_box, tf1, tf2);
+  compareShapeIntersection(box, convex_box, tf1, tf2, eps);
+  compareShapeDistance    (box, convex_box, tf1, tf2, eps);
 
   for (int i = 0; i < 1000; ++i) {
     generateRandomTransform(extents, tf2);
-    compareShapeIntersection(box, convex_box, tf1, tf2);
-    compareShapeDistance    (box, convex_box, tf1, tf2);
+    compareShapeIntersection(box, convex_box, tf1, tf2, eps);
+    compareShapeDistance    (box, convex_box, tf1, tf2, eps);
   }
 }

test/fcl_resources/manyframes/Model_1.xmlmodel.txt

-svm_type c_svc
-kernel_type rbf
-gamma 0.166667