Exposing gjk utils

include/hpp/fcl/collision_data.h

@@ -117,17 +117,28 @@ struct QueryResult;
 
 /// @brief base class for all query requests
 struct HPP_FCL_DLLAPI QueryRequest {
+  // @briefInitial guess to use for the GJK algorithm
+  GJKInitialGuess gjk_initial_guess;
+
   /// @brief whether enable gjk initial guess
+  /// @Deprecated Use gjk_initial_guess instead
+  HPP_FCL_DEPRECATED_MESSAGE("Use gjk_initial_guess instead")
   bool enable_cached_gjk_guess;
 
   /// @brief whether to enable the Nesterov accleration of GJK
   GJKVariant gjk_variant;
 
   /// @brief convergence criterion used to stop GJK
-  GJKConvergenceCriterion convergence_criterion;
+  GJKConvergenceCriterion gjk_convergence_criterion;
 
   /// @brief convergence criterion used to stop GJK
-  GJKConvergenceCriterionType convergence_criterion_type;
+  GJKConvergenceCriterionType gjk_convergence_criterion_type;
+
+  /// @brief tolerance for the GJK algorithm
+  FCL_REAL gjk_tolerance;
+
+  /// @brief maximum iteration for the GJK algorithm
+  size_t gjk_max_iterations;
 
   /// @brief the gjk initial guess set by user
   Vec3f cached_gjk_guess;
@@ -138,23 +149,40 @@ struct HPP_FCL_DLLAPI QueryRequest {
   /// @brief enable timings when performing collision/distance request
   bool enable_timings;
 
+  HPP_FCL_COMPILER_DIAGNOSTIC_PUSH
+  HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
+  /// @brief Default constructor.
   QueryRequest()
-      : enable_cached_gjk_guess(false),
+      : gjk_initial_guess(GJKInitialGuess::DefaultGuess),
+        enable_cached_gjk_guess(false),
         gjk_variant(GJKVariant::DefaultGJK),
-        convergence_criterion(GJKConvergenceCriterion::VDB),
-        convergence_criterion_type(GJKConvergenceCriterionType::Relative),
+        gjk_convergence_criterion(GJKConvergenceCriterion::VDB),
+        gjk_convergence_criterion_type(GJKConvergenceCriterionType::Relative),
+        gjk_tolerance(1e-6),
+        gjk_max_iterations(128),
         cached_gjk_guess(1, 0, 0),
         cached_support_func_guess(support_func_guess_t::Zero()),
         enable_timings(false) {}
 
+  /// @brief Copy  constructor.
+  QueryRequest(const QueryRequest& other) = default;
+
+  /// @brief Copy  assignment operator.
+  QueryRequest& operator=(const QueryRequest& other) = default;
+  HPP_FCL_COMPILER_DIAGNOSTIC_POP
+
   void updateGuess(const QueryResult& result);
 
   /// @brief whether two QueryRequest are the same or not
   inline bool operator==(const QueryRequest& other) const {
-    return enable_cached_gjk_guess == other.enable_cached_gjk_guess &&
+    HPP_FCL_COMPILER_DIAGNOSTIC_PUSH
+    HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
+    return gjk_initial_guess == other.gjk_initial_guess &&
+           enable_cached_gjk_guess == other.enable_cached_gjk_guess &&
            cached_gjk_guess == other.cached_gjk_guess &&
            cached_support_func_guess == other.cached_support_func_guess &&
            enable_timings == other.enable_timings;
+    HPP_FCL_COMPILER_DIAGNOSTIC_POP
   }
 };
 
@@ -175,10 +203,18 @@ struct HPP_FCL_DLLAPI QueryResult {
 };
 
 inline void QueryRequest::updateGuess(const QueryResult& result) {
+  if (gjk_initial_guess == GJKInitialGuess::CachedGuess) {
+    cached_gjk_guess = result.cached_gjk_guess;
+    cached_support_func_guess = result.cached_support_func_guess;
+  }
+  // TODO: use gjk_initial_guess instead
+  HPP_FCL_COMPILER_DIAGNOSTIC_PUSH
+  HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
   if (enable_cached_gjk_guess) {
     cached_gjk_guess = result.cached_gjk_guess;
     cached_support_func_guess = result.cached_support_func_guess;
   }
+  HPP_FCL_COMPILER_DIAGNOSTIC_POP
 }
 
 struct CollisionResult;

include/hpp/fcl/data_types.h

@@ -71,6 +71,14 @@ typedef Eigen::Matrix<Eigen::DenseIndex, Eigen::Dynamic, 3> Matrixx3i;
 typedef Eigen::Matrix<FCL_REAL, Eigen::Dynamic, Eigen::Dynamic> MatrixXf;
 typedef Eigen::Vector2i support_func_guess_t;
 
+/// @brief Initial guess to use for the GJK algorithm
+/// DefaultGuess: Vec3f(1, 0, 0)
+/// CachedGuess: previous vector found by GJK or guess cached by the user
+/// BoundingVolumeGuess: guess using the centers of the shapes' AABB
+/// WARNING: to use BoundingVolumeGuess, computeLocalAABB must have been called
+/// on the two shapes.
+enum GJKInitialGuess { DefaultGuess, CachedGuess, BoundingVolumeGuess };
+
 /// @brief Variant to use for the GJK algorithm
 enum GJKVariant { DefaultGJK, NesterovAcceleration };
 

include/hpp/fcl/fwd.hh

@@ -2,6 +2,7 @@
 // Software License Agreement (BSD License)
 //
 //  Copyright (c) 2014, CNRS-LAAS
+//  Copyright (c) 2022, Inria
 //  Author: Florent Lamiraux
 //
 //  All rights reserved.
@@ -41,6 +42,8 @@
 #include <sstream>
 
 #include <hpp/fcl/config.hh>
+#include <hpp/fcl/deprecated.hh>
+#include <hpp/fcl/warning.hh>
 
 #if _WIN32
 #define HPP_FCL_PRETTY_FUNCTION __FUNCSIG__
@@ -64,6 +67,22 @@
 #define HPP_FCL_WITH_CXX11_SUPPORT
 #endif
 
+#if defined(__GNUC__) || defined(__clang__)
+#define HPP_FCL_COMPILER_DIAGNOSTIC_PUSH _Pragma("GCC diagnostic push")
+#define HPP_FCL_COMPILER_DIAGNOSTIC_POP _Pragma("GCC diagnostic pop")
+#define HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS \
+  _Pragma("GCC diagnostic ignored \"-Wdeprecated-declarations\"")
+#elif defined(WIN32)
+#define HPP_FCL_COMPILER_DIAGNOSTIC_PUSH _Pragma("warning(push)")
+#define HPP_FCL_COMPILER_DIAGNOSTIC_POP _Pragma("warning(pop)")
+#define HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS \
+  _Pragma("warning(disable : 4996)")
+#else
+#define HPP_FCL_COMPILER_DIAGNOSTIC_PUSH
+#define HPP_FCL_COMPILER_DIAGNOSTIC_POP
+#define HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
+#endif  // __GNUC__
+
 namespace hpp {
 namespace fcl {
 using std::dynamic_pointer_cast;

include/hpp/fcl/internal/tools.h

@@ -45,8 +45,6 @@
 #include <iostream>
 #include <limits>
 
-#include <hpp/fcl/deprecated.hh>
-#include <hpp/fcl/config.hh>
 #include <hpp/fcl/data_types.h>
 
 namespace hpp {

include/hpp/fcl/narrowphase/gjk.h

@@ -172,6 +172,9 @@ struct HPP_FCL_DLLAPI GJK {
 
   MinkowskiDiff const* shape;
   Vec3f ray;
+  GJKVariant gjk_variant;
+  GJKConvergenceCriterion convergence_criterion;
+  GJKConvergenceCriterionType convergence_criterion_type;
   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
@@ -249,31 +252,11 @@ struct HPP_FCL_DLLAPI GJK {
     distance_upper_bound = dup;
   }
 
-  /// @brief Set which GJK version to use. Default is Vanilla.
-  inline void setGJKVariant(GJKVariant variant) { gjk_variant = variant; }
-  inline GJKVariant getGJKVariant() { return gjk_variant; }
-
   /// @brief Convergence check used to stop GJK when shapes are not in
   /// collision.
   bool checkConvergence(const Vec3f& w, const FCL_REAL& rl, FCL_REAL& alpha,
                         const FCL_REAL& omega);
 
-  inline void setConvergenceCriterion(
-      const GJKConvergenceCriterion& criterion) {
-    convergence_criterion = criterion;
-  }
-  inline GJKConvergenceCriterion getConvergenceCriterion() {
-    return convergence_criterion;
-  }
-
-  inline void setConvergenceCriterionType(
-      const GJKConvergenceCriterionType& criterion_type) {
-    convergence_criterion_type = criterion_type;
-  }
-  inline GJKConvergenceCriterionType getConvergenceCriterionType() {
-    return convergence_criterion_type;
-  }
-
   /// @brief Get GJK number of iterations.
   inline size_t getIterations() { return iterations; }
 
@@ -288,10 +271,7 @@ struct HPP_FCL_DLLAPI GJK {
   unsigned int max_iterations;
   FCL_REAL tolerance;
   FCL_REAL distance_upper_bound;
-  GJKVariant gjk_variant;
   size_t iterations;
-  GJKConvergenceCriterion convergence_criterion;
-  GJKConvergenceCriterionType convergence_criterion_type;
 
   /// @brief discard one vertex from the simplex
   inline void removeVertex(Simplex& simplex);