diff --git a/.github/workflows/ros_ci.yml b/.github/workflows/ros_ci.yml index e8b861496..9c672b88d 100644 --- a/.github/workflows/ros_ci.yml +++ b/.github/workflows/ros_ci.yml @@ -19,7 +19,7 @@ jobs: - {ROS_DISTRO: foxy, PRERELEASE: true} #- {ROS_DISTRO: galactic, PRERELEASE: true} ## Activate once eigenpy released to galactic env: - AFTER_RUN_TARGET_TEST: 'cd /root/target_ws/build/hpp-fcl && make test' + AFTER_RUN_TARGET_TEST: 'cd /root/target_ws/build/hpp-fcl && make build_tests && ctest --output-on-failure' CCACHE_DIR: /github/home/.ccache # Enable ccache runs-on: ubuntu-latest steps: diff --git a/include/hpp/fcl/collision_data.h b/include/hpp/fcl/collision_data.h index 7ab6296b3..40e9264a7 100644 --- a/include/hpp/fcl/collision_data.h +++ b/include/hpp/fcl/collision_data.h @@ -149,6 +149,9 @@ struct HPP_FCL_DLLAPI QueryRequest { /// @brief enable timings when performing collision/distance request bool enable_timings; + /// @brief threshold below which a collision is considered. + FCL_REAL collision_distance_threshold; + HPP_FCL_COMPILER_DIAGNOSTIC_PUSH HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS /// @brief Default constructor. @@ -162,7 +165,9 @@ struct HPP_FCL_DLLAPI QueryRequest { gjk_max_iterations(128), cached_gjk_guess(1, 0, 0), cached_support_func_guess(support_func_guess_t::Zero()), - enable_timings(false) {} + enable_timings(false), + collision_distance_threshold( + Eigen::NumTraits::dummy_precision()) {} /// @brief Copy constructor. QueryRequest(const QueryRequest& other) = default; @@ -306,6 +311,11 @@ struct HPP_FCL_DLLAPI CollisionResult : QueryResult { /// overhead). FCL_REAL distance_lower_bound; + /// @brief nearest points + /// available only when distance_lower_bound is inferior to + /// CollisionRequest::break_distance. + Vec3f nearest_points[2]; + public: CollisionResult() : distance_lower_bound((std::numeric_limits::max)()) {} @@ -343,6 +353,18 @@ struct HPP_FCL_DLLAPI CollisionResult : QueryResult { return contacts.back(); } + /// @brief set the i-th contact calculated + void setContact(size_t i, const Contact& c) { + if (contacts.size() == 0) + throw std::invalid_argument( + "The number of contacts is zero. No Contact can be returned."); + + if (i < contacts.size()) + contacts[i] = c; + else + contacts.back() = c; + } + /// @brief get all the contacts void getContacts(std::vector& contacts_) const { contacts_.resize(contacts.size()); diff --git a/include/hpp/fcl/internal/traversal_node_base.h b/include/hpp/fcl/internal/traversal_node_base.h index 173fb58a1..adc58042b 100644 --- a/include/hpp/fcl/internal/traversal_node_base.h +++ b/include/hpp/fcl/internal/traversal_node_base.h @@ -167,6 +167,28 @@ class DistanceTraversalNodeBase : public TraversalNodeBase { DistanceResult* result; }; +namespace internal { +inline void updateDistanceLowerBoundFromBV(const CollisionRequest& req, + CollisionResult& res, + const FCL_REAL& sqrDistLowerBound) { + // BV cannot find negative distance. + if (res.distance_lower_bound <= 0) return; + FCL_REAL new_dlb = std::sqrt(sqrDistLowerBound) - req.security_margin; + if (new_dlb < res.distance_lower_bound) res.distance_lower_bound = new_dlb; +} + +inline void updateDistanceLowerBoundFromLeaf(const CollisionRequest&, + CollisionResult& res, + const FCL_REAL& distance, + const Vec3f& p0, const Vec3f& p1) { + if (distance < res.distance_lower_bound) { + res.distance_lower_bound = distance; + res.nearest_points[0] = p0; + res.nearest_points[1] = p1; + } +} +} // namespace internal + ///@} } // namespace fcl diff --git a/include/hpp/fcl/internal/traversal_node_bvh_shape.h b/include/hpp/fcl/internal/traversal_node_bvh_shape.h index 8b2521ca5..7d0b1cd52 100644 --- a/include/hpp/fcl/internal/traversal_node_bvh_shape.h +++ b/include/hpp/fcl/internal/traversal_node_bvh_shape.h @@ -170,16 +170,19 @@ class MeshShapeCollisionTraversalNode bool BVDisjoints(unsigned int b1, unsigned int /*b2*/, FCL_REAL& sqrDistLowerBound) const { if (this->enable_statistics) this->num_bv_tests++; - bool res; + bool disjoint; if (RTIsIdentity) - res = !this->model1->getBV(b1).bv.overlap(this->model2_bv, this->request, - sqrDistLowerBound); + disjoint = !this->model1->getBV(b1).bv.overlap( + this->model2_bv, this->request, sqrDistLowerBound); else - res = !overlap(this->tf1.getRotation(), this->tf1.getTranslation(), - this->model2_bv, this->model1->getBV(b1).bv, this->request, - sqrDistLowerBound); - assert(!res || sqrDistLowerBound > 0); - return res; + disjoint = !overlap(this->tf1.getRotation(), this->tf1.getTranslation(), + this->model2_bv, this->model1->getBV(b1).bv, + this->request, sqrDistLowerBound); + if (disjoint) + internal::updateDistanceLowerBoundFromBV(this->request, *this->result, + sqrDistLowerBound); + assert(!disjoint || sqrDistLowerBound > 0); + return disjoint; } /// @brief Intersection testing between leaves (one triangle and one shape) @@ -211,24 +214,29 @@ class MeshShapeCollisionTraversalNode distance, c2, c1, normal); } + FCL_REAL distToCollision = distance - this->request.security_margin; if (collision) { + sqrDistLowerBound = 0; if (this->request.num_max_contacts > this->result->numContacts()) { this->result->addContact(Contact(this->model1, this->model2, primitive_id, Contact::NONE, c1, -normal, -distance)); assert(this->result->isCollision()); - return; } - } - sqrDistLowerBound = distance * distance; - assert(distance > 0); - if (this->request.security_margin > 0 && - distance <= this->request.security_margin) { - this->result->addContact(Contact(this->model1, this->model2, primitive_id, - Contact::NONE, .5 * (c1 + c2), - (c2 - c1).normalized(), -distance)); - } - assert(!this->result->isCollision() || sqrDistLowerBound > 0); + } else if (distToCollision <= this->request.collision_distance_threshold) { + sqrDistLowerBound = 0; + if (this->request.num_max_contacts > this->result->numContacts()) { + this->result->addContact( + Contact(this->model1, this->model2, primitive_id, Contact::NONE, + .5 * (c1 + c2), (c2 - c1).normalized(), -distance)); + } + } else + sqrDistLowerBound = distToCollision * distToCollision; + + internal::updateDistanceLowerBoundFromLeaf(this->request, *this->result, + distToCollision, c1, c2); + + assert(this->result->isCollision() || sqrDistLowerBound > 0); } Vec3f* vertices; @@ -273,16 +281,19 @@ class ShapeMeshCollisionTraversalNode bool BVDisjoints(unsigned int /*b1*/, unsigned int b2, FCL_REAL& sqrDistLowerBound) const { if (this->enable_statistics) this->num_bv_tests++; - bool res; + bool disjoint; if (RTIsIdentity) - res = !this->model2->getBV(b2).bv.overlap(this->model1_bv, - sqrDistLowerBound); + disjoint = !this->model2->getBV(b2).bv.overlap(this->model1_bv, + sqrDistLowerBound); else - res = !overlap(this->tf2.getRotation(), this->tf2.getTranslation(), - this->model1_bv, this->model2->getBV(b2).bv, - sqrDistLowerBound); - assert(!res || sqrDistLowerBound > 0); - return res; + disjoint = !overlap(this->tf2.getRotation(), this->tf2.getTranslation(), + this->model1_bv, this->model2->getBV(b2).bv, + sqrDistLowerBound); + if (disjoint) + internal::updateDistanceLowerBoundFromBV(this->request, *this->result, + sqrDistLowerBound); + assert(!disjoint || sqrDistLowerBound > 0); + return disjoint; } /// @brief Intersection testing between leaves (one shape and one triangle) @@ -314,24 +325,29 @@ class ShapeMeshCollisionTraversalNode c2, distance, normal); } + FCL_REAL distToCollision = distance - this->request.security_margin; if (collision) { + sqrDistLowerBound = 0; if (this->request.num_max_contacts > this->result->numContacts()) { this->result->addContact(Contact(this->model1, this->model2, Contact::NONE, primitive_id, c1, normal, -distance)); assert(this->result->isCollision()); - return; } - } - sqrDistLowerBound = distance * distance; - assert(distance > 0); - if (this->request.security_margin == 0 && - distance <= this->request.security_margin) { - this->result.addContact(Contact(this->model1, this->model2, Contact::NONE, - primitive_id, .5 * (c1 + c2), - (c2 - c1).normalized(), -distance)); - } - assert(!this->result->isCollision() || sqrDistLowerBound > 0); + } else if (distToCollision <= this->request.collision_distance_threshold) { + sqrDistLowerBound = 0; + if (this->request.num_max_contacts > this->result->numContacts()) { + this->result->addContact( + Contact(this->model1, this->model2, Contact::NONE, primitive_id, + .5 * (c1 + c2), (c2 - c1).normalized(), -distance)); + } + } else + sqrDistLowerBound = distToCollision * distToCollision; + + internal::updateDistanceLowerBoundFromLeaf(this->request, *this->result, + distToCollision, c1, c2); + + assert(this->result->isCollision() || sqrDistLowerBound > 0); } Vec3f* vertices; diff --git a/include/hpp/fcl/internal/traversal_node_bvhs.h b/include/hpp/fcl/internal/traversal_node_bvhs.h index 9e9cad789..9bcd9555a 100644 --- a/include/hpp/fcl/internal/traversal_node_bvhs.h +++ b/include/hpp/fcl/internal/traversal_node_bvhs.h @@ -163,16 +163,19 @@ class MeshCollisionTraversalNode : public BVHCollisionTraversalNode { bool BVDisjoints(unsigned int b1, unsigned int b2, FCL_REAL& sqrDistLowerBound) const { if (this->enable_statistics) this->num_bv_tests++; + bool disjoint; if (RTIsIdentity) - return !this->model1->getBV(b1).overlap(this->model2->getBV(b2), - this->request, sqrDistLowerBound); + disjoint = !this->model1->getBV(b1).overlap( + this->model2->getBV(b2), this->request, sqrDistLowerBound); else { - bool res = !overlap(RT._R(), RT._T(), this->model1->getBV(b1).bv, + disjoint = !overlap(RT._R(), RT._T(), this->model1->getBV(b1).bv, this->model2->getBV(b2).bv, this->request, sqrDistLowerBound); - assert(!res || sqrDistLowerBound > 0); - return res; } + if (disjoint) + internal::updateDistanceLowerBoundFromBV(this->request, *this->result, + sqrDistLowerBound); + return disjoint; } /// Intersection testing between leaves (two triangles) @@ -218,24 +221,28 @@ class MeshCollisionTraversalNode : public BVHCollisionTraversalNode { FCL_REAL distance; solver.shapeDistance(tri1, this->tf1, tri2, this->tf2, distance, p1, p2, normal); - FCL_REAL distToCollision = distance - this->request.security_margin; - sqrDistLowerBound = distance * distance; - if (distToCollision <= 0) { // collision - Vec3f p(p1); // contact point - FCL_REAL penetrationDepth(0); + + const FCL_REAL distToCollision = distance - this->request.security_margin; + if (distToCollision <= + this->request.collision_distance_threshold) { // collision + sqrDistLowerBound = 0; + Vec3f p(p1); // contact point if (this->result->numContacts() < this->request.num_max_contacts) { // How much (Q1, Q2, Q3) should be moved so that all vertices are // above (P1, P2, P3). - penetrationDepth = -distance; if (distance > 0) { normal = (p2 - p1).normalized(); p = .5 * (p1 + p2); } this->result->addContact(Contact(this->model1, this->model2, primitive_id1, primitive_id2, p, - normal, penetrationDepth)); + normal, -distance)); } - } + } else + sqrDistLowerBound = distToCollision * distToCollision; + + internal::updateDistanceLowerBoundFromLeaf(this->request, *this->result, + distToCollision, p1, p2); } Vec3f* vertices1; diff --git a/include/hpp/fcl/internal/traversal_node_hfield_shape.h b/include/hpp/fcl/internal/traversal_node_hfield_shape.h index 7b1066397..19f3a928e 100644 --- a/include/hpp/fcl/internal/traversal_node_hfield_shape.h +++ b/include/hpp/fcl/internal/traversal_node_hfield_shape.h @@ -243,18 +243,21 @@ class HeightFieldShapeCollisionTraversalNode bool BVDisjoints(unsigned int b1, unsigned int /*b2*/, FCL_REAL& sqrDistLowerBound) const { if (this->enable_statistics) this->num_bv_tests++; - bool res; + bool disjoint; if (RTIsIdentity) { assert(false && "must never happened"); - res = !this->model1->getBV(b1).bv.overlap(this->model2_bv, this->request, - sqrDistLowerBound); + disjoint = !this->model1->getBV(b1).bv.overlap( + this->model2_bv, this->request, sqrDistLowerBound); } else { - res = !overlap(this->tf1.getRotation(), this->tf1.getTranslation(), - this->model2_bv, this->model1->getBV(b1).bv, this->request, - sqrDistLowerBound); + disjoint = !overlap(this->tf1.getRotation(), this->tf1.getTranslation(), + this->model2_bv, this->model1->getBV(b1).bv, + this->request, sqrDistLowerBound); } - assert(!res || sqrDistLowerBound > 0); - return res; + if (disjoint) + internal::updateDistanceLowerBoundFromBV(this->request, *this->result, + sqrDistLowerBound); + assert(!disjoint || sqrDistLowerBound > 0); + return disjoint; } template @@ -326,24 +329,29 @@ class HeightFieldShapeCollisionTraversalNode this->shapeDistance(convex1, convex2, this->tf1, *(this->model2), this->tf2, distance, c1, c2, normal); + FCL_REAL distToCollision = distance - this->request.security_margin; if (collision) { + sqrDistLowerBound = 0; if (this->request.num_max_contacts > this->result->numContacts()) { this->result->addContact(Contact(this->model1, this->model2, (int)b1, (int)Contact::NONE, c1, normal, - distance)); + -distance)); assert(this->result->isCollision()); - return; } - } - sqrDistLowerBound = distance * distance; - // assert (distance > 0); - if (this->request.security_margin > 0 && - distance <= this->request.security_margin) { - this->result->addContact(Contact(this->model1, this->model2, (int)b1, - (int)Contact::NONE, .5 * (c1 + c2), - (c2 - c1).normalized(), distance)); - } - assert(!this->result->isCollision() || sqrDistLowerBound > 0); + } else if (distToCollision <= this->request.collision_distance_threshold) { + sqrDistLowerBound = 0; + if (this->request.num_max_contacts > this->result->numContacts()) { + this->result->addContact(Contact(this->model1, this->model2, (int)b1, + (int)Contact::NONE, .5 * (c1 + c2), + (c2 - c1).normalized(), -distance)); + } + } else + sqrDistLowerBound = distToCollision * distToCollision; + + internal::updateDistanceLowerBoundFromLeaf(this->request, *this->result, + distToCollision, c1, c2); + + assert(this->result->isCollision() || sqrDistLowerBound > 0); } const GJKSolver* nsolver; @@ -418,16 +426,20 @@ class ShapeHeightFieldCollisionTraversalNode bool BVDisjoints(unsigned int /*b1*/, unsigned int b2, FCL_REAL& sqrDistLowerBound) const { if (this->enable_statistics) this->num_bv_tests++; - bool res; + bool disjoint; if (RTIsIdentity) - res = !this->model2->getBV(b2).bv.overlap(this->model1_bv, - sqrDistLowerBound); + disjoint = !this->model2->getBV(b2).bv.overlap(this->model1_bv, + sqrDistLowerBound); else - res = !overlap(this->tf2.getRotation(), this->tf2.getTranslation(), - this->model1_bv, this->model2->getBV(b2).bv, - sqrDistLowerBound); - assert(!res || sqrDistLowerBound > 0); - return res; + disjoint = !overlap(this->tf2.getRotation(), this->tf2.getTranslation(), + this->model1_bv, this->model2->getBV(b2).bv, + sqrDistLowerBound); + + if (disjoint) + internal::updateDistanceLowerBoundFromBV(this->request, *this->result, + sqrDistLowerBound); + assert(!disjoint || sqrDistLowerBound > 0); + return disjoint; } template @@ -498,7 +510,9 @@ class ShapeHeightFieldCollisionTraversalNode this->shapeDistance(*(this->model1), this->tf1, convex1, convex2, this->tf2, distance, c1, c2, normal); + FCL_REAL distToCollision = distance - this->request.security_margin; if (collision) { + sqrDistLowerBound = 0; if (this->request.num_max_contacts > this->result->numContacts()) { this->result->addContact(Contact(this->model1, this->model2, Contact::NONE, b2, c1, normal, @@ -506,15 +520,20 @@ class ShapeHeightFieldCollisionTraversalNode assert(this->result->isCollision()); return; } - } - sqrDistLowerBound = distance * distance; - if (this->request.security_margin == 0 && - distance <= this->request.security_margin) { - this->result->addContact(Contact(this->model1, this->model2, - Contact::NONE, b2, .5 * (c1 + c2), - (c2 - c1).normalized(), distance)); - } - assert(!this->result->isCollision() || sqrDistLowerBound > 0); + } else if (distToCollision < 0) { + sqrDistLowerBound = 0; + if (this->request.num_max_contacts > this->result->numContacts()) { + this->result->addContact(Contact(this->model1, this->model2, + Contact::NONE, b2, .5 * (c1 + c2), + (c2 - c1).normalized(), distance)); + } + } else + sqrDistLowerBound = distToCollision * distToCollision; + + internal::updateDistanceLowerBoundFromLeaf(this->request, *this->result, + distToCollision, c1, c2); + + assert(this->result->isCollision() || sqrDistLowerBound > 0); } const GJKSolver* nsolver; diff --git a/include/hpp/fcl/internal/traversal_node_octree.h b/include/hpp/fcl/internal/traversal_node_octree.h index 3e6a88d22..d9ed85e98 100644 --- a/include/hpp/fcl/internal/traversal_node_octree.h +++ b/include/hpp/fcl/internal/traversal_node_octree.h @@ -46,6 +46,7 @@ #include #include #include +#include namespace hpp { namespace fcl { @@ -265,64 +266,8 @@ class HPP_FCL_DLLAPI OcTreeSolver { const AABB& bv1, const S& s, const OBB& obb2, const Transform3f& tf1, const Transform3f& tf2) const { - if (!root1) { - OBB obb1; - convertBV(bv1, tf1, obb1); - if (obb1.overlap(obb2)) { - Box box; - Transform3f box_tf; - constructBox(bv1, tf1, box, box_tf); - - FCL_REAL distance; - if (solver->shapeIntersect(box, box_tf, s, tf2, distance, false, NULL, - NULL)) { - AABB overlap_part; - AABB aabb1, aabb2; - computeBV(box, box_tf, aabb1); - computeBV(s, tf2, aabb2); - aabb1.overlap(aabb2, overlap_part); - } - } - - return false; - } else if (!tree1->nodeHasChildren(root1)) { - if (tree1->isNodeOccupied(root1)) // occupied area - { - OBB obb1; - convertBV(bv1, tf1, obb1); - if (obb1.overlap(obb2)) { - Box box; - Transform3f box_tf; - constructBox(bv1, tf1, box, box_tf); - - FCL_REAL distance; - if (!crequest->enable_contact) { - if (solver->shapeIntersect(box, box_tf, s, tf2, distance, false, - NULL, NULL)) { - if (cresult->numContacts() < crequest->num_max_contacts) - cresult->addContact(Contact( - tree1, &s, static_cast(root1 - tree1->getRoot()), - Contact::NONE)); - } - } else { - Vec3f contact; - Vec3f normal; - - if (solver->shapeIntersect(box, box_tf, s, tf2, distance, true, - &contact, &normal)) { - if (cresult->numContacts() < crequest->num_max_contacts) - cresult->addContact(Contact( - tree1, &s, static_cast(root1 - tree1->getRoot()), - Contact::NONE, contact, normal, distance)); - } - } - - return crequest->isSatisfied(*cresult); - } else - return false; - } else // free area - return false; - } + // Empty OcTree is considered free. + if (!root1) return false; /// stop when 1) bounding boxes of two objects not overlap; OR /// 2) at least of one the nodes is free; OR @@ -335,7 +280,36 @@ class HPP_FCL_DLLAPI OcTreeSolver { else { OBB obb1; convertBV(bv1, tf1, obb1); - if (!obb1.overlap(obb2)) return false; + FCL_REAL sqrDistLowerBound; + if (!obb1.overlap(obb2, *crequest, sqrDistLowerBound)) { + internal::updateDistanceLowerBoundFromBV(*crequest, *cresult, + sqrDistLowerBound); + return false; + } + } + + if (!tree1->nodeHasChildren(root1)) { + assert(tree1->isNodeOccupied(root1)); // it isn't free nor uncertain. + + Box box; + Transform3f box_tf; + constructBox(bv1, tf1, box, box_tf); + + bool contactNotAdded = + (cresult->numContacts() >= crequest->num_max_contacts); + std::size_t ncontact = ShapeShapeCollide( + &box, box_tf, &s, tf2, solver, *crequest, *cresult); + assert(ncontact == 0 || ncontact == 1); + if (!contactNotAdded && ncontact == 1) { + // Update contact information. + const Contact& c = cresult->getContact(cresult->numContacts() - 1); + cresult->setContact( + cresult->numContacts() - 1, + Contact(tree1, c.o2, static_cast(root1 - tree1->getRoot()), + c.b2, c.pos, c.normal, c.penetration_depth)); + } + + return crequest->isSatisfied(*cresult); } for (unsigned int i = 0; i < 8; ++i) { @@ -436,100 +410,20 @@ class HPP_FCL_DLLAPI OcTreeSolver { return false; } + /// \return True if the request is satisfied. template bool OcTreeMeshIntersectRecurse(const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1, const BVHModel* tree2, unsigned int root2, const Transform3f& tf1, const Transform3f& tf2) const { - if (!root1) { - if (tree2->getBV(root2).isLeaf()) { - OBB obb1, obb2; - convertBV(bv1, tf1, obb1); - convertBV(tree2->getBV(root2).bv, tf2, obb2); - if (obb1.overlap(obb2)) { - Box box; - Transform3f box_tf; - constructBox(bv1, tf1, box, box_tf); - - int primitive_id = tree2->getBV(root2).primitiveId(); - const Triangle& tri_id = tree2->tri_indices[primitive_id]; - const Vec3f& p1 = tree2->vertices[tri_id[0]]; - const Vec3f& p2 = tree2->vertices[tri_id[1]]; - const Vec3f& p3 = tree2->vertices[tri_id[2]]; - Vec3f c1, c2, normal; - FCL_REAL distance; - if (solver->shapeTriangleInteraction(box, box_tf, p1, p2, p3, tf2, - distance, c1, c2, normal)) { - AABB overlap_part; - AABB aabb1; - computeBV(box, box_tf, aabb1); - AABB aabb2(tf2.transform(p1), tf2.transform(p2), tf2.transform(p3)); - aabb1.overlap(aabb2, overlap_part); - } - } - - return false; - } else { - if (OcTreeMeshIntersectRecurse( - tree1, root1, bv1, tree2, - (unsigned int)tree2->getBV(root2).leftChild(), tf1, tf2)) - return true; - - if (OcTreeMeshIntersectRecurse( - tree1, root1, bv1, tree2, - (unsigned int)tree2->getBV(root2).rightChild(), tf1, tf2)) - return true; + // FIXME(jmirabel) I do not understand why the BVHModel was traversed. The + // code in this if(!root1) did not output anything so the empty OcTree is + // considered free. Should an empty OcTree be considered free ? - return false; - } - } else if (!tree1->nodeHasChildren(root1) && tree2->getBV(root2).isLeaf()) { - if (tree1->isNodeOccupied(root1)) { - OBB obb1, obb2; - convertBV(bv1, tf1, obb1); - convertBV(tree2->getBV(root2).bv, tf2, obb2); - if (obb1.overlap(obb2)) { - Box box; - Transform3f box_tf; - constructBox(bv1, tf1, box, box_tf); - - int primitive_id = tree2->getBV(root2).primitiveId(); - const Triangle& tri_id = tree2->tri_indices[primitive_id]; - const Vec3f& p1 = tree2->vertices[tri_id[0]]; - const Vec3f& p2 = tree2->vertices[tri_id[1]]; - const Vec3f& p3 = tree2->vertices[tri_id[2]]; - - if (!crequest->enable_contact) { - Vec3f c1, c2, normal; - FCL_REAL distance; - if (solver->shapeTriangleInteraction(box, box_tf, p1, p2, p3, tf2, - distance, c1, c2, normal)) { - if (cresult->numContacts() < crequest->num_max_contacts) - cresult->addContact(Contact(tree1, tree2, - (int)(root1 - tree1->getRoot()), - primitive_id)); - } - } else { - Vec3f c1, c2; - FCL_REAL distance; - Vec3f normal; - - if (solver->shapeTriangleInteraction(box, box_tf, p1, p2, p3, tf2, - distance, c1, c2, normal)) { - assert(crequest->security_margin == 0); - if (cresult->numContacts() < crequest->num_max_contacts) - cresult->addContact( - Contact(tree1, tree2, (int)(root1 - tree1->getRoot()), - primitive_id, c1, normal, -distance)); - } - } - - return crequest->isSatisfied(*cresult); - } else - return false; - } else // free area - return false; - } + // Empty OcTree is considered free. + if (!root1) return false; + BVNode const& bvn2 = tree2->getBV(root2); /// stop when 1) bounding boxes of two objects not overlap; OR /// 2) at least one of the nodes is free; OR @@ -542,13 +436,55 @@ class HPP_FCL_DLLAPI OcTreeSolver { else { OBB obb1, obb2; convertBV(bv1, tf1, obb1); - convertBV(tree2->getBV(root2).bv, tf2, obb2); - if (!obb1.overlap(obb2)) return false; + convertBV(bvn2.bv, tf2, obb2); + FCL_REAL sqrDistLowerBound; + if (!obb1.overlap(obb2, *crequest, sqrDistLowerBound)) { + internal::updateDistanceLowerBoundFromBV(*crequest, *cresult, + sqrDistLowerBound); + return false; + } } - if (tree2->getBV(root2).isLeaf() || - (tree1->nodeHasChildren(root1) && - (bv1.size() > tree2->getBV(root2).bv.size()))) { + // Check if leaf collides. + if (!tree1->nodeHasChildren(root1) && bvn2.isLeaf()) { + assert(tree1->isNodeOccupied(root1)); // it isn't free nor uncertain. + Box box; + Transform3f box_tf; + constructBox(bv1, tf1, box, box_tf); + + int primitive_id = bvn2.primitiveId(); + const Triangle& tri_id = tree2->tri_indices[primitive_id]; + const Vec3f& p1 = tree2->vertices[tri_id[0]]; + const Vec3f& p2 = tree2->vertices[tri_id[1]]; + const Vec3f& p3 = tree2->vertices[tri_id[2]]; + + Vec3f c1, c2, normal; + FCL_REAL distance; + + bool collision = solver->shapeTriangleInteraction( + box, box_tf, p1, p2, p3, tf2, distance, c1, c2, normal); + FCL_REAL distToCollision = distance - crequest->security_margin; + + if (cresult->numContacts() < crequest->num_max_contacts) { + if (collision) { + cresult->addContact(Contact(tree1, tree2, + (int)(root1 - tree1->getRoot()), + primitive_id, c1, normal, -distance)); + } else if (distToCollision < 0) { + cresult->addContact(Contact( + tree1, tree2, (int)(root1 - tree1->getRoot()), primitive_id, + .5 * (c1 + c2), (c2 - c1).normalized(), -distance)); + } + } + internal::updateDistanceLowerBoundFromLeaf(*crequest, *cresult, + distToCollision, c1, c2); + + return crequest->isSatisfied(*cresult); + } + + // Determine which tree to traverse first. + if (bvn2.isLeaf() || + (tree1->nodeHasChildren(root1) && (bv1.size() > bvn2.bv.size()))) { for (unsigned int i = 0; i < 8; ++i) { if (tree1->nodeChildExists(root1, i)) { const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); @@ -561,14 +497,12 @@ class HPP_FCL_DLLAPI OcTreeSolver { } } } else { - if (OcTreeMeshIntersectRecurse( - tree1, root1, bv1, tree2, - (unsigned int)tree2->getBV(root2).leftChild(), tf1, tf2)) + if (OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2, + (unsigned int)bvn2.leftChild(), tf1, tf2)) return true; - if (OcTreeMeshIntersectRecurse( - tree1, root1, bv1, tree2, - (unsigned int)tree2->getBV(root2).rightChild(), tf1, tf2)) + if (OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2, + (unsigned int)bvn2.rightChild(), tf1, tf2)) return true; } @@ -657,114 +591,9 @@ class HPP_FCL_DLLAPI OcTreeSolver { const OcTree::OcTreeNode* root2, const AABB& bv2, const Transform3f& tf1, const Transform3f& tf2) const { - if (!root1 && !root2) { - OBB obb1, obb2; - convertBV(bv1, tf1, obb1); - convertBV(bv2, tf2, obb2); - - if (obb1.overlap(obb2)) { - Box box1, box2; - Transform3f box1_tf, box2_tf; - constructBox(bv1, tf1, box1, box1_tf); - constructBox(bv2, tf2, box2, box2_tf); - - AABB overlap_part; - AABB aabb1, aabb2; - computeBV(box1, box1_tf, aabb1); - computeBV(box2, box2_tf, aabb2); - aabb1.overlap(aabb2, overlap_part); - } - - return false; - } else if (!root1 && root2) { - if (tree2->nodeHasChildren(root2)) { - for (unsigned int i = 0; i < 8; ++i) { - if (tree2->nodeChildExists(root2, i)) { - const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); - AABB child_bv; - computeChildBV(bv2, i, child_bv); - if (OcTreeIntersectRecurse(tree1, NULL, bv1, tree2, child, child_bv, - tf1, tf2)) - return true; - } else { - AABB child_bv; - computeChildBV(bv2, i, child_bv); - if (OcTreeIntersectRecurse(tree1, NULL, bv1, tree2, NULL, child_bv, - tf1, tf2)) - return true; - } - } - } else { - if (OcTreeIntersectRecurse(tree1, NULL, bv1, tree2, NULL, bv2, tf1, - tf2)) - return true; - } - - return false; - } else if (root1 && !root2) { - if (tree1->nodeHasChildren(root1)) { - for (unsigned int i = 0; i < 8; ++i) { - if (tree1->nodeChildExists(root1, i)) { - const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); - AABB child_bv; - computeChildBV(bv1, i, child_bv); - if (OcTreeIntersectRecurse(tree1, child, child_bv, tree2, NULL, bv2, - tf1, tf2)) - return true; - } else { - AABB child_bv; - computeChildBV(bv1, i, child_bv); - if (OcTreeIntersectRecurse(tree1, NULL, child_bv, tree2, NULL, bv2, - tf1, tf2)) - return true; - } - } - } else { - if (OcTreeIntersectRecurse(tree1, NULL, bv1, tree2, NULL, bv2, tf1, - tf2)) - return true; - } - - return false; - } else if (!tree1->nodeHasChildren(root1) && - !tree2->nodeHasChildren(root2)) { - if (tree1->isNodeOccupied(root1) && - tree2->isNodeOccupied(root2)) // occupied area - { - if (!crequest->enable_contact) { - OBB obb1, obb2; - convertBV(bv1, tf1, obb1); - convertBV(bv2, tf2, obb2); - - if (obb1.overlap(obb2)) { - if (cresult->numContacts() < crequest->num_max_contacts) - cresult->addContact(Contact( - tree1, tree2, static_cast(root1 - tree1->getRoot()), - static_cast(root2 - tree2->getRoot()))); - } - } else { - Box box1, box2; - Transform3f box1_tf, box2_tf; - constructBox(bv1, tf1, box1, box1_tf); - constructBox(bv2, tf2, box2, box2_tf); - - Vec3f contact; - FCL_REAL distance; - Vec3f normal; - if (solver->shapeIntersect(box1, box1_tf, box2, box2_tf, distance, - true, &contact, &normal)) { - if (cresult->numContacts() < crequest->num_max_contacts) - cresult->addContact(Contact( - tree1, tree2, static_cast(root1 - tree1->getRoot()), - static_cast(root2 - tree2->getRoot()), contact, normal, - distance)); - } - } - - return crequest->isSatisfied(*cresult); - } else // free area (at least one node is free) - return false; - } + // Empty OcTree is considered free. + if (!root1) return false; + if (!root2) return false; /// stop when 1) bounding boxes of two objects not overlap; OR /// 2) at least one of the nodes is free; OR @@ -774,13 +603,65 @@ class HPP_FCL_DLLAPI OcTreeSolver { return false; else if ((tree1->isNodeUncertain(root1) || tree2->isNodeUncertain(root2))) return false; - else { + + bool bothAreLeaves = + (!tree1->nodeHasChildren(root1) && !tree2->nodeHasChildren(root2)); + if (!bothAreLeaves || !crequest->enable_contact) { OBB obb1, obb2; convertBV(bv1, tf1, obb1); convertBV(bv2, tf2, obb2); - if (!obb1.overlap(obb2)) return false; + FCL_REAL sqrDistLowerBound; + if (!obb1.overlap(obb2, *crequest, sqrDistLowerBound)) { + if (cresult->distance_lower_bound > 0 && + sqrDistLowerBound < + cresult->distance_lower_bound * cresult->distance_lower_bound) + cresult->distance_lower_bound = + sqrt(sqrDistLowerBound) - crequest->security_margin; + return false; + } + if (!crequest->enable_contact) { // Overlap + if (cresult->numContacts() < crequest->num_max_contacts) + cresult->addContact( + Contact(tree1, tree2, static_cast(root1 - tree1->getRoot()), + static_cast(root2 - tree2->getRoot()))); + return crequest->isSatisfied(*cresult); + } + } + + // Both node are leaves + if (bothAreLeaves) { + assert(tree1->isNodeOccupied(root1) && tree2->isNodeOccupied(root2)); + + Box box1, box2; + Transform3f box1_tf, box2_tf; + constructBox(bv1, tf1, box1, box1_tf); + constructBox(bv2, tf2, box2, box2_tf); + + FCL_REAL distance; + Vec3f c1, c2, normal; + bool collision = solver->shapeDistance(box1, box1_tf, box2, box2_tf, + distance, c1, c2, normal); + FCL_REAL distToCollision = distance - crequest->security_margin; + + if (cresult->numContacts() < crequest->num_max_contacts) { + if (collision) + cresult->addContact( + Contact(tree1, tree2, static_cast(root1 - tree1->getRoot()), + static_cast(root2 - tree2->getRoot()), c1, normal, + -distance)); + else if (distToCollision <= 0) + cresult->addContact( + Contact(tree1, tree2, static_cast(root1 - tree1->getRoot()), + static_cast(root2 - tree2->getRoot()), + .5 * (c1 + c2), (c2 - c1).normalized(), -distance)); + } + internal::updateDistanceLowerBoundFromLeaf(*crequest, *cresult, + distToCollision, c1, c2); + + return crequest->isSatisfied(*cresult); } + // Determine which tree to traverse first. if (!tree2->nodeHasChildren(root2) || (tree1->nodeHasChildren(root1) && (bv1.size() > bv2.size()))) { for (unsigned int i = 0; i < 8; ++i) { @@ -831,8 +712,10 @@ class HPP_FCL_DLLAPI OcTreeCollisionTraversalNode bool BVDisjoints(unsigned, unsigned, FCL_REAL&) const { return false; } - void leafCollides(unsigned, unsigned, FCL_REAL&) const { + void leafCollides(unsigned, unsigned, FCL_REAL& sqrDistLowerBound) const { otsolver->OcTreeIntersect(model1, model2, tf1, tf2, request, *result); + sqrDistLowerBound = std::max((FCL_REAL)0, result->distance_lower_bound); + sqrDistLowerBound *= sqrDistLowerBound; } const OcTree* model1; @@ -862,8 +745,11 @@ class HPP_FCL_DLLAPI ShapeOcTreeCollisionTraversalNode return false; } - void leafCollides(unsigned int, unsigned int, FCL_REAL&) const { + void leafCollides(unsigned int, unsigned int, + FCL_REAL& sqrDistLowerBound) const { otsolver->OcTreeShapeIntersect(model2, *model1, tf2, tf1, request, *result); + sqrDistLowerBound = std::max((FCL_REAL)0, result->distance_lower_bound); + sqrDistLowerBound *= sqrDistLowerBound; } const S* model1; @@ -894,8 +780,11 @@ class HPP_FCL_DLLAPI OcTreeShapeCollisionTraversalNode return false; } - void leafCollides(unsigned int, unsigned int, FCL_REAL&) const { + void leafCollides(unsigned int, unsigned int, + FCL_REAL& sqrDistLowerBound) const { otsolver->OcTreeShapeIntersect(model1, *model2, tf1, tf2, request, *result); + sqrDistLowerBound = std::max((FCL_REAL)0, result->distance_lower_bound); + sqrDistLowerBound *= sqrDistLowerBound; } const OcTree* model1; @@ -925,8 +814,11 @@ class HPP_FCL_DLLAPI MeshOcTreeCollisionTraversalNode return false; } - void leafCollides(unsigned int, unsigned int, FCL_REAL&) const { + void leafCollides(unsigned int, unsigned int, + FCL_REAL& sqrDistLowerBound) const { otsolver->OcTreeMeshIntersect(model2, model1, tf2, tf1, request, *result); + sqrDistLowerBound = std::max((FCL_REAL)0, result->distance_lower_bound); + sqrDistLowerBound *= sqrDistLowerBound; } const BVHModel* model1; @@ -956,8 +848,12 @@ class HPP_FCL_DLLAPI OcTreeMeshCollisionTraversalNode return false; } - void leafCollides(unsigned int, unsigned int, FCL_REAL&) const { + void leafCollides(unsigned int, unsigned int, + FCL_REAL& sqrDistLowerBound) const { + std::cout << "leafCollides" << std::endl; otsolver->OcTreeMeshIntersect(model1, model2, tf1, tf2, request, *result); + sqrDistLowerBound = std::max((FCL_REAL)0, result->distance_lower_bound); + sqrDistLowerBound *= sqrDistLowerBound; } const OcTree* model1; diff --git a/include/hpp/fcl/narrowphase/narrowphase.h b/include/hpp/fcl/narrowphase/narrowphase.h index f63bb0fd2..4064f5753 100644 --- a/include/hpp/fcl/narrowphase/narrowphase.h +++ b/include/hpp/fcl/narrowphase/narrowphase.h @@ -202,7 +202,7 @@ struct HPP_FCL_DLLAPI GJKSolver { if (gjk.hasPenetrationInformation(shape)) { gjk.getClosestPoints(shape, w0, w1); distance = gjk.distance; - normal = tf1.getRotation() * (w0 - w1).normalized(); + normal.noalias() = tf1.getRotation() * (w0 - w1).normalized(); p1 = p2 = tf1.transform((w0 + w1) / 2); } else { details::EPA epa(epa_max_face_num, epa_max_vertex_num, diff --git a/src/collision_func_matrix.cpp b/src/collision_func_matrix.cpp index 67780e590..c7c921fcb 100644 --- a/src/collision_func_matrix.cpp +++ b/src/collision_func_matrix.cpp @@ -84,31 +84,28 @@ std::size_t ShapeShapeCollide(const CollisionGeometry* o1, o1, tf1, o2, tf2, nsolver, distanceRequest, distanceResult); size_t num_contacts = 0; - if (distance <= 0) { + const Vec3f& p1 = distanceResult.nearest_points[0]; + const Vec3f& p2 = distanceResult.nearest_points[1]; + FCL_REAL distToCollision = distance - request.security_margin; + + internal::updateDistanceLowerBoundFromLeaf(request, result, distToCollision, + p1, p2); + if (distToCollision <= request.collision_distance_threshold && + result.numContacts() < request.num_max_contacts) { if (result.numContacts() < request.num_max_contacts) { const Vec3f& p1 = distanceResult.nearest_points[0]; const Vec3f& p2 = distanceResult.nearest_points[1]; - Contact contact(o1, o2, distanceResult.b1, distanceResult.b2, - (p1 + p2) / 2, distanceResult.normal, - -distance + request.security_margin); + Contact contact( + o1, o2, distanceResult.b1, distanceResult.b2, (p1 + p2) / 2, + (distance <= 0 ? distanceResult.normal : (p2 - p1).normalized()), + -distance); result.addContact(contact); } num_contacts = result.numContacts(); - } else if (distance <= request.security_margin) { - if (result.numContacts() < request.num_max_contacts) { - const Vec3f& p1 = distanceResult.nearest_points[0]; - const Vec3f& p2 = distanceResult.nearest_points[1]; - - Contact contact(o1, o2, distanceResult.b1, distanceResult.b2, - .5 * (p1 + p2), (p2 - p1).normalized(), - -distance + request.security_margin); - result.addContact(contact); - } - num_contacts = result.numContacts(); } - result.updateDistanceLowerBound(distance); + return num_contacts; } diff --git a/src/collision_node.cpp b/src/collision_node.cpp index 0cdd1d6b1..75dc1a538 100644 --- a/src/collision_node.cpp +++ b/src/collision_node.cpp @@ -52,7 +52,16 @@ void collide(CollisionTraversalNodeBase* node, const CollisionRequest& request, collisionRecurse(node, 0, 0, front_list, sqrDistLowerBound); else collisionNonRecurse(node, front_list, sqrDistLowerBound); - result.updateDistanceLowerBound(sqrt(sqrDistLowerBound)); + + if (!std::isnan(sqrDistLowerBound)) { + if (sqrDistLowerBound == 0) { + assert(result.distance_lower_bound <= 0); + } else { + assert(result.distance_lower_bound * result.distance_lower_bound - + sqrDistLowerBound < + 1e-8); + } + } } } diff --git a/src/distance/sphere_sphere.cpp b/src/distance/sphere_sphere.cpp index 4e99c2098..ed1f044cc 100644 --- a/src/distance/sphere_sphere.cpp +++ b/src/distance/sphere_sphere.cpp @@ -38,6 +38,7 @@ #include #include #include +#include // Note that partial specialization of template functions is not allowed. // Therefore, two implementations with the default narrow phase solvers are @@ -106,22 +107,25 @@ std::size_t ShapeShapeCollide( const fcl::Vec3f& center2 = tf2.getTranslation(); FCL_REAL r1 = s1->radius; FCL_REAL r2 = s2->radius; - FCL_REAL margin(request.security_margin); + FCL_REAL margin = request.security_margin; Vec3f c1c2 = center2 - center1; FCL_REAL dist = c1c2.norm(); Vec3f unit(0, 0, 0); if (dist > epsilon) unit = c1c2 / dist; // Unlike in distance computation, we consider the security margin. - FCL_REAL penetrationDepth = r1 + r2 + margin - dist; - result.updateDistanceLowerBound(-penetrationDepth + margin); - bool collision = (penetrationDepth >= 0); - if (collision) { + FCL_REAL distToCollision = dist - (r1 + r2 + margin); + + internal::updateDistanceLowerBoundFromLeaf(request, result, distToCollision, + center1 + unit * r1, + center2 - unit * r2); + if (distToCollision <= request.collision_distance_threshold) { // Take contact point at the middle of intersection between each sphere // and segment [c1 c2]. FCL_REAL abscissa = .5 * r1 + .5 * (dist - r2); Vec3f contactPoint = center1 + abscissa * unit; - Contact contact(o1, o2, -1, -1, contactPoint, unit, penetrationDepth); + Contact contact(o1, o2, -1, -1, contactPoint, unit, + -(distToCollision + margin)); result.addContact(contact); return 1; } diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index dfc71feee..087327623 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -35,6 +35,7 @@ add_fcl_test(math math.cpp) add_fcl_test(collision collision.cpp) add_fcl_test(distance distance.cpp) add_fcl_test(distance_lower_bound distance_lower_bound.cpp) +add_fcl_test(security_margin security_margin.cpp) add_fcl_test(geometric_shapes geometric_shapes.cpp) #add_fcl_test(shape_mesh_consistency shape_mesh_consistency.cpp) add_fcl_test(frontlist frontlist.cpp) diff --git a/test/security_margin.cpp b/test/security_margin.cpp new file mode 100644 index 000000000..1647a4b7d --- /dev/null +++ b/test/security_margin.cpp @@ -0,0 +1,281 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, INRIA. + * All rights reserved. + * + * 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 Willow Garage, Inc. 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 OWNER 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. + */ + +#define BOOST_TEST_MODULE FCL_SECURITY_MARGIN +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "utility.h" + +using namespace hpp::fcl; +using hpp::fcl::CollisionGeometryPtr_t; +using hpp::fcl::CollisionObject; +using hpp::fcl::CollisionRequest; +using hpp::fcl::CollisionResult; +using hpp::fcl::DistanceRequest; +using hpp::fcl::DistanceResult; +using hpp::fcl::Transform3f; +using hpp::fcl::Vec3f; + +BOOST_AUTO_TEST_CASE(sphere_sphere) { + CollisionGeometryPtr_t s1(new hpp::fcl::Sphere(1)); + CollisionGeometryPtr_t s2(new hpp::fcl::Sphere(2)); + + const Transform3f tf1; + const Transform3f tf2_collision(Vec3f(0, 0, 3)); + + // No security margin - collision + { + CollisionRequest collisionRequest(CONTACT, 1); + CollisionResult collisionResult; + collide(s1.get(), tf1, s2.get(), tf2_collision, collisionRequest, + collisionResult); + BOOST_CHECK(collisionResult.isCollision()); + BOOST_CHECK_CLOSE(collisionResult.distance_lower_bound, 0, 1e-8); + BOOST_CHECK_SMALL(-collisionResult.getContact(0).penetration_depth, 1e-8); + } + + // No security margin - no collision + { + CollisionRequest collisionRequest(CONTACT, 1); + CollisionResult collisionResult; + const double distance = 0.01; + Transform3f tf2_no_collision( + Vec3f(tf2_collision.getTranslation() + Vec3f(0, 0, distance))); + collide(s1.get(), tf1, s2.get(), tf2_no_collision, collisionRequest, + collisionResult); + BOOST_CHECK(!collisionResult.isCollision()); + BOOST_CHECK_CLOSE(collisionResult.distance_lower_bound, distance, 1e-8); + } + + // Positive security margin + { + CollisionRequest collisionRequest(CONTACT, 1); + CollisionResult collisionResult; + const double distance = 0.01; + collisionRequest.security_margin = distance; + Transform3f tf2_no_collision( + Vec3f(tf2_collision.getTranslation() + Vec3f(0, 0, distance))); + collide(s1.get(), tf1, s2.get(), tf2_no_collision, collisionRequest, + collisionResult); + BOOST_CHECK(collisionResult.isCollision()); + BOOST_CHECK_CLOSE(collisionResult.distance_lower_bound, 0, 1e-8); + BOOST_CHECK_CLOSE(-collisionResult.getContact(0).penetration_depth, + distance, 1e-8); + } + + // Negative security margin - collion because the two spheres are in contact + { + CollisionRequest collisionRequest(CONTACT, 1); + CollisionResult collisionResult; + const double distance = -0.01; + collisionRequest.security_margin = distance; + Transform3f tf2( + Vec3f(tf2_collision.getTranslation() + Vec3f(0, 0, distance))); + collide(s1.get(), tf1, s2.get(), tf2, collisionRequest, collisionResult); + BOOST_CHECK(collisionResult.isCollision()); + BOOST_CHECK_SMALL(collisionResult.distance_lower_bound, 1e-8); + BOOST_CHECK_CLOSE(-collisionResult.getContact(0).penetration_depth, + distance, 1e-8); + } + + // Negative security margin - no collision + { + CollisionRequest collisionRequest(CONTACT, 1); + CollisionResult collisionResult; + collisionRequest.security_margin = -0.01; + collide(s1.get(), tf1, s2.get(), tf2_collision, collisionRequest, + collisionResult); + BOOST_CHECK(!collisionResult.isCollision()); + BOOST_CHECK_CLOSE(collisionResult.distance_lower_bound, 0.01, 1e-8); + } +} + +BOOST_AUTO_TEST_CASE(capsule_capsule) { + CollisionGeometryPtr_t c1(new hpp::fcl::Capsule(0.5, 1.)); + CollisionGeometryPtr_t c2(new hpp::fcl::Capsule(0.5, 1.)); + + const Transform3f tf1; + const Transform3f tf2_collision(Vec3f(0, 1., 0)); + + // No security margin - collision + { + CollisionRequest collisionRequest(CONTACT, 1); + CollisionResult collisionResult; + collide(c1.get(), tf1, c2.get(), tf2_collision, collisionRequest, + collisionResult); + BOOST_CHECK(collisionResult.isCollision()); + BOOST_CHECK_SMALL(collisionResult.distance_lower_bound, 1e-8); + BOOST_CHECK_SMALL(-collisionResult.getContact(0).penetration_depth, 1e-8); + } + + // No security margin - no collision + { + CollisionRequest collisionRequest(CONTACT, 1); + CollisionResult collisionResult; + const double distance = 0.01; + Transform3f tf2_no_collision( + Vec3f(tf2_collision.getTranslation() + Vec3f(0, distance, 0))); + collide(c1.get(), tf1, c2.get(), tf2_no_collision, collisionRequest, + collisionResult); + BOOST_CHECK(!collisionResult.isCollision()); + BOOST_CHECK_CLOSE(collisionResult.distance_lower_bound, distance, 1e-8); + } + + // Positive security margin - collision + { + CollisionRequest collisionRequest(CONTACT, 1); + CollisionResult collisionResult; + const double distance = 0.01; + collisionRequest.security_margin = distance; + Transform3f tf2_no_collision( + Vec3f(tf2_collision.getTranslation() + Vec3f(0, distance, 0))); + collide(c1.get(), tf1, c2.get(), tf2_no_collision, collisionRequest, + collisionResult); + BOOST_CHECK(collisionResult.isCollision()); + BOOST_CHECK_SMALL(collisionResult.distance_lower_bound, 1e-8); + BOOST_CHECK_CLOSE(-collisionResult.getContact(0).penetration_depth, + distance, 1e-8); + } + + // Negative security margin - collion because the two capsules are in contact + { + CollisionRequest collisionRequest(CONTACT, 1); + CollisionResult collisionResult; + const double distance = -0.01; + collisionRequest.security_margin = distance; + Transform3f tf2( + Vec3f(tf2_collision.getTranslation() + Vec3f(0, distance, 0))); + collide(c1.get(), tf1, c2.get(), tf2, collisionRequest, collisionResult); + BOOST_CHECK(collisionResult.isCollision()); + BOOST_CHECK_SMALL(collisionResult.distance_lower_bound, 1e-8); + BOOST_CHECK_CLOSE(-collisionResult.getContact(0).penetration_depth, + distance, 1e-8); + } + + // Negative security margin - no collision + { + CollisionRequest collisionRequest(CONTACT, 1); + CollisionResult collisionResult; + collisionRequest.security_margin = -0.01; + collide(c1.get(), tf1, c2.get(), tf2_collision, collisionRequest, + collisionResult); + BOOST_CHECK(!collisionResult.isCollision()); + BOOST_CHECK_CLOSE(collisionResult.distance_lower_bound, 0.01, 1e-8); + } +} + +BOOST_AUTO_TEST_CASE(box_box) { + CollisionGeometryPtr_t b1(new hpp::fcl::Box(1, 1, 1)); + CollisionGeometryPtr_t b2(new hpp::fcl::Box(1, 1, 1)); + + const Transform3f tf1; + const Transform3f tf2_collision(Vec3f(0, 0, 1)); + + const double tol = 1e-3; + + // No security margin - collision + { + CollisionRequest collisionRequest(CONTACT, 1); + CollisionResult collisionResult; + collide(b1.get(), tf1, b2.get(), tf2_collision, collisionRequest, + collisionResult); + BOOST_CHECK(collisionResult.isCollision()); + BOOST_CHECK_SMALL(collisionResult.distance_lower_bound, tol); + BOOST_CHECK_SMALL(-collisionResult.getContact(0).penetration_depth, 1e-8); + } + + // No security margin - no collision + { + CollisionRequest collisionRequest(CONTACT, 1); + const double distance = 0.01; + const Transform3f tf2_no_collision( + (tf2_collision.getTranslation() + Vec3f(0, 0, distance)).eval()); + + CollisionResult collisionResult; + collide(b1.get(), tf1, b2.get(), tf2_no_collision, collisionRequest, + collisionResult); + BOOST_CHECK(!collisionResult.isCollision()); + BOOST_CHECK_CLOSE(collisionResult.distance_lower_bound, distance, tol); + } + + // Positive security margin - collision + { + CollisionRequest collisionRequest(CONTACT, 1); + const double distance = 0.01; + collisionRequest.security_margin = distance; + CollisionResult collisionResult; + collide(b1.get(), tf1, b2.get(), tf2_collision, collisionRequest, + collisionResult); + BOOST_CHECK(collisionResult.isCollision()); + BOOST_CHECK_CLOSE(collisionResult.distance_lower_bound, + -collisionRequest.security_margin, tol); + BOOST_CHECK_SMALL(-collisionResult.getContact(0).penetration_depth, 1e-8); + } + + // Positive security margin - no collision + { + CollisionRequest collisionRequest(CONTACT, 1); + collisionRequest.security_margin = -0.01; + CollisionResult collisionResult; + collide(b1.get(), tf1, b2.get(), tf2_collision, collisionRequest, + collisionResult); + BOOST_CHECK(!collisionResult.isCollision()); + BOOST_CHECK_CLOSE(collisionResult.distance_lower_bound, + -collisionRequest.security_margin, tol); + } + + // Negative security margin - collision + { + CollisionRequest collisionRequest(CONTACT, 1); + collisionRequest.security_margin = -0.01; + CollisionResult collisionResult; + + const Transform3f tf2((tf2_collision.getTranslation() + + Vec3f(0, 0, collisionRequest.security_margin)) + .eval()); + collide(b1.get(), tf1, b2.get(), tf2, collisionRequest, collisionResult); + BOOST_CHECK(collisionResult.isCollision()); + BOOST_CHECK_SMALL(collisionResult.distance_lower_bound, tol); + BOOST_CHECK_CLOSE(-collisionResult.getContact(0).penetration_depth, + collisionRequest.security_margin, tol); + } +}