core: assertion check in initialize_gjk in GJKSolver

coal-library · Jun 3, 2022 · dce544c · dce544c
1 parent 492f0ff
commit dce544c
Showing 1 changed file with 1 addition and 0 deletions.
diff --git a/include/hpp/fcl/narrowphase/narrowphase.h b/include/hpp/fcl/narrowphase/narrowphase.h
@@ -65,6 +65,7 @@ struct HPP_FCL_DLLAPI GJKSolver {
         support_hint = support_func_cached_guess;
         break;
       case GJKInitialGuess::BoundingVolumeGuess:
+        assert((s1.aabb_radius > 0) && (s2.aabb_radius > 0));
         guess.noalias() =
             s1.aabb_center - (shape.oR1 * s2.aabb_center + shape.ot1);
         support_hint =