From 612d41231e471faf527cdea2ca804263e058603d Mon Sep 17 00:00:00 2001 From: Louis Montaut Date: Sun, 21 Apr 2024 20:58:56 +0200 Subject: [PATCH 1/8] BroadPhase: handle plane/halfspace in `DynamicAABBTree` --- include/hpp/fcl/BV/AABB.h | 33 ++++++++++++++ .../broadphase_dynamic_AABB_tree.cpp | 43 +++++++++++++++++-- 2 files changed, 73 insertions(+), 3 deletions(-) diff --git a/include/hpp/fcl/BV/AABB.h b/include/hpp/fcl/BV/AABB.h index ddd1d1a8a..7e4215a22 100644 --- a/include/hpp/fcl/BV/AABB.h +++ b/include/hpp/fcl/BV/AABB.h @@ -119,6 +119,39 @@ class HPP_FCL_DLLAPI AABB { return true; } + /// @brief Check whether AABB overlaps a plane defined by (normal, offset) + /// TODO: deal with inflation + inline bool overlapPlane(const Vec3f& normal, const FCL_REAL offset) const { + // Convert AABB to a (box, transform) representation and compute the support + // points in the directions normal and -normal. + // If both points lie on different sides of the plane, there is an overlap + // between the AABB and the plane. Otherwise, there is no overlap. + Vec3f halfside = (this->max_ - this->min_) / 2; + Vec3f center = (this->max_ + this->min_) / 2; + Vec3f support1 = (normal.array() > 0).select(halfside, -halfside) + center; + Vec3f support2 = + ((-normal).array() > 0).select(halfside, -halfside) + center; + int sign1 = (normal.dot(support1) - offset > 0) ? 1 : -1; + int sign2 = (normal.dot(support2) - offset > 0) ? 1 : -1; + return (sign1 != sign2); + } + + /// @brief Check whether AABB overlaps a halfspace defined by (normal, offset) + /// TODO: deal with inflation + inline bool overlapHalfspace(const Vec3f& normal, + const FCL_REAL offset) const { + // Convert AABB to a (box, transform) representation and compute the support + // points in the direction -normal. + // If the support is below the plane defined by the halfspace, there is an + // overlap between the AABB and the halfspace. Otherwise, there is no + // overlap. + Vec3f halfside = (this->max_ - this->min_) / 2; + Vec3f center = (this->max_ + this->min_) / 2; + Vec3f support = + ((-normal).array() > 0).select(halfside, -halfside) + center; + return ((normal.dot(support) - offset) < 0); + } + /// @brief Check whether two AABB are overlap bool overlap(const AABB& other, const CollisionRequest& request, FCL_REAL& sqrDistLowerBound) const; diff --git a/src/broadphase/broadphase_dynamic_AABB_tree.cpp b/src/broadphase/broadphase_dynamic_AABB_tree.cpp index 9338cc424..ea3025162 100644 --- a/src/broadphase/broadphase_dynamic_AABB_tree.cpp +++ b/src/broadphase/broadphase_dynamic_AABB_tree.cpp @@ -253,9 +253,46 @@ bool collisionRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, DynamicAABBTreeCollisionManager::DynamicAABBNode* root2, CollisionCallBackBase* callback) { if (root1->isLeaf() && root2->isLeaf()) { - if (!root1->bv.overlap(root2->bv)) return false; - return (*callback)(static_cast(root1->data), - static_cast(root2->data)); + CollisionObject* o1 = static_cast(root1->data); + CollisionObject* o2 = static_cast(root2->data); + bool overlap = false; + if (o1->getNodeType() == GEOM_HALFSPACE || + o1->getNodeType() == GEOM_PLANE) { + if (o1->getNodeType() == GEOM_HALFSPACE) { + const auto& halfspace = + static_cast(*(o1->collisionGeometryPtr())); + const Halfspace new_halfspace = + transform(halfspace, o1->getTransform()); + overlap = root2->bv.overlapHalfspace(new_halfspace.n, new_halfspace.d); + } else { + const auto& plane = + static_cast(*(o1->collisionGeometryPtr())); + const Plane new_plane = transform(plane, o1->getTransform()); + overlap = root2->bv.overlapPlane(new_plane.n, new_plane.d); + } + } else if (o2->getNodeType() == GEOM_HALFSPACE || + o2->getNodeType() == GEOM_PLANE) { + if (o2->getNodeType() == GEOM_HALFSPACE) { + const auto& halfspace = + static_cast(*(o2->collisionGeometryPtr())); + const Halfspace new_halfspace = + transform(halfspace, o2->getTransform()); + overlap = root1->bv.overlapHalfspace(new_halfspace.n, new_halfspace.d); + } else { + const auto& plane = + static_cast(*(o2->collisionGeometryPtr())); + const Plane new_plane = transform(plane, o2->getTransform()); + overlap = root1->bv.overlapPlane(new_plane.n, new_plane.d); + } + } else { + overlap = root1->bv.overlap(root2->bv); + } + + if (overlap) { + return (*callback)(static_cast(root1->data), + static_cast(root2->data)); + } + return false; } if (!root1->bv.overlap(root2->bv)) return false; From 949ed85e5fc25f4db24395d348c277bd648cf0ad Mon Sep 17 00:00:00 2001 From: Louis Montaut Date: Sun, 21 Apr 2024 21:11:47 +0200 Subject: [PATCH 2/8] BroadPhase: prettier overlap AABB-plane/halfspace --- include/hpp/fcl/BV/AABB.h | 33 +++---------------- src/BV/AABB.cpp | 33 +++++++++++++++++++ .../broadphase_dynamic_AABB_tree.cpp | 8 ++--- 3 files changed, 41 insertions(+), 33 deletions(-) diff --git a/include/hpp/fcl/BV/AABB.h b/include/hpp/fcl/BV/AABB.h index 7e4215a22..55505055d 100644 --- a/include/hpp/fcl/BV/AABB.h +++ b/include/hpp/fcl/BV/AABB.h @@ -44,6 +44,8 @@ namespace hpp { namespace fcl { struct CollisionRequest; +struct Plane; +struct Halfspace; /// @defgroup Bounding_Volume Bounding volumes /// Classes of different types of bounding volume. @@ -120,37 +122,10 @@ class HPP_FCL_DLLAPI AABB { } /// @brief Check whether AABB overlaps a plane defined by (normal, offset) - /// TODO: deal with inflation - inline bool overlapPlane(const Vec3f& normal, const FCL_REAL offset) const { - // Convert AABB to a (box, transform) representation and compute the support - // points in the directions normal and -normal. - // If both points lie on different sides of the plane, there is an overlap - // between the AABB and the plane. Otherwise, there is no overlap. - Vec3f halfside = (this->max_ - this->min_) / 2; - Vec3f center = (this->max_ + this->min_) / 2; - Vec3f support1 = (normal.array() > 0).select(halfside, -halfside) + center; - Vec3f support2 = - ((-normal).array() > 0).select(halfside, -halfside) + center; - int sign1 = (normal.dot(support1) - offset > 0) ? 1 : -1; - int sign2 = (normal.dot(support2) - offset > 0) ? 1 : -1; - return (sign1 != sign2); - } + inline bool overlap(const Plane& p) const; /// @brief Check whether AABB overlaps a halfspace defined by (normal, offset) - /// TODO: deal with inflation - inline bool overlapHalfspace(const Vec3f& normal, - const FCL_REAL offset) const { - // Convert AABB to a (box, transform) representation and compute the support - // points in the direction -normal. - // If the support is below the plane defined by the halfspace, there is an - // overlap between the AABB and the halfspace. Otherwise, there is no - // overlap. - Vec3f halfside = (this->max_ - this->min_) / 2; - Vec3f center = (this->max_ + this->min_) / 2; - Vec3f support = - ((-normal).array() > 0).select(halfside, -halfside) + center; - return ((normal.dot(support) - offset) < 0); - } + inline bool overlap(const Halfspace& hs) const; /// @brief Check whether two AABB are overlap bool overlap(const AABB& other, const CollisionRequest& request, diff --git a/src/BV/AABB.cpp b/src/BV/AABB.cpp index 665efc0b4..4abe6818d 100644 --- a/src/BV/AABB.cpp +++ b/src/BV/AABB.cpp @@ -36,6 +36,7 @@ /** \author Jia Pan */ #include +#include #include #include @@ -144,6 +145,38 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1, return bb1.overlap(b2, request, sqrDistLowerBound); } +/// @brief Check whether AABB overlaps a plane defined by (normal, offset) +/// TODO: deal with inflation +inline bool AABB::overlap(const Plane& p) const { + // Convert AABB to a (box, transform) representation and compute the support + // points in the directions normal and -normal. + // If both points lie on different sides of the plane, there is an overlap + // between the AABB and the plane. Otherwise, there is no overlap. + Vec3f halfside = (this->max_ - this->min_) / 2; + Vec3f center = (this->max_ + this->min_) / 2; + Vec3f support1 = (p.n.array() > 0).select(halfside, -halfside) + center; + Vec3f support2 = ((-p.n).array() > 0).select(halfside, -halfside) + center; + /// TODO: deal with swept-sphere radius + int sign1 = (p.n.dot(support1) - p.d > 0) ? 1 : -1; + int sign2 = (p.n.dot(support2) - p.d > 0) ? 1 : -1; + return (sign1 != sign2); +} + +/// @brief Check whether AABB overlaps a halfspace defined by (normal, offset) +/// TODO: deal with inflation +inline bool AABB::overlap(const Halfspace& hs) const { + // Convert AABB to a (box, transform) representation and compute the support + // points in the direction -normal. + // If the support is below the plane defined by the halfspace, there is an + // overlap between the AABB and the halfspace. Otherwise, there is no + // overlap. + Vec3f halfside = (this->max_ - this->min_) / 2; + Vec3f center = (this->max_ + this->min_) / 2; + /// TODO: deal with swept-sphere radius + Vec3f support = ((-hs.n).array() > 0).select(halfside, -halfside) + center; + return ((hs.n.dot(support) - hs.d) < 0); +} + } // namespace fcl } // namespace hpp diff --git a/src/broadphase/broadphase_dynamic_AABB_tree.cpp b/src/broadphase/broadphase_dynamic_AABB_tree.cpp index ea3025162..3ec1b7cc5 100644 --- a/src/broadphase/broadphase_dynamic_AABB_tree.cpp +++ b/src/broadphase/broadphase_dynamic_AABB_tree.cpp @@ -263,12 +263,12 @@ bool collisionRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, static_cast(*(o1->collisionGeometryPtr())); const Halfspace new_halfspace = transform(halfspace, o1->getTransform()); - overlap = root2->bv.overlapHalfspace(new_halfspace.n, new_halfspace.d); + overlap = root2->bv.overlap(new_halfspace); } else { const auto& plane = static_cast(*(o1->collisionGeometryPtr())); const Plane new_plane = transform(plane, o1->getTransform()); - overlap = root2->bv.overlapPlane(new_plane.n, new_plane.d); + overlap = root2->bv.overlap(new_plane); } } else if (o2->getNodeType() == GEOM_HALFSPACE || o2->getNodeType() == GEOM_PLANE) { @@ -277,12 +277,12 @@ bool collisionRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, static_cast(*(o2->collisionGeometryPtr())); const Halfspace new_halfspace = transform(halfspace, o2->getTransform()); - overlap = root1->bv.overlapHalfspace(new_halfspace.n, new_halfspace.d); + overlap = root1->bv.overlap(new_halfspace); } else { const auto& plane = static_cast(*(o2->collisionGeometryPtr())); const Plane new_plane = transform(plane, o2->getTransform()); - overlap = root1->bv.overlapPlane(new_plane.n, new_plane.d); + overlap = root1->bv.overlap(new_plane); } } else { overlap = root1->bv.overlap(root2->bv); From 5269f8cf018e98b9575975b912e5ba4657ad233e Mon Sep 17 00:00:00 2001 From: Louis Montaut Date: Sun, 21 Apr 2024 22:11:33 +0200 Subject: [PATCH 3/8] BroadPhase/plane-hspace: take inflation into account --- include/hpp/fcl/BV/AABB.h | 8 ++-- src/BV/AABB.cpp | 41 ++++++++++++++----- .../broadphase_dynamic_AABB_tree.cpp | 14 ++----- 3 files changed, 38 insertions(+), 25 deletions(-) diff --git a/include/hpp/fcl/BV/AABB.h b/include/hpp/fcl/BV/AABB.h index 55505055d..81abd6f31 100644 --- a/include/hpp/fcl/BV/AABB.h +++ b/include/hpp/fcl/BV/AABB.h @@ -44,8 +44,8 @@ namespace hpp { namespace fcl { struct CollisionRequest; -struct Plane; -struct Halfspace; +class Plane; +class Halfspace; /// @defgroup Bounding_Volume Bounding volumes /// Classes of different types of bounding volume. @@ -122,10 +122,10 @@ class HPP_FCL_DLLAPI AABB { } /// @brief Check whether AABB overlaps a plane defined by (normal, offset) - inline bool overlap(const Plane& p) const; + bool overlap(const Plane& p) const; /// @brief Check whether AABB overlaps a halfspace defined by (normal, offset) - inline bool overlap(const Halfspace& hs) const; + bool overlap(const Halfspace& hs) const; /// @brief Check whether two AABB are overlap bool overlap(const AABB& other, const CollisionRequest& request, diff --git a/src/BV/AABB.cpp b/src/BV/AABB.cpp index 4abe6818d..3565d0fbd 100644 --- a/src/BV/AABB.cpp +++ b/src/BV/AABB.cpp @@ -147,24 +147,44 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1, /// @brief Check whether AABB overlaps a plane defined by (normal, offset) /// TODO: deal with inflation -inline bool AABB::overlap(const Plane& p) const { +bool AABB::overlap(const Plane& p) const { // Convert AABB to a (box, transform) representation and compute the support // points in the directions normal and -normal. // If both points lie on different sides of the plane, there is an overlap // between the AABB and the plane. Otherwise, there is no overlap. - Vec3f halfside = (this->max_ - this->min_) / 2; - Vec3f center = (this->max_ + this->min_) / 2; - Vec3f support1 = (p.n.array() > 0).select(halfside, -halfside) + center; - Vec3f support2 = ((-p.n).array() > 0).select(halfside, -halfside) + center; - /// TODO: deal with swept-sphere radius - int sign1 = (p.n.dot(support1) - p.d > 0) ? 1 : -1; - int sign2 = (p.n.dot(support2) - p.d > 0) ? 1 : -1; + const Vec3f halfside = (this->max_ - this->min_) / 2; + const Vec3f center = (this->max_ + this->min_) / 2; + + const Vec3f support1 = (p.n.array() > 0).select(halfside, -halfside) + center; + const Vec3f support2 = + ((-p.n).array() > 0).select(halfside, -halfside) + center; + + const FCL_REAL dist1 = p.n.dot(support1) - p.d; + const FCL_REAL dist2 = p.n.dot(support2) - p.d; + const int sign1 = (dist1 > 0) ? 1 : -1; + const int sign2 = (dist2 > 0) ? 1 : -1; + + if (p.getSweptSphereRadius() > 0) { + if (sign1 != sign2) { + // Supports are on different sides of the plane. There is an overlap. + return true; + } + // Both supports are on the same side of the plane. + // We now need to check if they are on the same side of the plane inflated + // by the swept-sphere radius. + const FCL_REAL ssr_dist1 = std::abs(dist1) - p.getSweptSphereRadius(); + const FCL_REAL ssr_dist2 = std::abs(dist2) - p.getSweptSphereRadius(); + const int ssr_sign1 = (ssr_dist1 > 0) ? 1 : -1; + const int ssr_sign2 = (ssr_dist2 > 0) ? 1 : -1; + return ssr_sign1 != ssr_sign2; + } + return (sign1 != sign2); } /// @brief Check whether AABB overlaps a halfspace defined by (normal, offset) /// TODO: deal with inflation -inline bool AABB::overlap(const Halfspace& hs) const { +bool AABB::overlap(const Halfspace& hs) const { // Convert AABB to a (box, transform) representation and compute the support // points in the direction -normal. // If the support is below the plane defined by the halfspace, there is an @@ -172,9 +192,8 @@ inline bool AABB::overlap(const Halfspace& hs) const { // overlap. Vec3f halfside = (this->max_ - this->min_) / 2; Vec3f center = (this->max_ + this->min_) / 2; - /// TODO: deal with swept-sphere radius Vec3f support = ((-hs.n).array() > 0).select(halfside, -halfside) + center; - return ((hs.n.dot(support) - hs.d) < 0); + return (hs.signedDistance(support) < 0); } } // namespace fcl diff --git a/src/broadphase/broadphase_dynamic_AABB_tree.cpp b/src/broadphase/broadphase_dynamic_AABB_tree.cpp index 3ec1b7cc5..8c5e43b7b 100644 --- a/src/broadphase/broadphase_dynamic_AABB_tree.cpp +++ b/src/broadphase/broadphase_dynamic_AABB_tree.cpp @@ -261,28 +261,22 @@ bool collisionRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, if (o1->getNodeType() == GEOM_HALFSPACE) { const auto& halfspace = static_cast(*(o1->collisionGeometryPtr())); - const Halfspace new_halfspace = - transform(halfspace, o1->getTransform()); - overlap = root2->bv.overlap(new_halfspace); + overlap = root2->bv.overlap(transform(halfspace, o1->getTransform())); } else { const auto& plane = static_cast(*(o1->collisionGeometryPtr())); - const Plane new_plane = transform(plane, o1->getTransform()); - overlap = root2->bv.overlap(new_plane); + overlap = root2->bv.overlap(transform(plane, o1->getTransform())); } } else if (o2->getNodeType() == GEOM_HALFSPACE || o2->getNodeType() == GEOM_PLANE) { if (o2->getNodeType() == GEOM_HALFSPACE) { const auto& halfspace = static_cast(*(o2->collisionGeometryPtr())); - const Halfspace new_halfspace = - transform(halfspace, o2->getTransform()); - overlap = root1->bv.overlap(new_halfspace); + overlap = root1->bv.overlap(transform(halfspace, o2->getTransform())); } else { const auto& plane = static_cast(*(o2->collisionGeometryPtr())); - const Plane new_plane = transform(plane, o2->getTransform()); - overlap = root1->bv.overlap(new_plane); + overlap = root1->bv.overlap(transform(plane, o2->getTransform())); } } else { overlap = root1->bv.overlap(root2->bv); From d9d05bd3c67007ee8a37f5d9242ca26978e3f585 Mon Sep 17 00:00:00 2001 From: Louis Montaut Date: Wed, 24 Apr 2024 08:38:32 +0200 Subject: [PATCH 4/8] BroadPhase/BAABBTree: make separate `leafCollide` function --- .../broadphase_dynamic_AABB_tree.cpp | 83 +++++++++++-------- 1 file changed, 49 insertions(+), 34 deletions(-) diff --git a/src/broadphase/broadphase_dynamic_AABB_tree.cpp b/src/broadphase/broadphase_dynamic_AABB_tree.cpp index 8c5e43b7b..ca8f13a48 100644 --- a/src/broadphase/broadphase_dynamic_AABB_tree.cpp +++ b/src/broadphase/broadphase_dynamic_AABB_tree.cpp @@ -248,6 +248,52 @@ bool distanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, #endif +//============================================================================== +bool leafCollide(CollisionObject* o1, CollisionObject* o2, + CollisionCallBackBase* callback) { + if ((o1->getNodeType() == GEOM_HALFSPACE || + o1->getNodeType() == GEOM_PLANE) && + (o2->getNodeType() == GEOM_HALFSPACE || + o2->getNodeType() == GEOM_PLANE)) { + // Halfspace-plane / Halfspace-plane collision, there is no need to check + // AABBs. We can directly use the callback. + return (*callback)(o1, o2); + } + + bool overlap = false; + if (o1->getNodeType() == GEOM_HALFSPACE || o1->getNodeType() == GEOM_PLANE) { + if (o1->getNodeType() == GEOM_HALFSPACE) { + const auto& halfspace = + static_cast(*(o1->collisionGeometryPtr())); + overlap = o2->getAABB().overlap(transform(halfspace, o1->getTransform())); + } else { + const auto& plane = + static_cast(*(o1->collisionGeometryPtr())); + overlap = o2->getAABB().overlap(transform(plane, o1->getTransform())); + } + } // + else if (o2->getNodeType() == GEOM_HALFSPACE || + o2->getNodeType() == GEOM_PLANE) { + if (o2->getNodeType() == GEOM_HALFSPACE) { + const auto& halfspace = + static_cast(*(o2->collisionGeometryPtr())); + overlap = o1->getAABB().overlap(transform(halfspace, o2->getTransform())); + } else { + const auto& plane = + static_cast(*(o2->collisionGeometryPtr())); + overlap = o1->getAABB().overlap(transform(plane, o2->getTransform())); + } + } // + else { + overlap = o1->getAABB().overlap(o2->getAABB()); + } + + if (overlap) { + return (*callback)(o1, o2); + } + return false; +} + //============================================================================== bool collisionRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, DynamicAABBTreeCollisionManager::DynamicAABBNode* root2, @@ -255,38 +301,7 @@ bool collisionRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, if (root1->isLeaf() && root2->isLeaf()) { CollisionObject* o1 = static_cast(root1->data); CollisionObject* o2 = static_cast(root2->data); - bool overlap = false; - if (o1->getNodeType() == GEOM_HALFSPACE || - o1->getNodeType() == GEOM_PLANE) { - if (o1->getNodeType() == GEOM_HALFSPACE) { - const auto& halfspace = - static_cast(*(o1->collisionGeometryPtr())); - overlap = root2->bv.overlap(transform(halfspace, o1->getTransform())); - } else { - const auto& plane = - static_cast(*(o1->collisionGeometryPtr())); - overlap = root2->bv.overlap(transform(plane, o1->getTransform())); - } - } else if (o2->getNodeType() == GEOM_HALFSPACE || - o2->getNodeType() == GEOM_PLANE) { - if (o2->getNodeType() == GEOM_HALFSPACE) { - const auto& halfspace = - static_cast(*(o2->collisionGeometryPtr())); - overlap = root1->bv.overlap(transform(halfspace, o2->getTransform())); - } else { - const auto& plane = - static_cast(*(o2->collisionGeometryPtr())); - overlap = root1->bv.overlap(transform(plane, o2->getTransform())); - } - } else { - overlap = root1->bv.overlap(root2->bv); - } - - if (overlap) { - return (*callback)(static_cast(root1->data), - static_cast(root2->data)); - } - return false; + return leafCollide(o1, o2, callback); } if (!root1->bv.overlap(root2->bv)) return false; @@ -306,8 +321,8 @@ bool collisionRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, bool collisionRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root, CollisionObject* query, CollisionCallBackBase* callback) { if (root->isLeaf()) { - if (!root->bv.overlap(query->getAABB())) return false; - return (*callback)(static_cast(root->data), query); + CollisionObject* leaf = static_cast(root->data); + return leafCollide(leaf, query, callback); } if (!root->bv.overlap(query->getAABB())) return false; From 916a24b2b031f543577893c0b3299b8b26497375 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 24 Apr 2024 07:15:48 +0000 Subject: [PATCH 5/8] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- include/hpp/fcl/broadphase/default_broadphase_callbacks.h | 6 +++--- include/hpp/fcl/broadphase/detail/sparse_hash_table.h | 2 +- include/hpp/fcl/collision.h | 2 +- include/hpp/fcl/distance.h | 2 +- include/hpp/fcl/narrowphase/gjk.h | 2 +- include/hpp/fcl/shape/geometric_shapes.h | 4 ++-- 6 files changed, 9 insertions(+), 9 deletions(-) diff --git a/include/hpp/fcl/broadphase/default_broadphase_callbacks.h b/include/hpp/fcl/broadphase/default_broadphase_callbacks.h index 4dcdfc42c..b984226da 100644 --- a/include/hpp/fcl/broadphase/default_broadphase_callbacks.h +++ b/include/hpp/fcl/broadphase/default_broadphase_callbacks.h @@ -203,7 +203,7 @@ struct HPP_FCL_DLLAPI CollisionCallBackDefault : CollisionCallBackBase { CollisionData data; - virtual ~CollisionCallBackDefault(){}; + virtual ~CollisionCallBackDefault() {}; }; /// @brief Default distance callback to check collision between collision @@ -217,7 +217,7 @@ struct HPP_FCL_DLLAPI DistanceCallBackDefault : DistanceCallBackBase { DistanceData data; - virtual ~DistanceCallBackDefault(){}; + virtual ~DistanceCallBackDefault() {}; }; /// @brief Collision callback to collect collision pairs potentially in contacts @@ -244,7 +244,7 @@ struct HPP_FCL_DLLAPI CollisionCallBackCollect : CollisionCallBackBase { /// @brief Check whether a collision pair exists bool exist(CollisionObject* o1, CollisionObject* o2) const; - virtual ~CollisionCallBackCollect(){}; + virtual ~CollisionCallBackCollect() {}; protected: std::vector collision_pairs; diff --git a/include/hpp/fcl/broadphase/detail/sparse_hash_table.h b/include/hpp/fcl/broadphase/detail/sparse_hash_table.h index 4d8030087..5c28ccf3f 100644 --- a/include/hpp/fcl/broadphase/detail/sparse_hash_table.h +++ b/include/hpp/fcl/broadphase/detail/sparse_hash_table.h @@ -53,7 +53,7 @@ class unordered_map_hash_table : public std::unordered_map { typedef std::unordered_map Base; public: - unordered_map_hash_table() : Base(){}; + unordered_map_hash_table() : Base() {}; }; /// @brief A hash table implemented using unordered_map diff --git a/include/hpp/fcl/collision.h b/include/hpp/fcl/collision.h index e25f3ae27..0a28da308 100644 --- a/include/hpp/fcl/collision.h +++ b/include/hpp/fcl/collision.h @@ -126,7 +126,7 @@ class HPP_FCL_DLLAPI ComputeCollision { return !(*this == other); } - virtual ~ComputeCollision(){}; + virtual ~ComputeCollision() {}; protected: // These pointers are made mutable to let the derived classes to update diff --git a/include/hpp/fcl/distance.h b/include/hpp/fcl/distance.h index a146508c2..0c501a2bd 100644 --- a/include/hpp/fcl/distance.h +++ b/include/hpp/fcl/distance.h @@ -121,7 +121,7 @@ class HPP_FCL_DLLAPI ComputeDistance { return !(*this == other); } - virtual ~ComputeDistance(){}; + virtual ~ComputeDistance() {}; protected: // These pointers are made mutable to let the derived classes to update diff --git a/include/hpp/fcl/narrowphase/gjk.h b/include/hpp/fcl/narrowphase/gjk.h index 2b608705d..656979ba1 100644 --- a/include/hpp/fcl/narrowphase/gjk.h +++ b/include/hpp/fcl/narrowphase/gjk.h @@ -420,7 +420,7 @@ struct HPP_FCL_DLLAPI EPA { // (with 0 <= i <= 2). size_t pass; - SimplexFace() : n(Vec3f::Zero()), ignore(false){}; + SimplexFace() : n(Vec3f::Zero()), ignore(false) {}; }; /// @brief The simplex list of EPA is a linked list of faces. diff --git a/include/hpp/fcl/shape/geometric_shapes.h b/include/hpp/fcl/shape/geometric_shapes.h index cbe0dde47..fada3e07a 100644 --- a/include/hpp/fcl/shape/geometric_shapes.h +++ b/include/hpp/fcl/shape/geometric_shapes.h @@ -67,7 +67,7 @@ class HPP_FCL_DLLAPI ShapeBase : public CollisionGeometry { ShapeBase& operator=(const ShapeBase& other) = default; - virtual ~ShapeBase(){}; + virtual ~ShapeBase() {}; /// @brief Get object type: a geometric shape OBJECT_TYPE getObjectType() const { return OT_GEOM; } @@ -108,7 +108,7 @@ class HPP_FCL_DLLAPI ShapeBase : public CollisionGeometry { /// @brief Triangle stores the points instead of only indices of points class HPP_FCL_DLLAPI TriangleP : public ShapeBase { public: - TriangleP(){}; + TriangleP() {}; TriangleP(const Vec3f& a_, const Vec3f& b_, const Vec3f& c_) : ShapeBase(), a(a_), b(b_), c(c_) {} From 215f8b8ef9d5ea2c847ce31d8b350b558966847a Mon Sep 17 00:00:00 2001 From: Louis Montaut Date: Wed, 24 Apr 2024 09:18:45 +0200 Subject: [PATCH 6/8] AABB: fix documentation --- include/hpp/fcl/BV/AABB.h | 4 ++-- src/BV/AABB.cpp | 4 ---- 2 files changed, 2 insertions(+), 6 deletions(-) diff --git a/include/hpp/fcl/BV/AABB.h b/include/hpp/fcl/BV/AABB.h index 81abd6f31..bdcc8de64 100644 --- a/include/hpp/fcl/BV/AABB.h +++ b/include/hpp/fcl/BV/AABB.h @@ -121,10 +121,10 @@ class HPP_FCL_DLLAPI AABB { return true; } - /// @brief Check whether AABB overlaps a plane defined by (normal, offset) + /// @brief Check whether AABB overlaps a plane bool overlap(const Plane& p) const; - /// @brief Check whether AABB overlaps a halfspace defined by (normal, offset) + /// @brief Check whether AABB overlaps a halfspace bool overlap(const Halfspace& hs) const; /// @brief Check whether two AABB are overlap diff --git a/src/BV/AABB.cpp b/src/BV/AABB.cpp index 3565d0fbd..97f1b0723 100644 --- a/src/BV/AABB.cpp +++ b/src/BV/AABB.cpp @@ -145,8 +145,6 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1, return bb1.overlap(b2, request, sqrDistLowerBound); } -/// @brief Check whether AABB overlaps a plane defined by (normal, offset) -/// TODO: deal with inflation bool AABB::overlap(const Plane& p) const { // Convert AABB to a (box, transform) representation and compute the support // points in the directions normal and -normal. @@ -182,8 +180,6 @@ bool AABB::overlap(const Plane& p) const { return (sign1 != sign2); } -/// @brief Check whether AABB overlaps a halfspace defined by (normal, offset) -/// TODO: deal with inflation bool AABB::overlap(const Halfspace& hs) const { // Convert AABB to a (box, transform) representation and compute the support // points in the direction -normal. From 409b0ec65b7b7b0b1f1c620bdd6f81a83188ae33 Mon Sep 17 00:00:00 2001 From: Louis Montaut Date: Wed, 24 Apr 2024 09:20:37 +0200 Subject: [PATCH 7/8] Update changelog --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 59ee58056..360b601bb 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -5,6 +5,7 @@ All notable changes to this project will be documented in this file. The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/). ## [Unreleased] +- Enhance Broadphase DynamicAABBTree to better handle planes and halfspace ([#570](https://github.com/humanoid-path-planner/hpp-fcl/pull/570)) - [#558](https://github.com/humanoid-path-planner/hpp-fcl/pull/558): - [internal] Removed dead code in `narrowphase/details.h` ([#558](https://github.com/humanoid-path-planner/hpp-fcl/pull/558)) - [internal] Removed specializations of methods of `GJKSolver`. Now the specializations are all handled by `ShapeShapeDistance` in `shape_shape_func.h`. From 537f957fa7eab53332eb08782039330c4c8a3b12 Mon Sep 17 00:00:00 2001 From: Louis Montaut Date: Wed, 24 Apr 2024 11:25:41 +0200 Subject: [PATCH 8/8] BroadPhase/DAABBTree: check for plane/hspace - node collisions in tree --- .../broadphase_dynamic_AABB_tree.cpp | 54 ++++++++++++++++++- 1 file changed, 52 insertions(+), 2 deletions(-) diff --git a/src/broadphase/broadphase_dynamic_AABB_tree.cpp b/src/broadphase/broadphase_dynamic_AABB_tree.cpp index ca8f13a48..608d4628f 100644 --- a/src/broadphase/broadphase_dynamic_AABB_tree.cpp +++ b/src/broadphase/broadphase_dynamic_AABB_tree.cpp @@ -294,6 +294,44 @@ bool leafCollide(CollisionObject* o1, CollisionObject* o2, return false; } +//============================================================================== +bool nodeCollide(DynamicAABBTreeCollisionManager::DynamicAABBNode* node1, + DynamicAABBTreeCollisionManager::DynamicAABBNode* node2) { + // This function assumes that at least node1 or node2 is not a leaf of the + // tree. + if (node1->isLeaf()) { + CollisionObject* o1 = static_cast(node1->data); + if (o1->getNodeType() == GEOM_HALFSPACE || + o1->getNodeType() == GEOM_PLANE) { + if (o1->getNodeType() == GEOM_HALFSPACE) { + const auto& halfspace = + static_cast(*(o1->collisionGeometryPtr())); + return node2->bv.overlap(transform(halfspace, o1->getTransform())); + } + const auto& plane = + static_cast(*(o1->collisionGeometryPtr())); + return node2->bv.overlap(transform(plane, o1->getTransform())); + } + } + + if (node2->isLeaf()) { + CollisionObject* o2 = static_cast(node2->data); + if (o2->getNodeType() == GEOM_HALFSPACE || + o2->getNodeType() == GEOM_PLANE) { + if (o2->getNodeType() == GEOM_HALFSPACE) { + const auto& halfspace = + static_cast(*(o2->collisionGeometryPtr())); + return node1->bv.overlap(transform(halfspace, o2->getTransform())); + } + const auto& plane = + static_cast(*(o2->collisionGeometryPtr())); + return node1->bv.overlap(transform(plane, o2->getTransform())); + } + } + + return node1->bv.overlap(node2->bv); +} + //============================================================================== bool collisionRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, DynamicAABBTreeCollisionManager::DynamicAABBNode* root2, @@ -304,7 +342,9 @@ bool collisionRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, return leafCollide(o1, o2, callback); } - if (!root1->bv.overlap(root2->bv)) return false; + if (!nodeCollide(root1, root2)) { + return false; + } if (root2->isLeaf() || (!root1->isLeaf() && (root1->bv.size() > root2->bv.size()))) { @@ -325,7 +365,17 @@ bool collisionRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root, return leafCollide(leaf, query, callback); } - if (!root->bv.overlap(query->getAABB())) return false; + // Create a temporary node, attached to no tree. + // This allows to reuse the `nodeCollide` function, which only checks for + // overlap between the AABBs of two nodes. + DynamicAABBTreeCollisionManager::DynamicAABBNode query_node; + query_node.data = query; + query_node.bv = query->getAABB(); + query_node.parent = nullptr; + query_node.children[1] = nullptr; + if (!nodeCollide(root, &query_node)) { + return false; + } size_t select_res = select(query->getAABB(), *(root->children[0]), *(root->children[1]));