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());
+}

src/Capsule.cc

@@ -161,6 +161,14 @@ std::optional<gz::math::Inertiald> Capsule::CalculateInertial(double _density)
   }
 }
 
+/////////////////////////////////////////////////
+gz::math::AxisAlignedBox Capsule::AxisAlignedBox() const
+{
+  auto halfSize = gz::math::Vector3d(
+    this->Radius(), this->Radius(), this->Length() / 2);
+  return gz::math::AxisAlignedBox(-halfSize, halfSize);
+}
+
 /////////////////////////////////////////////////
 sdf::ElementPtr Capsule::ToElement() const
 {

src/Capsule_TEST.cc

@@ -285,3 +285,16 @@ TEST(DOMCapsule, ToElementErrorOutput)
   // Check nothing has been printed
   EXPECT_TRUE(buffer.str().empty()) << buffer.str();
 }
+
+/////////////////////////////////////////////////
+TEST(DOMCapsule, AxisAlignedBox)
+{
+  sdf::Capsule capsule;
+  capsule.SetRadius(0.5);
+  capsule.SetLength(2.0);
+
+  auto aabb = capsule.AxisAlignedBox();
+  EXPECT_EQ(gz::math::Vector3d(1.0, 1.0, 2.0), aabb.Size());
+  EXPECT_EQ(gz::math::Vector3d(-0.5, -0.5, -1.0), aabb.Min());
+  EXPECT_EQ(gz::math::Vector3d(0.5, 0.5, 1.0), aabb.Max());
+}

src/Cone.cc

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

src/Cone_TEST.cc

@@ -286,3 +286,16 @@ TEST(DOMCone, ToElementErrorOutput)
   // Check nothing has been printed
   EXPECT_TRUE(buffer.str().empty()) << buffer.str();
 }
+
+/////////////////////////////////////////////////
+TEST(DOMCone, AxisAlignedBox)
+{
+  sdf::Cone cone;
+  cone.SetRadius(0.2);
+  cone.SetLength(3.0);
+
+  auto aabb = cone.AxisAlignedBox();
+  EXPECT_EQ(gz::math::Vector3d(0.4, 0.4, 3.0), aabb.Size());
+  EXPECT_EQ(gz::math::Vector3d(-0.2, -0.2, -1.5), aabb.Min());
+  EXPECT_EQ(gz::math::Vector3d(0.2, 0.2, 1.5), aabb.Max());
+}

src/Cylinder.cc

@@ -159,6 +159,14 @@ std::optional<gz::math::Inertiald> Cylinder::CalculateInertial(double _density)
   }
 }
 
+/////////////////////////////////////////////////
+gz::math::AxisAlignedBox Cylinder::AxisAlignedBox() const
+{
+  auto halfSize = gz::math::Vector3d(
+    this->Radius(), this->Radius(), this->Length() / 2);
+  return gz::math::AxisAlignedBox(-halfSize, halfSize);
+}
+
 /////////////////////////////////////////////////
 sdf::ElementPtr Cylinder::ToElement() const
 {

src/Cylinder_TEST.cc

@@ -282,3 +282,16 @@ TEST(DOMCylinder, ToElementErrorOutput)
   // Check nothing has been printed
   EXPECT_TRUE(buffer.str().empty()) << buffer.str();
 }
+
+/////////////////////////////////////////////////
+TEST(DOMCylinder, AxisAlignedBox)
+{
+  sdf::Cylinder cylinder;
+  cylinder.SetRadius(0.25);
+  cylinder.SetLength(2.5);
+
+  auto aabb = cylinder.AxisAlignedBox();
+  EXPECT_EQ(gz::math::Vector3d(0.5, 0.5, 2.5), aabb.Size());
+  EXPECT_EQ(gz::math::Vector3d(-0.25, -0.25, -1.25), aabb.Min());
+  EXPECT_EQ(gz::math::Vector3d(0.25, 0.25, 1.25), aabb.Max());
+}

src/Ellipsoid.cc

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

src/Ellipsoid_TEST.cc

@@ -242,3 +242,15 @@ TEST(DOMEllipsoid, ToElementErrorOutput)
   // Check nothing has been printed
   EXPECT_TRUE(buffer.str().empty()) << buffer.str();
 }
+
+/////////////////////////////////////////////////
+TEST(DOMEllipsoid, AxisAlignedBox)
+{
+  sdf::Ellipsoid ellipsoid;
+  ellipsoid.SetRadii(gz::math::Vector3d(1.5, 0.5, 0.25));
+
+  auto aabb = ellipsoid.AxisAlignedBox();
+  EXPECT_EQ(gz::math::Vector3d(3.0, 1.0, 0.5), aabb.Size());
+  EXPECT_EQ(gz::math::Vector3d(-1.5, -0.5, -0.25), aabb.Min());
+  EXPECT_EQ(gz::math::Vector3d(1.5, 0.5, 0.25), aabb.Max());
+}

src/Geometry.cc

@@ -385,6 +385,45 @@ std::optional<gz::math::Inertiald> Geometry::CalculateInertial(
   return geomInertial;
 }
 
+/////////////////////////////////////////////////
+std::optional<gz::math::AxisAlignedBox> Geometry::AxisAlignedBox(
+  Mesh::AxisAlignedBoxCalculator _meshAabbCalculator) const
+{
+  // Initialized the AABB to an empty box (zero volume)
+  std::optional<gz::math::AxisAlignedBox> aabb;
+
+  switch (this->dataPtr->type)
+  {
+    case GeometryType::BOX:
+      aabb = this->dataPtr->box->AxisAlignedBox();
+      break;
+    case GeometryType::CAPSULE:
+      aabb = this->dataPtr->capsule->AxisAlignedBox();
+      break;
+    case GeometryType::CONE:
+      aabb = this->dataPtr->cone->AxisAlignedBox();
+      break;
+    case GeometryType::CYLINDER:
+      aabb = this->dataPtr->cylinder->AxisAlignedBox();
+      break;
+    case GeometryType::ELLIPSOID:
+      aabb = this->dataPtr->ellipsoid->AxisAlignedBox();
+      break;
+    case GeometryType::SPHERE:
+      aabb = this->dataPtr->sphere->AxisAlignedBox();
+      break;
+    case GeometryType::MESH:
+      aabb = this->dataPtr->mesh->AxisAlignedBox(_meshAabbCalculator);
+      break;
+    default:
+      aabb = std::nullopt;
+      break;
+  }
+
+  return aabb;
+}
+
+
 /////////////////////////////////////////////////
 sdf::ElementPtr Geometry::Element() const
 {