From f3cf2c431e0ecc828369d464f4a446146e15c780 Mon Sep 17 00:00:00 2001 From: Gabriel Pacheco Date: Tue, 18 Feb 2025 13:34:05 -0300 Subject: [PATCH 1/2] 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. Signed-off-by: Gabriel Pacheco --- include/sdf/Box.hh | 5 ++ include/sdf/Capsule.hh | 5 ++ include/sdf/Cone.hh | 5 ++ include/sdf/Cylinder.hh | 5 ++ include/sdf/Ellipsoid.hh | 5 ++ include/sdf/Geometry.hh | 8 ++ include/sdf/Mesh.hh | 12 +++ include/sdf/Sphere.hh | 5 ++ src/Box.cc | 7 ++ src/Box_TEST.cc | 12 +++ src/Capsule.cc | 8 ++ src/Capsule_TEST.cc | 13 +++ src/Cone.cc | 7 ++ src/Cone_TEST.cc | 13 +++ src/Cylinder.cc | 8 ++ src/Cylinder_TEST.cc | 13 +++ src/Ellipsoid.cc | 7 ++ src/Ellipsoid_TEST.cc | 12 +++ src/Geometry.cc | 39 +++++++++ src/Geometry_TEST.cc | 168 +++++++++++++++++++++++++++++++++++++++ src/Mesh.cc | 12 +++ src/Mesh_TEST.cc | 39 +++++++++ src/Sphere.cc | 7 ++ src/Sphere_TEST.cc | 12 +++ 24 files changed, 427 insertions(+) diff --git a/include/sdf/Box.hh b/include/sdf/Box.hh index 10ff8cc1a..f124dc00f 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 c385c1216..341e4ae8b 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 cc935a5e7..c573cc00b 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 9ea63c7a9..4b3b8a1d6 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 b328fcd8a..b337bc183 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 aec5076d0..53b3e227f 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 0ce5c525e..44650f04c 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 a56f37b16..49c2779cd 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 a2338c1fe..3ed4d2cd9 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 9fb631242..29c6ce63b 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()); +} diff --git a/src/Capsule.cc b/src/Capsule.cc index 1f8e2bd00..31c186193 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 0c77fcff7..f9d47bac8 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()); +} diff --git a/src/Cone.cc b/src/Cone.cc index 7e094bdd0..6480d509a 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 59c5dbf3e..fdebe5baf 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()); +} diff --git a/src/Cylinder.cc b/src/Cylinder.cc index 8c7779d00..8ec48eb6f 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 b07f298dd..f63362f4a 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()); +} diff --git a/src/Ellipsoid.cc b/src/Ellipsoid.cc index 294a0b87b..e280d037d 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 cdba939b9..deb35d3ae 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()); +} diff --git a/src/Geometry.cc b/src/Geometry.cc index 456d5e200..09bd2e846 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 05f4d7205..42c9cc52a 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()); + } +} diff --git a/src/Mesh.cc b/src/Mesh.cc index d2ee4b7a8..6bf5fea12 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 391283976..7ea1f3458 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 9d274530c..00a2bcc4b 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 e7da830dc..dc41f394b 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()); +} From 2ef02d4a097a789b23af4fc1e6620c8da6a1f9e0 Mon Sep 17 00:00:00 2001 From: Gabriel Pacheco Date: Thu, 20 Feb 2025 10:39:25 -0300 Subject: [PATCH 2/2] Add python bindings and corresponding tests Signed-off-by: Gabriel Pacheco --- include/sdf/Geometry.hh | 6 +- include/sdf/Mesh.hh | 7 +-- python/src/sdf/pyBox.cc | 2 + python/src/sdf/pyCapsule.cc | 2 + python/src/sdf/pyCone.cc | 2 + python/src/sdf/pyCylinder.cc | 2 + python/src/sdf/pyEllipsoid.cc | 2 + python/src/sdf/pyGeometry.cc | 3 + python/src/sdf/pyMesh.cc | 4 ++ python/src/sdf/pySphere.cc | 2 + python/test/pyBox_TEST.py | 11 +++- python/test/pyCapsule_TEST.py | 13 +++- python/test/pyCone_TEST.py | 11 ++++ python/test/pyCylinder_TEST.py | 12 +++- python/test/pyEllipsoid_TEST.py | 12 +++- python/test/pyGeometry_TEST.py | 102 +++++++++++++++++++++++++++++++- python/test/pyMesh_TEST.py | 25 +++++++- python/test/pySphere_TEST.py | 10 +++- src/Geometry.cc | 4 +- src/Mesh.cc | 2 +- 20 files changed, 216 insertions(+), 18 deletions(-) diff --git a/include/sdf/Geometry.hh b/include/sdf/Geometry.hh index 53b3e227f..501d15a5a 100644 --- a/include/sdf/Geometry.hh +++ b/include/sdf/Geometry.hh @@ -241,11 +241,11 @@ namespace sdf 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 + /// \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; + const Mesh::AxisAlignedBoxCalculator &_meshAabbCalculator) const; /// \brief Get a pointer to the SDF element that was used during /// load. diff --git a/include/sdf/Mesh.hh b/include/sdf/Mesh.hh index 44650f04c..6943c7d3e 100644 --- a/include/sdf/Mesh.hh +++ b/include/sdf/Mesh.hh @@ -211,12 +211,11 @@ namespace sdf 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. + /// \param[in] _aabbCalc A custom function that calculates the + /// AxisAlignedBox for the Mesh. /// \return A gz::math::AxisAlignedBox object. public: std::optional - AxisAlignedBox(AxisAlignedBoxCalculator _meshAabbCalculator) const; + 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 diff --git a/python/src/sdf/pyBox.cc b/python/src/sdf/pyBox.cc index 453438119..8ec805075 100644 --- a/python/src/sdf/pyBox.cc +++ b/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); }) diff --git a/python/src/sdf/pyCapsule.cc b/python/src/sdf/pyCapsule.cc index 50ca4337d..77bec7d43 100644 --- a/python/src/sdf/pyCapsule.cc +++ b/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_), diff --git a/python/src/sdf/pyCone.cc b/python/src/sdf/pyCone.cc index 05cb9cb15..9e7c651be 100644 --- a/python/src/sdf/pyCone.cc +++ b/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); }) diff --git a/python/src/sdf/pyCylinder.cc b/python/src/sdf/pyCylinder.cc index d8515ddc6..5e94f146e 100644 --- a/python/src/sdf/pyCylinder.cc +++ b/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); }) diff --git a/python/src/sdf/pyEllipsoid.cc b/python/src/sdf/pyEllipsoid.cc index 47ea60c96..aa446a83d 100644 --- a/python/src/sdf/pyEllipsoid.cc +++ b/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); }) diff --git a/python/src/sdf/pyGeometry.cc b/python/src/sdf/pyGeometry.cc index 8d7873bc3..a938b27d9 100644 --- a/python/src/sdf/pyGeometry.cc +++ b/python/src/sdf/pyGeometry.cc @@ -18,6 +18,7 @@ #include #include +#include #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); }) diff --git a/python/src/sdf/pyMesh.cc b/python/src/sdf/pyMesh.cc index b9fdafd17..dda62f719 100644 --- a/python/src/sdf/pyMesh.cc +++ b/python/src/sdf/pyMesh.cc @@ -17,6 +17,8 @@ #include "pyMesh.hh" #include +#include +#include #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); }) diff --git a/python/src/sdf/pySphere.cc b/python/src/sdf/pySphere.cc index ee6197b38..b5565a48f 100644 --- a/python/src/sdf/pySphere.cc +++ b/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); }) diff --git a/python/test/pyBox_TEST.py b/python/test/pyBox_TEST.py index 84ce40bee..9bb5db704 100644 --- a/python/test/pyBox_TEST.py +++ b/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() diff --git a/python/test/pyCapsule_TEST.py b/python/test/pyCapsule_TEST.py index 976e53c7b..e5b535006 100644 --- a/python/test/pyCapsule_TEST.py +++ b/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, -1.5), Vector3d(0.5, 0.5, 1.5)), + capsule.axis_aligned_box()) + + if __name__ == '__main__': unittest.main() diff --git a/python/test/pyCone_TEST.py b/python/test/pyCone_TEST.py index e36e65f63..22d3a8191 100644 --- a/python/test/pyCone_TEST.py +++ b/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() diff --git a/python/test/pyCylinder_TEST.py b/python/test/pyCylinder_TEST.py index b5387dd0b..5fba0b44c 100644 --- a/python/test/pyCylinder_TEST.py +++ b/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() diff --git a/python/test/pyEllipsoid_TEST.py b/python/test/pyEllipsoid_TEST.py index c12690ffe..7342bc34f 100644 --- a/python/test/pyEllipsoid_TEST.py +++ b/python/test/pyEllipsoid_TEST.py @@ -13,7 +13,7 @@ # limitations under the License. import copy -from gz_test_deps.math import Inertiald, MassMatrix3d, Pose3d, Vector3d +from gz_test_deps.math import AxisAlignedBox, Inertiald, MassMatrix3d, Pose3d, Vector3d import math from gz_test_deps.sdformat import Ellipsoid import unittest @@ -77,6 +77,7 @@ def test_shape(self): ellipsoid.shape().set_radii(expectedradii) self.assertEqual(expectedradii, ellipsoid.radii()) + def test_calculate_inertial(self): ellipsoid = Ellipsoid() @@ -116,5 +117,14 @@ def test_calculate_inertial(self): ellipsoidInertial.mass_matrix().mass()) self.assertEqual(expectedInertial.pose(), ellipsoidInertial.pose()) + def test_axis_aligned_box(self): + ellipsoid = Ellipsoid() + ellipsoid.set_radii(Vector3d(1.0, 2.0, 3.0)) + + self.assertEqual( + AxisAlignedBox(Vector3d(-1.0, -2.0, -3.0), Vector3d(1.0, 2.0, 3.0)), + ellipsoid.axis_aligned_box()) + + if __name__ == '__main__': unittest.main() diff --git a/python/test/pyGeometry_TEST.py b/python/test/pyGeometry_TEST.py index c124f2f71..ffc1359cf 100644 --- a/python/test/pyGeometry_TEST.py +++ b/python/test/pyGeometry_TEST.py @@ -15,7 +15,7 @@ import copy from gz_test_deps.sdformat import (Element, Error, Geometry, Box, Capsule, Cone, Cylinder, Ellipsoid, Heightmap, Mesh, ParserConfig, Plane, Sphere) -from gz_test_deps.math import Inertiald, MassMatrix3d, Pose3d, Vector3d, Vector2d +from gz_test_deps.math import AxisAlignedBox, Inertiald, MassMatrix3d, Pose3d, Vector3d, Vector2d import gz_test_deps.sdformat as sdf import math import unittest @@ -388,5 +388,105 @@ def test_calculate_inertial(self): self.assertEqual(expectedInertial.mass_matrix(), expectedMassMat) self.assertEqual(expectedInertial.pose(), sphereInertial.pose()) + def test_axis_aligned_box(self): + geom = Geometry() + + # Box + geom.set_type(sdf.GeometryType.BOX) + box = Box() + box.set_size(Vector3d(1, 2, 3)) + geom.set_box_shape(box) + + self.assertEqual( + AxisAlignedBox(Vector3d(-0.5, -1, -1.5), Vector3d(0.5, 1, 1.5)), + geom.axis_aligned_box(None)) + + # Capsule + geom.set_type(sdf.GeometryType.CAPSULE) + capsule = Capsule() + capsule.set_radius(0.5) + capsule.set_length(3.0) + geom.set_capsule_shape(capsule) + + self.assertEqual( + AxisAlignedBox(Vector3d(-0.5, -0.5, -1.5), Vector3d(0.5, 0.5, 1.5)), + geom.axis_aligned_box(None)) + + # Cone + geom.set_type(sdf.GeometryType.CONE) + cone = Cone() + cone.set_radius(0.5) + cone.set_length(3.0) + geom.set_cone_shape(cone) + + self.assertEqual( + AxisAlignedBox(Vector3d(-0.5, -0.5, -1.5), Vector3d(0.5, 0.5, 1.5)), + geom.axis_aligned_box(None)) + + # Cylinder + geom.set_type(sdf.GeometryType.CYLINDER) + cylinder = Cylinder() + cylinder.set_radius(0.5) + cylinder.set_length(3.0) + geom.set_cylinder_shape(cylinder) + + self.assertEqual( + AxisAlignedBox(Vector3d(-0.5, -0.5, -1.5), Vector3d(0.5, 0.5, 1.5)), + geom.axis_aligned_box(None)) + + # Ellipsoid + geom.set_type(sdf.GeometryType.ELLIPSOID) + ellipsoid = Ellipsoid() + ellipsoid.set_radii(Vector3d(1, 2, 3)) + geom.set_ellipsoid_shape(ellipsoid) + + self.assertEqual( + AxisAlignedBox(Vector3d(-1, -2, -3), Vector3d(1, 2, 3)), + geom.axis_aligned_box(None)) + + # Sphere + geom.set_type(sdf.GeometryType.SPHERE) + sphere = Sphere() + sphere.set_radius(0.5) + geom.set_sphere_shape(sphere) + + self.assertEqual( + AxisAlignedBox(Vector3d(-0.5, -0.5, -0.5), Vector3d(0.5, 0.5, 0.5)), + geom.axis_aligned_box(None)) + + # Mesh + geom.set_type(sdf.GeometryType.MESH) + mesh = Mesh() + geom.set_mesh_shape(mesh) + + self.assertEqual(None, geom.axis_aligned_box(None)) + + def mesh_aabb_calulator(sdf_mesh: Mesh) -> AxisAlignedBox: + return AxisAlignedBox(Vector3d(-1, -1, -1), Vector3d(1, 1, 1)) + + self.assertEqual( + AxisAlignedBox(Vector3d(-1, -1, -1), Vector3d(1, 1, 1)), + geom.axis_aligned_box(mesh_aabb_calulator)) + + # Plane - Non volumetric geometry (no AABB) + geom.set_type(sdf.GeometryType.PLANE) + plane = Plane() + geom.set_plane_shape(plane) + + self.assertEqual(None, geom.axis_aligned_box(None)) + + # Heightmap - Non volumetric geometry (no AABB) + geom.set_type(sdf.GeometryType.HEIGHTMAP) + heightmap = Heightmap() + geom.set_heightmap_shape(heightmap) + + self.assertEqual(None, geom.axis_aligned_box(None)) + + # Empty - Non volumetric geometry (no AABB) + geom.set_type(sdf.GeometryType.EMPTY) + + self.assertEqual(None, geom.axis_aligned_box(None)) + + if __name__ == '__main__': unittest.main() diff --git a/python/test/pyMesh_TEST.py b/python/test/pyMesh_TEST.py index 0990095d6..e79b30822 100644 --- a/python/test/pyMesh_TEST.py +++ b/python/test/pyMesh_TEST.py @@ -14,7 +14,7 @@ import copy from gz_test_deps.sdformat import Mesh, ConvexDecomposition -from gz_test_deps.math import Vector3d +from gz_test_deps.math import AxisAlignedBox, Vector3d import gz_test_deps.sdformat as sdf import unittest @@ -116,7 +116,6 @@ def test_deepcopy_construction(self): self.assertTrue(mesh2.center_submesh()) self.assertEqual("/pear", mesh2.file_path()) - def test_set(self): mesh = Mesh() @@ -160,6 +159,28 @@ def test_set(self): mesh.set_file_path("/mypath") self.assertEqual("/mypath", mesh.file_path()) + def test_axis_aligned_box(self): + mesh = Mesh() + self.assertEqual(None, mesh.axis_aligned_box(None)) + + # Customly defined Mesh AABB calculator + def mesh_aabb_calulator(sdf_mesh: Mesh) -> AxisAlignedBox: + if sdf_mesh.uri() == "banana": + # Banana mesh should return invalid AABB + return AxisAlignedBox() + + return AxisAlignedBox(Vector3d(-1, -1, -1), Vector3d(1, 1, 1)) + + mesh.set_uri("banana") + self.assertEqual( + AxisAlignedBox(), + mesh.axis_aligned_box(mesh_aabb_calulator)) + + mesh.set_uri("apple") + self.assertEqual( + AxisAlignedBox(Vector3d(-1, -1, -1), Vector3d(1, 1, 1)), + mesh.axis_aligned_box(mesh_aabb_calulator)) + if __name__ == '__main__': unittest.main() diff --git a/python/test/pySphere_TEST.py b/python/test/pySphere_TEST.py index f687f6815..032081b39 100644 --- a/python/test/pySphere_TEST.py +++ b/python/test/pySphere_TEST.py @@ -15,7 +15,7 @@ import copy import math from gz_test_deps.sdformat import Sphere -from gz_test_deps.math import Inertiald, MassMatrix3d, Pose3d, Vector3d +from gz_test_deps.math import AxisAlignedBox, Inertiald, MassMatrix3d, Pose3d, Vector3d import unittest class SphereTEST(unittest.TestCase): @@ -111,6 +111,14 @@ def test_calculate_inertial(self): sphereInertial.mass_matrix().mass()) self.assertEqual(expectedInertial.pose(), sphereInertial.pose()) + def test_axis_aligned_box(self): + sphere = Sphere() + sphere.set_radius(0.75) + + self.assertEqual( + AxisAlignedBox(Vector3d(-0.75, -0.75, -0.75), Vector3d(0.75, 0.75, 0.75)), + sphere.axis_aligned_box()) + if __name__ == '__main__': unittest.main() diff --git a/src/Geometry.cc b/src/Geometry.cc index 09bd2e846..8883f80ee 100644 --- a/src/Geometry.cc +++ b/src/Geometry.cc @@ -387,9 +387,9 @@ std::optional Geometry::CalculateInertial( ///////////////////////////////////////////////// std::optional Geometry::AxisAlignedBox( - Mesh::AxisAlignedBoxCalculator _meshAabbCalculator) const + const Mesh::AxisAlignedBoxCalculator &_meshAabbCalculator) const { - // Initialized the AABB to an empty box (zero volume) + // Initialize the AABB to an invalid box (aabb.Min > aabb.Max) std::optional aabb; switch (this->dataPtr->type) diff --git a/src/Mesh.cc b/src/Mesh.cc index 6bf5fea12..ca2e55c3d 100644 --- a/src/Mesh.cc +++ b/src/Mesh.cc @@ -400,7 +400,7 @@ std::optional Mesh::CalculateInertial(sdf::Errors &_errors, ///////////////////////////////////////////////// std::optional Mesh::AxisAlignedBox( - Mesh::AxisAlignedBoxCalculator _customCalculator) const + const Mesh::AxisAlignedBoxCalculator &_customCalculator) const { if (!_customCalculator) {