diff --git a/include/gz/math/Box.hh b/include/gz/math/Box.hh index 9412ca800..33551c17d 100644 --- 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 #include #include #include @@ -189,6 +190,14 @@ namespace gz /// could be due to an invalid size (<=0) or density (<=0). public: bool MassMatrix(MassMatrix3 &_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 > 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 index fe3cae477..1c3a124db 100644 --- 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 #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 > 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 index 4ab73d681..4c0a7b430 100644 --- 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 #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 > 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 index 830878462..9b5950984 100644 --- a/include/gz/math/detail/Box.hh +++ b/include/gz/math/detail/Box.hh @@ -19,6 +19,7 @@ #include "gz/math/Triangle3.hh" +#include #include #include #include @@ -310,6 +311,21 @@ bool Box::MassMatrix(MassMatrix3 &_massMat) const return _massMat.SetFromBox(this->material, this->size); } +///////////////////////////////////////////////// +template +std::optional< MassMatrix3 > Box::MassMatrix() const +{ + gz::math::MassMatrix3 _massMat; + + if(!_massMat.SetFromBox(this->material, this->size)) + { + return std::nullopt; + } + else + { + return std::make_optional(_massMat); + } +} ////////////////////////////////////////////////// template diff --git a/include/gz/math/detail/Cylinder.hh b/include/gz/math/detail/Cylinder.hh index f4db1b335..aaa6d69c1 100644 --- 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 + namespace gz { namespace math @@ -116,6 +119,24 @@ bool Cylinder::MassMatrix(MassMatrix3d &_massMat) const this->radius, this->rotOffset); } +////////////////////////////////////////////////// +template +std::optional < MassMatrix3 > Cylinder::MassMatrix() const +{ + gz::math::MassMatrix3 _massMat; + + if(!_massMat.SetFromCylinderZ( + this->material, this->length, + this->radius, this->rotOffset)) + { + return std::nullopt; + } + else + { + return std::make_optional(_massMat); + } +} + ////////////////////////////////////////////////// template T Cylinder::Volume() const diff --git a/include/gz/math/detail/Sphere.hh b/include/gz/math/detail/Sphere.hh index 597c76639..865c96598 100644 --- 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 #include "gz/math/Sphere.hh" namespace gz @@ -88,6 +89,23 @@ bool Sphere::MassMatrix(MassMatrix3d &_massMat) const return _massMat.SetFromSphere(this->material, this->radius); } +////////////////////////////////////////////////// +template +std::optional < MassMatrix3 > Sphere::MassMatrix() const +{ + gz::math::MassMatrix3 _massMat; + + if(!_massMat.SetFromSphere(this->material, this->radius)) + { + return std::nullopt; + } + else + { + return std::make_optional(_massMat); + } +} + + ////////////////////////////////////////////////// template T Sphere::Volume() const diff --git a/src/Box_TEST.cc b/src/Box_TEST.cc index f68f9e797..f8cd158c6 100644 --- 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 index 5730a7fe7..3811fd0ff 100644 --- 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 index 5996fbe1c..80d612f16 100644 --- 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 index c78cab4fd..109b19a23 100644 --- 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&> + (&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 &_plane) { diff --git a/src/python_pybind11/src/Cylinder.hh b/src/python_pybind11/src/Cylinder.hh index 8b788e430..496920440 100644 --- 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&> + (&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 index 2831ce350..f11b1a952 100644 --- 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&> + (&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 index 058507fb4..9e1a2f81f 100644 --- 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 index de4f7780d..8d64635ef 100644 --- 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 index afd75ab46..6e213bc98 100644 --- 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