Reduce memory footprint of MinkowskiDiff

include/hpp/fcl/narrowphase/gjk.h

@@ -60,11 +60,13 @@ struct MinkowskiDiff
   /// @brief points to two shapes
   const ShapeBase* shapes[2];
 
-  /// @brief rotation from shape0 to shape1
-  Matrix3f toshape1;
+  /// @brief rotation from shape1 to shape0
+  /// such that \f$ p_in_0 = oR1 * p_in_1 + ot1 \f$.
+  Matrix3f oR1;
 
-  /// @brief transform from shape1 to shape0 
-  Transform3f toshape0;
+  /// @brief translation from shape1 to shape0
+  /// such that \f$ p_in_0 = oR1 * p_in_1 + ot1 \f$.
+  Vec3f ot1;
 
   typedef void (*GetSupportFunction) (const MinkowskiDiff& minkowskiDiff,
       const Vec3f& dir, bool dirIsNormalized, Vec3f& support);
@@ -74,16 +76,22 @@ struct MinkowskiDiff
 
   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());
+  }
+
   /// @brief support function for shape0
   inline Vec3f support0(const Vec3f& d) const
   {
     return getSupport(shapes[0], d);
   }
-  
+
   /// @brief support function for shape1
   inline Vec3f support1(const Vec3f& d) const
   {
-    return toshape0.transform(getSupport(shapes[1], toshape1 * d));
+    return oR1 * getSupport(shapes[1], oR1.transpose() * d) + ot1;
   }
 
   /// @brief support function for the pair of shapes

include/hpp/fcl/narrowphase/narrowphase.h

@@ -63,8 +63,7 @@ namespace fcl
     
       details::MinkowskiDiff shape;
       shape.set (&s1, &s2);
-      shape.toshape1 = tf2.getRotation().transpose() * tf1.getRotation();
-      shape.toshape0 = tf1.inverseTimes(tf2);
+      shape.set (tf1, tf2);
   
       details::GJK gjk((unsigned int )gjk_max_iterations, gjk_tolerance);
       details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
@@ -115,8 +114,7 @@ namespace fcl
 
       details::MinkowskiDiff shape;
       shape.set (&s, &tri);
-      shape.toshape1 = tf2.getRotation().transpose() * tf1.getRotation();
-      shape.toshape0 = tf1.inverseTimes(tf2);
+      shape.set (tf1, tf2);
   
       details::GJK gjk((unsigned int )gjk_max_iterations, gjk_tolerance);
       details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
@@ -182,8 +180,7 @@ namespace fcl
 
       details::MinkowskiDiff shape;
       shape.set (&s1, &s2);
-      shape.toshape1 = tf2.getRotation().transpose() * tf1.getRotation();
-      shape.toshape0 = tf1.inverseTimes(tf2);
+      shape.set (tf1, tf2);
 
       details::GJK gjk((unsigned int) gjk_max_iterations, gjk_tolerance);
       details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);

src/narrowphase/gjk.cpp

@@ -203,13 +203,13 @@ Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir)
 
 template <typename Shape0, typename Shape1>
 void getSupportTpl (const Shape0* s0, const Shape1* s1,
-    const Matrix3f& oM1, const Vec3f& ot1,
+    const Matrix3f& oR1, const Vec3f& ot1,
     const Vec3f& dir, Vec3f& support)
 {
   getShapeSupport (s0, dir, support);
   Vec3f support1;
-  getShapeSupport (s1, - oM1.transpose() * dir, support1);
-  support.noalias() -= oM1 * support1 + ot1;
+  getShapeSupport (s1, - oR1.transpose() * dir, support1);
+  support.noalias() -= oR1 * support1 + ot1;
 }
 
 template <typename Shape0, typename Shape1>
@@ -223,8 +223,7 @@ void getSupportFuncTpl (const MinkowskiDiff& md,
   getSupportTpl<Shape0, Shape1> (
       static_cast <const Shape0*>(md.shapes[0]),
       static_cast <const Shape1*>(md.shapes[1]),
-      md.toshape0.getRotation(),
-      md.toshape0.getTranslation(),
+      md.oR1, md.ot1,
       (NeedNormalizedDir && !dirIsNormalized) ? dir.normalized() : dir,
       support);
 }

src/narrowphase/narrowphase.cpp

@@ -631,8 +631,7 @@ bool GJKSolver_indep::shapeDistance<Capsule, Capsule>
 
     details::MinkowskiDiff shape;
     shape.set (&s1, &s2);
-    shape.toshape1 = tf2.getRotation().transpose() * tf1.getRotation();
-    shape.toshape0 = tf1.inverseTimes(tf2);
+    shape.set (tf1, tf2);
 
     const Vec3f& P1 (s1.a);
     const Vec3f& P2 (s1.b);