diff --git a/include/geometric_shapes/bodies.h b/include/geometric_shapes/bodies.h index 4a0430b..c08a363 100644 --- a/include/geometric_shapes/bodies.h +++ b/include/geometric_shapes/bodies.h @@ -242,10 +242,8 @@ 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. - * \note This function should be pure virtual, but it can't in order to maintain ABI compatibility. - **/ - void computeBoundingBox(OBB& bbox) const; + pose. Scaling and padding are accounted for. */ + virtual void computeBoundingBox(OBB& bbox) const = 0; /** \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 @@ -314,7 +312,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; + void computeBoundingBox(OBB& bbox) const override; bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir, EigenSTL::vector_Vector3d* intersections = nullptr, unsigned int count = 0) const override; @@ -367,7 +365,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; + void computeBoundingBox(OBB& bbox) const override; bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir, EigenSTL::vector_Vector3d* intersections = nullptr, unsigned int count = 0) const override; @@ -430,7 +428,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; + void computeBoundingBox(OBB& bbox) const override; bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir, EigenSTL::vector_Vector3d* intersections = nullptr, unsigned int count = 0) const override; @@ -493,7 +491,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; + void computeBoundingBox(OBB& bbox) const override; 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 425588e..64d2275 100644 --- a/src/bodies.cpp +++ b/src/bodies.cpp @@ -136,30 +136,6 @@ 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_;