Add AxisAlignedBox getters for all relevant geometries

* Mesh is a special case since it forwards the calculations for
the caller. If callback is not set, the return is nullptr
* This has been done since this library does not depend on gz-common
* Geometry types that do not have a volume, return nullptr.

Author: gabrielfpacheco

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,10 @@ 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.
+    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,10 @@ 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.
+    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,10 @@ 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.
+    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,10 @@ 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.
+    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,10 @@ 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.
+    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(
+      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,9 @@ namespace sdf
   /// Geometry.
   class SDFORMAT_VISIBLE Mesh
   {
+    public: using AxisAlignedBoxCalculator =
+      std::function<gz::math::AxisAlignedBox(const sdf::Mesh &_sdfMesh)>;
+
     /// \brief Constructor
     public: Mesh();
 
@@ -206,6 +210,14 @@ namespace sdf
                               const sdf::ElementPtr _autoInertiaParams,
                               const ParserConfig &_config);
 
+    /// \brief Get the Axis-aligned box for this Mesh.
+    /// \param[out] _errors A vector of Errors object. Each object
+    /// would contain an error code and an error message.
+    /// \param[in] _config Parser Configuration object.
+    /// \return A gz::math::AxisAlignedBox object.
+    public: std::optional<gz::math::AxisAlignedBox>
+            AxisAlignedBox(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
     /// 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,10 @@ 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.
+    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

src/Box.cc

@@ -140,6 +140,13 @@ std::optional<gz::math::Inertiald> Box::CalculateInertial(double _density)
   }
 }
 
+/////////////////////////////////////////////////
+gz::math::AxisAlignedBox Box::AxisAlignedBox() const
+{
+  auto halfSize = this->Size() / 2;
+  return gz::math::AxisAlignedBox(-halfSize, halfSize);
+}
+
 /////////////////////////////////////////////////
 sdf::ElementPtr Box::ToElement() const
 {

src/Box_TEST.cc

@@ -246,3 +246,15 @@ TEST(DOMBox, ToElementErrorOutput)
   // Check nothing has been printed
   EXPECT_TRUE(buffer.str().empty()) << buffer.str();
 }
+
+/////////////////////////////////////////////////
+TEST(DOMBox, AxisAlignedBox)
+{
+  sdf::Box box;
+  box.SetSize(gz::math::Vector3d(1, 2, 3));
+
+  auto aabb = box.AxisAlignedBox();
+  EXPECT_EQ(box.Size(), aabb.Size());
+  EXPECT_EQ(gz::math::Vector3d(-0.5, -1, -1.5), aabb.Min());
+  EXPECT_EQ(gz::math::Vector3d(0.5, 1, 1.5), aabb.Max());
+}