Fix Transform3f::inverse.

include/hpp/fcl/math/transform.h

@@ -204,7 +204,7 @@ public:
   }
 
   /// @brief inverse transform
-  inline Transform3f& inverse()
+  inline Transform3f& inverseInPlace()
   {
     matrix_set = false;
     q = q.conjugate();
@@ -212,6 +212,12 @@ public:
     return *this;
   }
 
+  /// @brief inverse transform
+  inline Transform3f inverse()
+  {
+    return Transform3f (R.transpose(), - R.transpose() * T);
+  }
+
   /// @brief inverse the transform and multiply with another
   inline Transform3f inverseTimes(const Transform3f& other) const
   {
@@ -272,4 +278,4 @@ inline Quaternion3f fromAxisAngle(const Eigen::MatrixBase<Derived>& axis, FCL_RE
 }
 } // namespace hpp
 
-#endif
+#endif
\ No newline at end of file

src/math/transform.cpp

@@ -55,12 +55,6 @@ const Matrix3f& Transform3f::getRotationInternal() const
   return R;
 }
 
-Transform3f inverse(const Transform3f& tf)
-{
-  Transform3f res(tf);
-  return res.inverse();
-}
-
 void relativeTransform(const Transform3f& tf1, const Transform3f& tf2,
                        Transform3f& tf)
 {

src/narrowphase/details.h

@@ -73,9 +73,6 @@ namespace fcl {
        const Capsule& s2, const Transform3f& tf2,
        Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal_)
     {
-      Transform3f tf2_inv (tf2);
-      tf2_inv.inverse();
-
       Vec3f pos1 (tf2.transform (Vec3f (0., 0., 0.5 * s2.lz))); // from distance function
       Vec3f pos2 (tf2.transform (Vec3f (0., 0., -0.5 * s2.lz)));
       Vec3f s_c = tf1.getTranslation ();
@@ -111,9 +108,6 @@ namespace fcl {
        const Capsule& s2, const Transform3f& tf2,
        FCL_REAL& dist, Vec3f& p1, Vec3f& p2, Vec3f& normal)
     {
-      Transform3f tf2_inv(tf2);
-      tf2_inv.inverse();
-
       Vec3f pos1 (tf2.transform (Vec3f (0., 0., 0.5 * s2.lz)));
       Vec3f pos2 (tf2.transform (Vec3f (0., 0., -0.5 * s2.lz)));
       Vec3f s_c = tf1.getTranslation ();