From 126955b1d32ea7c9170e2868844ae5fa35c0ed61 Mon Sep 17 00:00:00 2001 From: seb Date: Fri, 3 May 2024 13:47:26 -0400 Subject: [PATCH 1/3] reversed white space change --- include/geometric_shapes/body_operations.h | 1 + 1 file changed, 1 insertion(+) diff --git a/include/geometric_shapes/body_operations.h b/include/geometric_shapes/body_operations.h index cc6c8810..877c362f 100644 --- a/include/geometric_shapes/body_operations.h +++ b/include/geometric_shapes/body_operations.h @@ -25,6 +25,7 @@ // 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: Ioan Sucan, E. Gil Jones */ #ifndef GEOMETRIC_SHAPES_BODY_OPERATIONS_ From 94ea0df99b6ef93c79efcf56e3dccae2f9ed722e Mon Sep 17 00:00:00 2001 From: seb Date: Fri, 3 May 2024 17:56:33 +0000 Subject: [PATCH 2/3] removed commented depends --- package.xml | 9 +-------- 1 file changed, 1 insertion(+), 8 deletions(-) diff --git a/package.xml b/package.xml index 19b226ac..0df31ecd 100644 --- a/package.xml +++ b/package.xml @@ -36,14 +36,7 @@ visualization_msgs assimp-dev - - - - - + eigen libfcl-dev pkg-config libboost-dev From 48bba6f1a4fed830a89caeb22d0196b5d0501fb0 Mon Sep 17 00:00:00 2001 From: seb Date: Fri, 3 May 2024 18:38:23 +0000 Subject: [PATCH 3/3] toFCL fromFCL removed, FCL 0.5 checks removed --- src/aabb.cpp | 9 --------- src/obb.cpp | 52 +++------------------------------------------------- 2 files changed, 3 insertions(+), 58 deletions(-) diff --git a/src/aabb.cpp b/src/aabb.cpp index 45c09200..5c31a62b 100644 --- a/src/aabb.cpp +++ b/src/aabb.cpp @@ -31,13 +31,10 @@ #include #include -#if FCL_MAJOR_VERSION > 0 || FCL_MINOR_VERSION > 5 #include -#endif void bodies::AABB::extendWithTransformedBox(const Eigen::Isometry3d& transform, const Eigen::Vector3d& box) { -#if FCL_MAJOR_VERSION == 0 && FCL_MINOR_VERSION == 5 // 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 in FCL 0.5 @@ -53,10 +50,4 @@ void bodies::AABB::extendWithTransformedBox(const Eigen::Isometry3d& transform, const Eigen::Vector3d v_delta(x_range, y_range, z_range); extend(t + v_delta); extend(t - v_delta); -#else - fcl::AABBd aabb; - fcl::computeBV(fcl::Boxd(box), transform, aabb); - extend(aabb.min_); - extend(aabb.max_); -#endif } diff --git a/src/obb.cpp b/src/obb.cpp index 885e48e1..3afebcea 100644 --- a/src/obb.cpp +++ b/src/obb.cpp @@ -2,37 +2,12 @@ #include -#if FCL_MAJOR_VERSION == 0 && FCL_MINOR_VERSION == 5 -#include -typedef fcl::Vec3f FCL_Vec3; -typedef fcl::OBB FCL_OBB; -#else #include typedef fcl::Vector3d FCL_Vec3; typedef fcl::OBB FCL_OBB; -#endif namespace bodies { -#if FCL_MAJOR_VERSION == 0 && FCL_MINOR_VERSION == 5 -inline FCL_Vec3 toFcl(const Eigen::Vector3d& eigenVec) -{ - FCL_Vec3 result; - Eigen::Map(result.data.vs, 3, 1) = eigenVec; - return result; -} -#else -#define toFcl -#endif - -#if FCL_MAJOR_VERSION == 0 && FCL_MINOR_VERSION == 5 -Eigen::Vector3d fromFcl(const FCL_Vec3& fclVec) -{ - return Eigen::Map(fclVec.data.vs, 3, 1); -} -#else -#define fromFcl -#endif class OBBPrivate : public FCL_OBB { @@ -44,17 +19,10 @@ OBB::OBB() { obb_.reset(new OBBPrivate); // Initialize the OBB to position 0, with 0 extents and identity rotation -#if FCL_MAJOR_VERSION > 0 || FCL_MINOR_VERSION > 5 // FCL 0.6+ does not zero-initialize the OBB. obb_->extent.setZero(); obb_->To.setZero(); obb_->axis.setIdentity(); -#else - // FCL 0.5 zero-initializes the OBB, so we just put the identity into orientation. - obb_->axis[0][0] = 1.0; - obb_->axis[1][1] = 1.0; - obb_->axis[2][2] = 1.0; -#endif } OBB::OBB(const OBB& other) @@ -78,22 +46,14 @@ void OBB::setPoseAndExtents(const Eigen::Isometry3d& pose, const Eigen::Vector3d { const auto rotation = pose.linear(); -#if FCL_MAJOR_VERSION == 0 && FCL_MINOR_VERSION == 5 - obb_->axis[0] = toFcl(rotation.col(0)); - obb_->axis[1] = toFcl(rotation.col(1)); - obb_->axis[2] = toFcl(rotation.col(2)); -#else obb_->axis = rotation; -#endif - - obb_->To = toFcl(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 * fromFcl(obb_->extent); + extents = 2 * obb_->extent; } Eigen::Vector3d OBB::getExtents() const @@ -106,14 +66,8 @@ Eigen::Vector3d OBB::getExtents() const void OBB::getPose(Eigen::Isometry3d& pose) const { pose = Eigen::Isometry3d::Identity(); - pose.translation() = fromFcl(obb_->To); -#if FCL_MAJOR_VERSION == 0 && FCL_MINOR_VERSION == 5 - pose.linear().col(0) = fromFcl(obb_->axis[0]); - pose.linear().col(1) = fromFcl(obb_->axis[1]); - pose.linear().col(2) = fromFcl(obb_->axis[2]); -#else + pose.translation() = obb_->To; pose.linear() = obb_->axis; -#endif } Eigen::Isometry3d OBB::getPose() const @@ -158,7 +112,7 @@ OBB* OBB::extendApprox(const OBB& box) bool OBB::contains(const Eigen::Vector3d& point) const { - return obb_->contain(toFcl(point)); + return obb_->contain(point); } bool OBB::overlaps(const bodies::OBB& other) const