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

…re (#538) * Added std::optional MassMatrix function to box, sphere & cylinder Signed-off-by: Jasmeet Singh <[email protected]> * Used py::overload_cast with py::const_ for MassMatrix functions in py bindings Signed-off-by: Jasmeet Singh <[email protected]> * Included <optional> in Cylinder, Sphere & Box headers Signed-off-by: Jasmeet Singh <[email protected]> * Added unit tests for std::optional MassMatrix() for box, cylinder & sphere Signed-off-by: Jasmeet Singh <[email protected]> * Added python tests for new MassMatrix() function for Box, sphere & Cylinder Signed-off-by: Jasmeet Singh <[email protected]> * Included <optional> in detail Cylinder, Sphere & Box headers Signed-off-by: Jasmeet Singh <[email protected]> --------- Signed-off-by: Jasmeet Singh <[email protected]>
gazebosim · Jun 30, 2023 · cf5d7f3 · cf5d7f3
1 parent 2267187
commit cf5d7f3
Show file tree

Hide file tree

Showing 15 changed files with 147 additions and 15 deletions.
diff --git a/include/gz/math/Box.hh b/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.

diff --git a/include/gz/math/Cylinder.hh b/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;

diff --git a/include/gz/math/Sphere.hh b/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;

diff --git a/include/gz/math/detail/Box.hh b/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>

diff --git a/include/gz/math/detail/Cylinder.hh b/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

diff --git a/include/gz/math/detail/Sphere.hh b/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

diff --git a/src/Box_TEST.cc b/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());
 }
diff --git a/src/Cylinder_TEST.cc b/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());
 }
diff --git a/src/Sphere_TEST.cc b/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());
 }
 
 //////////////////////////////////////////////////

diff --git a/src/python_pybind11/src/Box.hh b/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)
          {

diff --git a/src/python_pybind11/src/Cylinder.hh b/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);
     })

diff --git a/src/python_pybind11/src/Sphere.hh b/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);
     })

diff --git a/src/python_pybind11/test/Box_TEST.py b/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()
diff --git a/src/python_pybind11/test/Cylinder_TEST.py b/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()
diff --git a/src/python_pybind11/test/Sphere_TEST.py b/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