core: initialize GJKSolver default/cached guess/aabb center

coal-library · Jun 2, 2022 · c749e7a · c749e7a
1 parent 8a54b97
commit c749e7a
Showing 1 changed file with 35 additions and 28 deletions.
diff --git a/include/hpp/fcl/narrowphase/narrowphase.h b/include/hpp/fcl/narrowphase/narrowphase.h
@@ -50,30 +50,48 @@ namespace fcl {
 /// @brief collision and distance solver based on GJK algorithm implemented in
 /// fcl (rewritten the code from the GJK in bullet)
 struct HPP_FCL_DLLAPI GJKSolver {
+  /// @brief initialize GJK
+  template <typename S1, typename S2>
+  void initialize_gjk(details::GJK& gjk, const details::MinkowskiDiff& shape,
+                      const S1& s1, const S2& s2, Vec3f& guess,
+                      support_func_guess_t& support_hint) const {
+    switch (gjk_initial_guess) {
+      case GJKInitialGuess::DefaultGuess:
+        guess = Vec3f(1, 0, 0);
+        support_hint.setZero();
+        break;
+      case GJKInitialGuess::CachedGuess:
+        guess = cached_guess;
+        support_hint = support_func_cached_guess;
+        break;
+      case GJKInitialGuess::BoundingVolumeGuess:
+        guess = s1.aabb_center - (shape.oR1 * s2.aabb_center + shape.ot1);
+        support_hint =
+            support_func_cached_guess;  // we could also put it to (0, 0)
+        break;
+      default:
+        throw std::logic_error("Wrong initial guess for GJK.");
+    }
+    gjk.setDistanceEarlyBreak(distance_upper_bound);
+
+    gjk.gjk_variant = gjk_variant;
+    gjk.convergence_criterion = gjk_convergence_criterion;
+    gjk.convergence_criterion_type = gjk_convergence_criterion_type;
+  }
+
   /// @brief intersection checking between two shapes
   template <typename S1, typename S2>
   bool shapeIntersect(const S1& s1, const Transform3f& tf1, const S2& s2,
                       const Transform3f& tf2, FCL_REAL& distance_lower_bound,
                       bool enable_penetration, Vec3f* contact_points,
                       Vec3f* normal) const {
-    Vec3f guess(1, 0, 0);
-    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);
 
+    Vec3f guess;
+    support_func_guess_t support_hint;
     details::GJK gjk((unsigned int)gjk_max_iterations, gjk_tolerance);
-
-    gjk.setDistanceEarlyBreak(distance_upper_bound);
-
-    gjk.gjk_variant = gjk_variant;
-    gjk.convergence_criterion = gjk_convergence_criterion;
-    gjk.convergence_criterion_type = gjk_convergence_criterion_type;
+    initialize_gjk(gjk, shape, s1, s2, guess, support_hint);
 
     details::GJK::Status gjk_status = gjk.evaluate(shape, guess, support_hint);
     if (enable_cached_guess) {
@@ -217,24 +235,13 @@ struct HPP_FCL_DLLAPI GJKSolver {
 #ifndef NDEBUG
     FCL_REAL eps(sqrt(std::numeric_limits<FCL_REAL>::epsilon()));
 #endif
-    Vec3f guess(1, 0, 0);
-    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);
 
+    Vec3f guess;
+    support_func_guess_t support_hint;
     details::GJK gjk((unsigned int)gjk_max_iterations, gjk_tolerance);
-
-    gjk.setDistanceEarlyBreak(distance_upper_bound);
-
-    gjk.gjk_variant = gjk_variant;
-    gjk.convergence_criterion = gjk_convergence_criterion;
-    gjk.convergence_criterion_type = gjk_convergence_criterion_type;
+    initialize_gjk(gjk, shape, s1, s2, guess, support_hint);
 
     details::GJK::Status gjk_status = gjk.evaluate(shape, guess, support_hint);
     if (enable_cached_guess) {