core: simplify setup of GJKSolver

coal-library · Jun 7, 2022 · bb13f10 · bb13f10
1 parent 59ee1a1
commit bb13f10
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 65 deletions.
diff --git a/src/collision.cpp b/src/collision.cpp
@@ -75,21 +75,8 @@ std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1,
     result.clear();
     return false;
   }
-  GJKSolver solver;
-  solver.gjk_initial_guess = request.gjk_initial_guess;
-  // TODO: use gjk_initial_guess instead
-  solver.enable_cached_guess = request.enable_cached_gjk_guess;
-  solver.gjk_variant = request.gjk_variant;
-  solver.gjk_convergence_criterion = request.gjk_convergence_criterion;
-  solver.gjk_convergence_criterion_type =
-      request.gjk_convergence_criterion_type;
-  solver.gjk_tolerance = request.gjk_tolerance;
-  solver.gjk_max_iterations = request.gjk_max_iterations;
-  if (solver.gjk_initial_guess == GJKInitialGuess::CachedGuess ||
-      solver.enable_cached_guess) {
-    solver.cached_guess = request.cached_gjk_guess;
-    solver.support_func_cached_guess = request.cached_support_func_guess;
-  }
+
+  GJKSolver solver(request);
 
   const CollisionFunctionMatrix& looktable = getCollisionFunctionLookTable();
   std::size_t res;
@@ -194,23 +181,7 @@ std::size_t ComputeCollision::operator()(const Transform3f& tf1,
                                          CollisionResult& result) const
 
 {
-  GJKInitialGuess gjk_initial_guess = request.gjk_initial_guess;
-  solver.gjk_initial_guess = gjk_initial_guess;
-  bool cached = (gjk_initial_guess == GJKInitialGuess::CachedGuess ||
-                 request.enable_cached_gjk_guess);
-  solver.enable_cached_guess = cached;
-  if (cached) {
-    solver.cached_guess = request.cached_gjk_guess;
-    solver.support_func_cached_guess = request.cached_support_func_guess;
-  }
-
-  solver.distance_upper_bound = request.distance_upper_bound;
-  solver.gjk_tolerance = request.gjk_tolerance;
-  solver.gjk_max_iterations = request.gjk_max_iterations;
-  solver.gjk_variant = request.gjk_variant;
-  solver.gjk_convergence_criterion = request.gjk_convergence_criterion;
-  solver.gjk_convergence_criterion_type =
-      request.gjk_convergence_criterion_type;
+  solver.set(request);
 
   std::size_t res;
   if (request.enable_timings) {
@@ -220,7 +191,8 @@ std::size_t ComputeCollision::operator()(const Transform3f& tf1,
   } else
     res = run(tf1, tf2, request, result);
 
-  if (cached) {
+  if (solver.gjk_initial_guess == GJKInitialGuess::CachedGuess ||
+      solver.enable_cached_guess) {
     result.cached_gjk_guess = solver.cached_guess;
     result.cached_support_func_guess = solver.support_func_cached_guess;
   }

diff --git a/src/distance.cpp b/src/distance.cpp
@@ -60,21 +60,7 @@ FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2,
 FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1,
                   const CollisionGeometry* o2, const Transform3f& tf2,
                   const DistanceRequest& request, DistanceResult& result) {
-  GJKSolver solver;
-  solver.gjk_initial_guess = request.gjk_initial_guess;
-  // TODO: use gjk_initial_guess instead
-  solver.enable_cached_guess = request.enable_cached_gjk_guess;
-  solver.gjk_variant = request.gjk_variant;
-  solver.gjk_convergence_criterion = request.gjk_convergence_criterion;
-  solver.gjk_convergence_criterion_type =
-      request.gjk_convergence_criterion_type;
-  solver.gjk_tolerance = request.gjk_tolerance;
-  solver.gjk_max_iterations = request.gjk_max_iterations;
-  if (solver.gjk_initial_guess == GJKInitialGuess::CachedGuess ||
-      solver.enable_cached_guess) {
-    solver.cached_guess = request.cached_gjk_guess;
-    solver.support_func_cached_guess = request.cached_support_func_guess;
-  }
+  GJKSolver solver(request);
 
   const DistanceFunctionMatrix& looktable = getDistanceFunctionLookTable();
 
@@ -179,22 +165,7 @@ FCL_REAL ComputeDistance::operator()(const Transform3f& tf1,
                                      const Transform3f& tf2,
                                      const DistanceRequest& request,
                                      DistanceResult& result) const {
-  GJKInitialGuess gjk_initial_guess = request.gjk_initial_guess;
-  solver.gjk_initial_guess = gjk_initial_guess;
-  bool cached = (gjk_initial_guess == GJKInitialGuess::CachedGuess ||
-                 request.enable_cached_gjk_guess);
-  solver.enable_cached_guess = cached;
-  if (cached) {
-    solver.cached_guess = request.cached_gjk_guess;
-    solver.support_func_cached_guess = request.cached_support_func_guess;
-  }
-
-  solver.gjk_tolerance = request.gjk_tolerance;
-  solver.gjk_max_iterations = request.gjk_max_iterations;
-  solver.gjk_variant = request.gjk_variant;
-  solver.gjk_convergence_criterion = request.gjk_convergence_criterion;
-  solver.gjk_convergence_criterion_type =
-      request.gjk_convergence_criterion_type;
+  solver.set(request);
 
   FCL_REAL res;
   if (request.enable_timings) {
@@ -204,7 +175,8 @@ FCL_REAL ComputeDistance::operator()(const Transform3f& tf1,
   } else
     res = run(tf1, tf2, request, result);
 
-  if (cached) {
+  if (solver.gjk_initial_guess == GJKInitialGuess::CachedGuess ||
+      solver.enable_cached_guess) {
     result.cached_gjk_guess = solver.cached_guess;
     result.cached_support_func_guess = solver.support_func_cached_guess;
   }