core: check in initialize_gjk in GJKSolver

coal-library · Jun 3, 2022 · 1d92924 · 1d92924
1 parent 492f0ff
commit 1d92924
Showing 1 changed file with 5 additions and 0 deletions.
diff --git a/include/hpp/fcl/narrowphase/narrowphase.h b/include/hpp/fcl/narrowphase/narrowphase.h
@@ -65,6 +65,11 @@ struct HPP_FCL_DLLAPI GJKSolver {
         support_hint = support_func_cached_guess;
         break;
       case GJKInitialGuess::BoundingVolumeGuess:
+        if (s1.aabb_radius < 0 || s2.aabb_radius < 0) {
+          throw std::logic_error(
+              "computeLocalAABB must have been called on the shapes when using "
+              "GJKInitialGuess::BoundingVolumeGuess.");
+        }
         guess.noalias() =
             s1.aabb_center - (shape.oR1 * s2.aabb_center + shape.ot1);
         support_hint =