Merge pull request #187 from jmirabel/devel

Logarithmic or linear support function for Convex depending of its size + Python update

include/hpp/fcl/narrowphase/gjk.h

@@ -55,15 +55,20 @@ Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, bool dirIsNormalized,
 
 /// @brief Minkowski difference class of two shapes
 ///
-/// @todo template this by the two shapes. The triangle / triangle case can be
-///       easily optimized computing once the triangle shapes[1] into frame0
-///
 /// @note The Minkowski difference is expressed in the frame of the first shape.
 struct HPP_FCL_DLLAPI MinkowskiDiff
 {
   /// @brief points to two shapes
   const ShapeBase* shapes[2];
 
+  struct ShapeData {
+    std::vector<int8_t> visited;
+  };
+
+  /// @brief Store temporary data for the computation of the support point for
+  /// each shape.
+  ShapeData data[2];
+
   /// @brief rotation from shape1 to shape0
   /// such that @f$ p_in_0 = oR1 * p_in_1 + ot1 @f$.
   Matrix3f oR1;
@@ -76,12 +81,18 @@ struct HPP_FCL_DLLAPI MinkowskiDiff
   /// The 2 values correspond to the inflation of shape 0 and shape 1.
   Eigen::Array<FCL_REAL, 1, 2> inflation;
 
+  /// @brief Number of points in a Convex object from which using a logarithmic
+  /// support function is faster than a linear one.
+  /// It defaults to 32.
+  /// \note It must set before the call to \ref set.
+  int linear_log_convex_threshold;
+
   typedef void (*GetSupportFunction) (const MinkowskiDiff& minkowskiDiff,
       const Vec3f& dir, bool dirIsNormalized, Vec3f& support0, Vec3f& support1,
-      support_func_guess_t& hint);
+      support_func_guess_t& hint, ShapeData data[2]);
   GetSupportFunction getSupportFunc;
 
-  MinkowskiDiff() : getSupportFunc (NULL) {}
+  MinkowskiDiff() : linear_log_convex_threshold (32), getSupportFunc (NULL) {}
 
   /// Set the two shapes,
   /// assuming the relative transformation between them is identity.
@@ -107,7 +118,7 @@ struct HPP_FCL_DLLAPI MinkowskiDiff
   inline void support(const Vec3f& d, bool dIsNormalized, Vec3f& supp0, Vec3f& supp1, support_func_guess_t& hint) const
   {
     assert(getSupportFunc != NULL);
-    getSupportFunc(*this, d, dIsNormalized, supp0, supp1, hint);
+    getSupportFunc(*this, d, dIsNormalized, supp0, supp1, hint, const_cast<ShapeData*>(data));
   }
 };
 
@@ -158,6 +169,13 @@ struct HPP_FCL_DLLAPI GJK
   Simplex simplices[2];
 
 
+  /// \param max_iterations_ number of iteration before GJK returns failure.
+  /// \param tolerance_ precision of the algorithm.
+  ///
+  /// The tolerance argument is useful for continuous shapes and for polyhedron
+  /// with some vertices closer than this threshold.
+  ///
+  /// Suggested values are 100 iterations and a tolerance of 1e-6.
   GJK(unsigned int max_iterations_, FCL_REAL tolerance_)  : max_iterations(max_iterations_),
                                                             tolerance(tolerance_)
   {

python/gjk.cc

@@ -76,9 +76,8 @@ void exposeGJK()
 
   if(!eigenpy::register_symbolic_link_to_registered_type<MinkowskiDiff>())
   {
-    class_ <MinkowskiDiff> ("MinkowskiDiff",
-        doxygen::class_doc<MinkowskiDiff>(), init<>(
-          doxygen::constructor_doc<MinkowskiDiff>()))
+    class_ <MinkowskiDiff> ("MinkowskiDiff", doxygen::class_doc<MinkowskiDiff>(), no_init)
+      .def (doxygen::visitor::init<MinkowskiDiff>())
       .def ("set", static_cast < void (MinkowskiDiff::*)(
             const ShapeBase*, const ShapeBase*)> (&MinkowskiDiff::set),
           doxygen::member_func_doc(
@@ -100,16 +99,17 @@ void exposeGJK()
 
   if(!eigenpy::register_symbolic_link_to_registered_type<GJK>())
   {
-    class_ <GJK> ("GJK",
-        doxygen::class_doc<GJK>(), init<unsigned int, FCL_REAL>(
-          doxygen::constructor_doc<GJK, unsigned int, FCL_REAL>()))
+    class_ <GJK> ("GJK", doxygen::class_doc<GJK>(), no_init)
+     .def (doxygen::visitor::init<GJK, unsigned int, FCL_REAL>())
       .DEF_RW_CLASS_ATTRIB (GJK, distance)
-      .DEF_RW_CLASS_ATTRIB (GJK, ray     )
+      .DEF_RW_CLASS_ATTRIB (GJK, ray)
+      .DEF_RW_CLASS_ATTRIB (GJK, support_hint)
       .DEF_CLASS_FUNC(GJK, evaluate)
       .DEF_CLASS_FUNC(GJK, hasClosestPoints)
       .DEF_CLASS_FUNC(GJK, hasPenetrationInformation)
       .DEF_CLASS_FUNC(GJK, getClosestPoints)
       .DEF_CLASS_FUNC(GJK, setDistanceEarlyBreak)
+      .DEF_CLASS_FUNC(GJK, getGuessFromSimplex)
       ;
   }
 }

python/math.cc

@@ -81,13 +81,13 @@ void exposeMaths ()
   eigenpy::enableEigenPySpecific<Matrix3f>();
   eigenpy::enableEigenPySpecific<Vec3f   >();
 
-  class_ <Transform3f> ("Transform3f", doxygen::class_doc<Transform3f>(), init<>(arg("self"),doxygen::constructor_doc<Transform3f>()))
-    .def (init<Matrix3f, Vec3f>    (doxygen::constructor_doc<Transform3f, const Matrix3f::MatrixBase&, const Vec3f::MatrixBase&>()))
-    .def (init<Quaternion3f, Vec3f>(doxygen::constructor_doc<Transform3f, const Quaternion3f&        , const Vec3f::MatrixBase&>()))
-    .def (init<Matrix3f>           (doxygen::constructor_doc<Transform3f, const Matrix3f&                                      >()))
-    .def (init<Quaternion3f>       (doxygen::constructor_doc<Transform3f, const Quaternion3f&                                  >()))
-    .def (init<Vec3f>              (doxygen::constructor_doc<Transform3f, const Vec3f&                                         >()))
-    .def (init<Transform3f>        (doxygen::constructor_doc<Transform3f, const Transform3f&                                   >()))
+  class_ <Transform3f> ("Transform3f", doxygen::class_doc<Transform3f>(), no_init)
+    .def (dv::init<Transform3f>())
+    .def (dv::init<Transform3f, const Matrix3f::MatrixBase&, const Vec3f::MatrixBase&>())
+    .def (dv::init<Transform3f, const Quaternion3f&        , const Vec3f::MatrixBase&>())
+    .def (dv::init<Transform3f, const Matrix3f&                                      >())
+    .def (dv::init<Transform3f, const Quaternion3f&                                  >())
+    .def (dv::init<Transform3f, const Vec3f&                                         >())
 
     .def (dv::member_func("getQuatRotation", &Transform3f::getQuatRotation))
     .def ("getTranslation", &Transform3f::getTranslation, doxygen::member_func_doc(&Transform3f::getTranslation), return_value_policy<copy_const_reference>())

src/narrowphase/gjk.cpp

@@ -97,7 +97,7 @@ template <> struct HPP_FCL_LOCAL shape_traits<ConvexBase> : shape_traits_base
   };
 };
 
-void getShapeSupport(const TriangleP* triangle, const Vec3f& dir, Vec3f& support, int&)
+void getShapeSupport(const TriangleP* triangle, const Vec3f& dir, Vec3f& support, int&, MinkowskiDiff::ShapeData*)
 {
   FCL_REAL dota = dir.dot(triangle->a);
   FCL_REAL dotb = dir.dot(triangle->b);
@@ -118,25 +118,25 @@ void getShapeSupport(const TriangleP* triangle, const Vec3f& dir, Vec3f& support
   }
 }
 
-inline void getShapeSupport(const Box* box, const Vec3f& dir, Vec3f& support, int&)
+inline void getShapeSupport(const Box* box, const Vec3f& dir, Vec3f& support, int&, MinkowskiDiff::ShapeData*)
 {
   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, int&)
+inline void getShapeSupport(const Sphere*, const Vec3f& /*dir*/, Vec3f& support, int&, MinkowskiDiff::ShapeData*)
 {
   support.setZero();
 }
 
-inline void getShapeSupport(const Capsule* capsule, const Vec3f& dir, Vec3f& support, int&)
+inline void getShapeSupport(const Capsule* capsule, const Vec3f& dir, Vec3f& support, int&, MinkowskiDiff::ShapeData*)
 {
   support.head<2>().setZero();
   if (dir[2] > 0) support[2] =   capsule->halfLength;
   else            support[2] = - capsule->halfLength;
 }
 
-void getShapeSupport(const Cone* cone, const Vec3f& dir, Vec3f& support, int&)
+void getShapeSupport(const Cone* cone, const Vec3f& dir, Vec3f& support, int&, MinkowskiDiff::ShapeData*)
 {
   // The cone radius is, for -h < z < h, (h - z) * r / (2*h)
   static const FCL_REAL inflate = 1.00001;
@@ -174,7 +174,7 @@ void getShapeSupport(const Cone* cone, const Vec3f& dir, Vec3f& support, int&)
   }
 }
 
-void getShapeSupport(const Cylinder* cylinder, const Vec3f& dir, Vec3f& support, int&)
+void getShapeSupport(const Cylinder* cylinder, const Vec3f& dir, Vec3f& support, int&, MinkowskiDiff::ShapeData*)
 {
   // The inflation makes the object look strictly convex to GJK and EPA. This
   // helps solving particular cases (e.g. a cylinder with itself at the same
@@ -196,8 +196,13 @@ void getShapeSupport(const Cylinder* cylinder, const Vec3f& dir, Vec3f& support,
       < sqrt(std::numeric_limits<FCL_REAL>::epsilon()));
 }
 
-void getShapeSupport(const ConvexBase* convex, const Vec3f& dir, Vec3f& support, int& hint)
+struct SmallConvex : ShapeBase{};
+struct LargeConvex : ShapeBase{};
+
+void getShapeSupportLog(const ConvexBase* convex, const Vec3f& dir, Vec3f& support, int& hint, MinkowskiDiff::ShapeData* data)
 {
+  assert(data != NULL);
+
   const Vec3f* pts = convex->points;
   const ConvexBase::Neighbors* nn = convex->neighbors;
 
@@ -205,16 +210,30 @@ void getShapeSupport(const ConvexBase* convex, const Vec3f& dir, Vec3f& support,
     hint = 0;
   FCL_REAL maxdot = pts[hint].dot(dir);
   FCL_REAL dot;
-  bool found = true;
+  std::vector<int8_t>& visited = data->visited;
+  visited.assign(convex->num_points, false);
+  visited[hint] = true;
+  // when the first face is orthogonal to dir, all the dot products will be
+  // equal. Yet, the neighbors must be visited.
+  bool found = true, loose_check = true;
   while (found)
   {
     const ConvexBase::Neighbors& n = nn[hint];
     found = false;
     for (int in = 0; in < n.count(); ++in) {
-      dot = pts[n[in]].dot(dir);
+      const int ip = n[in];
+      if (visited[ip]) continue;
+      visited[ip] = true;
+      dot = pts[ip].dot(dir);
+      bool better = false;
       if (dot > maxdot) {
+        better = true;
+        loose_check = false;
+      } else if (loose_check && dot == maxdot)
+        better = true;
+      if (better) {
         maxdot = dot;
-        hint = n[in];
+        hint = ip;
         found = true;
       }
     }
@@ -223,11 +242,49 @@ void getShapeSupport(const ConvexBase* convex, const Vec3f& dir, Vec3f& support,
   support = pts[hint];
 }
 
+void getShapeSupportLinear(const ConvexBase* convex, const Vec3f& dir, Vec3f& support, int& hint, MinkowskiDiff::ShapeData*)
+{
+  const Vec3f* pts = convex->points;
+
+  hint = 0;
+  FCL_REAL maxdot = pts[0].dot(dir);
+  for (int i = 1; i < convex->num_points; ++i) {
+    FCL_REAL dot = pts[i].dot(dir);
+    if (dot > maxdot) {
+      maxdot = dot;
+      hint = i;
+    }
+  }
+  support = pts[hint];
+}
+
+void getShapeSupport(const ConvexBase* convex, const Vec3f& dir, Vec3f& support, int& hint, MinkowskiDiff::ShapeData*)
+{
+  // TODO add benchmark to set a proper value for switching between linear and
+  // logarithmic.
+  if (convex->num_points > 32) {
+    MinkowskiDiff::ShapeData data;
+    getShapeSupportLog(convex, dir, support, hint, &data);
+  }
+  else
+    getShapeSupportLinear(convex, dir, support, hint, NULL);
+}
+
+inline void getShapeSupport(const SmallConvex* convex, const Vec3f& dir, Vec3f& support, int& hint, MinkowskiDiff::ShapeData* data)
+{
+  getShapeSupportLinear(reinterpret_cast<const ConvexBase*>(convex), dir, support, hint, data);
+}
+
+inline void getShapeSupport(const LargeConvex* convex, const Vec3f& dir, Vec3f& support, int& hint, MinkowskiDiff::ShapeData* data)
+{
+  getShapeSupportLog(reinterpret_cast<const ConvexBase*>(convex), dir, support, hint, data);
+}
+
 #define CALL_GET_SHAPE_SUPPORT(ShapeType)                                      \
   getShapeSupport (static_cast<const ShapeType*>(shape),                       \
       (shape_traits<ShapeType>::NeedNormalizedDir && !dirIsNormalized)         \
       ? dir.normalized() : dir,                                                \
-      support, hint)
+      support, hint, NULL)
 
 Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, bool dirIsNormalized, int& hint)
 {
@@ -271,13 +328,13 @@ template <typename Shape0, typename Shape1, bool TransformIsIdentity>
 void getSupportTpl (const Shape0* s0, const Shape1* s1,
     const Matrix3f& oR1, const Vec3f& ot1,
     const Vec3f& dir, Vec3f& support0, Vec3f& support1,
-    support_func_guess_t& hint)
+    support_func_guess_t& hint, MinkowskiDiff::ShapeData data[2])
 {
-  getShapeSupport (s0, dir, support0, hint[0]);
+  getShapeSupport (s0, dir, support0, hint[0], &(data[0]));
   if (TransformIsIdentity)
-    getShapeSupport (s1, - dir, support1, hint[1]);
+    getShapeSupport (s1, - dir, support1, hint[1], &(data[1]));
   else {
-    getShapeSupport (s1, - oR1.transpose() * dir, support1, hint[1]);
+    getShapeSupport (s1, - oR1.transpose() * dir, support1, hint[1], &(data[1]));
     support1 = oR1 * support1 + ot1;
   }
 }
@@ -285,7 +342,7 @@ void getSupportTpl (const Shape0* s0, const Shape1* s1,
 template <typename Shape0, typename Shape1, bool TransformIsIdentity>
 void getSupportFuncTpl (const MinkowskiDiff& md,
     const Vec3f& dir, bool dirIsNormalized, Vec3f& support0, Vec3f& support1,
-    support_func_guess_t& hint)
+    support_func_guess_t& hint, MinkowskiDiff::ShapeData data[2])
 {
   enum { NeedNormalizedDir =
     bool ( (bool)shape_traits<Shape0>::NeedNormalizedDir
@@ -304,12 +361,12 @@ void getSupportFuncTpl (const MinkowskiDiff& md,
       static_cast <const Shape1*>(md.shapes[1]),
       md.oR1, md.ot1,
       (NeedNormalizedDir && !dirIsNormalized) ? dir.normalized() : dir,
-      support0, support1, hint);
+      support0, support1, hint, data);
 }
 
 template <typename Shape0>
 MinkowskiDiff::GetSupportFunction makeGetSupportFunction1 (const ShapeBase* s1, bool identity,
-    Eigen::Array<FCL_REAL, 1, 2>& inflation)
+    Eigen::Array<FCL_REAL, 1, 2>& inflation, int linear_log_convex_threshold)
 {
   inflation[1] = 0;
   switch(s1->getNodeType())
@@ -335,41 +392,49 @@ MinkowskiDiff::GetSupportFunction makeGetSupportFunction1 (const ShapeBase* s1,
     if (identity) return getSupportFuncTpl<Shape0, Cylinder, true >;
     else          return getSupportFuncTpl<Shape0, Cylinder, false>;
   case GEOM_CONVEX:
-    if (identity) return getSupportFuncTpl<Shape0, ConvexBase, true >;
-    else          return getSupportFuncTpl<Shape0, ConvexBase, false>;
+    if (static_cast<const ConvexBase*>(s1)->num_points > linear_log_convex_threshold) {
+      if (identity) return getSupportFuncTpl<Shape0, LargeConvex, true >;
+      else          return getSupportFuncTpl<Shape0, LargeConvex, false>;
+    } else {
+      if (identity) return getSupportFuncTpl<Shape0, SmallConvex, true >;
+      else          return getSupportFuncTpl<Shape0, SmallConvex, false>;
+    }
   default:
     throw std::logic_error ("Unsupported geometric shape");
   }
 }
 
 MinkowskiDiff::GetSupportFunction makeGetSupportFunction0 (const ShapeBase* s0, const ShapeBase* s1, bool identity,
-    Eigen::Array<FCL_REAL, 1, 2>& inflation)
+    Eigen::Array<FCL_REAL, 1, 2>& inflation, int linear_log_convex_threshold)
 {
   inflation[0] = 0;
   switch(s0->getNodeType())
   {
   case GEOM_TRIANGLE:
-    return makeGetSupportFunction1<TriangleP> (s1, identity, inflation);
+    return makeGetSupportFunction1<TriangleP> (s1, identity, inflation, linear_log_convex_threshold);
     break;
   case GEOM_BOX:
-    return makeGetSupportFunction1<Box> (s1, identity, inflation);
+    return makeGetSupportFunction1<Box> (s1, identity, inflation, linear_log_convex_threshold);
     break;
   case GEOM_SPHERE:
     inflation[0] = static_cast<const Sphere*>(s0)->radius;
-    return makeGetSupportFunction1<Sphere> (s1, identity, inflation);
+    return makeGetSupportFunction1<Sphere> (s1, identity, inflation, linear_log_convex_threshold);
     break;
   case GEOM_CAPSULE:
     inflation[0] = static_cast<const Capsule*>(s0)->radius;
-    return makeGetSupportFunction1<Capsule> (s1, identity, inflation);
+    return makeGetSupportFunction1<Capsule> (s1, identity, inflation, linear_log_convex_threshold);
     break;
   case GEOM_CONE:
-    return makeGetSupportFunction1<Cone> (s1, identity, inflation);
+    return makeGetSupportFunction1<Cone> (s1, identity, inflation, linear_log_convex_threshold);
     break;
   case GEOM_CYLINDER:
-    return makeGetSupportFunction1<Cylinder> (s1, identity, inflation);
+    return makeGetSupportFunction1<Cylinder> (s1, identity, inflation, linear_log_convex_threshold);
     break;
   case GEOM_CONVEX:
-    return makeGetSupportFunction1<ConvexBase> (s1, identity, inflation);
+    if (static_cast<const ConvexBase*>(s0)->num_points > linear_log_convex_threshold)
+      return makeGetSupportFunction1<LargeConvex> (s1, identity, inflation, linear_log_convex_threshold);
+    else
+      return makeGetSupportFunction1<SmallConvex> (s1, identity, inflation, linear_log_convex_threshold);
     break;
   default:
     throw std::logic_error ("Unsupported geometric shape");
@@ -387,7 +452,7 @@ void MinkowskiDiff::set (const ShapeBase* shape0, const ShapeBase* shape1,
 
   bool identity = (oR1.isIdentity() && ot1.isZero());
 
-  getSupportFunc = makeGetSupportFunction0 (shape0, shape1, identity, inflation);
+  getSupportFunc = makeGetSupportFunction0 (shape0, shape1, identity, inflation, linear_log_convex_threshold);
 }
 
 void MinkowskiDiff::set (const ShapeBase* shape0, const ShapeBase* shape1)
@@ -398,7 +463,7 @@ void MinkowskiDiff::set (const ShapeBase* shape0, const ShapeBase* shape1)
   oR1.setIdentity();
   ot1.setZero();
 
-  getSupportFunc = makeGetSupportFunction0 (shape0, shape1, true, inflation);
+  getSupportFunc = makeGetSupportFunction0 (shape0, shape1, true, inflation, linear_log_convex_threshold);
 }
 
 void GJK::initialize()