Merge pull request #165 from jmirabel/devel

Add hint to the support function + Fix usage of GJK guess.

include/hpp/fcl/narrowphase/gjk.h

@@ -50,7 +50,8 @@ namespace details
 {
 
 /// @brief the support function for shape
-Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, bool dirIsNormalized); 
+/// \param hint use to initialize the search when shape is a ConvexBase object.
+Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, bool dirIsNormalized, int& hint); 
 
 /// @brief Minkowski difference class of two shapes
 ///
@@ -60,6 +61,8 @@ Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, bool dirIsNormalized)
 /// @note The Minkowski difference is expressed in the frame of the first shape.
 struct MinkowskiDiff
 {
+  typedef Eigen::Vector2i hint_t;
+
   /// @brief points to two shapes
   const ShapeBase* shapes[2];
 
@@ -76,7 +79,8 @@ struct MinkowskiDiff
   Eigen::Array<FCL_REAL, 1, 2> inflation;
 
   typedef void (*GetSupportFunction) (const MinkowskiDiff& minkowskiDiff,
-      const Vec3f& dir, bool dirIsNormalized, Vec3f& support0, Vec3f& support1);
+      const Vec3f& dir, bool dirIsNormalized, Vec3f& support0, Vec3f& support1,
+      hint_t& hint);
   GetSupportFunction getSupportFunc;
 
   MinkowskiDiff() : getSupportFunc (NULL) {}
@@ -90,22 +94,22 @@ struct MinkowskiDiff
       const Transform3f& tf0, const Transform3f& tf1);
 
   /// @brief support function for shape0
-  inline Vec3f support0(const Vec3f& d, bool dIsNormalized) const
+  inline Vec3f support0(const Vec3f& d, bool dIsNormalized, int& hint) const
   {
-    return getSupport(shapes[0], d, dIsNormalized);
+    return getSupport(shapes[0], d, dIsNormalized, hint);
   }
 
   /// @brief support function for shape1
-  inline Vec3f support1(const Vec3f& d, bool dIsNormalized) const
+  inline Vec3f support1(const Vec3f& d, bool dIsNormalized, int& hint) const
   {
-    return oR1 * getSupport(shapes[1], oR1.transpose() * d, dIsNormalized) + ot1;
+    return oR1 * getSupport(shapes[1], oR1.transpose() * d, dIsNormalized, hint) + ot1;
   }
 
   /// @brief support function for the pair of shapes
-  inline void support(const Vec3f& d, bool dIsNormalized, Vec3f& supp0, Vec3f& supp1) const
+  inline void support(const Vec3f& d, bool dIsNormalized, Vec3f& supp0, Vec3f& supp1, hint_t& hint) const
   {
     assert(getSupportFunc != NULL);
-    getSupportFunc(*this, d, dIsNormalized, supp0, supp1);
+    getSupportFunc(*this, d, dIsNormalized, supp0, supp1, hint);
   }
 };
 
@@ -123,6 +127,7 @@ struct GJK
   };
 
   typedef unsigned char vertex_id_t;
+  typedef MinkowskiDiff::hint_t support_hint_t;
 
   struct Simplex
   {
@@ -138,6 +143,7 @@ struct GJK
 
   MinkowskiDiff const* shape;
   Vec3f ray;
+  support_hint_t support_hint;
   /// The distance computed by GJK. The possible values are
   /// - \f$ d = - R - 1 \f$ when a collision is detected and GJK
   ///   cannot compute penetration informations.
@@ -164,12 +170,14 @@ struct GJK
   void initialize();
 
   /// @brief GJK algorithm, given the initial value guess
-  Status evaluate(const MinkowskiDiff& shape, const Vec3f& guess);
+  Status evaluate(const MinkowskiDiff& shape, const Vec3f& guess,
+      const support_hint_t& supportHint = support_hint_t::Zero());
 
   /// @brief apply the support function along a direction, the result is return in sv
-  inline void getSupport(const Vec3f& d, bool dIsNormalized, SimplexV& sv) const
+  inline void getSupport(const Vec3f& d, bool dIsNormalized, SimplexV& sv,
+      support_hint_t& hint) const
   {
-    shape->support(d, dIsNormalized, sv.w0, sv.w1);
+    shape->support(d, dIsNormalized, sv.w0, sv.w1, hint);
     sv.w.noalias() = sv.w0 - sv.w1;
   }
 
@@ -229,7 +237,8 @@ private:
   inline void removeVertex(Simplex& simplex);
 
   /// @brief append one vertex to the simplex
-  inline void appendVertex(Simplex& simplex, const Vec3f& v, bool isNormalized = false);
+  inline void appendVertex(Simplex& simplex, const Vec3f& v, bool isNormalized,
+      support_hint_t& hint);
 
   /// @brief Project origin (0) onto line a-b
   bool projectLineOrigin(const Simplex& current, Simplex& next);

include/hpp/fcl/narrowphase/narrowphase.h

@@ -52,6 +52,8 @@ namespace fcl
   /// @brief collision and distance solver based on GJK algorithm implemented in fcl (rewritten the code from the GJK in bullet)
   struct GJKSolver
   {
+    typedef details::GJK::support_hint_t support_func_guess_t;
+
     /// @brief intersection checking between two shapes
     template<typename S1, typename S2>
       bool shapeIntersect(const S1& s1, const Transform3f& tf1,
@@ -61,14 +63,22 @@ namespace fcl
                           Vec3f* contact_points, Vec3f* normal) const
     {
       Vec3f guess(1, 0, 0);
-      if(enable_cached_guess) guess = cached_guess;
+      support_func_guess_t support_hint;
+      if(enable_cached_guess) {
+        guess = cached_guess;
+        support_hint = support_func_cached_guess;
+      } else
+        support_hint.setZero();
     
       details::MinkowskiDiff shape;
       shape.set (&s1, &s2, tf1, tf2);
   
       details::GJK gjk((unsigned int )gjk_max_iterations, gjk_tolerance);
-      details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
-      if(enable_cached_guess) cached_guess = gjk.getGuessFromSimplex();
+      details::GJK::Status gjk_status = gjk.evaluate(shape, guess, support_hint);
+      if(enable_cached_guess) {
+        cached_guess = gjk.getGuessFromSimplex();
+        support_func_cached_guess = gjk.support_hint;
+      }
     
       Vec3f w0, w1;
       switch(gjk_status) {
@@ -127,14 +137,22 @@ namespace fcl
           tf_1M2.transform (P3));
 
       Vec3f guess(1, 0, 0);
-      if(enable_cached_guess) guess = cached_guess;
+      support_func_guess_t support_hint;
+      if(enable_cached_guess) {
+        guess = cached_guess;
+        support_hint = support_func_cached_guess;
+      } else
+        support_hint.setZero();
 
       details::MinkowskiDiff shape;
       shape.set (&s, &tri);
   
       details::GJK gjk((unsigned int )gjk_max_iterations, gjk_tolerance);
-      details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
-      if(enable_cached_guess) cached_guess = gjk.getGuessFromSimplex();
+      details::GJK::Status gjk_status = gjk.evaluate(shape, guess, support_hint);
+      if(enable_cached_guess) {
+        cached_guess = gjk.getGuessFromSimplex();
+        support_func_cached_guess = gjk.support_hint;
+      }
 
       Vec3f w0, w1;
       switch(gjk_status) {
@@ -197,14 +215,22 @@ namespace fcl
       FCL_REAL eps (sqrt(std::numeric_limits<FCL_REAL>::epsilon()));
 #endif
       Vec3f guess(1, 0, 0);
-      if(enable_cached_guess) guess = cached_guess;
+      support_func_guess_t support_hint;
+      if(enable_cached_guess) {
+        guess = cached_guess;
+        support_hint = support_func_cached_guess;
+      } else
+        support_hint.setZero();
 
       details::MinkowskiDiff shape;
       shape.set (&s1, &s2, tf1, tf2);
 
       details::GJK gjk((unsigned int) gjk_max_iterations, gjk_tolerance);
-      details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
-      if(enable_cached_guess) cached_guess = gjk.getGuessFromSimplex();
+      details::GJK::Status gjk_status = gjk.evaluate(shape, guess, support_hint);
+      if(enable_cached_guess) {
+        cached_guess = gjk.getGuessFromSimplex();
+        support_func_cached_guess = gjk.support_hint;
+      }
 
       if(gjk_status == details::GJK::Failed)
       {
@@ -279,6 +305,7 @@ namespace fcl
       epa_tolerance = 1e-6;
       enable_cached_guess = false;
       cached_guess = Vec3f(1, 0, 0);
+      support_func_cached_guess = support_func_guess_t::Zero();
     }
 
     void enableCachedGuess(bool if_enable) const
@@ -319,6 +346,9 @@ namespace fcl
 
     /// @brief smart guess
     mutable Vec3f cached_guess;
+
+    /// @brief smart guess for the support function
+    mutable support_func_guess_t support_func_cached_guess;
   };
 
 #if __cplusplus < 201103L

include/hpp/fcl/shape/details/convex.hxx

@@ -63,6 +63,7 @@ Convex<PolygonT>::Convex(const Convex<PolygonT>& other) :
   num_polygons (other.num_polygons)
 {
   if (own_storage_) {
+    delete [] polygons;
     polygons = new PolygonT[num_polygons];
     memcpy(polygons, other.polygons, sizeof(PolygonT) * num_polygons);
   }

src/narrowphase/gjk.cpp

@@ -97,7 +97,7 @@ template <> struct shape_traits<ConvexBase> : shape_traits_base
   };
 };
 
-void getShapeSupport(const TriangleP* triangle, const Vec3f& dir, Vec3f& support)
+void getShapeSupport(const TriangleP* triangle, const Vec3f& dir, Vec3f& support, int&)
 {
   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)
+inline void getShapeSupport(const Box* box, const Vec3f& dir, Vec3f& support, int&)
 {
   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)
+inline void getShapeSupport(const Sphere*, const Vec3f& /*dir*/, Vec3f& support, int&)
 {
   support.setZero();
 }
 
-inline void getShapeSupport(const Capsule* capsule, const Vec3f& dir, Vec3f& support)
+inline void getShapeSupport(const Capsule* capsule, const Vec3f& dir, Vec3f& support, int&)
 {
   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)
+void getShapeSupport(const Cone* cone, const Vec3f& dir, Vec3f& support, int&)
 {
   // 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)
   }
 }
 
-void getShapeSupport(const Cylinder* cylinder, const Vec3f& dir, Vec3f& support)
+void getShapeSupport(const Cylinder* cylinder, const Vec3f& dir, Vec3f& support, int&)
 {
   // 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,39 +196,40 @@ 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)
+void getShapeSupport(const ConvexBase* convex, const Vec3f& dir, Vec3f& support, int& hint)
 {
   const Vec3f* pts = convex->points;
   const ConvexBase::Neighbors* nn = convex->neighbors;
 
-  int i = 0;
-  FCL_REAL maxdot = pts[i].dot(dir);
+  if (hint < 0 || hint >= convex->num_points)
+    hint = 0;
+  FCL_REAL maxdot = pts[hint].dot(dir);
   FCL_REAL dot;
   bool found = true;
   while (found)
   {
-    const ConvexBase::Neighbors& n = nn[i];
+    const ConvexBase::Neighbors& n = nn[hint];
     found = false;
     for (int in = 0; in < n.count(); ++in) {
       dot = pts[n[in]].dot(dir);
       if (dot > maxdot) {
         maxdot = dot;
-        i = n[in];
+        hint = n[in];
         found = true;
       }
     }
   }
 
-  support = pts[i];
+  support = pts[hint];
 }
 
 #define CALL_GET_SHAPE_SUPPORT(ShapeType)                                      \
   getShapeSupport (static_cast<const ShapeType*>(shape),                       \
       (shape_traits<ShapeType>::NeedNormalizedDir && !dirIsNormalized)         \
       ? dir.normalized() : dir,                                                \
-      support)
+      support, hint)
 
-Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, bool dirIsNormalized)
+Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, bool dirIsNormalized, int& hint)
 {
   Vec3f support;
   switch(shape->getNodeType())
@@ -269,20 +270,22 @@ Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, bool dirIsNormalized)
 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)
+    const Vec3f& dir, Vec3f& support0, Vec3f& support1,
+    MinkowskiDiff::hint_t& hint)
 {
-  getShapeSupport (s0, dir, support0);
+  getShapeSupport (s0, dir, support0, hint[0]);
   if (TransformIsIdentity)
-    getShapeSupport (s1, - dir, support1);
+    getShapeSupport (s1, - dir, support1, hint[1]);
   else {
-    getShapeSupport (s1, - oR1.transpose() * dir, support1);
+    getShapeSupport (s1, - oR1.transpose() * dir, support1, hint[1]);
     support1 = oR1 * support1 + ot1;
   }
 }
 
 template <typename Shape0, typename Shape1, bool TransformIsIdentity>
 void getSupportFuncTpl (const MinkowskiDiff& md,
-    const Vec3f& dir, bool dirIsNormalized, Vec3f& support0, Vec3f& support1)
+    const Vec3f& dir, bool dirIsNormalized, Vec3f& support0, Vec3f& support1,
+    MinkowskiDiff::hint_t& hint)
 {
   enum { NeedNormalizedDir =
     bool ( (bool)shape_traits<Shape0>::NeedNormalizedDir
@@ -301,7 +304,7 @@ void getSupportFuncTpl (const MinkowskiDiff& md,
       static_cast <const Shape1*>(md.shapes[1]),
       md.oR1, md.ot1,
       (NeedNormalizedDir && !dirIsNormalized) ? dir.normalized() : dir,
-      support0, support1);
+      support0, support1, hint);
 }
 
 template <typename Shape0>
@@ -508,7 +511,8 @@ bool GJK::getClosestPoints (const MinkowskiDiff& shape, Vec3f& w0, Vec3f& w1)
   return true;
 }
 
-GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess)
+GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess,
+    const MinkowskiDiff::hint_t& supportHint)
 {
   size_t iterations = 0;
   FCL_REAL alpha = 0;
@@ -526,19 +530,15 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess)
   shape = &shape_;
   distance = 0.0;
   simplices[0].rank = 0;
-  ray = guess;
-
-  if (ray.squaredNorm() > 0) appendVertex(simplices[0], -ray);
-  else                       appendVertex(simplices[0], Vec3f(1, 0, 0), true);
-  ray = simplices[0].vertex[0]->w;
-
-  FCL_REAL rl = ray.norm();
-  if (rl == 0) {
-    status = Inside;
-    distance = - inflation - 1.;
-    simplex = &simplices[0];
-    return status;
-  }
+  support_hint = supportHint;
+
+  FCL_REAL rl = guess.norm();
+  if (rl < tolerance) {
+    ray = Vec3f(-1,0,0);
+    rl = 1;
+  } else
+    ray = guess;
+
   do
   {
     vertex_id_t next = (vertex_id_t)(1 - current);
@@ -558,7 +558,7 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess)
       break;
     }
 
-    appendVertex(curr_simplex, -ray); // see below, ray points away from origin
+    appendVertex(curr_simplex, -ray, false, support_hint); // see below, ray points away from origin
 
     // check removed (by ?): when the new support point is close to previous support points, stop (as the new simplex is degenerated)
     const Vec3f& w = curr_simplex.vertex[curr_simplex.rank - 1]->w;
@@ -573,9 +573,12 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess)
 
     // check C: when the new support point is close to the sub-simplex where the ray point lies, stop (as the new simplex again is degenerated)
     alpha = std::max(alpha, omega);
-    if((rl - alpha) - tolerance * rl <= 0)
+    FCL_REAL diff (rl - alpha);
+    if (iterations == 0) diff = std::abs(diff);
+    if(diff - tolerance * rl <= 0)
     {
-      removeVertex(simplices[current]);
+      if (iterations > 0)
+        removeVertex(simplices[current]);
       distance = rl - inflation;
       // TODO When inflation is strictly positive, the distance may be exactly
       // zero (so the ray is not zero) and we are not in the case rl < tolerance.
@@ -589,6 +592,13 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess)
     bool inside;
     switch(curr_simplex.rank)
     {
+    case 1: // Only at the first iteration
+      assert(iterations == 0);
+      ray = w;
+      inside = false;
+      next_simplex.rank = 1;
+      next_simplex.vertex[0] = curr_simplex.vertex[0];
+      break;
     case 2:
       inside = projectLineOrigin (curr_simplex, next_simplex);
       break;
@@ -598,6 +608,8 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess)
     case 4:
       inside = projectTetrahedraOrigin (curr_simplex, next_simplex);
       break;