Added std::optional MassMatrix() functions for Box, Cylinder & Sphere

include/gz/math/Box.hh

@@ -17,6 +17,7 @@
 #ifndef GZ_MATH_BOX_HH_
 #define GZ_MATH_BOX_HH_
 
+#include <optional>
 #include <gz/math/config.hh>
 #include <gz/math/MassMatrix3.hh>
 #include <gz/math/Material.hh>
@@ -189,6 +190,14 @@ namespace gz
       /// could be due to an invalid size (<=0) or density (<=0).
       public: bool MassMatrix(MassMatrix3<Precision> &_massMat) const;
 
+      /// \brief Get the mass matrix for this box. This function
+      /// is only meaningful if the box's size and material
+      /// have been set.
+      /// \return The computed mass matrix if parameters are valid
+      /// (radius > 0), (length > 0), and (density > 0). Otherwise
+      /// std::nullopt is returned.
+      public: std::optional< MassMatrix3<Precision> > MassMatrix() const;
+
       /// \brief Get intersection between a plane and the box's edges.
       /// Edges contained on the plane are ignored.
       /// \param[in] _plane The plane against which we are testing intersection.

include/gz/math/Cylinder.hh

@@ -17,6 +17,7 @@
 #ifndef GZ_MATH_CYLINDER_HH_
 #define GZ_MATH_CYLINDER_HH_
 
+#include <optional>
 #include "gz/math/MassMatrix3.hh"
 #include "gz/math/Material.hh"
 #include "gz/math/Quaternion.hh"
@@ -115,6 +116,14 @@ namespace gz
       /// (<=0).
       public: bool MassMatrix(MassMatrix3d &_massMat) const;
 
+      /// \brief Get the mass matrix for this cylinder. This function
+      /// is only meaningful if the cylinder's radius, length, and material
+      /// have been set. Optionally, set the rotational offset.
+      /// \return The computed mass matrix if parameters are valid
+      /// (radius > 0), (length > 0) and (density > 0). Otherwise
+      /// std::nullopt is returned.
+      public: std::optional< MassMatrix3<Precision> > MassMatrix() const;
+
       /// \brief Check if this cylinder is equal to the provided cylinder.
       /// Radius, length, and material properties will be checked.
       public: bool operator==(const Cylinder &_cylinder) const;

include/gz/math/Sphere.hh

@@ -17,6 +17,7 @@
 #ifndef GZ_MATH_SPHERE_HH_
 #define GZ_MATH_SPHERE_HH_
 
+#include <optional>
 #include "gz/math/MassMatrix3.hh"
 #include "gz/math/Material.hh"
 #include "gz/math/Quaternion.hh"
@@ -77,6 +78,13 @@ namespace gz
       /// could be due to an invalid radius (<=0) or density (<=0).
       public: bool MassMatrix(MassMatrix3d &_massMat) const;
 
+      /// \brief Get the mass matrix for this sphere. This function
+      /// is only meaningful if the sphere's radius and material have been set.
+      /// \return The computed mass matrix if parameters are valid
+      /// (radius > 0) and (density > 0). Otherwise
+      /// std::nullopt is returned.
+      public: std::optional< MassMatrix3<Precision> > MassMatrix() const;
+
       /// \brief Check if this sphere is equal to the provided sphere.
       /// Radius and material properties will be checked.
       public: bool operator==(const Sphere &_sphere) const;

include/gz/math/detail/Box.hh

@@ -310,6 +310,21 @@ bool Box<T>::MassMatrix(MassMatrix3<T> &_massMat) const
   return _massMat.SetFromBox(this->material, this->size);
 }
 
+/////////////////////////////////////////////////
+template<typename T>
+std::optional< MassMatrix3<T> > Box<T>::MassMatrix() const
+{
+  gz::math::MassMatrix3<T> _massMat;
+
+  if(!_massMat.SetFromBox(this->material, this->size))
+  {
+    return std::nullopt;
+  }
+  else
+  {
+    return std::make_optional(_massMat);
+  }
+}
 
 //////////////////////////////////////////////////
 template<typename T>

include/gz/math/detail/Cylinder.hh

@@ -116,6 +116,24 @@ bool Cylinder<T>::MassMatrix(MassMatrix3d &_massMat) const
       this->radius, this->rotOffset);
 }
 
+//////////////////////////////////////////////////
+template<typename T>
+std::optional < MassMatrix3<T> > Cylinder<T>::MassMatrix() const
+{
+  gz::math::MassMatrix3<T> _massMat;
+
+  if(!_massMat.SetFromCylinderZ(
+      this->material, this->length,
+      this->radius, this->rotOffset))
+  {
+    return std::nullopt;
+  }
+  else
+  {
+    return std::make_optional(_massMat);
+  }
+}
+
 //////////////////////////////////////////////////
 template<typename T>
 T Cylinder<T>::Volume() const

include/gz/math/detail/Sphere.hh

@@ -88,6 +88,23 @@ bool Sphere<T>::MassMatrix(MassMatrix3d &_massMat) const
   return _massMat.SetFromSphere(this->material, this->radius);
 }
 
+//////////////////////////////////////////////////
+template<typename T>
+std::optional < MassMatrix3<T> > Sphere<T>::MassMatrix() const
+{
+  gz::math::MassMatrix3<T> _massMat;
+
+  if(!_massMat.SetFromSphere(this->material, this->radius))
+  {
+    return std::nullopt;
+  }
+  else
+  {
+    return std::make_optional(_massMat);
+  }
+}
+
+
 //////////////////////////////////////////////////
 template<typename T>
 T Sphere<T>::Volume() const

src/Box_TEST.cc

@@ -498,4 +498,10 @@ TEST(BoxTest, Mass)
   box.MassMatrix(massMat);
   EXPECT_EQ(expectedMassMat, massMat);
   EXPECT_DOUBLE_EQ(expectedMassMat.Mass(), massMat.Mass());
+
+  auto massMatOpt = box.MassMatrix();
+  ASSERT_NE(std::nullopt, massMatOpt);
+  EXPECT_EQ(expectedMassMat, *massMatOpt);
+  EXPECT_EQ(expectedMassMat.DiagonalMoments(), massMatOpt->DiagonalMoments());
+  EXPECT_DOUBLE_EQ(expectedMassMat.Mass(), massMatOpt->Mass());
 }

src/Cylinder_TEST.cc

@@ -136,4 +136,10 @@ TEST(CylinderTest, Mass)
   cylinder.MassMatrix(massMat);
   EXPECT_EQ(expectedMassMat, massMat);
   EXPECT_DOUBLE_EQ(expectedMassMat.Mass(), massMat.Mass());
+
+  auto massMatOpt = cylinder.MassMatrix();
+  ASSERT_NE(std::nullopt, massMatOpt);
+  EXPECT_EQ(expectedMassMat, *massMatOpt);
+  EXPECT_EQ(expectedMassMat.DiagonalMoments(), massMatOpt->DiagonalMoments());
+  EXPECT_DOUBLE_EQ(expectedMassMat.Mass(), massMatOpt->Mass());
 }

src/Sphere_TEST.cc

@@ -129,6 +129,12 @@ TEST(SphereTest, Mass)
   sphere.MassMatrix(massMat);
   EXPECT_EQ(expectedMassMat, massMat);
   EXPECT_DOUBLE_EQ(expectedMassMat.Mass(), massMat.Mass());
+
+  auto massMatOpt = sphere.MassMatrix();
+  ASSERT_NE(std::nullopt, massMatOpt);
+  EXPECT_EQ(expectedMassMat, *massMatOpt);
+  EXPECT_EQ(expectedMassMat.DiagonalMoments(), massMatOpt->DiagonalMoments());
+  EXPECT_DOUBLE_EQ(expectedMassMat.Mass(), massMatOpt->Mass());
 }
 
 //////////////////////////////////////////////////

src/python_pybind11/src/Box.hh

@@ -105,11 +105,17 @@ void defineMathBox(py::module &m, const std::string &typestr)
     .def("set_density_from_mass",
          &Class::SetDensityFromMass,
          "Set the density of this box based on a mass value.")
-    .def("mass_matrix",
-         &Class::MassMatrix,
-         "Get the mass matrix for this box. This function "
-         "is only meaningful if the box's size and material "
-         "have been set.")
+     .def("mass_matrix",
+          py::overload_cast<>(&Class::MassMatrix, py::const_),
+          "Get the mass matrix for this box. This function "
+          "is only meaningful if the box's size and material "
+          "have been set.")
+     .def("mass_matrix",
+          py::overload_cast<gz::math::MassMatrix3<T>&>
+          (&Class::MassMatrix, py::const_),
+          "Get the mass matrix for this box. This function "
+          "is only meaningful if the box's size and material "
+          "have been set.")
     .def("intersections",
          [](const Class &self, const Plane<T> &_plane)
          {

src/python_pybind11/src/Cylinder.hh

@@ -97,11 +97,17 @@ void defineMathCylinder(py::module &m, const std::string &typestr)
     .def("set_density_from_mass",
          &Class::SetDensityFromMass,
          "Set the density of this box based on a mass value.")
-    .def("mass_matrix",
-         &Class::MassMatrix,
-         "Get the mass matrix for this box. This function "
-         "is only meaningful if the box's size and material "
-         "have been set.")
+     .def("mass_matrix",
+          py::overload_cast<>(&Class::MassMatrix, py::const_),
+          "Get the mass matrix for this box. This function "
+          "is only meaningful if the box's size and material "
+          "have been set.")
+     .def("mass_matrix",
+          py::overload_cast<gz::math::MassMatrix3<T>&>
+          (&Class::MassMatrix, py::const_),
+          "Get the mass matrix for this box. This function "
+          "is only meaningful if the box's size and material "
+          "have been set.")
     .def("__copy__", [](const Class &self) {
       return Class(self);
     })

src/python_pybind11/src/Sphere.hh

@@ -83,11 +83,17 @@ void defineMathSphere(py::module &m, const std::string &typestr)
     .def("set_density_from_mass",
          &Class::SetDensityFromMass,
          "Set the density of this box based on a mass value.")
-    .def("mass_matrix",
-         &Class::MassMatrix,
-         "Get the mass matrix for this box. This function "
-         "is only meaningful if the box's size and material "
-         "have been set.")
+     .def("mass_matrix",
+          py::overload_cast<>(&Class::MassMatrix, py::const_),
+          "Get the mass matrix for this box. This function "
+          "is only meaningful if the box's size and material "
+          "have been set.")
+     .def("mass_matrix",
+          py::overload_cast<gz::math::MassMatrix3<T>&>
+          (&Class::MassMatrix, py::const_),
+          "Get the mass matrix for this box. This function "
+          "is only meaningful if the box's size and material "
+          "have been set.")
     .def("__copy__", [](const Class &self) {
       return Class(self);
     })

src/python_pybind11/test/Box_TEST.py

@@ -401,6 +401,11 @@ def test_mass(self):
         self.assertEqual(expectedMassMat, massMat)
         self.assertEqual(expectedMassMat.mass(), massMat.mass())
 
+        massMat2 = box.mass_matrix()
+        self.assertEqual(expectedMassMat, massMat2)
+        self.assertEqual(expectedMassMat.diagonal_moments(), massMat2.diagonal_moments())
+        self.assertEqual(expectedMassMat.mass(), massMat2.mass())
+
 
 if __name__ == '__main__':
     unittest.main()

src/python_pybind11/test/Cylinder_TEST.py

@@ -114,6 +114,11 @@ def test_mass(self):
         self.assertEqual(expectedMassMat, massMat)
         self.assertEqual(expectedMassMat.mass(), massMat.mass())
 
+        massMat2 = cylinder.mass_matrix()
+        self.assertEqual(expectedMassMat, massMat2)
+        self.assertEqual(expectedMassMat.diagonal_moments(), massMat2.diagonal_moments())
+        self.assertEqual(expectedMassMat.mass(), massMat2.mass())
+
 
 if __name__ == '__main__':
     unittest.main()

src/python_pybind11/test/Sphere_TEST.py

@@ -108,6 +108,11 @@ def test_mass(self):
         self.assertEqual(expectedMassMat, massMat)
         self.assertEqual(expectedMassMat.mass(), massMat.mass())
 
+        massMat2 = sphere.mass_matrix()
+        self.assertEqual(expectedMassMat, massMat2)
+        self.assertEqual(expectedMassMat.diagonal_moments(), massMat2.diagonal_moments())
+        self.assertEqual(expectedMassMat.mass(), massMat2.mass())
+
     def test_volume_below(self):
 
         r = 2