Add Matrix6 class

eigen3/include/ignition/math/eigen3/Conversions.hh

@@ -21,6 +21,7 @@
 #include <Eigen/Geometry>
 #include <ignition/math/AxisAlignedBox.hh>
 #include <ignition/math/Matrix3.hh>
+#include <ignition/math/Matrix6.hh>
 #include <ignition/math/Pose3.hh>
 #include <ignition/math/Quaternion.hh>
 #include <ignition/math/Vector3.hh>
@@ -66,6 +67,27 @@ namespace ignition
         return matrix;
       }
 
+      /// \brief Convert from ignition::math::Matrix6d to
+      /// Eigen::Matrix<Precision, 6, 6>.
+      /// \param[in] _m ignition::math::Matrix6d to convert.
+      /// \return The equivalent Eigen::Matrix<Precision, 6, 6>.
+      /// \tparam Precision Precision such as int, double or float.
+      template<typename Precision>
+      inline
+      Eigen::Matrix<Precision, 6, 6> convert(const Matrix6<Precision> &_m)
+      {
+        Eigen::Matrix<Precision, 6, 6> matrix;
+        for (std::size_t i = 0; i < 6; ++i)
+        {
+          for (std::size_t j = 0; j < 6; ++j)
+          {
+            matrix(i, j) = _m(i, j);
+          }
+        }
+
+        return matrix;
+      }
+
       /// \brief Convert ignition::math::Quaterniond to Eigen::Quaterniond.
       /// \param[in] _q ignition::math::Quaterniond to convert.
       /// \return The equivalent Eigen::Quaterniond.
@@ -135,6 +157,27 @@ namespace ignition
         return matrix;
       }
 
+      /// \brief Convert Eigen::Matrix<Precision, 6, 6> to
+      /// ignition::math::Matrix6d.
+      /// \param[in] _m Eigen::Matrix<Precision, 6, 6> to convert.
+      /// \return The equivalent ignition::math::Matrix6d.
+      /// \tparam Precision Precision such as int, double or float.
+      template<typename Precision>
+      inline
+      Matrix6<Precision> convert(const Eigen::Matrix<Precision, 6, 6> &_m)
+      {
+        Matrix6<Precision> matrix;
+        for (std::size_t i = 0; i < 6; ++i)
+        {
+          for (std::size_t j = 0; j < 6; ++j)
+          {
+            matrix(i, j) = _m(i, j);
+          }
+        }
+
+        return matrix;
+      }
+
       /// \brief Convert Eigen::Quaterniond to ignition::math::Quaterniond.
       /// \param[in] _q Eigen::Quaterniond to convert.
       /// \return The equivalent ignition::math::Quaterniond.

eigen3/src/Conversions_TEST.cc

@@ -144,6 +144,46 @@ TEST(EigenConversions, ConvertMatrix3)
   }
 }
 
+/////////////////////////////////////////////////
+/// Check Matrix6 conversions
+TEST(EigenConversions, ConvertMatrix6)
+{
+  {
+    ignition::math::Matrix6d iMat, iMat2;
+    auto eMat = ignition::math::eigen3::convert(iMat);
+    for (std::size_t i = 0; i < 6; ++i)
+    {
+      for (std::size_t j = 0; j < 6; ++j)
+      {
+        EXPECT_DOUBLE_EQ(0.0, eMat(i, j));
+      }
+    }
+    iMat2 = ignition::math::eigen3::convert(eMat);
+    EXPECT_EQ(iMat, iMat2);
+  }
+
+  {
+    ignition::math::Matrix6d iMat(
+        0.0, 1.0, 2.0, 3.0, 4.0, 5.0,
+        6.0, 7.0, 8.0, 9.0, 10.0, 11.0,
+        12.0, 13.0, 14.0, 15.0, 16.0, 17.0,
+        18.0, 19.0, 20.0, 21.0, 22.0, 23.0,
+        24.0, 25.0, 26.0, 27.0, 28.0, 29.0,
+        30.0, 31.0, 32.0, 33.0, 34.0, 35.0);
+    ignition::math::Matrix6d iMat2;
+    auto eMat = ignition::math::eigen3::convert(iMat);
+    for (std::size_t i = 0; i < 6; ++i)
+    {
+      for (std::size_t j = 0; j < 6; ++j)
+      {
+        EXPECT_DOUBLE_EQ(iMat(i, j), eMat(i, j));
+      }
+    }
+    iMat2 = ignition::math::eigen3::convert(eMat);
+    EXPECT_EQ(iMat, iMat2);
+  }
+}
+
 /////////////////////////////////////////////////
 /// Check Pose conversions
 TEST(EigenConversions, ConvertPose3)