core: remove useless lines

They were related to shape deflations

include/hpp/fcl/narrowphase/gjk.h

@@ -51,7 +51,7 @@ namespace details {
 /// @brief the support function for shape
 /// \param hint use to initialize the search when shape is a ConvexBase object.
 Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, bool dirIsNormalized,
-                 int& hint, const FCL_REAL& shape_deflation);
+                 int& hint);
 
 /// @brief Minkowski difference class of two shapes
 ///
@@ -83,10 +83,6 @@ struct HPP_FCL_DLLAPI MinkowskiDiff {
   /// These inflation values are used for Sphere and Capsule.
   Array2d inflation;
 
-  /// @brief Deflation values associated to each support function.
-  /// The values are zero by default.
-  Array2d shape_deflation;
-
   /// @brief Number of points in a Convex object from which using a logarithmic
   /// support function is faster than a linear one.
   /// It defaults to 32.
@@ -106,8 +102,7 @@ struct HPP_FCL_DLLAPI MinkowskiDiff {
   GetSupportFunction getSupportFunc;
 
   MinkowskiDiff()
-      : shape_deflation(0, 0),
-        linear_log_convex_threshold(32),
+      : linear_log_convex_threshold(32),
         normalize_support_direction(false),
         getSupportFunc(NULL) {}
 
@@ -121,13 +116,13 @@ struct HPP_FCL_DLLAPI MinkowskiDiff {
 
   /// @brief support function for shape0
   inline Vec3f support0(const Vec3f& d, bool dIsNormalized, int& hint) const {
-    return getSupport(shapes[0], d, dIsNormalized, hint, shape_deflation[0]);
+    return getSupport(shapes[0], d, dIsNormalized, hint);
   }
 
   /// @brief support function for shape1
   inline Vec3f support1(const Vec3f& d, bool dIsNormalized, int& hint) const {
-    return oR1 * getSupport(shapes[1], oR1.transpose() * d, dIsNormalized, hint,
-                            shape_deflation[1]) +
+    return oR1 *
+               getSupport(shapes[1], oR1.transpose() * d, dIsNormalized, hint) +
            ot1;
   }
 

include/hpp/fcl/narrowphase/narrowphase.h

@@ -107,7 +107,6 @@ struct HPP_FCL_DLLAPI GJKSolver {
                       Vec3f* normal) const {
     details::MinkowskiDiff shape;
     shape.set(&s1, &s2, tf1, tf2);
-    shape.shape_deflation = shape_deflation;
 
     Vec3f guess;
     support_func_guess_t support_hint;
@@ -351,7 +350,6 @@ struct HPP_FCL_DLLAPI GJKSolver {
     gjk_variant = GJKVariant::DefaultGJK;
     gjk_convergence_criterion = GJKConvergenceCriterion::VDB;
     gjk_convergence_criterion_type = GJKConvergenceCriterionType::Relative;
-    shape_deflation.setZero();
   }
 
   /// @brief Constructor from a DistanceRequest
@@ -362,7 +360,6 @@ struct HPP_FCL_DLLAPI GJKSolver {
     cached_guess = Vec3f(1, 0, 0);
     support_func_cached_guess = support_func_guess_t::Zero();
     distance_upper_bound = (std::numeric_limits<FCL_REAL>::max)();
-    shape_deflation.setZero();
 
     // EPS settings
     epa_max_face_num = 128;
@@ -401,7 +398,6 @@ struct HPP_FCL_DLLAPI GJKSolver {
     cached_guess = Vec3f(1, 0, 0);
     support_func_cached_guess = support_func_guess_t::Zero();
     distance_upper_bound = (std::numeric_limits<FCL_REAL>::max)();
-    shape_deflation.setZero();
 
     // EPS settings
     epa_max_face_num = 128;
@@ -464,15 +460,6 @@ struct HPP_FCL_DLLAPI GJKSolver {
 
   bool operator!=(const GJKSolver& other) const { return !(*this == other); }
 
-  void resetShapeDeflation() const { shape_deflation.setZero(); }
-
-  void setShapeDeflation(const FCL_REAL shape1_deflation,
-                         const FCL_REAL shape2_deflation) const {
-    shape_deflation << shape1_deflation, shape2_deflation;
-    assert((shape_deflation <= 0).all() &&
-           "The deflation values should be negative.");
-  }
-
   /// @brief maximum number of simplex face used in EPA algorithm
   unsigned int epa_max_face_num;
 
@@ -520,12 +507,6 @@ struct HPP_FCL_DLLAPI GJKSolver {
   ///        the two shapes have a distance greather than distance_upper_bound.
   mutable FCL_REAL distance_upper_bound;
 
-  /// @brief Inflation values provided to the support functions associated to
-  /// each shape.
-  /// @note These inflation values are used to accound for negative
-  /// security_margin
-  mutable Array2d shape_deflation;
-
  public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };

src/distance/convex_halfspace.cpp

@@ -45,18 +45,13 @@ namespace fcl {
 template <>
 FCL_REAL ShapeShapeDistance<ConvexBase, Halfspace>(
     const CollisionGeometry* o1, const Transform3f& tf1,
-    const CollisionGeometry* o2, const Transform3f& tf2,
-    const GJKSolver* nsolver, const DistanceRequest&, DistanceResult& result) {
-  assert(nsolver->shape_deflation[0] <= 0 &&
-         "Shape deflation for the ConvexBase object should be negative");
-  assert(nsolver->shape_deflation[1] == 0 &&
-         "Shape deflation for the HalfSpace object should be zero");
-
+    const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*,
+    const DistanceRequest&, DistanceResult& result) {
   const ConvexBase& s1 = static_cast<const ConvexBase&>(*o1);
   const Halfspace& s2 = static_cast<const Halfspace&>(*o2);
   details::halfspaceDistance(s2, tf2, s1, tf1, result.min_distance,
                              result.nearest_points[1], result.nearest_points[0],
-                             result.normal, nsolver->shape_deflation[0]);
+                             result.normal);
   result.o1 = o1;
   result.o2 = o2;
   result.b1 = -1;
@@ -68,18 +63,13 @@ FCL_REAL ShapeShapeDistance<ConvexBase, Halfspace>(
 template <>
 FCL_REAL ShapeShapeDistance<Halfspace, ConvexBase>(
     const CollisionGeometry* o1, const Transform3f& tf1,
-    const CollisionGeometry* o2, const Transform3f& tf2,
-    const GJKSolver* nsolver, const DistanceRequest&, DistanceResult& result) {
-  assert(nsolver->shape_deflation[0] == 0 &&
-         "Shape deflation for the HalfSpace object should be zero");
-  assert(nsolver->shape_deflation[1] <= 0 &&
-         "Shape deflation for the ConvexBase object should be negative");
-
+    const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*,
+    const DistanceRequest&, DistanceResult& result) {
   const Halfspace& s1 = static_cast<const Halfspace&>(*o1);
   const ConvexBase& s2 = static_cast<const ConvexBase&>(*o2);
   details::halfspaceDistance(s1, tf1, s2, tf2, result.min_distance,
                              result.nearest_points[0], result.nearest_points[1],
-                             result.normal, nsolver->shape_deflation[1]);
+                             result.normal);
   result.o1 = o1;
   result.o2 = o2;
   result.b1 = -1;

src/distance/triangle_halfspace.cpp

@@ -45,18 +45,13 @@ namespace fcl {
 template <>
 FCL_REAL ShapeShapeDistance<TriangleP, Halfspace>(
     const CollisionGeometry* o1, const Transform3f& tf1,
-    const CollisionGeometry* o2, const Transform3f& tf2,
-    const GJKSolver* nsolver, const DistanceRequest&, DistanceResult& result) {
-  assert(nsolver->shape_deflation[0] == 0 &&
-         "Shape deflation for the TriangleP object should be zero");
-  assert(nsolver->shape_deflation[1] == 0 &&
-         "Shape deflation for the HalfSpace object should be zero");
-
+    const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*,
+    const DistanceRequest&, DistanceResult& result) {
   const TriangleP& s1 = static_cast<const TriangleP&>(*o1);
   const Halfspace& s2 = static_cast<const Halfspace&>(*o2);
   details::halfspaceDistance(s2, tf2, s1, tf1, result.min_distance,
                              result.nearest_points[1], result.nearest_points[0],
-                             result.normal, nsolver->shape_deflation[0]);
+                             result.normal);
   result.o1 = o1;
   result.o2 = o2;
   result.b1 = -1;
@@ -68,18 +63,13 @@ FCL_REAL ShapeShapeDistance<TriangleP, Halfspace>(
 template <>
 FCL_REAL ShapeShapeDistance<Halfspace, TriangleP>(
     const CollisionGeometry* o1, const Transform3f& tf1,
-    const CollisionGeometry* o2, const Transform3f& tf2,
-    const GJKSolver* nsolver, const DistanceRequest&, DistanceResult& result) {
-  assert(nsolver->shape_deflation[0] == 0 &&
-         "Shape deflation for the HalfSpace object should be zero");
-  assert(nsolver->shape_deflation[1] == 0 &&
-         "Shape deflation for the TriangleP object should be zero");
-
+    const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*,
+    const DistanceRequest&, DistanceResult& result) {
   const Halfspace& s1 = static_cast<const Halfspace&>(*o1);
   const TriangleP& s2 = static_cast<const TriangleP&>(*o2);
   details::halfspaceDistance(s1, tf1, s2, tf2, result.min_distance,
                              result.nearest_points[0], result.nearest_points[1],
-                             result.normal, nsolver->shape_deflation[1]);
+                             result.normal);
   result.o1 = o1;
   result.o2 = o2;
   result.b1 = -1;

src/narrowphase/details.h

@@ -1619,8 +1619,6 @@ inline bool convexHalfspaceIntersect(
     const ConvexBase& s1, const Transform3f& tf1, const Halfspace& s2,
     const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth,
     Vec3f* normal) {
-  // No need to account for the shape_deflation in the case of Convex,Halfspace
-  // as Halfspace objects have an infinite value possible for deflation.
   Halfspace new_s2 = transform(s2, tf2);
 
   Vec3f v;
@@ -1810,11 +1808,11 @@ inline bool halfspaceIntersect(const Halfspace& s1, const Transform3f& tf1,
 inline bool halfspaceDistance(const Halfspace& h, const Transform3f& tf1,
                               const ShapeBase& s, const Transform3f& tf2,
                               FCL_REAL& dist, Vec3f& p1, Vec3f& p2,
-                              Vec3f& normal, const FCL_REAL shape_deflation) {
+                              Vec3f& normal) {
   Vec3f n_w = tf1.getRotation() * h.n;
   Vec3f n_2(tf2.getRotation().transpose() * n_w);
   int hint = 0;
-  p2 = getSupport(&s, -n_2, true, hint, shape_deflation);
+  p2 = getSupport(&s, -n_2, true, hint);
   p2 = tf2.transform(p2);
 
   dist = (p2 - tf1.getTranslation()).dot(n_w) - h.d;