diff --git a/include/geometric_shapes/bodies.h b/include/geometric_shapes/bodies.h index c08a363..4a0430b 100644 --- a/include/geometric_shapes/bodies.h +++ b/include/geometric_shapes/bodies.h @@ -242,8 +242,10 @@ class Body virtual void computeBoundingBox(AABB& bbox) const = 0; /** \brief Compute the oriented bounding box for the body, in its current - pose. Scaling and padding are accounted for. */ - virtual void computeBoundingBox(OBB& bbox) const = 0; + * pose. Scaling and padding are accounted for. + * \note This function should be pure virtual, but it can't in order to maintain ABI compatibility. + **/ + void computeBoundingBox(OBB& bbox) const; /** \brief Get a clone of this body, but one that is located at the pose \e pose */ inline BodyPtr cloneAt(const Eigen::Isometry3d& pose) const @@ -312,7 +314,7 @@ class Sphere : public Body void computeBoundingSphere(BoundingSphere& sphere) const override; void computeBoundingCylinder(BoundingCylinder& cylinder) const override; void computeBoundingBox(AABB& bbox) const override; - void computeBoundingBox(OBB& bbox) const override; + void computeBoundingBox(OBB& bbox) const; bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir, EigenSTL::vector_Vector3d* intersections = nullptr, unsigned int count = 0) const override; @@ -365,7 +367,7 @@ class Cylinder : public Body void computeBoundingSphere(BoundingSphere& sphere) const override; void computeBoundingCylinder(BoundingCylinder& cylinder) const override; void computeBoundingBox(AABB& bbox) const override; - void computeBoundingBox(OBB& bbox) const override; + void computeBoundingBox(OBB& bbox) const; bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir, EigenSTL::vector_Vector3d* intersections = nullptr, unsigned int count = 0) const override; @@ -428,7 +430,7 @@ class Box : public Body void computeBoundingSphere(BoundingSphere& sphere) const override; void computeBoundingCylinder(BoundingCylinder& cylinder) const override; void computeBoundingBox(AABB& bbox) const override; - void computeBoundingBox(OBB& bbox) const override; + void computeBoundingBox(OBB& bbox) const; bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir, EigenSTL::vector_Vector3d* intersections = nullptr, unsigned int count = 0) const override; @@ -491,7 +493,7 @@ class ConvexMesh : public Body void computeBoundingSphere(BoundingSphere& sphere) const override; void computeBoundingCylinder(BoundingCylinder& cylinder) const override; void computeBoundingBox(AABB& bbox) const override; - void computeBoundingBox(OBB& bbox) const override; + void computeBoundingBox(OBB& bbox) const; bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir, EigenSTL::vector_Vector3d* intersections = nullptr, unsigned int count = 0) const override; diff --git a/src/bodies.cpp b/src/bodies.cpp index 64d2275..425588e 100644 --- a/src/bodies.cpp +++ b/src/bodies.cpp @@ -136,6 +136,30 @@ bool bodies::Body::samplePointInside(random_numbers::RandomNumberGenerator& rng, return false; } +void bodies::Body::computeBoundingBox(bodies::OBB& bbox) const +{ + // NOTE: this switch is needed only because computeBoundingBox(OBB) could not be defined virtual to keep ABI + // compatibility; if porting to new ROS (2) releases, this function should have no base implementation for a generic + // body and should be pure virtual + switch (this->getType()) + { + case shapes::SPHERE: + static_cast(this)->computeBoundingBox(bbox); + break; + case shapes::CYLINDER: + static_cast(this)->computeBoundingBox(bbox); + break; + case shapes::BOX: + static_cast(this)->computeBoundingBox(bbox); + break; + case shapes::MESH: + static_cast(this)->computeBoundingBox(bbox); + break; + default: + throw std::runtime_error("Unknown body type: " + std::to_string(this->getType())); + } +} + bool bodies::Sphere::containsPoint(const Eigen::Vector3d& p, bool /* verbose */) const { return (center_ - p).squaredNorm() <= radius2_;