Clean API of MinkowskiDiff

include/hpp/fcl/narrowphase/gjk.h

@@ -54,6 +54,9 @@ Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir);
 
 /// @brief Minkowski difference class of two shapes
 ///
+/// \todo template this by the two shapes. The triangle / triangle case can be
+///       easily optimized computing once the triangle shapes[1] into frame0
+///
 /// \note The Minkowski difference is expressed in the frame of the first shape.
 struct MinkowskiDiff
 {
@@ -94,27 +97,12 @@ struct MinkowskiDiff
     return oR1 * getSupport(shapes[1], oR1.transpose() * d) + ot1;
   }
 
-  /// @brief support function for the pair of shapes
-  inline Vec3f support(const Vec3f& d) const
-  {
-    return support0(d) - support1(-d);
-  }
-
   /// @brief support function for the pair of shapes
   inline void support(const Vec3f& d, bool dIsNormalized, Vec3f& supp) const
   {
     assert(getSupportFunc != NULL);
     getSupportFunc(*this, d, dIsNormalized, supp);
   }
-
-  /// @brief support function for the d-th shape (d = 0 or 1)
-  inline Vec3f support(const Vec3f& d, size_t index) const
-  {
-    if(index)
-      return support1(d);
-    else
-      return support0(d);
-  }
 };
 
 /// @brief class for GJK algorithm

include/hpp/fcl/narrowphase/narrowphase.h

@@ -80,7 +80,7 @@ namespace fcl
                 Vec3f w0 (Vec3f::Zero());
                 for(size_t i = 0; i < epa.result.rank; ++i)
                   {
-                    w0 += shape.support(epa.result.vertex[i]->d, 0) *
+                    w0 += shape.support0(epa.result.vertex[i]->d) *
                       epa.result.coefficient[i];
                   }
                 if(penetration_depth) *penetration_depth = -epa.depth;
@@ -131,7 +131,7 @@ namespace fcl
             Vec3f w0 (Vec3f::Zero());
             for(size_t i = 0; i < epa.result.rank; ++i)
               {
-                w0 += shape.support(epa.result.vertex[i]->d, 0) *
+                w0 += shape.support0(epa.result.vertex[i]->d) *
                   epa.result.coefficient[i];
               }
             distance = -epa.depth;
@@ -148,8 +148,8 @@ namespace fcl
             for(size_t i = 0; i < gjk.getSimplex()->rank; ++i)
               {
                 FCL_REAL p = gjk.getSimplex()->coefficient[i];
-                w0 += shape.support(gjk.getSimplex()->vertex[i]->d, 0) * p;
-                w1 += shape.support(-gjk.getSimplex()->vertex[i]->d, 1) * p;
+                w0 += shape.support0( gjk.getSimplex()->vertex[i]->d) * p;
+                w1 += shape.support1(-gjk.getSimplex()->vertex[i]->d) * p;
               }
             distance = (w0 - w1).norm();
             p1 = tf1.transform (w0);
@@ -194,8 +194,8 @@ namespace fcl
         for(size_t i = 0; i < gjk.getSimplex()->rank; ++i)
         {
           FCL_REAL p = gjk.getSimplex()->coefficient[i];
-          w0 += shape.support(gjk.getSimplex()->vertex[i]->d, 0) * p;
-          w1 += shape.support(-gjk.getSimplex()->vertex[i]->d, 1) * p;
+          w0 += shape.support0( gjk.getSimplex()->vertex[i]->d) * p;
+          w1 += shape.support1(-gjk.getSimplex()->vertex[i]->d) * p;
         }
         distance = 0;
         p1 = p2 = tf1.transform (.5* (w0 + w1));
@@ -208,8 +208,8 @@ namespace fcl
           for(size_t i = 0; i < gjk.getSimplex()->rank; ++i)
             {
               FCL_REAL p = gjk.getSimplex()->coefficient[i];
-              w0 += shape.support(gjk.getSimplex()->vertex[i]->d, 0) * p;
-              w1 += shape.support(-gjk.getSimplex()->vertex[i]->d, 1) * p;
+              w0 += shape.support0( gjk.getSimplex()->vertex[i]->d) * p;
+              w1 += shape.support1(-gjk.getSimplex()->vertex[i]->d) * p;
             }
 
           distance = (w0 - w1).norm();
@@ -231,7 +231,7 @@ namespace fcl
                   Vec3f w0 (Vec3f::Zero());
                   for(size_t i = 0; i < epa.result.rank; ++i)
                     {
-                      w0 += shape.support(epa.result.vertex[i]->d, 0) *
+                      w0 += shape.support0(epa.result.vertex[i]->d) *
                         epa.result.coefficient[i];
                     }
                   assert (epa.depth >= -eps);
@@ -246,8 +246,8 @@ namespace fcl
               for(size_t i = 0; i < gjk.getSimplex()->rank; ++i)
                 {
                   FCL_REAL p = gjk.getSimplex()->coefficient[i];
-                  w0 += shape.support(gjk.getSimplex()->vertex[i]->d, 0) * p;
-                  w1 += shape.support(-gjk.getSimplex()->vertex[i]->d, 1) * p;
+                  w0 += shape.support0( gjk.getSimplex()->vertex[i]->d) * p;
+                  w1 += shape.support1(-gjk.getSimplex()->vertex[i]->d) * p;
                 }
               distance = 0;
 

src/narrowphase/narrowphase.cpp

@@ -648,8 +648,8 @@ bool GJKSolver_indep::shapeDistance<Capsule, Capsule>
     for(size_t i = 0; i < gjk.getSimplex()->rank; ++i)
     {
       FCL_REAL p = gjk.getSimplex()->coefficient[i];
-      w0 += shape.support(gjk.getSimplex()->vertex[i]->d, 0) * p;
-      w1 += shape.support(-gjk.getSimplex()->vertex[i]->d, 1) * p;
+      w0 += shape.support0( gjk.getSimplex()->vertex[i]->d) * p;
+      w1 += shape.support1(-gjk.getSimplex()->vertex[i]->d) * p;
     }
     if((gjk_status == details::GJK::Valid) ||
        (gjk_status == details::GJK::Failed))