From 3c0c84968e2f682aa464d784a59e07495343c05f Mon Sep 17 00:00:00 2001 From: Gabriel Pacheco Date: Fri, 2 May 2025 16:04:58 -0300 Subject: [PATCH] Add AxisAlignedBox getters for all relevant geometries (#1547) This adds AxisAlignedBox support for SDF geometries. Now each volumetric geometry type such as Box, Capsule, Cone, Cylinder, Ellipsoid, Sphere, and Mesh has an AxisAlignedBox() function that computes its bounding box from the geometry dimensions. The Geometry class uses these functions to determine the correct AABB, allowing a custom calculator for meshes , which is a special case since no mesh loading is performed in the sdf::Mesh class. Signed-off-by: Gabriel Pacheco (cherry picked from commit e073ffa5986474c9d52c6ac1c574f49bc659bfc5) --- include/sdf/Box.hh | 6 ++ include/sdf/Capsule.hh | 6 ++ include/sdf/Cone.hh | 6 ++ include/sdf/Cylinder.hh | 6 ++ include/sdf/Ellipsoid.hh | 6 ++ include/sdf/Geometry.hh | 8 ++ include/sdf/Mesh.hh | 13 +++ include/sdf/Sphere.hh | 6 ++ 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 | 103 ++++++++++++++++++- python/test/pyMesh_TEST.py | 34 ++++++- python/test/pySphere_TEST.py | 10 +- 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 | 169 ++++++++++++++++++++++++++++++++ src/Mesh.cc | 12 +++ src/Mesh_TEST.cc | 48 +++++++++ src/Sphere.cc | 7 ++ src/Sphere_TEST.cc | 12 +++ 40 files changed, 661 insertions(+), 8 deletions(-) diff --git a/include/sdf/Box.hh b/include/sdf/Box.hh index ae3515f39..d9b35fdfe 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,11 @@ namespace sdf public: std::optional 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 diff --git a/include/sdf/Capsule.hh b/include/sdf/Capsule.hh index 97f6dce0b..1015fbb4e 100644 --- a/include/sdf/Capsule.hh +++ b/include/sdf/Capsule.hh @@ -21,6 +21,7 @@ #include #include +#include #include #include #include @@ -83,6 +84,11 @@ namespace sdf public: std::optional 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 diff --git a/include/sdf/Cone.hh b/include/sdf/Cone.hh index 641c5a304..a0ae804bf 100644 --- a/include/sdf/Cone.hh +++ b/include/sdf/Cone.hh @@ -23,6 +23,7 @@ #include #include +#include #include #include #include @@ -85,6 +86,11 @@ namespace sdf public: std::optional 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 diff --git a/include/sdf/Cylinder.hh b/include/sdf/Cylinder.hh index bba37ba24..aae626c5d 100644 --- a/include/sdf/Cylinder.hh +++ b/include/sdf/Cylinder.hh @@ -21,6 +21,7 @@ #include #include +#include #include #include #include @@ -83,6 +84,11 @@ namespace sdf public: std::optional 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 diff --git a/include/sdf/Ellipsoid.hh b/include/sdf/Ellipsoid.hh index 33a046c2c..c1b908b0c 100644 --- a/include/sdf/Ellipsoid.hh +++ b/include/sdf/Ellipsoid.hh @@ -21,6 +21,7 @@ #include #include +#include #include #include #include @@ -75,6 +76,11 @@ namespace sdf public: std::optional 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 diff --git a/include/sdf/Geometry.hh b/include/sdf/Geometry.hh index 5d75b860d..69310527d 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( + 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 diff --git a/include/sdf/Mesh.hh b/include/sdf/Mesh.hh index c77cb7bff..4933afb16 100644 --- a/include/sdf/Mesh.hh +++ b/include/sdf/Mesh.hh @@ -22,6 +22,7 @@ #include #include +#include #include #include #include @@ -94,6 +95,10 @@ namespace sdf /// Geometry. class SDFORMAT_VISIBLE Mesh { + public: using AxisAlignedBoxCalculator = + std::function( + 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 + 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. diff --git a/include/sdf/Sphere.hh b/include/sdf/Sphere.hh index 05eb072e3..91bf45a3f 100644 --- a/include/sdf/Sphere.hh +++ b/include/sdf/Sphere.hh @@ -21,6 +21,7 @@ #include #include +#include #include #include @@ -76,6 +77,11 @@ namespace sdf public: std::optional 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 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..56a5505b8 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, -2.0), Vector3d(0.5, 0.5, 2.0)), + 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..a846ff124 100644 --- a/python/test/pyGeometry_TEST.py +++ b/python/test/pyGeometry_TEST.py @@ -15,9 +15,10 @@ 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 +from typing import Optional import unittest @@ -388,5 +389,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, -2.0), Vector3d(0.5, 0.5, 2.0)), + 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) -> Optional[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..2c8946e7d 100644 --- a/python/test/pyMesh_TEST.py +++ b/python/test/pyMesh_TEST.py @@ -14,8 +14,9 @@ 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 +from typing import Optional import unittest @@ -116,7 +117,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 +160,36 @@ 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) -> Optional[AxisAlignedBox]: + if sdf_mesh.uri() == "no_mesh": + return None + + 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("no_mesh") + self.assertEqual( + None, + mesh.axis_aligned_box(mesh_aabb_calulator)) + + 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/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..50fc6de39 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 = this->Radius() * gz::math::Vector3d::One + + this->Length() / 2 * gz::math::Vector3d::UnitZ; + 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..d6a9bb86a 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, 3.0), aabb.Size()); + EXPECT_EQ(gz::math::Vector3d(-0.5, -0.5, -1.5), aabb.Min()); + EXPECT_EQ(gz::math::Vector3d(0.5, 0.5, 1.5), 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..8883f80ee 100644 --- a/src/Geometry.cc +++ b/src/Geometry.cc @@ -385,6 +385,45 @@ std::optional Geometry::CalculateInertial( return geomInertial; } +///////////////////////////////////////////////// +std::optional Geometry::AxisAlignedBox( + const Mesh::AxisAlignedBoxCalculator &_meshAabbCalculator) const +{ + // Initialize the AABB to an invalid box (aabb.Min > aabb.Max) + 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..78a3979ee 100644 --- a/src/Geometry_TEST.cc +++ b/src/Geometry_TEST.cc @@ -1186,3 +1186,172 @@ 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.2 - 1.7), aabb->Min()); + EXPECT_EQ(gz::math::Vector3d(1.2, 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 &) -> + std::optional + { + 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..ca2e55c3d 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( + const 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..6afa3790c 100644 --- a/src/Mesh_TEST.cc +++ b/src/Mesh_TEST.cc @@ -433,3 +433,51 @@ 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) -> std::optional + { + if(_mesh.Uri() == "no_mesh") + { + return std::nullopt; + } + + 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) + ) + ); + }; + + // "apple" mesh should return valid aabb + 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()); + + // "banana" mesh should return invalid aabb + mesh.SetUri("banana"); + aabb = mesh.AxisAlignedBox(customCalculator); + EXPECT_TRUE(aabb.has_value()); + EXPECT_EQ(gz::math::AxisAlignedBox(), aabb.value()); + + // "no_mesh" should return std::nullopt + mesh.SetUri("no_mesh"); + EXPECT_EQ(std::nullopt, mesh.AxisAlignedBox(customCalculator)); +} 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()); +}