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

* Added std::optional MassMatrix function to box, sphere & cylinder

Signed-off-by: Jasmeet Singh <jasmeet0915@gmail.com>

* Used py::overload_cast with py::const_ for MassMatrix functions in py bindings

Signed-off-by: Jasmeet Singh <jasmeet0915@gmail.com>

* Included <optional> in Cylinder, Sphere & Box headers

Signed-off-by: Jasmeet Singh <jasmeet0915@gmail.com>

* Added unit tests for std::optional MassMatrix() for box, cylinder & sphere

Signed-off-by: Jasmeet Singh <jasmeet0915@gmail.com>

* Added python tests for new MassMatrix() function for Box, sphere & Cylinder

Signed-off-by: Jasmeet Singh <jasmeet0915@gmail.com>

* Included <optional> in detail Cylinder, Sphere & Box headers

Signed-off-by: Jasmeet Singh <jasmeet0915@gmail.com>

---------

Signed-off-by: Jasmeet Singh <jasmeet0915@gmail.com>

Author: jasmeet0915

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

@@ -19,6 +19,7 @@
 
 #include "gz/math/Triangle3.hh"
 
+#include <optional>
 #include <algorithm>
 #include <set>
 #include <utility>
@@ -310,6 +311,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

@@ -16,6 +16,9 @@
 */
 #ifndef GZ_MATH_DETAIL_CYLINDER_HH_
 #define GZ_MATH_DETAIL_CYLINDER_HH_
+
+#include <optional>
+
 namespace gz
 {
 namespace math
@@ -116,6 +119,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

@@ -17,6 +17,7 @@
 #ifndef GZ_MATH_DETAIL_SPHERE_HH_
 #define GZ_MATH_DETAIL_SPHERE_HH_
 
+#include <optional>
 #include "gz/math/Sphere.hh"
 
 namespace gz
@@ -88,6 +89,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)
          {