Templatize relativeTransform()

- Reference: https://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html
GradyLiugy · Jul 28, 2016 · 150903e · 150903e
1 parent 934af56
commit 150903e
Show file tree

Hide file tree

Showing 2 changed files with 48 additions and 7 deletions.
diff --git a/include/fcl/math/geometry.h b/include/fcl/math/geometry.h
@@ -169,15 +169,56 @@ void generateCoordinateSystem(Matrix3<T>& axis)
   }
 }
 
-template<typename T>
-void relativeTransform(const Matrix3<T>& R1, const Vector3<T>& t1,
-                       const Matrix3<T>& R2, const Vector3<T>& t2,
-                       Matrix3<T>& R, Vector3<T>& t)
+template <typename Derived1, typename Derived2, typename Derived3, typename Derived4>
+void relativeTransform(
+    const Eigen::MatrixBase<Derived1>& R1, const Eigen::MatrixBase<Derived2>& t1,
+    const Eigen::MatrixBase<Derived1>& R2, const Eigen::MatrixBase<Derived2>& t2,
+    Eigen::MatrixBase<Derived3>& R, Eigen::MatrixBase<Derived4>& t)
 {
+  EIGEN_STATIC_ASSERT(
+        Derived1::RowsAtCompileTime == 3
+        && Derived1::ColsAtCompileTime == 3,
+        THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
+
+  EIGEN_STATIC_ASSERT(
+        Derived2::RowsAtCompileTime == 3
+        && Derived2::ColsAtCompileTime == 1,
+        THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
+
+  EIGEN_STATIC_ASSERT(
+        Derived3::RowsAtCompileTime == 3
+        && Derived3::ColsAtCompileTime == 3,
+        THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
+
+  EIGEN_STATIC_ASSERT(
+        Derived4::RowsAtCompileTime == 3
+        && Derived4::ColsAtCompileTime == 1,
+        THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
+
   R = R1.transpose() * R2;
   t = R1.transpose() * (t2 - t1);
 }
 
+template <typename Scalar, typename Derived1, typename Derived2>
+void relativeTransform(
+    const Eigen::Transform<Scalar, 3, Eigen::Isometry>& T1,
+    const Eigen::Transform<Scalar, 3, Eigen::Isometry>& T2,
+    Eigen::MatrixBase<Derived1>& R, Eigen::MatrixBase<Derived2>& t)
+{
+  EIGEN_STATIC_ASSERT(
+        Derived1::RowsAtCompileTime == 3
+        && Derived1::ColsAtCompileTime == 3,
+        THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
+
+  EIGEN_STATIC_ASSERT(
+        Derived2::RowsAtCompileTime == 3
+        && Derived2::ColsAtCompileTime == 1,
+        THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
+
+  relativeTransform(
+        T1.linear(), T1.translation(), T2.linear(), T2.translation(), R, t);
+}
+
 } // namespace fcl
 
 #endif
diff --git a/src/traversal/traversal_node_setup.cpp b/src/traversal/traversal_node_setup.cpp
@@ -70,7 +70,7 @@ static inline bool setupMeshCollisionOrientedNode(OrientedNode& node,
 
   node.cost_density = model1.cost_density * model2.cost_density;
 
-  relativeTransform(tf1.linear().eval(), tf1.translation().eval(), tf2.linear().eval(), tf2.translation().eval(), node.R, node.T);
+  relativeTransform(tf1, tf2, node.R, node.T);
 
   return true;
 }
@@ -143,7 +143,7 @@ static inline bool setupMeshDistanceOrientedNode(OrientedNode& node,
   node.tri_indices1 = model1.tri_indices;
   node.tri_indices2 = model2.tri_indices;
 
-  relativeTransform(tf1.linear().eval(), tf1.translation().eval(), tf2.linear().eval(), tf2.translation().eval(), node.R, node.T);
+  relativeTransform(tf1, tf2, node.R, node.T);
 
   return true;
 }
@@ -203,7 +203,7 @@ static inline bool setupMeshConservativeAdvancementOrientedDistanceNode(Oriented
 
   node.w = w;
 
-  relativeTransform(tf1.linear().eval(), tf1.translation().eval(), tf2.linear().eval(), tf2.translation().eval(), node.R, node.T);
+  relativeTransform(tf1, tf2, node.R, node.T);
 
   return true;
 }