Add support function cached guess in queries and merge query attribute.

include/hpp/fcl/collision_data.h

@@ -136,6 +136,47 @@ struct Contact
   }
 };
 
+struct QueryResult;
+
+/// @brief base class for all query requests
+struct QueryRequest
+{
+  /// @brief whether enable gjk intial guess
+  bool enable_cached_gjk_guess;
+  
+  /// @brief the gjk intial guess set by user
+  Vec3f cached_gjk_guess;
+
+  /// @brief the support function intial guess set by user
+  support_func_guess_t cached_support_func_guess;
+
+  QueryRequest () :
+    enable_cached_gjk_guess (false),
+    cached_gjk_guess (1,0,0),
+    cached_support_func_guess(support_func_guess_t::Zero())
+  {}
+
+  void updateGuess(const QueryResult& result);
+};
+
+/// @brief base class for all query results
+struct QueryResult
+{
+  /// @brief stores the last GJK ray when relevant.
+  Vec3f cached_gjk_guess;
+
+  /// @brief stores the last support function vertex index, when relevant.
+  support_func_guess_t cached_support_func_guess;
+};
+
+inline void QueryRequest::updateGuess(const QueryResult& result)
+{
+  if (enable_cached_gjk_guess) {
+    cached_gjk_guess = result.cached_gjk_guess;
+    cached_support_func_guess = result.cached_support_func_guess;
+  }
+}
+
 struct CollisionResult;
 
 /// @brief flag declaration for specifying required params in CollisionResult
@@ -147,7 +188,7 @@ enum CollisionRequestFlag
 };
 
 /// @brief request to the collision algorithm
-struct CollisionRequest
+struct CollisionRequest : QueryRequest
 {  
   /// @brief The maximum number of contacts will return
   size_t num_max_contacts;
@@ -158,12 +199,6 @@ struct CollisionRequest
   /// Whether a lower bound on distance is returned when objects are disjoint
   bool enable_distance_lower_bound;
 
-  /// @brief whether enable gjk intial guess
-  bool enable_cached_gjk_guess;
-  
-  /// @brief the gjk intial guess set by user
-  Vec3f cached_gjk_guess;
-
   /// @brief Distance below which objects are considered in collision.
   /// See \ref hpp_fcl_collision_and_distance_lower_bound_computation
   FCL_REAL security_margin;
@@ -187,8 +222,6 @@ struct CollisionRequest
     security_margin (0),
     break_distance (1e-3)
   {
-    enable_cached_gjk_guess = false;
-    cached_gjk_guess = Vec3f(1, 0, 0);
   }
 
   CollisionRequest() :
@@ -198,23 +231,19 @@ struct CollisionRequest
       security_margin (0),
       break_distance (1e-3)
     {
-      enable_cached_gjk_guess = false;
-      cached_gjk_guess = Vec3f(1, 0, 0);
     }
 
   bool isSatisfied(const CollisionResult& result) const;
 };
 
 /// @brief collision result
-struct CollisionResult
+struct CollisionResult : QueryResult
 {
 private:
   /// @brief contact information
   std::vector<Contact> contacts;
 
 public:
-  Vec3f cached_gjk_guess;
-
   /// 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
@@ -289,11 +318,10 @@ public:
   friend void invertResults(CollisionResult& result);
 };
 
-
 struct DistanceResult;
 
 /// @brief request to the distance computation
-struct DistanceRequest
+struct DistanceRequest : QueryRequest
 {
   /// @brief whether to return the nearest points
   bool enable_nearest_points;
@@ -328,9 +356,8 @@ struct DistanceRequest
   bool isSatisfied(const DistanceResult& result) const;
 };
 
-
 /// @brief distance result
-struct DistanceResult
+struct DistanceResult : QueryResult
 {
 
 public:
@@ -456,7 +483,6 @@ public:
 
 };
 
-
 inline CollisionRequestFlag operator~(CollisionRequestFlag a)
 {return static_cast<CollisionRequestFlag>(~static_cast<const int>(a));}
 

include/hpp/fcl/data_types.h

@@ -72,6 +72,7 @@ typedef boost::uint32_t FCL_UINT32;
 typedef boost::int32_t FCL_INT32;
 typedef Eigen::Matrix<FCL_REAL, 3, 1> Vec3f;
 typedef Eigen::Matrix<FCL_REAL, 3, 3> Matrix3f;
+typedef Eigen::Vector2i support_func_guess_t;
 
 /// @brief Triangle with 3 indices for points
 class Triangle

include/hpp/fcl/narrowphase/gjk.h

@@ -61,8 +61,6 @@ 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];
 
@@ -80,7 +78,7 @@ struct MinkowskiDiff
 
   typedef void (*GetSupportFunction) (const MinkowskiDiff& minkowskiDiff,
       const Vec3f& dir, bool dirIsNormalized, Vec3f& support0, Vec3f& support1,
-      hint_t& hint);
+      support_func_guess_t& hint);
   GetSupportFunction getSupportFunc;
 
   MinkowskiDiff() : getSupportFunc (NULL) {}
@@ -106,7 +104,7 @@ struct MinkowskiDiff
   }
 
   /// @brief support function for the pair of shapes
-  inline void support(const Vec3f& d, bool dIsNormalized, Vec3f& supp0, Vec3f& supp1, hint_t& hint) const
+  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);
@@ -127,7 +125,6 @@ struct GJK
   };
 
   typedef unsigned char vertex_id_t;
-  typedef MinkowskiDiff::hint_t support_hint_t;
 
   struct Simplex
   {
@@ -143,7 +140,7 @@ struct GJK
 
   MinkowskiDiff const* shape;
   Vec3f ray;
-  support_hint_t support_hint;
+  support_func_guess_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.
@@ -171,11 +168,11 @@ struct GJK
 
   /// @brief GJK algorithm, given the initial value guess
   Status evaluate(const MinkowskiDiff& shape, const Vec3f& guess,
-      const support_hint_t& supportHint = support_hint_t::Zero());
+      const support_func_guess_t& supportHint = support_func_guess_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,
-      support_hint_t& hint) const
+      support_func_guess_t& hint) const
   {
     shape->support(d, dIsNormalized, sv.w0, sv.w1, hint);
     sv.w.noalias() = sv.w0 - sv.w1;
@@ -238,7 +235,7 @@ private:
 
   /// @brief append one vertex to the simplex
   inline void appendVertex(Simplex& simplex, const Vec3f& v, bool isNormalized,
-      support_hint_t& hint);
+      support_func_guess_t& hint);
 
   /// @brief Project origin (0) onto line a-b
   bool projectLineOrigin(const Simplex& current, Simplex& next);

include/hpp/fcl/narrowphase/narrowphase.h

@@ -52,8 +52,6 @@ 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,

src/collision.cpp

@@ -85,8 +85,10 @@ std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1,
 {
   GJKSolver solver;
   solver.enable_cached_guess = request.enable_cached_gjk_guess;
-  if (solver.enable_cached_guess)
+  if (solver.enable_cached_guess) {
     solver.cached_guess = request.cached_gjk_guess;
+    solver.support_func_cached_guess = request.cached_support_func_guess;
+  }
 
   const CollisionFunctionMatrix& looktable = getCollisionFunctionLookTable();
   std::size_t res;
@@ -126,8 +128,10 @@ std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1,
         res = looktable.collision_matrix[node_type1][node_type2](o1, tf1, o2, tf2, &solver, request, result);
     }
   }
-  if (solver.enable_cached_guess)
+  if (solver.enable_cached_guess) {
     result.cached_gjk_guess = solver.cached_guess;
+    result.cached_support_func_guess = solver.support_func_cached_guess;
+  }
 
   return res;
 }

src/collision_data.cpp

@@ -61,10 +61,7 @@ bool DistanceRequest::isSatisfied(const DistanceResult& result) const
     enable_distance_lower_bound (enable_distance_lower_bound_),
     security_margin (0),
     break_distance (1e-3)
-  {
-    enable_cached_gjk_guess = false;
-    cached_gjk_guess = Vec3f(1, 0, 0);
-  }
+  {}
 }
 
 } // namespace hpp

src/distance.cpp

@@ -65,6 +65,11 @@ FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1,
                   const DistanceRequest& request, DistanceResult& result)
 {
   GJKSolver solver;
+  solver.enable_cached_guess = request.enable_cached_gjk_guess;
+  if (solver.enable_cached_guess) {
+    solver.cached_guess = request.cached_gjk_guess;
+    solver.support_func_cached_guess = request.cached_support_func_guess;
+  }
 
   const DistanceFunctionMatrix& looktable = getDistanceFunctionLookTable();
 
@@ -106,6 +111,10 @@ FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1,
       res = looktable.distance_matrix[node_type1][node_type2](o1, tf1, o2, tf2, &solver, request, result);    
     }
   }
+  if (solver.enable_cached_guess) {
+    result.cached_gjk_guess = solver.cached_guess;
+    result.cached_support_func_guess = solver.support_func_cached_guess;
+  }
 
   return res;
 }

src/narrowphase/gjk.cpp

@@ -271,7 +271,7 @@ 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,
-    MinkowskiDiff::hint_t& hint)
+    support_func_guess_t& hint)
 {
   getShapeSupport (s0, dir, support0, hint[0]);
   if (TransformIsIdentity)
@@ -285,7 +285,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,
-    MinkowskiDiff::hint_t& hint)
+    support_func_guess_t& hint)
 {
   enum { NeedNormalizedDir =
     bool ( (bool)shape_traits<Shape0>::NeedNormalizedDir
@@ -512,7 +512,7 @@ bool GJK::getClosestPoints (const MinkowskiDiff& shape, Vec3f& w0, Vec3f& w1)
 }
 
 GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess,
-    const MinkowskiDiff::hint_t& supportHint)
+    const support_func_guess_t& supportHint)
 {
   size_t iterations = 0;
   FCL_REAL alpha = 0;
@@ -635,7 +635,7 @@ inline void GJK::removeVertex(Simplex& simplex)
   free_v[nfree++] = simplex.vertex[--simplex.rank];
 }
 
-inline void GJK::appendVertex(Simplex& simplex, const Vec3f& v, bool isNormalized, support_hint_t& hint)
+inline void GJK::appendVertex(Simplex& simplex, const Vec3f& v, bool isNormalized, support_func_guess_t& hint)
 {
   simplex.vertex[simplex.rank] = free_v[--nfree]; // set the memory
   getSupport (v, isNormalized, *simplex.vertex[simplex.rank++], hint);
@@ -644,7 +644,7 @@ inline void GJK::appendVertex(Simplex& simplex, const Vec3f& v, bool isNormalize
 bool GJK::encloseOrigin()
 {
   Vec3f axis(Vec3f::Zero());
-  support_hint_t hint = support_hint_t::Zero();
+  support_func_guess_t hint = support_func_guess_t::Zero();
   switch(simplex->rank)
   {
   case 1:
@@ -1272,7 +1272,7 @@ EPA::SimplexF* EPA::findBest()
 EPA::Status EPA::evaluate(GJK& gjk, const Vec3f& guess)
 {
   GJK::Simplex& simplex = *gjk.getSimplex();
-  MinkowskiDiff::hint_t hint (gjk.support_hint);
+  support_func_guess_t hint (gjk.support_hint);
   if((simplex.rank > 1) && gjk.encloseOrigin())
   {
     while(hull.root)