[GJK] Put shapes in the same frame when possible.

include/hpp/fcl/narrowphase/gjk.h

@@ -77,13 +77,13 @@ struct MinkowskiDiff
 
   MinkowskiDiff() : getSupportFunc (NULL) {}
 
+  /// Set the two shapes,
+  /// assuming the relative transformation between them is identity.
   void set (const ShapeBase* shape0, const ShapeBase* shape1);
 
-  void set (const Transform3f& tf0, const Transform3f& tf1)
-  {
-    oR1 = tf0.getRotation().transpose() * tf1.getRotation();
-    ot1 = tf0.getRotation().transpose() * (tf1.getTranslation() - tf0.getTranslation());
-  }
+  /// Set the two shapes, with a relative transformation.
+  void set (const ShapeBase* shape0, const ShapeBase* shape1,
+      const Transform3f& tf0, const Transform3f& tf1);
 
   /// @brief support function for shape0
   inline Vec3f support0(const Vec3f& d, bool dIsNormalized) const

include/hpp/fcl/narrowphase/narrowphase.h

@@ -62,8 +62,7 @@ namespace fcl
       if(enable_cached_guess) guess = cached_guess;
     
       details::MinkowskiDiff shape;
-      shape.set (&s1, &s2);
-      shape.set (tf1, tf2);
+      shape.set (&s1, &s2, tf1, tf2);
   
       details::GJK gjk((unsigned int )gjk_max_iterations, gjk_tolerance);
       details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
@@ -103,14 +102,18 @@ namespace fcl
      Vec3f& p1, Vec3f& p2, Vec3f& normal) const
     {
       bool col;
-      TriangleP tri(P1, P2, P3);
+      // Express everything in frame 1
+      const Transform3f tf_1M2 (tf1.inverseTimes(tf2));
+      TriangleP tri(
+          tf_1M2.transform (P1),
+          tf_1M2.transform (P2),
+          tf_1M2.transform (P3));
 
       Vec3f guess(1, 0, 0);
       if(enable_cached_guess) guess = cached_guess;
 
       details::MinkowskiDiff shape;
       shape.set (&s, &tri);
-      shape.set (tf1, tf2);
   
       details::GJK gjk((unsigned int )gjk_max_iterations, gjk_tolerance);
       details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
@@ -170,8 +173,7 @@ namespace fcl
       if(enable_cached_guess) guess = cached_guess;
 
       details::MinkowskiDiff shape;
-      shape.set (&s1, &s2);
-      shape.set (tf1, tf2);
+      shape.set (&s1, &s2, tf1, tf2);
 
       details::GJK gjk((unsigned int) gjk_max_iterations, gjk_tolerance);
       details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);

src/narrowphase/details.h

@@ -2626,6 +2626,21 @@ namespace fcl {
 
       return true;
     }
+
+    /// See the prototype below
+    inline FCL_REAL computePenetration
+      (const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
+       const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3,
+       Vec3f& normal)
+    {
+      Vec3f u ((P2-P1).cross (P3-P1));
+      normal = u.normalized ();
+      FCL_REAL depth1 ((P1-Q1).dot (normal));
+      FCL_REAL depth2 ((P1-Q2).dot (normal));
+      FCL_REAL depth3 ((P1-Q3).dot (normal));
+      return std::max (depth1, std::max (depth2, depth3));
+    }
+
     // Compute penetration distance and normal of two triangles in collision
     // Normal is normal of triangle 1 (P1, P2, P3), penetration depth is the
     // minimal distance (Q1, Q2, Q3) should be translated along the normal so
@@ -2644,14 +2659,8 @@ namespace fcl {
       Vec3f globalQ1 (tf2.transform (Q1));
       Vec3f globalQ2 (tf2.transform (Q2));
       Vec3f globalQ3 (tf2.transform (Q3));
-      Vec3f u ((globalP2-globalP1).cross (globalP3-globalP1));
-      normal = u.normalized ();
-      FCL_REAL depth;
-      FCL_REAL depth1 ((globalP1-globalQ1).dot (normal));
-      FCL_REAL depth2 ((globalP1-globalQ2).dot (normal));
-      FCL_REAL depth3 ((globalP1-globalQ3).dot (normal));
-      depth = std::max (depth1, std::max (depth2, depth3));
-      return depth;
+      return computePenetration (globalP1, globalP2, globalP3,
+          globalQ1, globalQ2, globalQ3, normal);
     }
   } // details
 } // namespace fcl

src/narrowphase/gjk.cpp

@@ -210,17 +210,21 @@ Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, bool dirIsNormalized)
 
 #undef CALL_GET_SHAPE_SUPPORT
 
-template <typename Shape0, typename Shape1>
+template <typename Shape0, typename Shape1, bool TransformIsIdentity>
 void getSupportTpl (const Shape0* s0, const Shape1* s1,
     const Matrix3f& oR1, const Vec3f& ot1,
     const Vec3f& dir, Vec3f& support0, Vec3f& support1)
 {
   getShapeSupport (s0, dir, support0);
-  getShapeSupport (s1, - oR1.transpose() * dir, support1);
-  support1 = oR1 * support1 + ot1;
+  if (TransformIsIdentity)
+    getShapeSupport (s1, - dir, support1);
+  else {
+    getShapeSupport (s1, - oR1.transpose() * dir, support1);
+    support1 = oR1 * support1 + ot1;
+  }
 }
 
-template <typename Shape0, typename Shape1>
+template <typename Shape0, typename Shape1, bool TransformIsIdentity>
 void getSupportFuncTpl (const MinkowskiDiff& md,
     const Vec3f& dir, bool dirIsNormalized, Vec3f& support0, Vec3f& support1)
 {
@@ -228,7 +232,7 @@ void getSupportFuncTpl (const MinkowskiDiff& md,
     bool ( (bool)shape_traits<Shape0>::NeedNormalizedDir
         || (bool)shape_traits<Shape1>::NeedNormalizedDir)
   };
-  getSupportTpl<Shape0, Shape1> (
+  getSupportTpl<Shape0, Shape1, TransformIsIdentity> (
       static_cast <const Shape0*>(md.shapes[0]),
       static_cast <const Shape1*>(md.shapes[1]),
       md.oR1, md.ot1,
@@ -237,62 +241,91 @@ void getSupportFuncTpl (const MinkowskiDiff& md,
 }
 
 template <typename Shape0>
-MinkowskiDiff::GetSupportFunction makeGetSupportFunction1 (const ShapeBase* s1)
+MinkowskiDiff::GetSupportFunction makeGetSupportFunction1 (const ShapeBase* s1, bool identity)
 {
   switch(s1->getNodeType())
   {
   case GEOM_TRIANGLE:
-    return getSupportFuncTpl<Shape0, TriangleP>;
+    if (identity) return getSupportFuncTpl<Shape0, TriangleP, true >;
+    else          return getSupportFuncTpl<Shape0, TriangleP, false>;
   case GEOM_BOX:
-    return getSupportFuncTpl<Shape0, Box>;
+    if (identity) return getSupportFuncTpl<Shape0, Box, true >;
+    else          return getSupportFuncTpl<Shape0, Box, false>;
   case GEOM_SPHERE:
-    return getSupportFuncTpl<Shape0, Sphere>;
+    if (identity) return getSupportFuncTpl<Shape0, Sphere, true >;
+    else          return getSupportFuncTpl<Shape0, Sphere, false>;
   case GEOM_CAPSULE:
-    return getSupportFuncTpl<Shape0, Capsule>;
+    if (identity) return getSupportFuncTpl<Shape0, Capsule, true >;
+    else          return getSupportFuncTpl<Shape0, Capsule, false>;
   case GEOM_CONE:
-    return getSupportFuncTpl<Shape0, Cone>;
+    if (identity) return getSupportFuncTpl<Shape0, Cone, true >;
+    else          return getSupportFuncTpl<Shape0, Cone, false>;
   case GEOM_CYLINDER:
-    return getSupportFuncTpl<Shape0, Cylinder>;
+    if (identity) return getSupportFuncTpl<Shape0, Cylinder, true >;
+    else          return getSupportFuncTpl<Shape0, Cylinder, false>;
   case GEOM_CONVEX:
-    return getSupportFuncTpl<Shape0, Convex>;
+    if (identity) return getSupportFuncTpl<Shape0, Convex, true >;
+    else          return getSupportFuncTpl<Shape0, Convex, false>;
   default:
     throw std::logic_error ("Unsupported geometric shape");
   }
 }
 
-void MinkowskiDiff::set (const ShapeBase* shape0, const ShapeBase* shape1)
+MinkowskiDiff::GetSupportFunction makeGetSupportFunction0 (const ShapeBase* s0, const ShapeBase* s1, bool identity)
 {
-  shapes[0] = shape0;
-  shapes[1] = shape1;
-
-  switch(shape0->getNodeType())
+  switch(s0->getNodeType())
   {
   case GEOM_TRIANGLE:
-    getSupportFunc = makeGetSupportFunction1<TriangleP> (shape1);
+    return makeGetSupportFunction1<TriangleP> (s1, identity);
     break;
   case GEOM_BOX:
-    getSupportFunc = makeGetSupportFunction1<Box> (shape1);
+    return makeGetSupportFunction1<Box> (s1, identity);
     break;
   case GEOM_SPHERE:
-    getSupportFunc = makeGetSupportFunction1<Sphere> (shape1);
+    return makeGetSupportFunction1<Sphere> (s1, identity);
     break;
   case GEOM_CAPSULE:
-    getSupportFunc = makeGetSupportFunction1<Capsule> (shape1);
+    return makeGetSupportFunction1<Capsule> (s1, identity);
     break;
   case GEOM_CONE:
-    getSupportFunc = makeGetSupportFunction1<Cone> (shape1);
+    return makeGetSupportFunction1<Cone> (s1, identity);
     break;
   case GEOM_CYLINDER:
-    getSupportFunc = makeGetSupportFunction1<Cylinder> (shape1);
+    return makeGetSupportFunction1<Cylinder> (s1, identity);
     break;
   case GEOM_CONVEX:
-    getSupportFunc = makeGetSupportFunction1<Convex> (shape1);
+    return makeGetSupportFunction1<Convex> (s1, identity);
     break;
   default:
     throw std::logic_error ("Unsupported geometric shape");
   }
 }
 
+void MinkowskiDiff::set (const ShapeBase* shape0, const ShapeBase* shape1,
+    const Transform3f& tf0, const Transform3f& tf1)
+{
+  shapes[0] = shape0;
+  shapes[1] = shape1;
+
+  oR1 = tf0.getRotation().transpose() * tf1.getRotation();
+  ot1 = tf0.getRotation().transpose() * (tf1.getTranslation() - tf0.getTranslation());
+
+  bool identity = (oR1.isIdentity() && ot1.isZero());
+
+  getSupportFunc = makeGetSupportFunction0 (shape0, shape1, identity);
+}
+
+void MinkowskiDiff::set (const ShapeBase* shape0, const ShapeBase* shape1)
+{
+  shapes[0] = shape0;
+  shapes[1] = shape1;
+
+  oR1.setIdentity();
+  ot1.setZero();
+
+  getSupportFunc = makeGetSupportFunction0 (shape0, shape1, true);
+}
+
 void GJK::initialize()
 {
   nfree = 0;

src/narrowphase/narrowphase.cpp

@@ -625,20 +625,16 @@ bool GJKSolver_indep::shapeDistance<Capsule, Capsule>
      const TriangleP& s2, const Transform3f& tf2,
      FCL_REAL& dist, Vec3f& p1, Vec3f& p2, Vec3f& normal) const
   {
+    const TriangleP
+      t1 (tf1.transform(s1.a), tf1.transform(s1.b), tf1.transform(s1.c)),
+      t2 (tf2.transform(s2.a), tf2.transform(s2.b), tf2.transform(s2.c));
+
     Vec3f guess(1, 0, 0);
     if(enable_cached_guess) guess = cached_guess;
     bool enable_penetration (true);
 
     details::MinkowskiDiff shape;
-    shape.set (&s1, &s2);
-    shape.set (tf1, tf2);
-
-    const Vec3f& P1 (s1.a);
-    const Vec3f& P2 (s1.b);
-    const Vec3f& P3 (s1.c);
-    const Vec3f& Q1 (s2.a);
-    const Vec3f& Q2 (s2.b);
-    const Vec3f& Q3 (s2.c);
+    shape.set (&t1, &t2);
 
     details::GJK gjk((unsigned int) gjk_max_iterations, gjk_tolerance);
     details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
@@ -653,19 +649,14 @@ bool GJKSolver_indep::shapeDistance<Capsule, Capsule>
       // (i.e. an object face normal is colinear to gjk.ray
       // assert (dist == (w0 - w1).norm());
       dist = gjk.distance;
-      p1 = tf1.transform (p1);
-      p2 = tf1.transform (p2);
 
       return true;
     }
     else if (gjk_status == details::GJK::Inside)
     {
-      p1 = tf1.transform (p1);
-      p2 = tf1.transform (p2);
-
       if (enable_penetration) {
         FCL_REAL penetrationDepth = details::computePenetration
-          (P1, P2, P3, Q1, Q2, Q3, tf1, tf2, normal);
+          (t1.a, t1.b, t1.c, t2.a, t2.b, t2.c, normal);
         dist = -penetrationDepth;
         assert (dist <= 1e-6);
         // GJK says Inside when below GJK.tolerance. So non intersecting