diff --git a/include/sdf/Box.hh b/include/sdf/Box.hh index 10ff8cc1a7..f124dc00f4 100644 --- a/include/sdf/Box.hh +++ b/include/sdf/Box.hh @@ -22,6 +22,7 @@ #include #include #include +#include #include #include #include @@ -76,6 +77,10 @@ namespace sdf public: std::optional 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 diff --git a/include/sdf/Capsule.hh b/include/sdf/Capsule.hh index c385c1216d..341e4ae8be 100644 --- a/include/sdf/Capsule.hh +++ b/include/sdf/Capsule.hh @@ -21,6 +21,7 @@ #include #include +#include #include #include #include @@ -83,6 +84,10 @@ namespace sdf public: std::optional 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 diff --git a/include/sdf/Cone.hh b/include/sdf/Cone.hh index cc935a5e74..c573cc00b3 100644 --- a/include/sdf/Cone.hh +++ b/include/sdf/Cone.hh @@ -23,6 +23,7 @@ #include #include +#include #include #include #include @@ -85,6 +86,10 @@ namespace sdf public: std::optional 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 diff --git a/include/sdf/Cylinder.hh b/include/sdf/Cylinder.hh index 9ea63c7a95..4b3b8a1d68 100644 --- a/include/sdf/Cylinder.hh +++ b/include/sdf/Cylinder.hh @@ -21,6 +21,7 @@ #include #include +#include #include #include #include @@ -83,6 +84,10 @@ namespace sdf public: std::optional 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 diff --git a/include/sdf/Ellipsoid.hh b/include/sdf/Ellipsoid.hh index b328fcd8ac..b337bc1831 100644 --- a/include/sdf/Ellipsoid.hh +++ b/include/sdf/Ellipsoid.hh @@ -21,6 +21,7 @@ #include #include +#include #include #include #include @@ -75,6 +76,10 @@ namespace sdf public: std::optional 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 diff --git a/include/sdf/Geometry.hh b/include/sdf/Geometry.hh index aec5076d0a..c7b030f105 100644 --- a/include/sdf/Geometry.hh +++ b/include/sdf/Geometry.hh @@ -22,6 +22,7 @@ #include #include +#include #include #include #include @@ -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 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 diff --git a/include/sdf/Mesh.hh b/include/sdf/Mesh.hh index 0ce5c525e9..44650f04c2 100644 --- a/include/sdf/Mesh.hh +++ b/include/sdf/Mesh.hh @@ -22,6 +22,7 @@ #include #include +#include #include #include #include @@ -94,6 +95,9 @@ namespace sdf /// Geometry. class SDFORMAT_VISIBLE Mesh { + public: using AxisAlignedBoxCalculator = + std::function; + /// \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 + 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. diff --git a/include/sdf/Sphere.hh b/include/sdf/Sphere.hh index a56f37b16d..49c2779cd0 100644 --- a/include/sdf/Sphere.hh +++ b/include/sdf/Sphere.hh @@ -21,6 +21,7 @@ #include #include +#include #include #include @@ -76,6 +77,10 @@ namespace sdf public: std::optional 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 diff --git a/src/Box.cc b/src/Box.cc index a2338c1fe8..3ed4d2cd94 100644 --- a/src/Box.cc +++ b/src/Box.cc @@ -140,6 +140,13 @@ std::optional 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 { diff --git a/src/Box_TEST.cc b/src/Box_TEST.cc index 9fb631242d..4242c18d2f 100644 --- a/src/Box_TEST.cc +++ b/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()); +} \ No newline at end of file diff --git a/src/Capsule.cc b/src/Capsule.cc index 1f8e2bd009..31c1861935 100644 --- a/src/Capsule.cc +++ b/src/Capsule.cc @@ -161,6 +161,14 @@ std::optional 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 { diff --git a/src/Capsule_TEST.cc b/src/Capsule_TEST.cc index 0c77fcff73..b8aef8e239 100644 --- a/src/Capsule_TEST.cc +++ b/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()); +} \ No newline at end of file diff --git a/src/Cone.cc b/src/Cone.cc index 7e094bdd0a..6480d509ab 100644 --- a/src/Cone.cc +++ b/src/Cone.cc @@ -164,6 +164,13 @@ std::optional 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 { diff --git a/src/Cone_TEST.cc b/src/Cone_TEST.cc index 59c5dbf3e2..6ce2cb878b 100644 --- a/src/Cone_TEST.cc +++ b/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()); +} \ No newline at end of file diff --git a/src/Cylinder.cc b/src/Cylinder.cc index 8c7779d00c..8ec48eb6ff 100644 --- a/src/Cylinder.cc +++ b/src/Cylinder.cc @@ -159,6 +159,14 @@ std::optional 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 { diff --git a/src/Cylinder_TEST.cc b/src/Cylinder_TEST.cc index b07f298dd0..e92e1e7794 100644 --- a/src/Cylinder_TEST.cc +++ b/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()); +} \ No newline at end of file diff --git a/src/Ellipsoid.cc b/src/Ellipsoid.cc index 294a0b87b9..e280d037d5 100644 --- a/src/Ellipsoid.cc +++ b/src/Ellipsoid.cc @@ -140,6 +140,13 @@ std::optional 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 { diff --git a/src/Ellipsoid_TEST.cc b/src/Ellipsoid_TEST.cc index cdba939b95..24d29eefd7 100644 --- a/src/Ellipsoid_TEST.cc +++ b/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()); +} \ No newline at end of file diff --git a/src/Geometry.cc b/src/Geometry.cc index 456d5e200c..09bd2e8462 100644 --- a/src/Geometry.cc +++ b/src/Geometry.cc @@ -385,6 +385,45 @@ std::optional Geometry::CalculateInertial( return geomInertial; } +///////////////////////////////////////////////// +std::optional Geometry::AxisAlignedBox( + Mesh::AxisAlignedBoxCalculator _meshAabbCalculator) const +{ + // Initialized the AABB to an empty box (zero volume) + std::optional 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 { diff --git a/src/Geometry_TEST.cc b/src/Geometry_TEST.cc index 05f4d72055..2f2258b587 100644 --- a/src/Geometry_TEST.cc +++ b/src/Geometry_TEST.cc @@ -1186,3 +1186,171 @@ TEST(DOMGeometry, ToElementErrorOutput) // Check nothing has been printed EXPECT_TRUE(buffer.str().empty()) << buffer.str(); } + +///////////////////////////////////////////////// +TEST(DOMGeometry, AxisAlignedBox) +{ + // Box + { + sdf::Geometry geom; + geom.SetType(sdf::GeometryType::BOX); + + sdf::Box box; + box.SetSize({1, 2, 3}); + geom.SetBoxShape(box); + + auto aabb = geom.AxisAlignedBox(nullptr); + EXPECT_TRUE(aabb.has_value()); + EXPECT_EQ(gz::math::Vector3d(-0.5, -1, -1.5), aabb->Min()); + EXPECT_EQ(gz::math::Vector3d(0.5, 1, 1.5), aabb->Max()); + } + + // Capsule + { + sdf::Geometry geom; + geom.SetType(sdf::GeometryType::CAPSULE); + + sdf::Capsule capsule; + capsule.SetRadius(1.2); + capsule.SetLength(3.4); + geom.SetCapsuleShape(capsule); + + auto aabb = geom.AxisAlignedBox(nullptr); + EXPECT_TRUE(aabb.has_value()); + EXPECT_EQ(gz::math::Vector3d(-1.2, -1.2, -1.7), aabb->Min()); + EXPECT_EQ(gz::math::Vector3d(1.2, 1.2, 1.7), aabb->Max()); + } + + // Cone + { + sdf::Geometry geom; + geom.SetType(sdf::GeometryType::CONE); + + sdf::Cone cone; + cone.SetRadius(1.2); + cone.SetLength(3.4); + geom.SetConeShape(cone); + + auto aabb = geom.AxisAlignedBox(nullptr); + EXPECT_TRUE(aabb.has_value()); + EXPECT_EQ(gz::math::Vector3d(-1.2, -1.2, -1.7), aabb->Min()); + EXPECT_EQ(gz::math::Vector3d(1.2, 1.2, 1.7), aabb->Max()); + } + + // Cylinder + { + sdf::Geometry geom; + geom.SetType(sdf::GeometryType::CYLINDER); + + sdf::Cylinder cylinder; + cylinder.SetRadius(1.2); + cylinder.SetLength(3.4); + geom.SetCylinderShape(cylinder); + + auto aabb = geom.AxisAlignedBox(nullptr); + EXPECT_TRUE(aabb.has_value()); + EXPECT_EQ(gz::math::Vector3d(-1.2, -1.2, -1.7), aabb->Min()); + EXPECT_EQ(gz::math::Vector3d(1.2, 1.2, 1.7), aabb->Max()); + } + + // Ellipsoid + { + sdf::Geometry geom; + geom.SetType(sdf::GeometryType::ELLIPSOID); + + sdf::Ellipsoid ellipsoid; + ellipsoid.SetRadii({1.2, 3.4, 5.6}); + geom.SetEllipsoidShape(ellipsoid); + + auto aabb = geom.AxisAlignedBox(nullptr); + EXPECT_TRUE(aabb.has_value()); + EXPECT_EQ(gz::math::Vector3d(-1.2, -3.4, -5.6), aabb->Min()); + EXPECT_EQ(gz::math::Vector3d(1.2, 3.4, 5.6), aabb->Max()); + } + + // Sphere + { + sdf::Geometry geom; + geom.SetType(sdf::GeometryType::SPHERE); + + sdf::Sphere sphere; + sphere.SetRadius(1.2); + geom.SetSphereShape(sphere); + + auto aabb = geom.AxisAlignedBox(nullptr); + EXPECT_TRUE(aabb.has_value()); + EXPECT_EQ(gz::math::Vector3d(-1.2, -1.2, -1.2), aabb->Min()); + EXPECT_EQ(gz::math::Vector3d(1.2, 1.2, 1.2), aabb->Max()); + } + + // Mesh + { + sdf::Geometry geom; + geom.SetType(sdf::GeometryType::MESH); + + sdf::Mesh mesh; + mesh.SetUri("mesh://model.sdf"); + geom.SetMeshShape(mesh); + + auto aabb = geom.AxisAlignedBox(nullptr); + EXPECT_FALSE(aabb.has_value()); + + auto meshAabbCalculator = [](const sdf::Mesh &) -> gz::math::AxisAlignedBox + { + return gz::math::AxisAlignedBox( + gz::math::Vector3d(-1, -2, -3), + gz::math::Vector3d(1, 2, 3)); + }; + + aabb = geom.AxisAlignedBox(meshAabbCalculator); + EXPECT_TRUE(aabb.has_value()); + EXPECT_EQ(gz::math::Vector3d(-1, -2, -3), aabb->Min()); + EXPECT_EQ(gz::math::Vector3d(1, 2, 3), aabb->Max()); + } + + // Heightmap - no axis aligned box + { + sdf::Geometry geom; + geom.SetType(sdf::GeometryType::HEIGHTMAP); + + sdf::Heightmap heightmap; + heightmap.SetUri("heightmap://model.sdf"); + geom.SetHeightmapShape(heightmap); + + auto aabb = geom.AxisAlignedBox(nullptr); + EXPECT_FALSE(aabb.has_value()); + } + + // Polyline - no axis aligned box + { + sdf::Geometry geom; + geom.SetType(sdf::GeometryType::POLYLINE); + + sdf::Polyline polyline; + geom.SetPolylineShape({polyline}); + + auto aabb = geom.AxisAlignedBox(nullptr); + EXPECT_FALSE(aabb.has_value()); + } + + // Plane - no axis aligned box + { + sdf::Geometry geom; + geom.SetType(sdf::GeometryType::PLANE); + + sdf::Plane plane; + geom.SetPlaneShape(plane); + + auto aabb = geom.AxisAlignedBox(nullptr); + EXPECT_FALSE(aabb.has_value()); + } + + // Empty - no axis aligned box + { + sdf::Geometry geom; + geom.SetType(sdf::GeometryType::EMPTY); + + auto aabb = geom.AxisAlignedBox(nullptr); + EXPECT_FALSE(aabb.has_value()); + } +} \ No newline at end of file diff --git a/src/Mesh.cc b/src/Mesh.cc index d2ee4b7a80..6bf5fea126 100644 --- a/src/Mesh.cc +++ b/src/Mesh.cc @@ -398,6 +398,18 @@ std::optional Mesh::CalculateInertial(sdf::Errors &_errors, return customCalculator(_errors, calcInterface); } +///////////////////////////////////////////////// +std::optional Mesh::AxisAlignedBox( + Mesh::AxisAlignedBoxCalculator _customCalculator) const +{ + if (!_customCalculator) + { + return std::nullopt; + } + + return _customCalculator(*this); +} + ///////////////////////////////////////////////// sdf::ElementPtr Mesh::ToElement() const { diff --git a/src/Mesh_TEST.cc b/src/Mesh_TEST.cc index 3912839762..552fb9059c 100644 --- a/src/Mesh_TEST.cc +++ b/src/Mesh_TEST.cc @@ -433,3 +433,42 @@ TEST(DOMMesh, ToElementErrorOutput) // Check nothing has been printed EXPECT_TRUE(buffer.str().empty()) << buffer.str(); } + +///////////////////////////////////////////////// +TEST(DOMMesh, AxisAlignedBox) +{ + sdf::Mesh mesh; + + // No custom calculator should return nullopt + EXPECT_EQ(std::nullopt, mesh.AxisAlignedBox(nullptr)); + + auto customCalculator = []( + const sdf::Mesh &_mesh) -> gz::math::AxisAlignedBox + { + if (_mesh.Uri() == "banana") + { + // Invalid mesh leads to empty AABB + return gz::math::AxisAlignedBox(); + } + + return gz::math::AxisAlignedBox( + gz::math::AxisAlignedBox( + gz::math::Vector3d(1, 2, 3), + gz::math::Vector3d(4, 5, 6) + ) + ); + }; + + // Set to valid "apple" mesh + mesh.SetUri("apple"); + auto aabb = mesh.AxisAlignedBox(customCalculator); + EXPECT_TRUE(aabb.has_value()); + EXPECT_EQ(gz::math::Vector3d(1, 2, 3), aabb->Min()); + EXPECT_EQ(gz::math::Vector3d(4, 5, 6), aabb->Max()); + + // Set to "invalid" banana mesh + mesh.SetUri("banana"); + aabb = mesh.AxisAlignedBox(customCalculator); + EXPECT_TRUE(aabb.has_value()); + EXPECT_EQ(gz::math::AxisAlignedBox(), aabb.value()); +} diff --git a/src/Sphere.cc b/src/Sphere.cc index 9d274530c0..00a2bcc4b4 100644 --- a/src/Sphere.cc +++ b/src/Sphere.cc @@ -138,6 +138,13 @@ std::optional Sphere::CalculateInertial(double _density) } } +///////////////////////////////////////////////// +gz::math::AxisAlignedBox Sphere::AxisAlignedBox() const +{ + auto halfSize = this->Radius() * gz::math::Vector3d::One; + return gz::math::AxisAlignedBox(-halfSize, halfSize); +} + ///////////////////////////////////////////////// sdf::ElementPtr Sphere::ToElement() const { diff --git a/src/Sphere_TEST.cc b/src/Sphere_TEST.cc index e7da830dc5..23faed60ae 100644 --- a/src/Sphere_TEST.cc +++ b/src/Sphere_TEST.cc @@ -229,3 +229,15 @@ TEST(DOMSphere, ToElementErrorOutput) // Check nothing has been printed EXPECT_TRUE(buffer.str().empty()) << buffer.str(); } + +///////////////////////////////////////////////// +TEST(DOMSphere, AxisAlignedBox) +{ + sdf::Sphere sphere; + sphere.SetRadius(1.75); + + auto aabb = sphere.AxisAlignedBox(); + EXPECT_EQ(gz::math::Vector3d(3.5, 3.5, 3.5), aabb.Size()); + EXPECT_EQ(gz::math::Vector3d(-1.75, -1.75, -1.75), aabb.Min()); + EXPECT_EQ(gz::math::Vector3d(1.75, 1.75, 1.75), aabb.Max()); +} \ No newline at end of file