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`. diff --git a/include/hpp/fcl/BV/AABB.h b/include/hpp/fcl/BV/AABB.h index ddd1d1a8a..bdcc8de64 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; +class Plane; +class Halfspace; /// @defgroup Bounding_Volume Bounding volumes /// Classes of different types of bounding volume. @@ -119,6 +121,12 @@ class HPP_FCL_DLLAPI AABB { return true; } + /// @brief Check whether AABB overlaps a plane + bool overlap(const Plane& p) const; + + /// @brief Check whether AABB overlaps a halfspace + bool overlap(const Halfspace& hs) const; + /// @brief Check whether two AABB are overlap bool overlap(const AABB& other, const CollisionRequest& request, FCL_REAL& sqrDistLowerBound) const; 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_) {} diff --git a/src/BV/AABB.cpp b/src/BV/AABB.cpp index 665efc0b4..97f1b0723 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,53 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1, return bb1.overlap(b2, request, sqrDistLowerBound); } +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. + 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); +} + +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; + Vec3f support = ((-hs.n).array() > 0).select(halfside, -halfside) + center; + return (hs.signedDistance(support) < 0); +} + } // namespace fcl } // namespace hpp diff --git a/src/broadphase/broadphase_dynamic_AABB_tree.cpp b/src/broadphase/broadphase_dynamic_AABB_tree.cpp index 9338cc424..608d4628f 100644 --- a/src/broadphase/broadphase_dynamic_AABB_tree.cpp +++ b/src/broadphase/broadphase_dynamic_AABB_tree.cpp @@ -248,17 +248,103 @@ 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 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, 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); + 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()))) { @@ -275,11 +361,21 @@ 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; + // 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]));