Add AxisAlignedBox getters for all relevant geometries

include/sdf/Box.hh

@@ -22,6 +22,7 @@
 #include <gz/math/Box.hh>
 #include <gz/math/Vector3.hh>
 #include <gz/math/Inertial.hh>
+#include <gz/math/AxisAlignedBox.hh>
 #include <gz/utils/ImplPtr.hh>
 #include <sdf/Error.hh>
 #include <sdf/Element.hh>
@@ -76,6 +77,11 @@ namespace sdf
     public: std::optional<gz::math::Inertiald>
             CalculateInertial(double _density);
 
+    /// \brief Get the Axis-aligned box for this Box.
+    /// \return A gz::math::AxisAlignedBox object centered at this
+    /// Box's center.
+    public: gz::math::AxisAlignedBox AxisAlignedBox() const;
+
     /// \brief Create and return an SDF element filled with data from this
     /// box.
     /// Note that parameter passing functionality is not captured with this

include/sdf/Capsule.hh

@@ -21,6 +21,7 @@
 
 #include <gz/math/Capsule.hh>
 #include <gz/math/Inertial.hh>
+#include <gz/math/AxisAlignedBox.hh>
 #include <gz/utils/ImplPtr.hh>
 #include <sdf/Error.hh>
 #include <sdf/Element.hh>
@@ -83,6 +84,11 @@ namespace sdf
     public: std::optional<gz::math::Inertiald> CalculateInertial(
       double _density);
 
+    /// \brief Get the Axis-aligned box for this Capsule.
+    /// \return A gz::math::AxisAlignedBox object centered at this
+    /// Capsules's center.
+    public: gz::math::AxisAlignedBox AxisAlignedBox() const;
+
     /// \brief Create and return an SDF element filled with data from this
     /// capsule.
     /// Note that parameter passing functionality is not captured with this

include/sdf/Cone.hh

@@ -23,6 +23,7 @@
 
 #include <gz/math/Cone.hh>
 #include <gz/math/Inertial.hh>
+#include <gz/math/AxisAlignedBox.hh>
 #include <gz/utils/ImplPtr.hh>
 #include <sdf/Error.hh>
 #include <sdf/Element.hh>
@@ -85,6 +86,11 @@ namespace sdf
     public: std::optional<gz::math::Inertiald>
             CalculateInertial(double _density);
 
+    /// \brief Get the Axis-aligned box for this Cone.
+    /// \return A gz::math::AxisAlignedBox object centered at this
+    /// Cones's center.
+    public: gz::math::AxisAlignedBox AxisAlignedBox() const;
+
     /// \brief Create and return an SDF element filled with data from this
     /// cone.
     /// Note that parameter passing functionality is not captured with this

include/sdf/Cylinder.hh

@@ -21,6 +21,7 @@
 
 #include <gz/math/Cylinder.hh>
 #include <gz/math/Inertial.hh>
+#include <gz/math/AxisAlignedBox.hh>
 #include <gz/utils/ImplPtr.hh>
 #include <sdf/Error.hh>
 #include <sdf/Element.hh>
@@ -83,6 +84,11 @@ namespace sdf
     public: std::optional<gz::math::Inertiald>
             CalculateInertial(double _density);
 
+    /// \brief Get the Axis-aligned box for this Cylinder.
+    /// \return A gz::math::AxisAlignedBox object centered at this
+    /// Cylinder's center.
+    public: gz::math::AxisAlignedBox AxisAlignedBox() const;
+
     /// \brief Create and return an SDF element filled with data from this
     /// cylinder.
     /// Note that parameter passing functionality is not captured with this

include/sdf/Ellipsoid.hh

@@ -21,6 +21,7 @@
 
 #include <gz/math/Inertial.hh>
 #include <gz/math/Ellipsoid.hh>
+#include <gz/math/AxisAlignedBox.hh>
 #include <gz/utils/ImplPtr.hh>
 #include <sdf/Error.hh>
 #include <sdf/Element.hh>
@@ -75,6 +76,11 @@ namespace sdf
     public: std::optional<gz::math::Inertiald>
             CalculateInertial(double _density);
 
+    /// \brief Get the Axis-aligned box for this Ellipsoid.
+    /// \return A gz::math::AxisAlignedBox object centered at this
+    /// Ellipsoids's center.
+    public: gz::math::AxisAlignedBox AxisAlignedBox() const;
+
     /// \brief Create and return an SDF element filled with data from this
     /// ellipsoid.
     /// Note that parameter passing functionality is not captured with this

include/sdf/Geometry.hh

@@ -22,6 +22,7 @@
 
 #include <gz/utils/ImplPtr.hh>
 #include <gz/math/Inertial.hh>
+#include <gz/math/AxisAlignedBox.hh>
 #include <sdf/Error.hh>
 #include <sdf/Element.hh>
 #include <sdf/config.hh>
@@ -239,6 +240,13 @@ namespace sdf
       sdf::Errors &_errors, const ParserConfig &_config,
       double _density, sdf::ElementPtr _autoInertiaParams);
 
+    /// \brief Calculate and return the AxisAlignedBox for the Geometry
+    /// \param _meshAabbCalculator The function to calculate the AABB of a mesh
+    /// \return std::optional with gz::math::AxisAlignedBox object or
+    /// std::nullopt if the geometry type does not support AABB calculation
+    public: std::optional<gz::math::AxisAlignedBox> AxisAlignedBox(
+      const Mesh::AxisAlignedBoxCalculator &_meshAabbCalculator) const;
+
     /// \brief Get a pointer to the SDF element that was used during
     /// load.
     /// \return SDF element pointer. The value will be nullptr if Load has

include/sdf/Mesh.hh

@@ -22,6 +22,7 @@
 
 #include <gz/math/Vector3.hh>
 #include <gz/math/Inertial.hh>
+#include <gz/math/AxisAlignedBox.hh>
 #include <gz/utils/ImplPtr.hh>
 #include <sdf/CustomInertiaCalcProperties.hh>
 #include <sdf/Element.hh>
@@ -94,6 +95,10 @@ namespace sdf
   /// Geometry.
   class SDFORMAT_VISIBLE Mesh
   {
+    public: using AxisAlignedBoxCalculator =
+      std::function<std::optional<gz::math::AxisAlignedBox>(
+        const sdf::Mesh &_sdfMesh)>;
+
     /// \brief Constructor
     public: Mesh();
 
@@ -206,6 +211,14 @@ namespace sdf
                               const sdf::ElementPtr _autoInertiaParams,
                               const ParserConfig &_config);
 
+    /// \brief Get the Axis-aligned box for this Mesh.
+    /// \param[in] _aabbCalc A custom function that calculates the
+    /// AxisAlignedBox for the Mesh.
+    /// \return A gz::math::AxisAlignedBox object centered at this
+    /// Mesh's origin or std::nullopt.
+    public: std::optional<gz::math::AxisAlignedBox>
+            AxisAlignedBox(const AxisAlignedBoxCalculator &_aabbCalc) const;
+
     /// \brief Get a pointer to the SDF element that was used during load.
     /// \return SDF element pointer. The value will be nullptr if Load has
     /// not been called.

include/sdf/Sphere.hh

@@ -21,6 +21,7 @@
 
 #include <gz/math/Inertial.hh>
 #include <gz/math/Sphere.hh>
+#include <gz/math/AxisAlignedBox.hh>
 #include <gz/utils/ImplPtr.hh>
 
 #include <sdf/Error.hh>
@@ -76,6 +77,11 @@ namespace sdf
     public: std::optional<gz::math::Inertiald>
             CalculateInertial(double _density);
 
+    /// \brief Get the Axis-aligned box for this Sphere.
+    /// \return A gz::math::AxisAlignedBox object centered at this
+    /// Sphere's center.
+    public: gz::math::AxisAlignedBox AxisAlignedBox() const;
+
     /// \brief Create and return an SDF element filled with data from this
     /// sphere.
     /// Note that parameter passing functionality is not captured with this

python/src/sdf/pyBox.cc

@@ -45,6 +45,8 @@ void defineBox(pybind11::object module)
         "Get a mutable Gazebo Math representation of this Box.")
     .def("calculate_inertial", &sdf::Box::CalculateInertial,
          "Calculate and return the Inertial values for the Box.")
+    .def("axis_aligned_box", &sdf::Box::AxisAlignedBox,
+         "Get the axis-aligned box that contains this box.")
     .def("__copy__", [](const sdf::Box &self) {
       return sdf::Box(self);
     })

python/src/sdf/pyCapsule.cc

@@ -44,6 +44,8 @@ void defineCapsule(pybind11::object module)
          "Set the capsule's length in meters.")
     .def("calculate_inertial", &sdf::Capsule::CalculateInertial,
          "Calculate and return the Inertial values for the Capsule.")
+    .def("axis_aligned_box", &sdf::Capsule::AxisAlignedBox,
+         "Get the axis-aligned box that contains this Capsule.")
     .def(
         "shape",
         pybind11::overload_cast<>(&sdf::Capsule::Shape, pybind11::const_),

python/src/sdf/pyCone.cc

@@ -48,6 +48,8 @@ void defineCone(pybind11::object module)
         pybind11::overload_cast<>(&sdf::Cone::Shape, pybind11::const_),
         pybind11::return_value_policy::reference,
         "Get a mutable Gazebo Math representation of this Cone.")
+    .def("axis_aligned_box", &sdf::Cone::AxisAlignedBox,
+         "Get the axis-aligned box that contains this Cone.")
     .def("__copy__", [](const sdf::Cone &self) {
       return sdf::Cone(self);
     })

python/src/sdf/pyCylinder.cc

@@ -49,6 +49,8 @@ void defineCylinder(pybind11::object module)
         pybind11::overload_cast<>(&sdf::Cylinder::Shape, pybind11::const_),
         pybind11::return_value_policy::reference,
         "Get a mutable Gazebo Math representation of this Cylinder.")
+    .def("axis_aligned_box", &sdf::Cylinder::AxisAlignedBox,
+         "Get the axis-aligned box that contains this Cylinder.")
     .def("__copy__", [](const sdf::Cylinder &self) {
       return sdf::Cylinder(self);
     })

python/src/sdf/pyEllipsoid.cc

@@ -45,6 +45,8 @@ void defineEllipsoid(pybind11::object module)
         pybind11::overload_cast<>(&sdf::Ellipsoid::Shape, pybind11::const_),
         pybind11::return_value_policy::reference,
         "Get a mutable Gazebo Math representation of this Ellipsoid.")
+    .def("axis_aligned_box", &sdf::Ellipsoid::AxisAlignedBox,
+         "Get the axis-aligned box that contains this Ellipsoid.")
     .def("__copy__", [](const sdf::Ellipsoid &self) {
       return sdf::Ellipsoid(self);
     })

python/src/sdf/pyGeometry.cc

@@ -18,6 +18,7 @@
 
 #include <pybind11/pybind11.h>
 #include <pybind11/stl.h>
+#include <pybind11/functional.h>
 
 #include "sdf/ParserConfig.hh"
 
@@ -107,6 +108,8 @@ void defineGeometry(pybind11::object module)
     .def("heightmap_shape", &sdf::Geometry::HeightmapShape,
           pybind11::return_value_policy::reference,
           "Get the heightmap geometry.")
+    .def("axis_aligned_box", &sdf::Geometry::AxisAlignedBox,
+         "Get the axis-aligned box that contains the Geometry.")
     .def("__copy__", [](const sdf::Geometry &self) {
       return sdf::Geometry(self);
     })

python/src/sdf/pyMesh.cc

@@ -17,6 +17,8 @@
 #include "pyMesh.hh"
 
 #include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
+#include <pybind11/functional.h>
 
 #include "sdf/ParserConfig.hh"
 #include "sdf/Mesh.hh"
@@ -78,6 +80,8 @@ void defineMesh(pybind11::object module)
     .def("set_center_submesh", &sdf::Mesh::SetCenterSubmesh,
          "Set whether the submesh should be centered. See CenterSubmesh() "
          "for more information.")
+    .def("axis_aligned_box", &sdf::Mesh::AxisAlignedBox,
+         "Get the axis-aligned box that contains this Mesh.")
     .def("__copy__", [](const sdf::Mesh &self) {
       return sdf::Mesh(self);
     })

python/src/sdf/pySphere.cc

@@ -45,6 +45,8 @@ void defineSphere(pybind11::object module)
         pybind11::overload_cast<>(&sdf::Sphere::Shape, pybind11::const_),
         pybind11::return_value_policy::reference,
         "Get a mutable Gazebo Math representation of this Sphere.")
+    .def("axis_aligned_box", &sdf::Sphere::AxisAlignedBox,
+         "Get the axis-aligned box that contains this Sphere.")
     .def("__copy__", [](const sdf::Sphere &self) {
       return sdf::Sphere(self);
     })

python/test/pyBox_TEST.py

@@ -12,7 +12,7 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-from gz_test_deps.math import Inertiald, MassMatrix3d, Pose3d, Vector3d
+from gz_test_deps.math import AxisAlignedBox, Inertiald, MassMatrix3d, Pose3d, Vector3d
 from gz_test_deps.sdformat import Box
 import unittest
 
@@ -89,5 +89,14 @@ def test_calculate_inertial(self):
       boxInertial.mass_matrix().mass())
     self.assertEqual(expectedInertial.pose(), boxInertial.pose())
 
+  def test_axis_aligned_box(self):
+    box = Box()
+    box.set_size(Vector3d(1, 2, 3))
+
+    self.assertEqual(
+      AxisAlignedBox(Vector3d(-0.5, -1, -1.5), Vector3d(0.5, 1, 1.5)),
+      box.axis_aligned_box())
+
+
 if __name__ == '__main__':
     unittest.main()

python/test/pyCapsule_TEST.py

@@ -16,7 +16,7 @@
 
 import math
 
-from gz_test_deps.math import Inertiald, MassMatrix3d, Pose3d, Vector3d
+from gz_test_deps.math import AxisAlignedBox, Inertiald, MassMatrix3d, Pose3d, Vector3d
 from gz_test_deps.sdformat import Capsule
 
 import unittest
@@ -107,6 +107,7 @@ def test_shape(self):
     self.assertEqual(0.123, capsule.radius())
     self.assertEqual(0.456, capsule.length())
 
+
   def test_calculate_inertial(self):
     capsule = Capsule()
 
@@ -145,5 +146,15 @@ def test_calculate_inertial(self):
       capsuleInertial.mass_matrix().mass())
     self.assertEqual(expectedInertial.pose(), capsuleInertial.pose())
 
+  def test_axis_aligned_box(self):
+    capsule = Capsule()
+    capsule.set_radius(0.5)
+    capsule.set_length(3.0)
+
+    self.assertEqual(
+      AxisAlignedBox(Vector3d(-0.5, -0.5, -2.0), Vector3d(0.5, 0.5, 2.0)),
+      capsule.axis_aligned_box())
+
+
 if __name__ == '__main__':
     unittest.main()

python/test/pyCone_TEST.py

@@ -18,6 +18,7 @@
 
 import math
 
+from gz_test_deps.math import AxisAlignedBox, Vector3d
 from gz_test_deps.sdformat import Cone
 
 import unittest
@@ -82,6 +83,7 @@ def test_copy_construction(self):
     self.assertEqual(0.2, cone2.radius())
     self.assertEqual(3.0, cone2.length())
 
+
   def test_deepcopy(self):
     cone = Cone();
     cone.set_radius(0.2)
@@ -109,6 +111,15 @@ def test_shape(self):
     self.assertEqual(0.123, cone.radius())
     self.assertEqual(0.456, cone.length())
 
+  def test_axis_aligned_box(self):
+    cone = Cone()
+    cone.set_radius(1.0)
+    cone.set_length(2.0)
+
+    self.assertEqual(
+      AxisAlignedBox(Vector3d(-1, -1, -1), Vector3d(1, 1, 1)),
+      cone.axis_aligned_box())
+
 
 if __name__ == '__main__':
     unittest.main()

python/test/pyCylinder_TEST.py

@@ -16,7 +16,7 @@
 
 import math
 
-from gz_test_deps.math import Inertiald, MassMatrix3d, Pose3d, Vector3d
+from gz_test_deps.math import AxisAlignedBox, Inertiald, MassMatrix3d, Pose3d, Vector3d
 from gz_test_deps.sdformat import Cylinder
 
 import unittest
@@ -147,5 +147,15 @@ def test_calculate_interial(self):
       cylinderInertial.mass_matrix().mass())
     self.assertEqual(expectedInertial.pose(), cylinderInertial.pose())
 
+  def test_axis_aligned_box(self):
+    cylinder = Cylinder()
+    cylinder.set_radius(0.8)
+    cylinder.set_length(1.8)
+
+    self.assertEqual(
+      AxisAlignedBox(Vector3d(-0.8, -0.8, -0.9), Vector3d(0.8, 0.8, 0.9)),
+      cylinder.axis_aligned_box())
+
+
 if __name__ == '__main__':
     unittest.main()