diff --git a/CMakeLists.txt b/CMakeLists.txt index c0b0ed08..aa7505e6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -52,6 +52,7 @@ find_package(rclcpp REQUIRED) find_package(resource_retriever REQUIRED) find_package(shape_msgs REQUIRED) find_package(visualization_msgs REQUIRED) +find_package(fcl REQUIRED) include(ConfigExtras) set(THIS_PACKAGE_EXPORT_DEPENDS @@ -80,6 +81,7 @@ add_library(${PROJECT_NAME} SHARED src/bodies.cpp src/body_operations.cpp src/mesh_operations.cpp + src/obb.cpp src/shape_extents.cpp src/shape_operations.cpp src/shape_to_marker.cpp @@ -87,6 +89,7 @@ add_library(${PROJECT_NAME} SHARED ) set_target_properties(${PROJECT_NAME} PROPERTIES VERSION ${${PROJECT_NAME}_VERSION} WINDOWS_EXPORT_ALL_SYMBOLS TRUE) target_compile_options(${PROJECT_NAME} PRIVATE ${PROJECT_COMPILE_OPTIONS}) +target_link_libraries(${PROJECT_NAME} fcl) ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_EXPORT_DEPENDS} ) diff --git a/include/geometric_shapes/bodies.h b/include/geometric_shapes/bodies.h index c8bf1ec0..c08a363f 100644 --- a/include/geometric_shapes/bodies.h +++ b/include/geometric_shapes/bodies.h @@ -33,6 +33,7 @@ #define _USE_MATH_DEFINES #include "geometric_shapes/aabb.h" +#include "geometric_shapes/obb.h" #include "geometric_shapes/shapes.h" #include #include @@ -240,6 +241,10 @@ class Body pose. Scaling and padding are accounted for. */ 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; + /** \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 { @@ -307,6 +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; bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir, EigenSTL::vector_Vector3d* intersections = nullptr, unsigned int count = 0) const override; @@ -359,6 +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; bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir, EigenSTL::vector_Vector3d* intersections = nullptr, unsigned int count = 0) const override; @@ -421,6 +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; bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir, EigenSTL::vector_Vector3d* intersections = nullptr, unsigned int count = 0) const override; @@ -483,6 +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; bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir, EigenSTL::vector_Vector3d* intersections = nullptr, unsigned int count = 0) const override; diff --git a/include/geometric_shapes/body_operations.h b/include/geometric_shapes/body_operations.h index 5e1cf984..877c362f 100644 --- a/include/geometric_shapes/body_operations.h +++ b/include/geometric_shapes/body_operations.h @@ -67,6 +67,9 @@ void mergeBoundingSpheres(const std::vector& spheres, BoundingSp /** \brief Compute an axis-aligned bounding box to enclose a set of bounding boxes. */ void mergeBoundingBoxes(const std::vector& boxes, AABB& mergedBox); +/** \brief Compute an approximate oriented bounding box to enclose a set of bounding boxes. */ +void mergeBoundingBoxesApprox(const std::vector& boxes, OBB& mergedBox); + /** \brief Compute the bounding sphere for a set of \e bodies and store the resulting sphere in \e mergedSphere */ void computeBoundingSphere(const std::vector& bodies, BoundingSphere& mergedSphere); } // namespace bodies diff --git a/include/geometric_shapes/obb.h b/include/geometric_shapes/obb.h new file mode 100644 index 00000000..f7b700c0 --- /dev/null +++ b/include/geometric_shapes/obb.h @@ -0,0 +1,142 @@ +// Copyright 2024 Open Robotics +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Open Robotics nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +/* Author: Martin Pecka */ + +#ifndef GEOMETRIC_SHAPES_OBB_H +#define GEOMETRIC_SHAPES_OBB_H + +#include + +#include +#include + +#include +#include + +namespace bodies +{ +class OBBPrivate; + +/** \brief Represents an oriented bounding box. */ +class OBB +{ +public: + /** \brief Initialize an oriented bounding box at position 0, with 0 extents and + * identity orientation. */ + OBB(); + OBB(const OBB& other); + OBB(const Eigen::Isometry3d& pose, const Eigen::Vector3d& extents); + virtual ~OBB(); + + OBB& operator=(const OBB& other); + + /** + * \brief Set both the pose and extents of the OBB. + * \param [in] pose New pose of the OBB. + * \param [in] extents New extents of the OBB. + */ + void setPoseAndExtents(const Eigen::Isometry3d& pose, const Eigen::Vector3d& extents); + + /** + * \brief Get the extents of the OBB. + * \return The extents. + */ + Eigen::Vector3d getExtents() const; + + /** + * \brief Get the extents of the OBB. + * \param extents [out] The extents. + */ + void getExtents(Eigen::Vector3d& extents) const; + + /** + * \brief Get the pose of the OBB. + * \return The pose. + */ + Eigen::Isometry3d getPose() const; + + /** + * \brief Get The pose of the OBB. + * \param pose The pose. + */ + void getPose(Eigen::Isometry3d& pose) const; + + /** + * \brief Convert this OBB to an axis-aligned BB. + * \return The AABB. + */ + AABB toAABB() const; + + /** + * \brief Convert this OBB to an axis-aligned BB. + * \param aabb The AABB. + */ + void toAABB(AABB& aabb) const; + + /** + * \brief Add the other OBB to this one and compute an approximate enclosing OBB. + * \param box The other box to add. + * \return Pointer to this OBB after the update. + */ + OBB* extendApprox(const OBB& box); + + /** + * \brief Check if this OBB contains the given point. + * \param point The point to check. + * \return Whether the point is inside or not. + */ + bool contains(const Eigen::Vector3d& point) const; + + /** + * \brief Check whether this and the given OBBs have nonempty intersection. + * \param other The other OBB to check. + * \return Whether the OBBs overlap. + */ + bool overlaps(const OBB& other) const; + + /** + * \brief Check if this OBB contains whole other OBB. + * \param point The point to check. + * \return Whether the point is inside or not. + */ + bool contains(const OBB& obb) const; + + /** + * \brief Compute coordinates of the 8 vertices of this OBB. + * \return The vertices. + */ + EigenSTL::vector_Vector3d computeVertices() const; + +protected: + /** \brief PIMPL pointer */ + std::unique_ptr obb_; +}; +} // namespace bodies + +#endif // GEOMETRIC_SHAPES_OBB_H diff --git a/package.xml b/package.xml index e0d7a676..f4b03620 100644 --- a/package.xml +++ b/package.xml @@ -36,6 +36,8 @@ visualization_msgs assimp-dev + eigen + libfcl-dev pkg-config libboost-dev libboost-filesystem-dev diff --git a/src/aabb.cpp b/src/aabb.cpp index c323e5ff..2956186f 100644 --- a/src/aabb.cpp +++ b/src/aabb.cpp @@ -30,14 +30,15 @@ #include +#include + void bodies::AABB::extendWithTransformedBox(const Eigen::Isometry3d& transform, const Eigen::Vector3d& box) { // Method adapted from FCL src/shape/geometric_shapes_utility.cpp#computeBV(...) (BSD-licensed code): // https://github.com/flexible-collision-library/fcl/blob/fcl-0.4/src/shape/geometric_shapes_utility.cpp#L292 - // We don't call their code because it would need creating temporary objects, and their method is in floats. + // We don't call their code because it would need creating temporary objects, and their method is in floats in FCL 0.5 // // Here's a nice explanation why it works: https://zeuxcg.org/2010/10/17/aabb-from-obb-with-component-wise-abs/ - const Eigen::Matrix3d& r = transform.rotation(); const Eigen::Vector3d& t = transform.translation(); @@ -45,7 +46,8 @@ void bodies::AABB::extendWithTransformedBox(const Eigen::Isometry3d& transform, double y_range = 0.5 * (fabs(r(1, 0) * box[0]) + fabs(r(1, 1) * box[1]) + fabs(r(1, 2) * box[2])); double z_range = 0.5 * (fabs(r(2, 0) * box[0]) + fabs(r(2, 1) * box[1]) + fabs(r(2, 2) * box[2])); - const Eigen::Vector3d v_delta(x_range, y_range, z_range); - extend(t + v_delta); - extend(t - v_delta); + fcl::AABBd aabb; + fcl::computeBV(fcl::Boxd(box), transform, aabb); + extend(aabb.min_); + extend(aabb.max_); } diff --git a/src/bodies.cpp b/src/bodies.cpp index 0742001a..64d22751 100644 --- a/src/bodies.cpp +++ b/src/bodies.cpp @@ -206,6 +206,15 @@ 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(); + + 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 { @@ -427,6 +436,11 @@ 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_)); +} + bool bodies::Cylinder::intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir, EigenSTL::vector_Vector3d* intersections, unsigned int count) const { @@ -663,6 +677,11 @@ 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_)); +} + bool bodies::Box::intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir, EigenSTL::vector_Vector3d* intersections, unsigned int count) const { @@ -1155,6 +1174,11 @@ void bodies::ConvexMesh::computeBoundingBox(bodies::AABB& 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 { unsigned int numplanes = mesh_data_->planes_.size(); diff --git a/src/body_operations.cpp b/src/body_operations.cpp index 559259a6..75f00715 100644 --- a/src/body_operations.cpp +++ b/src/body_operations.cpp @@ -259,3 +259,9 @@ void bodies::mergeBoundingBoxes(const std::vector& boxes, bodies:: for (const auto& box : boxes) mergedBox.extend(box); } + +void bodies::mergeBoundingBoxesApprox(const std::vector& boxes, bodies::OBB& mergedBox) +{ + for (const auto& box : boxes) + mergedBox.extendApprox(box); +} diff --git a/src/obb.cpp b/src/obb.cpp new file mode 100644 index 00000000..10841194 --- /dev/null +++ b/src/obb.cpp @@ -0,0 +1,190 @@ +// Copyright 2024 Open Robotics +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Open Robotics nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include + +#include + +#include +typedef fcl::Vector3d FCL_Vec3; +typedef fcl::OBB FCL_OBB; + +namespace bodies +{ + +class OBBPrivate : public FCL_OBB +{ +public: + using FCL_OBB::OBB; +}; + +OBB::OBB() +{ + obb_.reset(new OBBPrivate); + // Initialize the OBB to position 0, with 0 extents and identity rotation + // FCL 0.6+ does not zero-initialize the OBB. + obb_->extent.setZero(); + obb_->To.setZero(); + obb_->axis.setIdentity(); +} + +OBB::OBB(const OBB& other) +{ + obb_.reset(new OBBPrivate(*other.obb_)); +} + +OBB::OBB(const Eigen::Isometry3d& pose, const Eigen::Vector3d& extents) +{ + obb_.reset(new OBBPrivate); + setPoseAndExtents(pose, extents); +} + +OBB& OBB::operator=(const OBB& other) +{ + *obb_ = *other.obb_; + return *this; +} + +void OBB::setPoseAndExtents(const Eigen::Isometry3d& pose, const Eigen::Vector3d& extents) +{ + const auto rotation = pose.linear(); + + obb_->axis = rotation; + + obb_->To = pose.translation(); + + obb_->extent = { extents[0] / 2.0, extents[1] / 2.0, extents[2] / 2.0 }; +} + +void OBB::getExtents(Eigen::Vector3d& extents) const +{ + extents = 2 * obb_->extent; +} + +Eigen::Vector3d OBB::getExtents() const +{ + Eigen::Vector3d extents; + getExtents(extents); + return extents; +} + +void OBB::getPose(Eigen::Isometry3d& pose) const +{ + pose = Eigen::Isometry3d::Identity(); + pose.translation() = obb_->To; + pose.linear() = obb_->axis; +} + +Eigen::Isometry3d OBB::getPose() const +{ + Eigen::Isometry3d pose; + getPose(pose); + return pose; +} + +AABB OBB::toAABB() const +{ + AABB result; + toAABB(result); + return result; +} + +void OBB::toAABB(AABB& aabb) const +{ + aabb.extendWithTransformedBox(getPose(), getExtents()); +} + +OBB* OBB::extendApprox(const OBB& box) +{ + if (this->getExtents() == Eigen::Vector3d::Zero()) + { + *obb_ = *box.obb_; + return this; + } + + if (this->contains(box)) + return this; + + if (box.contains(*this)) + { + *obb_ = *box.obb_; + return this; + } + + *this->obb_ += *box.obb_; + return this; +} + +bool OBB::contains(const Eigen::Vector3d& point) const +{ + return obb_->contain(point); +} + +bool OBB::overlaps(const bodies::OBB& other) const +{ + return obb_->overlap(*other.obb_); +} + +EigenSTL::vector_Vector3d OBB::computeVertices() const +{ + const Eigen::Vector3d e = getExtents() / 2; // do not use auto type, Eigen would be inefficient + // clang-format off + EigenSTL::vector_Vector3d result = { + { -e[0], -e[1], -e[2] }, + { -e[0], -e[1], e[2] }, + { -e[0], e[1], -e[2] }, + { -e[0], e[1], e[2] }, + { e[0], -e[1], -e[2] }, + { e[0], -e[1], e[2] }, + { e[0], e[1], -e[2] }, + { e[0], e[1], e[2] }, + }; + // clang-format on + + const auto pose = getPose(); + for (auto& v : result) + { + v = pose * v; + } + + return result; +} + +bool OBB::contains(const OBB& obb) const +{ + for (const auto& v : obb.computeVertices()) + { + if (!contains(v)) + return false; + } + return true; +} + +OBB::~OBB() = default; + +} // namespace bodies diff --git a/test/test_bounding_box.cpp b/test/test_bounding_box.cpp index c4b9d5f5..6d85f8fa 100644 --- a/test/test_bounding_box.cpp +++ b/test/test_bounding_box.cpp @@ -41,6 +41,8 @@ TEST(SphereBoundingBox, Sphere1) bodies::Sphere body(&shape); bodies::AABB bbox; body.computeBoundingBox(bbox); + bodies::OBB obbox; + body.computeBoundingBox(obbox); EXPECT_NEAR(-1.0, bbox.min().x(), 1e-4); EXPECT_NEAR(-1.0, bbox.min().y(), 1e-4); @@ -48,6 +50,15 @@ TEST(SphereBoundingBox, Sphere1) EXPECT_NEAR(1.0, bbox.max().x(), 1e-4); EXPECT_NEAR(1.0, bbox.max().y(), 1e-4); EXPECT_NEAR(1.0, bbox.max().z(), 1e-4); + + EXPECT_NEAR(2.0, obbox.getExtents().x(), 1e-4); + EXPECT_NEAR(2.0, obbox.getExtents().y(), 1e-4); + EXPECT_NEAR(2.0, obbox.getExtents().z(), 1e-4); + EXPECT_NEAR(0.0, obbox.getPose().translation().x(), 1e-4); + EXPECT_NEAR(0.0, obbox.getPose().translation().y(), 1e-4); + EXPECT_NEAR(0.0, obbox.getPose().translation().z(), 1e-4); + + EXPECT_TRUE(obbox.getPose().linear().isApprox(Eigen::Matrix3d::Identity(), 1e-4)); } TEST(SphereBoundingBox, Sphere2) @@ -60,6 +71,8 @@ TEST(SphereBoundingBox, Sphere2) body.setPose(pose); bodies::AABB bbox; body.computeBoundingBox(bbox); + bodies::OBB obbox; + body.computeBoundingBox(obbox); EXPECT_NEAR(-1.0, bbox.min().x(), 1e-4); EXPECT_NEAR(0.0, bbox.min().y(), 1e-4); @@ -68,9 +81,16 @@ TEST(SphereBoundingBox, Sphere2) EXPECT_NEAR(4.0, bbox.max().y(), 1e-4); EXPECT_NEAR(5.0, bbox.max().z(), 1e-4); + EXPECT_NEAR(4.0, obbox.getExtents().x(), 1e-4); + EXPECT_NEAR(4.0, obbox.getExtents().y(), 1e-4); + EXPECT_NEAR(4.0, obbox.getExtents().z(), 1e-4); + + EXPECT_TRUE(obbox.getPose().isApprox(pose, 1e-4)); + pose *= Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d(1, 1, 1).normalized()); body.setPose(pose); body.computeBoundingBox(bbox); + body.computeBoundingBox(obbox); EXPECT_NEAR(-1.0, bbox.min().x(), 1e-4); EXPECT_NEAR(0.0, bbox.min().y(), 1e-4); @@ -79,6 +99,14 @@ TEST(SphereBoundingBox, Sphere2) EXPECT_NEAR(4.0, bbox.max().y(), 1e-4); EXPECT_NEAR(5.0, bbox.max().z(), 1e-4); + EXPECT_NEAR(4.0, obbox.getExtents().x(), 1e-4); + EXPECT_NEAR(4.0, obbox.getExtents().y(), 1e-4); + EXPECT_NEAR(4.0, obbox.getExtents().z(), 1e-4); + + // oriented bounding box doesn't rotate with the sphere + EXPECT_TRUE(obbox.getPose().translation().isApprox(pose.translation(), 1e-4)); + EXPECT_TRUE(obbox.getPose().linear().isApprox(Eigen::Matrix3d::Identity(), 1e-4)); + // verify the bounding box is rotation-invariant random_numbers::RandomNumberGenerator gen; @@ -95,7 +123,9 @@ TEST(SphereBoundingBox, Sphere2) pose.linear() = quat.toRotationMatrix(); body.setPose(pose); bodies::AABB bbox2; + bodies::OBB obbox2; body.computeBoundingBox(bbox2); + body.computeBoundingBox(obbox2); EXPECT_NEAR(bbox2.min().x(), bbox.min().x(), 1e-4); EXPECT_NEAR(bbox2.min().y(), bbox.min().y(), 1e-4); @@ -103,6 +133,12 @@ TEST(SphereBoundingBox, Sphere2) EXPECT_NEAR(bbox2.max().x(), bbox.max().x(), 1e-4); EXPECT_NEAR(bbox2.max().y(), bbox.max().y(), 1e-4); EXPECT_NEAR(bbox2.max().z(), bbox.max().z(), 1e-4); + + EXPECT_NEAR(obbox.getExtents().x(), obbox2.getExtents().x(), 1e-4); + EXPECT_NEAR(obbox.getExtents().y(), obbox2.getExtents().y(), 1e-4); + EXPECT_NEAR(obbox.getExtents().z(), obbox2.getExtents().z(), 1e-4); + + EXPECT_TRUE(obbox2.getPose().isApprox(obbox.getPose(), 1e-4)); } } @@ -112,6 +148,8 @@ TEST(BoxBoundingBox, Box1) bodies::Box body(&shape); bodies::AABB bbox; body.computeBoundingBox(bbox); + bodies::OBB obbox; + body.computeBoundingBox(obbox); EXPECT_NEAR(-0.5, bbox.min().x(), 1e-4); EXPECT_NEAR(-1.0, bbox.min().y(), 1e-4); @@ -119,6 +157,15 @@ TEST(BoxBoundingBox, Box1) EXPECT_NEAR(0.5, bbox.max().x(), 1e-4); EXPECT_NEAR(1.0, bbox.max().y(), 1e-4); EXPECT_NEAR(1.5, bbox.max().z(), 1e-4); + + EXPECT_NEAR(1.0, obbox.getExtents().x(), 1e-4); + EXPECT_NEAR(2.0, obbox.getExtents().y(), 1e-4); + EXPECT_NEAR(3.0, obbox.getExtents().z(), 1e-4); + EXPECT_NEAR(0.0, obbox.getPose().translation().x(), 1e-4); + EXPECT_NEAR(0.0, obbox.getPose().translation().y(), 1e-4); + EXPECT_NEAR(0.0, obbox.getPose().translation().z(), 1e-4); + + EXPECT_TRUE(obbox.getPose().linear().isApprox(Eigen::Matrix3d::Identity(), 1e-4)); } TEST(BoxBoundingBox, Box2) @@ -131,6 +178,8 @@ TEST(BoxBoundingBox, Box2) body.setPose(pose); bodies::AABB bbox; body.computeBoundingBox(bbox); + bodies::OBB obbox; + body.computeBoundingBox(obbox); EXPECT_NEAR(0.5, bbox.min().x(), 1e-4); EXPECT_NEAR(1.0, bbox.min().y(), 1e-4); @@ -139,9 +188,19 @@ TEST(BoxBoundingBox, Box2) EXPECT_NEAR(3.0, bbox.max().y(), 1e-4); EXPECT_NEAR(4.5, bbox.max().z(), 1e-4); + EXPECT_NEAR(1.0, obbox.getExtents().x(), 1e-4); + EXPECT_NEAR(2.0, obbox.getExtents().y(), 1e-4); + EXPECT_NEAR(3.0, obbox.getExtents().z(), 1e-4); + EXPECT_NEAR(1.0, obbox.getPose().translation().x(), 1e-4); + EXPECT_NEAR(2.0, obbox.getPose().translation().y(), 1e-4); + EXPECT_NEAR(3.0, obbox.getPose().translation().z(), 1e-4); + + EXPECT_TRUE(obbox.getPose().linear().isApprox(Eigen::Matrix3d::Identity(), 1e-4)); + pose *= Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d(1, 1, 1).normalized()); body.setPose(pose); body.computeBoundingBox(bbox); + body.computeBoundingBox(obbox); EXPECT_NEAR(-0.7767, bbox.min().x(), 1e-4); EXPECT_NEAR(0.8452, bbox.min().y(), 1e-4); @@ -149,6 +208,15 @@ TEST(BoxBoundingBox, Box2) EXPECT_NEAR(2.7767, bbox.max().x(), 1e-4); EXPECT_NEAR(3.1547, bbox.max().y(), 1e-4); EXPECT_NEAR(4.5326, bbox.max().z(), 1e-4); + + EXPECT_NEAR(1.0, obbox.getExtents().x(), 1e-4); + EXPECT_NEAR(2.0, obbox.getExtents().y(), 1e-4); + EXPECT_NEAR(3.0, obbox.getExtents().z(), 1e-4); + EXPECT_NEAR(1.0, obbox.getPose().translation().x(), 1e-4); + EXPECT_NEAR(2.0, obbox.getPose().translation().y(), 1e-4); + EXPECT_NEAR(3.0, obbox.getPose().translation().z(), 1e-4); + + EXPECT_TRUE(obbox.getPose().linear().isApprox(pose.linear(), 1e-4)); } TEST(CylinderBoundingBox, Cylinder1) @@ -157,6 +225,8 @@ TEST(CylinderBoundingBox, Cylinder1) bodies::Cylinder body(&shape); bodies::AABB bbox; body.computeBoundingBox(bbox); + bodies::OBB obbox; + body.computeBoundingBox(obbox); EXPECT_NEAR(-1.0, bbox.min().x(), 1e-4); EXPECT_NEAR(-1.0, bbox.min().y(), 1e-4); @@ -164,6 +234,15 @@ TEST(CylinderBoundingBox, Cylinder1) EXPECT_NEAR(1.0, bbox.max().x(), 1e-4); EXPECT_NEAR(1.0, bbox.max().y(), 1e-4); EXPECT_NEAR(1.0, bbox.max().z(), 1e-4); + + EXPECT_NEAR(2.0, obbox.getExtents().x(), 1e-4); + EXPECT_NEAR(2.0, obbox.getExtents().y(), 1e-4); + EXPECT_NEAR(2.0, obbox.getExtents().z(), 1e-4); + EXPECT_NEAR(0.0, obbox.getPose().translation().x(), 1e-4); + EXPECT_NEAR(0.0, obbox.getPose().translation().y(), 1e-4); + EXPECT_NEAR(0.0, obbox.getPose().translation().z(), 1e-4); + + EXPECT_TRUE(obbox.getPose().linear().isApprox(Eigen::Matrix3d::Identity(), 1e-4)); } TEST(CylinderBoundingBox, Cylinder2) @@ -176,6 +255,8 @@ TEST(CylinderBoundingBox, Cylinder2) body.setPose(pose); bodies::AABB bbox; body.computeBoundingBox(bbox); + bodies::OBB obbox; + body.computeBoundingBox(obbox); EXPECT_NEAR(0.0, bbox.min().x(), 1e-4); EXPECT_NEAR(1.0, bbox.min().y(), 1e-4); @@ -184,9 +265,19 @@ TEST(CylinderBoundingBox, Cylinder2) EXPECT_NEAR(3.0, bbox.max().y(), 1e-4); EXPECT_NEAR(4.0, bbox.max().z(), 1e-4); + EXPECT_NEAR(2.0, obbox.getExtents().x(), 1e-4); + EXPECT_NEAR(2.0, obbox.getExtents().y(), 1e-4); + EXPECT_NEAR(2.0, obbox.getExtents().z(), 1e-4); + EXPECT_NEAR(1.0, obbox.getPose().translation().x(), 1e-4); + EXPECT_NEAR(2.0, obbox.getPose().translation().y(), 1e-4); + EXPECT_NEAR(3.0, obbox.getPose().translation().z(), 1e-4); + + EXPECT_TRUE(obbox.getPose().linear().isApprox(Eigen::Matrix3d::Identity(), 1e-4)); + pose *= Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d(1, 1, 1).normalized()); body.setPose(pose); body.computeBoundingBox(bbox); + body.computeBoundingBox(obbox); EXPECT_NEAR(-0.3238, bbox.min().x(), 1e-4); EXPECT_NEAR(0.7862, bbox.min().y(), 1e-4); @@ -195,6 +286,15 @@ TEST(CylinderBoundingBox, Cylinder2) EXPECT_NEAR(3.2138, bbox.max().y(), 1e-4); EXPECT_NEAR(4.2761, bbox.max().z(), 1e-4); + EXPECT_NEAR(2.0, obbox.getExtents().x(), 1e-4); + EXPECT_NEAR(2.0, obbox.getExtents().y(), 1e-4); + EXPECT_NEAR(2.0, obbox.getExtents().z(), 1e-4); + EXPECT_NEAR(1.0, obbox.getPose().translation().x(), 1e-4); + EXPECT_NEAR(2.0, obbox.getPose().translation().y(), 1e-4); + EXPECT_NEAR(3.0, obbox.getPose().translation().z(), 1e-4); + + EXPECT_TRUE(obbox.getPose().linear().isApprox(pose.linear(), 1e-4)); + // verify the bounding box is yaw-invariant random_numbers::RandomNumberGenerator gen(0); @@ -206,6 +306,7 @@ TEST(CylinderBoundingBox, Cylinder2) body.computeBoundingBox(bbox); bodies::AABB bbox2; + bodies::OBB obbox2; for (size_t i = 0; i < 10; ++i) { const auto angle = gen.uniformReal(-M_PI, M_PI); @@ -213,6 +314,7 @@ TEST(CylinderBoundingBox, Cylinder2) pose.linear() = (rollPitch * yaw).toRotationMatrix(); body.setPose(pose); body.computeBoundingBox(bbox2); + body.computeBoundingBox(obbox2); EXPECT_NEAR(bbox2.min().x(), bbox.min().x(), 1e-4); EXPECT_NEAR(bbox2.min().y(), bbox.min().y(), 1e-4); @@ -220,6 +322,16 @@ TEST(CylinderBoundingBox, Cylinder2) EXPECT_NEAR(bbox2.max().x(), bbox.max().x(), 1e-4); EXPECT_NEAR(bbox2.max().y(), bbox.max().y(), 1e-4); EXPECT_NEAR(bbox2.max().z(), bbox.max().z(), 1e-4); + + EXPECT_NEAR(2.0, obbox2.getExtents().x(), 1e-4); + EXPECT_NEAR(2.0, obbox2.getExtents().y(), 1e-4); + EXPECT_NEAR(2.0, obbox2.getExtents().z(), 1e-4); + EXPECT_NEAR(1.0, obbox2.getPose().translation().x(), 1e-4); + EXPECT_NEAR(2.0, obbox2.getPose().translation().y(), 1e-4); + EXPECT_NEAR(3.0, obbox2.getPose().translation().z(), 1e-4); + + // oriented bounding boxes are not yaw-invariant + EXPECT_TRUE(obbox2.getPose().linear().isApprox(pose.linear(), 1e-4)); } } @@ -317,6 +429,8 @@ TEST(MeshBoundingBox, Mesh1) bodies::ConvexMesh body(m); bodies::AABB bbox; body.computeBoundingBox(bbox); + bodies::OBB obbox; + body.computeBoundingBox(obbox); EXPECT_NEAR(-1.0, bbox.min().x(), 1e-4); EXPECT_NEAR(-1.0, bbox.min().y(), 1e-4); @@ -324,6 +438,15 @@ TEST(MeshBoundingBox, Mesh1) EXPECT_NEAR(1.0, bbox.max().x(), 1e-4); EXPECT_NEAR(1.0, bbox.max().y(), 1e-4); EXPECT_NEAR(1.0, bbox.max().z(), 1e-4); + + EXPECT_NEAR(2.0, obbox.getExtents().x(), 1e-4); + EXPECT_NEAR(2.0, obbox.getExtents().y(), 1e-4); + EXPECT_NEAR(2.0, obbox.getExtents().z(), 1e-4); + EXPECT_NEAR(0.0, obbox.getPose().translation().x(), 1e-4); + EXPECT_NEAR(0.0, obbox.getPose().translation().y(), 1e-4); + EXPECT_NEAR(0.0, obbox.getPose().translation().z(), 1e-4); + + EXPECT_TRUE(obbox.getPose().linear().isApprox(Eigen::Matrix3d::Identity(), 1e-4)); delete m; } @@ -338,6 +461,8 @@ TEST(MeshBoundingBox, Mesh2) body.setPose(pose); bodies::AABB bbox; body.computeBoundingBox(bbox); + bodies::OBB obbox; + body.computeBoundingBox(obbox); EXPECT_NEAR(0.5, bbox.min().x(), 1e-4); EXPECT_NEAR(1.0, bbox.min().y(), 1e-4); @@ -346,9 +471,19 @@ TEST(MeshBoundingBox, Mesh2) EXPECT_NEAR(3.0, bbox.max().y(), 1e-4); EXPECT_NEAR(4.5, bbox.max().z(), 1e-4); + EXPECT_NEAR(1.0, obbox.getExtents().x(), 1e-4); + EXPECT_NEAR(2.0, obbox.getExtents().y(), 1e-4); + EXPECT_NEAR(3.0, obbox.getExtents().z(), 1e-4); + EXPECT_NEAR(1.0, obbox.getPose().translation().x(), 1e-4); + EXPECT_NEAR(2.0, obbox.getPose().translation().y(), 1e-4); + EXPECT_NEAR(3.0, obbox.getPose().translation().z(), 1e-4); + + EXPECT_TRUE(obbox.getPose().linear().isApprox(Eigen::Matrix3d::Identity(), 1e-4)); + pose *= Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d(1, 1, 1).normalized()); body.setPose(pose); body.computeBoundingBox(bbox); + body.computeBoundingBox(obbox); EXPECT_NEAR(-0.7767, bbox.min().x(), 1e-4); EXPECT_NEAR(0.8452, bbox.min().y(), 1e-4); @@ -357,6 +492,15 @@ TEST(MeshBoundingBox, Mesh2) EXPECT_NEAR(3.1547, bbox.max().y(), 1e-4); EXPECT_NEAR(4.5326, bbox.max().z(), 1e-4); + EXPECT_NEAR(1.0, obbox.getExtents().x(), 1e-4); + EXPECT_NEAR(2.0, obbox.getExtents().y(), 1e-4); + EXPECT_NEAR(3.0, obbox.getExtents().z(), 1e-4); + EXPECT_NEAR(1.0, obbox.getPose().translation().x(), 1e-4); + EXPECT_NEAR(2.0, obbox.getPose().translation().y(), 1e-4); + EXPECT_NEAR(3.0, obbox.getPose().translation().z(), 1e-4); + + EXPECT_TRUE(obbox.getPose().linear().isApprox(pose.linear(), 1e-4)); + delete m; } @@ -377,6 +521,137 @@ TEST(MergeBoundingBoxes, Merge1) EXPECT_NEAR(1.0, bbox.max().z(), 1e-4); } +TEST(MergeBoundingBoxes, OBBInvalid) +{ + auto pose = Eigen::Isometry3d::Identity(); + pose.linear() = Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d(1, 1, 1).normalized()).matrix(); + + bodies::OBB b1; // uninitialized OBB, extents == 0 and invalid rotation + pose.translation() = -0.6 * Eigen::Vector3d::Ones(); + bodies::OBB b2(pose, Eigen::Vector3d(0.1, 0.1, 0.1)); + + // invalid b1 extended with valid b2 should equal b2 + b1.extendApprox(b2); + + EXPECT_TRUE(b1.overlaps(b2)); + EXPECT_TRUE(b2.overlaps(b1)); + + EXPECT_NEAR(0.1, b1.getExtents().x(), 1e-12); + EXPECT_NEAR(0.1, b1.getExtents().y(), 1e-12); + EXPECT_NEAR(0.1, b1.getExtents().z(), 1e-12); + EXPECT_NEAR(-0.6, b1.getPose().translation().x(), 1e-12); + EXPECT_NEAR(-0.6, b1.getPose().translation().y(), 1e-12); + EXPECT_NEAR(-0.6, b1.getPose().translation().z(), 1e-12); + EXPECT_TRUE(b1.getPose().linear().isApprox(pose.linear(), 1e-12)); +} + +TEST(MergeBoundingBoxes, OBBContains1) +{ + auto pose = Eigen::Isometry3d::Identity(); + + pose.translation() = -0.5 * Eigen::Vector3d::Ones(); + bodies::OBB b1(pose, Eigen::Vector3d(1, 1, 1)); + pose.translation() = -0.6 * Eigen::Vector3d::Ones(); + pose.linear() = Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d(1, 1, 1).normalized()).matrix(); + bodies::OBB b2(pose, Eigen::Vector3d(0.1, 0.1, 0.1)); + + EXPECT_TRUE(b1.contains(b2)); + EXPECT_FALSE(b2.contains(b1)); + EXPECT_TRUE(b1.overlaps(b2)); + EXPECT_TRUE(b2.overlaps(b1)); + + // b1 contains whole b2, so the extended b1 should be equal to the original b1 + b1.extendApprox(b2); + + EXPECT_TRUE(b1.contains(b2)); + EXPECT_FALSE(b2.contains(b1)); + EXPECT_TRUE(b1.overlaps(b2)); + EXPECT_TRUE(b2.overlaps(b1)); + + auto u = b1.computeVertices(); + auto v = b2.computeVertices(); + EXPECT_NEAR(1, b1.getExtents().x(), 1e-12); + EXPECT_NEAR(1, b1.getExtents().y(), 1e-12); + EXPECT_NEAR(1, b1.getExtents().z(), 1e-12); + EXPECT_NEAR(-0.5, b1.getPose().translation().x(), 1e-12); + EXPECT_NEAR(-0.5, b1.getPose().translation().y(), 1e-12); + EXPECT_NEAR(-0.5, b1.getPose().translation().z(), 1e-12); + EXPECT_TRUE(b1.getPose().linear().isApprox(Eigen::Matrix3d::Identity(), 1e-12)); +} + +TEST(MergeBoundingBoxes, OBBContains2) +{ + auto pose = Eigen::Isometry3d::Identity(); + + pose.translation() = -0.5 * Eigen::Vector3d::Ones(); + bodies::OBB b1(pose, Eigen::Vector3d(1, 1, 1)); + pose.translation() = -0.6 * Eigen::Vector3d::Ones(); + pose.linear() = Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d(1, 1, 1).normalized()).matrix(); + bodies::OBB b2(pose, Eigen::Vector3d(0.1, 0.1, 0.1)); + + EXPECT_TRUE(b1.contains(b2)); + EXPECT_FALSE(b2.contains(b1)); + EXPECT_TRUE(b1.overlaps(b2)); + EXPECT_TRUE(b2.overlaps(b1)); + + // b1 contains whole b2, so the extended b2 should be equal to the original b1 + b2.extendApprox(b1); + + EXPECT_TRUE(b1.overlaps(b2)); + EXPECT_TRUE(b2.overlaps(b1)); + + EXPECT_NEAR(1, b2.getExtents().x(), 1e-12); + EXPECT_NEAR(1, b2.getExtents().y(), 1e-12); + EXPECT_NEAR(1, b2.getExtents().z(), 1e-12); + EXPECT_NEAR(-0.5, b2.getPose().translation().x(), 1e-12); + EXPECT_NEAR(-0.5, b2.getPose().translation().y(), 1e-12); + EXPECT_NEAR(-0.5, b2.getPose().translation().z(), 1e-12); + EXPECT_TRUE(b2.getPose().linear().isApprox(Eigen::Matrix3d::Identity(), 1e-12)); +} + +TEST(MergeBoundingBoxes, OBBApprox1) +{ + std::vector boxes; + auto pose = Eigen::Isometry3d::Identity(); + + pose.translation() = -0.5 * Eigen::Vector3d::Ones(); + boxes.emplace_back(pose, Eigen::Vector3d(1, 1, 1)); + pose.translation() = 0.5 * Eigen::Vector3d::Ones(); + boxes.emplace_back(pose, Eigen::Vector3d(1, 1, 1)); + + bodies::OBB bbox; + // check that the empty constructor constructs a valid empty OBB + EXPECT_EQ(0.0, bbox.getPose().translation().x()); + EXPECT_EQ(0.0, bbox.getPose().translation().y()); + EXPECT_EQ(0.0, bbox.getPose().translation().z()); + EXPECT_EQ(0.0, bbox.getExtents().x()); + EXPECT_EQ(0.0, bbox.getExtents().y()); + EXPECT_EQ(0.0, bbox.getExtents().z()); + EXPECT_TRUE(bbox.getPose().rotation().isApprox(Eigen::Matrix3d::Identity())); + + bodies::mergeBoundingBoxesApprox(boxes, bbox); + + // the resulting bounding box might not be tight, so we only do some sanity checks + + EXPECT_GE(2.1, bbox.getExtents().x()); + EXPECT_GE(2.1, bbox.getExtents().y()); + EXPECT_GE(2.1, bbox.getExtents().z()); + EXPECT_GE(0.1, bbox.getPose().translation().x()); + EXPECT_GE(0.1, bbox.getPose().translation().y()); + EXPECT_GE(0.1, bbox.getPose().translation().z()); + EXPECT_LE(2.0, bbox.getExtents().x()); + EXPECT_LE(2.0, bbox.getExtents().y()); + EXPECT_LE(2.0, bbox.getExtents().z()); + EXPECT_LE(-0.1, bbox.getPose().translation().x()); + EXPECT_LE(-0.1, bbox.getPose().translation().y()); + EXPECT_LE(-0.1, bbox.getPose().translation().z()); + + EXPECT_TRUE(bbox.contains(boxes[0].getPose().translation())); + EXPECT_TRUE(bbox.contains(boxes[1].getPose().translation())); + EXPECT_TRUE(bbox.overlaps(boxes[0])); + EXPECT_TRUE(bbox.overlaps(boxes[1])); +} + int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv);