diff --git a/include/geometric_shapes/bodies.h b/include/geometric_shapes/bodies.h index c08a363..c261121 100644 --- a/include/geometric_shapes/bodies.h +++ b/include/geometric_shapes/bodies.h @@ -243,7 +243,7 @@ class Body /** \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; + // 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 @@ -312,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 override; + // 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; @@ -365,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 override; + // 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; @@ -428,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 override; + // 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; @@ -491,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 override; + // 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 64d2275..033c623 100644 --- a/src/bodies.cpp +++ b/src/bodies.cpp @@ -206,14 +206,14 @@ void bodies::Sphere::computeBoundingBox(bodies::AABB& bbox) const bbox.extendWithTransformedBox(transform, Eigen::Vector3d(2 * radiusU_, 2 * radiusU_, 2 * radiusU_)); } -void bodies::Sphere::computeBoundingBox(bodies::OBB& bbox) const -{ - // it's a sphere, so we do not rotate the bounding box - Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); - transform.translation() = getPose().translation(); +// void bodies::Sphere::computeBoundingBox(bodies::OBB& bbox) const +// { +// // it's a sphere, so we do not rotate the bounding box +// Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); +// transform.translation() = getPose().translation(); - bbox.setPoseAndExtents(transform, 2 * Eigen::Vector3d(radiusU_, radiusU_, radiusU_)); -} +// bbox.setPoseAndExtents(transform, 2 * Eigen::Vector3d(radiusU_, radiusU_, radiusU_)); +// } bool bodies::Sphere::samplePointInside(random_numbers::RandomNumberGenerator& rng, unsigned int max_attempts, Eigen::Vector3d& result) const @@ -436,10 +436,10 @@ void bodies::Cylinder::computeBoundingBox(bodies::AABB& bbox) const bbox.extend(pb + e); } -void bodies::Cylinder::computeBoundingBox(bodies::OBB& bbox) const -{ - bbox.setPoseAndExtents(getPose(), 2 * Eigen::Vector3d(radiusU_, radiusU_, length2_)); -} +// void bodies::Cylinder::computeBoundingBox(bodies::OBB& bbox) const +// { +// bbox.setPoseAndExtents(getPose(), 2 * Eigen::Vector3d(radiusU_, radiusU_, length2_)); +// } bool bodies::Cylinder::intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir, EigenSTL::vector_Vector3d* intersections, unsigned int count) const @@ -677,10 +677,10 @@ void bodies::Box::computeBoundingBox(bodies::AABB& bbox) const bbox.extendWithTransformedBox(getPose(), 2 * Eigen::Vector3d(length2_, width2_, height2_)); } -void bodies::Box::computeBoundingBox(bodies::OBB& bbox) const -{ - bbox.setPoseAndExtents(getPose(), 2 * Eigen::Vector3d(length2_, width2_, height2_)); -} +// void bodies::Box::computeBoundingBox(bodies::OBB& bbox) const +// { +// bbox.setPoseAndExtents(getPose(), 2 * Eigen::Vector3d(length2_, width2_, height2_)); +// } bool bodies::Box::intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir, EigenSTL::vector_Vector3d* intersections, unsigned int count) const @@ -1174,10 +1174,10 @@ void bodies::ConvexMesh::computeBoundingBox(bodies::AABB& bbox) const bounding_box_.computeBoundingBox(bbox); } -void bodies::ConvexMesh::computeBoundingBox(bodies::OBB& bbox) const -{ - bounding_box_.computeBoundingBox(bbox); -} +// void bodies::ConvexMesh::computeBoundingBox(bodies::OBB& bbox) const +// { +// bounding_box_.computeBoundingBox(bbox); +// } bool bodies::ConvexMesh::isPointInsidePlanes(const Eigen::Vector3d& point) const {