From 0c6bd4c046a6a652f7888b3f4e893ca0e9b07958 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Mon, 6 Jun 2022 13:23:11 +0200 Subject: [PATCH 01/52] gjk: add more comments --- src/narrowphase/gjk.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/narrowphase/gjk.cpp b/src/narrowphase/gjk.cpp index 4a417b5c8..ac7afb07d 100644 --- a/src/narrowphase/gjk.cpp +++ b/src/narrowphase/gjk.cpp @@ -769,8 +769,8 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess, FCL_REAL frank_wolfe_duality_gap = 2 * ray.dot(ray - w); if (frank_wolfe_duality_gap - tolerance <= 0) { removeVertex(simplices[current]); - current_gjk_variant = DefaultGJK; - continue; // continue to next iteration + current_gjk_variant = DefaultGJK; // move back to classic GJK + continue; // continue to next iteration } } @@ -783,7 +783,7 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess, if (iterations > 0 && cv_check_passed) { if (iterations > 0) removeVertex(simplices[current]); if (current_gjk_variant != DefaultGJK) { - current_gjk_variant = DefaultGJK; + current_gjk_variant = DefaultGJK; // move back to classic GJK continue; } distance = rl - inflation; From 17c202f3a577346e376b7839afcf7ea504dbe0f4 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Mon, 6 Jun 2022 18:28:59 +0200 Subject: [PATCH 02/52] shapes: add missing constructor for Ellipsoid --- include/hpp/fcl/shape/geometric_shapes.h | 2 ++ python/collision-geometries.cc | 1 + 2 files changed, 3 insertions(+) diff --git a/include/hpp/fcl/shape/geometric_shapes.h b/include/hpp/fcl/shape/geometric_shapes.h index 15b845e9d..f4414caa5 100644 --- a/include/hpp/fcl/shape/geometric_shapes.h +++ b/include/hpp/fcl/shape/geometric_shapes.h @@ -200,6 +200,8 @@ class HPP_FCL_DLLAPI Ellipsoid : public ShapeBase { Ellipsoid(FCL_REAL rx, FCL_REAL ry, FCL_REAL rz) : ShapeBase(), radii(rx, ry, rz) {} + Ellipsoid(const Vec3f& radii) : radii(radii) {} + Ellipsoid(const Ellipsoid& other) : ShapeBase(other), radii(other.radii) {} /// @brief Clone *this into a new Ellipsoid diff --git a/python/collision-geometries.cc b/python/collision-geometries.cc index 598356d9a..5abc2e872 100644 --- a/python/collision-geometries.cc +++ b/python/collision-geometries.cc @@ -322,6 +322,7 @@ void exposeShapes() { class_, shared_ptr >( "Ellipsoid", doxygen::class_doc(), no_init) .def(dv::init()) + .def(dv::init()) .DEF_RW_CLASS_ATTRIB(Ellipsoid, radii) .def("clone", &Ellipsoid::clone, doxygen::member_func_doc(&Ellipsoid::clone), From 588c3f1fbb1df2e2b6fdd96c968d25994047be94 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Mon, 6 Jun 2022 22:40:32 +0200 Subject: [PATCH 03/52] convex: fix warnings --- src/shape/convex.cpp | 48 +++++++++++++++++++++++++++----------------- 1 file changed, 30 insertions(+), 18 deletions(-) diff --git a/src/shape/convex.cpp b/src/shape/convex.cpp index 58fb6d24b..3841f0059 100644 --- a/src/shape/convex.cpp +++ b/src/shape/convex.cpp @@ -57,7 +57,7 @@ ConvexBase* ConvexBase::convexHull(const Vec3f* pts, unsigned int num_points, Qhull qh; const char* command = qhullCommand ? qhullCommand : (keepTriangles ? "Qt" : ""); - qh.runQhull("", 3, num_points, pts[0].data(), command); + qh.runQhull("", 3, static_cast(num_points), pts[0].data(), command); if (qh.qhullStatus() != qh_ERRnone) { if (qh.hasQhullMessage()) std::cerr << qh.qhullMessage() << std::endl; @@ -71,14 +71,14 @@ ConvexBase* ConvexBase::convexHull(const Vec3f* pts, unsigned int num_points, std::vector pts_to_vertices(num_points, -1); // Initialize the vertices - int nvertex = qh.vertexCount(); + int nvertex = (qh.vertexCount()); Vec3f* vertices = new Vec3f[nvertex]; QhullVertexList vertexList(qh.vertexList()); int i_vertex = 0; for (QhullVertexList::const_iterator v = vertexList.begin(); v != vertexList.end(); ++v) { QhullPoint pt((*v).point()); - pts_to_vertices[pt.id()] = i_vertex; + pts_to_vertices[(size_t)pt.id()] = (int)i_vertex; vertices[i_vertex] = Vec3f(pt[0], pt[1], pt[2]); ++i_vertex; } @@ -90,13 +90,13 @@ ConvexBase* ConvexBase::convexHull(const Vec3f* pts, unsigned int num_points, convex = convex_tri = new Convex(); else convex = new ConvexBase; - convex->initialize(true, vertices, nvertex); + convex->initialize(true, vertices, static_cast(nvertex)); // Build the neighbors convex->neighbors = new Neighbors[nvertex]; - std::vector > nneighbors(nvertex); + std::vector > nneighbors(static_cast(nvertex)); if (keepTriangles) { - convex_tri->num_polygons = qh.facetCount(); + convex_tri->num_polygons = static_cast(qh.facetCount()); convex_tri->polygons = new Triangle[convex_tri->num_polygons]; convex_tri->computeCenter(); } @@ -110,18 +110,22 @@ ConvexBase* ConvexBase::convexHull(const Vec3f* pts, unsigned int num_points, if (facet.isSimplicial()) { // In 3D, simplicial faces have 3 vertices. We mark them as neighbors. QhullVertexSet f_vertices(facet.vertices()); - int n = f_vertices.count(); + size_t n = static_cast(f_vertices.count()); assert(n == 3); - Triangle tri(pts_to_vertices[f_vertices[0].point().id()], - pts_to_vertices[f_vertices[1].point().id()], - pts_to_vertices[f_vertices[2].point().id()]); + Triangle tri( + static_cast( + pts_to_vertices[static_cast(f_vertices[0].point().id())]), + static_cast( + pts_to_vertices[static_cast(f_vertices[1].point().id())]), + static_cast(pts_to_vertices[static_cast( + f_vertices[2].point().id())])); if (keepTriangles) { reorderTriangle(convex_tri, tri); convex_tri->polygons[i_polygon++] = tri; } - for (size_type j = 0; j < n; ++j) { - size_type i = (j == 0) ? n - 1 : j - 1; - size_type k = (j == n - 1) ? 0 : j + 1; + for (size_t j = 0; j < n; ++j) { + size_t i = (j == 0) ? n - 1 : j - 1; + size_t k = (j == n - 1) ? 0 : j + 1; // Update neighbors of pj; if (nneighbors[tri[j]].insert(tri[i]).second) c_nneighbors++; if (nneighbors[tri[j]].insert(tri[k]).second) c_nneighbors++; @@ -138,11 +142,19 @@ ConvexBase* ConvexBase::convexHull(const Vec3f* pts, unsigned int num_points, QhullRidgeSet f_ridges(facet.ridges()); for (size_type j = 0; j < f_ridges.count(); ++j) { assert(f_ridges[j].vertices().count() == 2); - index_type pi = pts_to_vertices[f_ridges[j].vertices()[0].point().id()], - pj = pts_to_vertices[f_ridges[j].vertices()[1].point().id()]; + int pi = pts_to_vertices[static_cast( + f_ridges[j].vertices()[0].point().id())], + pj = pts_to_vertices[static_cast( + f_ridges[j].vertices()[1].point().id())]; // Update neighbors of pi and pj; - if (nneighbors[pj].insert(pi).second) c_nneighbors++; - if (nneighbors[pi].insert(pj).second) c_nneighbors++; + if (nneighbors[static_cast(pj)] + .insert(static_cast(pi)) + .second) + c_nneighbors++; + if (nneighbors[static_cast(pi)] + .insert(static_cast(pj)) + .second) + c_nneighbors++; } } } @@ -151,7 +163,7 @@ ConvexBase* ConvexBase::convexHull(const Vec3f* pts, unsigned int num_points, // Fill the neighbor attribute of the returned object. convex->nneighbors_ = new unsigned int[c_nneighbors]; unsigned int* p_nneighbors = convex->nneighbors_; - for (int i = 0; i < nvertex; ++i) { + for (size_t i = 0; i < static_cast(nvertex); ++i) { Neighbors& n = convex->neighbors[i]; if (nneighbors[i].size() >= (std::numeric_limits::max)()) throw std::logic_error("Too many neighbors."); From 574055c4634a07398c65efa23b042e83615ccfbb Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Tue, 7 Jun 2022 17:59:36 +0200 Subject: [PATCH 04/52] core: add precision to isIdentity --- include/hpp/fcl/math/transform.h | 6 +++++- python/math.cc | 5 ++++- 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/include/hpp/fcl/math/transform.h b/include/hpp/fcl/math/transform.h index 11e3e02e5..9b4cec0a6 100644 --- a/include/hpp/fcl/math/transform.h +++ b/include/hpp/fcl/math/transform.h @@ -184,7 +184,11 @@ class HPP_FCL_DLLAPI Transform3f { } /// @brief check whether the transform is identity - inline bool isIdentity() const { return R.isIdentity() && T.isZero(); } + inline bool isIdentity( + const FCL_REAL& prec = + Eigen::NumTraits::dummy_precision()) const { + return R.isIdentity(prec) && T.isZero(prec); + } /// @brief set the transform to be identity transform inline void setIdentity() { diff --git a/python/math.cc b/python/math.cc index 5308c8c3f..fdd7dd09f 100644 --- a/python/math.cc +++ b/python/math.cc @@ -90,7 +90,10 @@ void exposeMaths() { return_value_policy()) .def("getRotation", &Transform3f::getRotation, return_value_policy()) - .def(dv::member_func("isIdentity", &Transform3f::isIdentity)) + .def("isIdentity", &Transform3f::isIdentity, + (bp::arg("self"), + bp::arg("prec") = Eigen::NumTraits::dummy_precision()), + doxygen::member_func_doc(&Transform3f::getTranslation)) .def(dv::member_func("setQuatRotation", &Transform3f::setQuatRotation)) .def("setTranslation", &Transform3f::setTranslation) From 0cd38111156cf6dabdcbe3df27685bd6ecd19a80 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Tue, 7 Jun 2022 18:00:20 +0200 Subject: [PATCH 05/52] core: add inflated method to basic shapes --- include/hpp/fcl/shape/geometric_shapes.h | 117 +++++++++++++++++++++++ 1 file changed, 117 insertions(+) diff --git a/include/hpp/fcl/shape/geometric_shapes.h b/include/hpp/fcl/shape/geometric_shapes.h index f4414caa5..86e22f432 100644 --- a/include/hpp/fcl/shape/geometric_shapes.h +++ b/include/hpp/fcl/shape/geometric_shapes.h @@ -59,6 +59,14 @@ class HPP_FCL_DLLAPI ShapeBase : public CollisionGeometry { virtual ~ShapeBase(){}; + /// \brief Inflate the shape by an amount given by value + /// + /// \param[in] value of the shape inflation. + /// + /// \returns a new inflated shape + virtual std::pair inflated( + const FCL_REAL value) const = 0; + /// @brief Get object type: a geometric shape OBJECT_TYPE getObjectType() const { return OT_GEOM; } }; @@ -84,6 +92,20 @@ class HPP_FCL_DLLAPI TriangleP : public ShapeBase { NODE_TYPE getNodeType() const { return GEOM_TRIANGLE; } + std::pair inflated(const FCL_REAL value) const { + if (value == 0) return std::make_pair(new TriangleP(*this), Transform3f()); + Vec3f AB(b - a), BC(c - b), CA(a - c); + AB.normalize(); + BC.normalize(); + CA.normalize(); + + Vec3f new_a(a + value * Vec3f(-AB + CA).normalized()); + Vec3f new_b(b + value * Vec3f(-BC + AB).normalized()); + Vec3f new_c(c + value * Vec3f(-CA + BC).normalized()); + + return std::make_pair(new TriangleP(new_a, new_b, new_c), Transform3f()); + } + Vec3f a, b, c; private: @@ -139,6 +161,17 @@ class HPP_FCL_DLLAPI Box : public ShapeBase { return (Vec3f(s[1] + s[2], s[0] + s[2], s[0] + s[1]) / 3).asDiagonal(); } + std::pair inflated(const FCL_REAL value) const { + const FCL_REAL minimal_value = -halfSide.minCoeff(); + if (value <= minimal_value) + HPP_FCL_THROW_PRETTY( + "value (" << value << ") " + << "is two small. It should be at least: " << minimal_value, + std::invalid_argument); + return std::make_pair(new Box(2 * (halfSide + Vec3f::Constant(value))), + Transform3f()); + } + private: virtual bool isEqual(const CollisionGeometry& _other) const { const Box* other_ptr = dynamic_cast(&_other); @@ -181,6 +214,15 @@ class HPP_FCL_DLLAPI Sphere : public ShapeBase { radius / 3; } + std::pair inflated(const FCL_REAL value) const { + const FCL_REAL minimal_value = -radius; + if (value <= minimal_value) + HPP_FCL_THROW_PRETTY( + "value is two small. It should be at least: " << minimal_value, + std::invalid_argument); + return std::make_pair(new Sphere(radius + value), Transform3f()); + } + private: virtual bool isEqual(const CollisionGeometry& _other) const { const Sphere* other_ptr = dynamic_cast(&_other); @@ -232,6 +274,16 @@ class HPP_FCL_DLLAPI Ellipsoid : public ShapeBase { radii[2] / 3; } + std::pair inflated(const FCL_REAL value) const { + const FCL_REAL minimal_value = -radii.minCoeff(); + if (value <= minimal_value) + HPP_FCL_THROW_PRETTY( + "value is two small. It should be at least: " << minimal_value, + std::invalid_argument); + return std::make_pair(new Ellipsoid(radii + Vec3f::Constant(value)), + Transform3f()); + } + private: virtual bool isEqual(const CollisionGeometry& _other) const { const Ellipsoid* other_ptr = dynamic_cast(&_other); @@ -293,6 +345,16 @@ class HPP_FCL_DLLAPI Capsule : public ShapeBase { return (Matrix3f() << ix, 0, 0, 0, ix, 0, 0, 0, iz).finished(); } + std::pair inflated(const FCL_REAL value) const { + const FCL_REAL minimal_value = -radius; + if (value <= minimal_value) + HPP_FCL_THROW_PRETTY( + "value is two small. It should be at least: " << minimal_value, + std::invalid_argument); + return std::make_pair(new Capsule(radius + value, 2 * halfLength), + Transform3f()); + } + private: virtual bool isEqual(const CollisionGeometry& _other) const { const Capsule* other_ptr = dynamic_cast(&_other); @@ -349,6 +411,27 @@ class HPP_FCL_DLLAPI Cone : public ShapeBase { Vec3f computeCOM() const { return Vec3f(0, 0, -0.5 * halfLength); } + std::pair inflated(const FCL_REAL value) const { + const FCL_REAL minimal_value = -(std::min)(radius, halfLength); + if (value <= minimal_value) + HPP_FCL_THROW_PRETTY( + "value is two small. It should be at least: " << minimal_value, + std::invalid_argument); + + // tan(alpha) = 2*halfLength/radius; + const FCL_REAL tan_alpha = 2 * halfLength / radius; + const FCL_REAL sin_alpha = tan_alpha / std::sqrt(1 + tan_alpha * tan_alpha); + const FCL_REAL top_inflation = value / sin_alpha; + const FCL_REAL bottom_inflation = value; + + const FCL_REAL new_lz = 2 * halfLength + top_inflation + bottom_inflation; + const FCL_REAL new_cz = (top_inflation + bottom_inflation) / 2.; + const FCL_REAL new_radius = new_lz / tan_alpha; + + return std::make_pair(new Cone(new_radius, new_lz), + Transform3f(Vec3f(0., 0., new_cz))); + } + private: virtual bool isEqual(const CollisionGeometry& _other) const { const Cone* other_ptr = dynamic_cast(&_other); @@ -408,6 +491,16 @@ class HPP_FCL_DLLAPI Cylinder : public ShapeBase { return (Matrix3f() << ix, 0, 0, 0, ix, 0, 0, 0, iz).finished(); } + std::pair inflated(const FCL_REAL value) const { + const FCL_REAL minimal_value = -(std::min)(radius, halfLength); + if (value <= minimal_value) + HPP_FCL_THROW_PRETTY( + "value is two small. It should be at least: " << minimal_value, + std::invalid_argument); + return std::make_pair( + new Cylinder(radius + value, 2 * (halfLength + value)), Transform3f()); + } + private: virtual bool isEqual(const CollisionGeometry& _other) const { const Cylinder* other_ptr = dynamic_cast(&_other); @@ -503,6 +596,16 @@ class HPP_FCL_DLLAPI ConvexBase : public ShapeBase { /// is guaranteed in the internal of the polytope (as it is convex) Vec3f center; + /// @copydoc ShapeBase::inflated + /// @remarks Inflated should not be applied on Convex objects. + std::pair inflated(const FCL_REAL value) const { + HPP_FCL_UNUSED_VARIABLE(value); + HPP_FCL_THROW_PRETTY( + "ConvexBase::inflated has not been implemented so far.", + std::logic_error); + return std::make_pair(static_cast(NULL), Transform3f()); + } + protected: /// @brief Construct an uninitialized convex object /// Initialization is done with ConvexBase::initialize. @@ -615,6 +718,13 @@ class HPP_FCL_DLLAPI Halfspace : public ShapeBase { /// @brief Plane offset FCL_REAL d; + /// @copydoc ShapeBase::inflated + /// @remarks Inflated method has no effect on the Halfspace + std::pair inflated(const FCL_REAL value) const { + HPP_FCL_UNUSED_VARIABLE(value); + return std::make_pair(clone(), Transform3f()); + } + protected: /// @brief Turn non-unit normal into unit void unitNormalTest(); @@ -676,6 +786,13 @@ class HPP_FCL_DLLAPI Plane : public ShapeBase { /// @brief Plane offset FCL_REAL d; + /// @copydoc ShapeBase::inflated + /// @remarks Inflated method has no effect on the Plane + std::pair inflated(const FCL_REAL value) const { + HPP_FCL_UNUSED_VARIABLE(value); + return std::make_pair(clone(), Transform3f()); + } + protected: /// @brief Turn non-unit normal into unit void unitNormalTest(); From dd2dc9cc11032c425477a9daf62b2227d10897ae Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Tue, 7 Jun 2022 18:00:39 +0200 Subject: [PATCH 06/52] test: test shape inflation --- test/CMakeLists.txt | 1 + test/shape_inflation.cpp | 196 +++++++++++++++++++++++++++++++++++++++ 2 files changed, 197 insertions(+) create mode 100644 test/shape_inflation.cpp diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 087327623..6056713e6 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -37,6 +37,7 @@ 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_inflation shape_inflation.cpp) #add_fcl_test(shape_mesh_consistency shape_mesh_consistency.cpp) add_fcl_test(frontlist frontlist.cpp) SET_TESTS_PROPERTIES(frontlist PROPERTIES TIMEOUT 7200) diff --git a/test/shape_inflation.cpp b/test/shape_inflation.cpp new file mode 100644 index 000000000..3558e2185 --- /dev/null +++ b/test/shape_inflation.cpp @@ -0,0 +1,196 @@ +/* + * 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 + +#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; + +#define MATH_SQUARED(x) (x * x) + +template +bool isApprox(const Shape &s1, const Shape &s2, const FCL_REAL tol); + +bool isApprox(const FCL_REAL &v1, const FCL_REAL &v2, const FCL_REAL tol) { + typedef Eigen::Matrix ScalarMatrix; + ScalarMatrix m1; + m1 << v1; + ScalarMatrix m2; + m2 << v2; + return m1.isApprox(m2, tol); +} + +bool isApprox(const Box &s1, const Box &s2, const FCL_REAL tol) { + return s1.halfSide.isApprox(s2.halfSide, tol); +} + +bool isApprox(const Sphere &s1, const Sphere &s2, const FCL_REAL tol) { + return isApprox(s1.radius, s2.radius, tol); +} + +bool isApprox(const Ellipsoid &s1, const Ellipsoid &s2, const FCL_REAL tol) { + return s1.radii.isApprox(s2.radii, tol); +} + +bool isApprox(const Capsule &s1, const Capsule &s2, const FCL_REAL tol) { + return isApprox(s1.radius, s2.radius, tol) && + isApprox(s1.halfLength, s2.halfLength, tol); +} + +bool isApprox(const Cylinder &s1, const Cylinder &s2, const FCL_REAL tol) { + return isApprox(s1.radius, s2.radius, tol) && + isApprox(s1.halfLength, s2.halfLength, tol); +} + +bool isApprox(const Cone &s1, const Cone &s2, const FCL_REAL tol) { + return isApprox(s1.radius, s2.radius, tol) && + isApprox(s1.halfLength, s2.halfLength, tol); +} + +bool isApprox(const TriangleP &s1, const TriangleP &s2, const FCL_REAL tol) { + return s1.a.isApprox(s2.a, tol) && s1.b.isApprox(s2.b, tol) && + s1.c.isApprox(s2.c, tol); +} + +template +void test(const Shape &original_shape, const FCL_REAL inflation, + const FCL_REAL tol = 1e-8) { + // Zero inflation + { + const FCL_REAL inflation = 0.; + const auto &inflation_result = original_shape.inflated(inflation); + const Transform3f &shift = inflation_result.second; + std::shared_ptr inflated_shape_ptr( + static_cast(inflation_result.first)); + + BOOST_CHECK(isApprox(original_shape, *inflated_shape_ptr, tol)); + BOOST_CHECK(shift.isIdentity(tol)); + } + + // Positive inflation + { + const auto &inflation_result = original_shape.inflated(inflation); + std::shared_ptr inflated_shape_ptr( + static_cast(inflation_result.first)); + const Transform3f &inflation_shift = inflation_result.second; + + BOOST_CHECK(!isApprox(original_shape, *inflated_shape_ptr, tol)); + + const auto &deflation_result = inflated_shape_ptr->inflated(-inflation); + std::shared_ptr deflated_shape_ptr( + static_cast(deflation_result.first)); + const Transform3f &deflation_shift = deflation_result.second; + + BOOST_CHECK(isApprox(original_shape, *deflated_shape_ptr, tol)); + BOOST_CHECK((inflation_shift * deflation_shift).isIdentity(tol)); + } + + // Negative inflation + { + const auto &inflation_result = original_shape.inflated(-inflation); + std::shared_ptr inflated_shape_ptr( + static_cast(inflation_result.first)); + const Transform3f &inflation_shift = inflation_result.second; + + BOOST_CHECK(!isApprox(original_shape, *inflated_shape_ptr, tol)); + + const auto &deflation_result = inflated_shape_ptr->inflated(+inflation); + std::shared_ptr deflated_shape_ptr( + static_cast(deflation_result.first)); + const Transform3f &deflation_shift = deflation_result.second; + + BOOST_CHECK(isApprox(original_shape, *deflated_shape_ptr, tol)); + BOOST_CHECK((inflation_shift * deflation_shift).isIdentity(tol)); + } +} + +template +void test_throw(const Shape &shape, const FCL_REAL inflation) { + BOOST_REQUIRE_THROW(shape.inflated(inflation), std::invalid_argument); +} + +template +void test_no_throw(const Shape &shape, const FCL_REAL inflation) { + BOOST_REQUIRE_NO_THROW(shape.inflated(inflation)); +} + +BOOST_AUTO_TEST_CASE(test_inflate) { + const hpp::fcl::Sphere sphere(1); + test(sphere, 0.01, 1e-8); + test_throw(sphere, -1.1); + test_no_throw(sphere, 1.); + + const hpp::fcl::Box box(1, 1, 1); + test(box, 0.01, 1e-8); + test_throw(box, -0.6); + + const hpp::fcl::Ellipsoid ellipsoid(1, 2, 3); + test(ellipsoid, 0.01, 1e-8); + test_throw(ellipsoid, -1.1); + + const hpp::fcl::Capsule capsule(1., 2.); + test(capsule, 0.01, 1e-8); + test_throw(capsule, -1.1); + + const hpp::fcl::Cylinder cylinder(1., 2.); + test(cylinder, 0.01, 1e-8); + test_throw(cylinder, -1.1); + + const hpp::fcl::Cone cone(1., 4.); + test(cone, 0.01, 1e-8); + test_throw(cone, -1.1); + + const hpp::fcl::TriangleP triangle(Vec3f::UnitX(), Vec3f::UnitY(), + Vec3f::UnitZ()); + test(triangle, 0.01, 1e-8); +} From b01cbcb91b764088123d49f60e1c3da3e26e20ca Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Tue, 7 Jun 2022 18:29:48 +0200 Subject: [PATCH 07/52] test: fix compilation warnings --- test/convex.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/test/convex.cpp b/test/convex.cpp index c7e15d217..d55cab48a 100644 --- a/test/convex.cpp +++ b/test/convex.cpp @@ -225,8 +225,8 @@ BOOST_AUTO_TEST_CASE(convex_hull_quad) { Vec3f(0, 0, 1), }); - ConvexBase* convexHull = - ConvexBase::convexHull(points.data(), (int)points.size(), false, NULL); + ConvexBase* convexHull = ConvexBase::convexHull( + points.data(), (unsigned int)points.size(), false, NULL); BOOST_REQUIRE_EQUAL(convexHull->num_points, 4); BOOST_CHECK_EQUAL(convexHull->neighbors[0].count(), 3); @@ -249,8 +249,8 @@ BOOST_AUTO_TEST_CASE(convex_hull_box_like) { Vec3f(0, 0, 0.99), }); - ConvexBase* convexHull = - ConvexBase::convexHull(points.data(), (int)points.size(), false, NULL); + ConvexBase* convexHull = ConvexBase::convexHull( + points.data(), (unsigned int)points.size(), false, NULL); BOOST_REQUIRE_EQUAL(8, convexHull->num_points); for (int i = 0; i < 8; ++i) { @@ -259,8 +259,8 @@ BOOST_AUTO_TEST_CASE(convex_hull_box_like) { } delete convexHull; - convexHull = - ConvexBase::convexHull(points.data(), (int)points.size(), true, NULL); + convexHull = ConvexBase::convexHull(points.data(), + (unsigned int)points.size(), true, NULL); Convex* convex_tri = dynamic_cast*>(convexHull); BOOST_CHECK(convex_tri != NULL); From 59aa8fee83e4e3ba6c36dc254503f1173a3476c3 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Tue, 7 Jun 2022 21:03:03 +0200 Subject: [PATCH 08/52] doc: fix table --- src/narrowphase/narrowphase.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/narrowphase/narrowphase.cpp b/src/narrowphase/narrowphase.cpp index 1d63d044c..51fa9cddf 100644 --- a/src/narrowphase/narrowphase.cpp +++ b/src/narrowphase/narrowphase.cpp @@ -364,7 +364,7 @@ bool GJKSolver::shapeTriangleInteraction(const Plane& s, const Transform3f& tf1, // +------------+-----+--------+---------+------+----------+-------+------------+----------+ // | box | | O | | | | | | | // +------------+-----+--------+---------+------+----------+-------+------------+----------+ -// | sphere |/////| O | O | | | | | | +// | sphere |/////| O | O | | O | | | | // +------------+-----+--------+---------+------+----------+-------+------------+----------+ // | capsule |/////|////////| O | | | | | | // +------------+-----+--------+---------+------+----------+-------+------------+----------+ From e8de9438aaa43e5242594f9e05793fb02c99a900 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Tue, 14 Jun 2022 12:48:39 +0200 Subject: [PATCH 09/52] core: rename Collide -> OctreeCollide --- src/collision_func_matrix.cpp | 99 ++++++++++++++++++++--------------- 1 file changed, 57 insertions(+), 42 deletions(-) diff --git a/src/collision_func_matrix.cpp b/src/collision_func_matrix.cpp index c7c921fcb..c033110ac 100644 --- a/src/collision_func_matrix.cpp +++ b/src/collision_func_matrix.cpp @@ -49,10 +49,11 @@ namespace fcl { #ifdef HPP_FCL_HAS_OCTOMAP template -std::size_t Collide(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver* nsolver, const CollisionRequest& request, - CollisionResult& result) { +std::size_t OctreeCollide(const CollisionGeometry* o1, const Transform3f& tf1, + const CollisionGeometry* o2, const Transform3f& tf2, + const GJKSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result) { if (request.isSatisfied(result)) return result.numContacts(); typename TraversalTraitsCollision::CollisionTraversal_t node( @@ -640,51 +641,65 @@ CollisionFunctionMatrix::CollisionFunctionMatrix() { collision_matrix[BV_OBBRSS][BV_OBBRSS] = &BVHCollide; #ifdef HPP_FCL_HAS_OCTOMAP - collision_matrix[GEOM_OCTREE][GEOM_BOX] = &Collide; - collision_matrix[GEOM_OCTREE][GEOM_SPHERE] = &Collide; - collision_matrix[GEOM_OCTREE][GEOM_CAPSULE] = &Collide; - collision_matrix[GEOM_OCTREE][GEOM_CONE] = &Collide; - collision_matrix[GEOM_OCTREE][GEOM_CYLINDER] = &Collide; - collision_matrix[GEOM_OCTREE][GEOM_CONVEX] = &Collide; - collision_matrix[GEOM_OCTREE][GEOM_PLANE] = &Collide; - collision_matrix[GEOM_OCTREE][GEOM_HALFSPACE] = &Collide; - collision_matrix[GEOM_OCTREE][GEOM_ELLIPSOID] = &Collide; - - collision_matrix[GEOM_BOX][GEOM_OCTREE] = &Collide; - collision_matrix[GEOM_SPHERE][GEOM_OCTREE] = &Collide; - collision_matrix[GEOM_CAPSULE][GEOM_OCTREE] = &Collide; - collision_matrix[GEOM_CONE][GEOM_OCTREE] = &Collide; - collision_matrix[GEOM_CYLINDER][GEOM_OCTREE] = &Collide; - collision_matrix[GEOM_CONVEX][GEOM_OCTREE] = &Collide; - collision_matrix[GEOM_PLANE][GEOM_OCTREE] = &Collide; - collision_matrix[GEOM_HALFSPACE][GEOM_OCTREE] = &Collide; - - collision_matrix[GEOM_OCTREE][GEOM_OCTREE] = &Collide; - - collision_matrix[GEOM_OCTREE][BV_AABB] = &Collide >; - collision_matrix[GEOM_OCTREE][BV_OBB] = &Collide >; - collision_matrix[GEOM_OCTREE][BV_RSS] = &Collide >; + collision_matrix[GEOM_OCTREE][GEOM_BOX] = &OctreeCollide; + collision_matrix[GEOM_OCTREE][GEOM_SPHERE] = &OctreeCollide; + collision_matrix[GEOM_OCTREE][GEOM_CAPSULE] = &OctreeCollide; + collision_matrix[GEOM_OCTREE][GEOM_CONE] = &OctreeCollide; + collision_matrix[GEOM_OCTREE][GEOM_CYLINDER] = + &OctreeCollide; + collision_matrix[GEOM_OCTREE][GEOM_CONVEX] = + &OctreeCollide; + collision_matrix[GEOM_OCTREE][GEOM_PLANE] = &OctreeCollide; + collision_matrix[GEOM_OCTREE][GEOM_HALFSPACE] = + &OctreeCollide; + collision_matrix[GEOM_OCTREE][GEOM_ELLIPSOID] = + &OctreeCollide; + + collision_matrix[GEOM_BOX][GEOM_OCTREE] = &OctreeCollide; + collision_matrix[GEOM_SPHERE][GEOM_OCTREE] = &OctreeCollide; + collision_matrix[GEOM_CAPSULE][GEOM_OCTREE] = &OctreeCollide; + collision_matrix[GEOM_CONE][GEOM_OCTREE] = &OctreeCollide; + collision_matrix[GEOM_CYLINDER][GEOM_OCTREE] = + &OctreeCollide; + collision_matrix[GEOM_CONVEX][GEOM_OCTREE] = + &OctreeCollide; + collision_matrix[GEOM_PLANE][GEOM_OCTREE] = &OctreeCollide; + collision_matrix[GEOM_HALFSPACE][GEOM_OCTREE] = + &OctreeCollide; + + collision_matrix[GEOM_OCTREE][GEOM_OCTREE] = &OctreeCollide; + + collision_matrix[GEOM_OCTREE][BV_AABB] = + &OctreeCollide >; + collision_matrix[GEOM_OCTREE][BV_OBB] = + &OctreeCollide >; + collision_matrix[GEOM_OCTREE][BV_RSS] = + &OctreeCollide >; collision_matrix[GEOM_OCTREE][BV_OBBRSS] = - &Collide >; - collision_matrix[GEOM_OCTREE][BV_kIOS] = &Collide >; + &OctreeCollide >; + collision_matrix[GEOM_OCTREE][BV_kIOS] = + &OctreeCollide >; collision_matrix[GEOM_OCTREE][BV_KDOP16] = - &Collide > >; + &OctreeCollide > >; collision_matrix[GEOM_OCTREE][BV_KDOP18] = - &Collide > >; + &OctreeCollide > >; collision_matrix[GEOM_OCTREE][BV_KDOP24] = - &Collide > >; - - collision_matrix[BV_AABB][GEOM_OCTREE] = &Collide, OcTree>; - collision_matrix[BV_OBB][GEOM_OCTREE] = &Collide, OcTree>; - collision_matrix[BV_RSS][GEOM_OCTREE] = &Collide, OcTree>; - collision_matrix[BV_OBBRSS][GEOM_OCTREE] = &Collide, OcTree>; - collision_matrix[BV_kIOS][GEOM_OCTREE] = &Collide, OcTree>; + &OctreeCollide > >; + + collision_matrix[BV_AABB][GEOM_OCTREE] = + &OctreeCollide, OcTree>; + collision_matrix[BV_OBB][GEOM_OCTREE] = &OctreeCollide, OcTree>; + collision_matrix[BV_RSS][GEOM_OCTREE] = &OctreeCollide, OcTree>; + collision_matrix[BV_OBBRSS][GEOM_OCTREE] = + &OctreeCollide, OcTree>; + collision_matrix[BV_kIOS][GEOM_OCTREE] = + &OctreeCollide, OcTree>; collision_matrix[BV_KDOP16][GEOM_OCTREE] = - &Collide >, OcTree>; + &OctreeCollide >, OcTree>; collision_matrix[BV_KDOP18][GEOM_OCTREE] = - &Collide >, OcTree>; + &OctreeCollide >, OcTree>; collision_matrix[BV_KDOP24][GEOM_OCTREE] = - &Collide >, OcTree>; + &OctreeCollide >, OcTree>; #endif } // template struct CollisionFunctionMatrix; From 74bc6b90f134452ae6e69e61bc92f7f2a339db21 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Tue, 14 Jun 2022 12:51:42 +0200 Subject: [PATCH 10/52] core: fix --- include/hpp/fcl/internal/traversal_node_hfield_shape.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/include/hpp/fcl/internal/traversal_node_hfield_shape.h b/include/hpp/fcl/internal/traversal_node_hfield_shape.h index 19f3a928e..5236a9fd7 100644 --- a/include/hpp/fcl/internal/traversal_node_hfield_shape.h +++ b/include/hpp/fcl/internal/traversal_node_hfield_shape.h @@ -516,16 +516,16 @@ class ShapeHeightFieldCollisionTraversalNode if (this->request.num_max_contacts > this->result->numContacts()) { this->result->addContact(Contact(this->model1, this->model2, Contact::NONE, b2, c1, normal, - distance)); + -distance)); assert(this->result->isCollision()); return; } - } else if (distToCollision < 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, b2, .5 * (c1 + c2), - (c2 - c1).normalized(), distance)); + (c2 - c1).normalized(), -distance)); } } else sqrDistLowerBound = distToCollision * distToCollision; From b3189be6d6512cf6f7c863e7e79b49ae5d249446 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Tue, 14 Jun 2022 13:15:33 +0200 Subject: [PATCH 11/52] cmake: add missing file --- CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index db65d593b..8c15e5ea9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -232,6 +232,7 @@ SET(${PROJECT_NAME}_HEADERS include/hpp/fcl/mesh_loader/loader.h include/hpp/fcl/internal/BV_fitter.h include/hpp/fcl/internal/BV_splitter.h + include/hpp/fcl/internal/shape_shape_func.h include/hpp/fcl/internal/intersect.h include/hpp/fcl/internal/tools.h include/hpp/fcl/internal/traversal_node_base.h From 95660dc8da8aa4416a4028e26a153377c6ed6d6c Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Tue, 14 Jun 2022 13:15:57 +0200 Subject: [PATCH 12/52] core: move content to the right file --- include/hpp/fcl/collision_data.h | 22 +++++++++++++++++++ .../hpp/fcl/internal/traversal_node_base.h | 22 ------------------- 2 files changed, 22 insertions(+), 22 deletions(-) diff --git a/include/hpp/fcl/collision_data.h b/include/hpp/fcl/collision_data.h index 40e9264a7..ab1d432cf 100644 --- a/include/hpp/fcl/collision_data.h +++ b/include/hpp/fcl/collision_data.h @@ -534,6 +534,28 @@ struct HPP_FCL_DLLAPI DistanceResult : QueryResult { } }; +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 + inline CollisionRequestFlag operator~(CollisionRequestFlag a) { return static_cast(~static_cast(a)); } diff --git a/include/hpp/fcl/internal/traversal_node_base.h b/include/hpp/fcl/internal/traversal_node_base.h index adc58042b..173fb58a1 100644 --- a/include/hpp/fcl/internal/traversal_node_base.h +++ b/include/hpp/fcl/internal/traversal_node_base.h @@ -167,28 +167,6 @@ 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 From d09cfcea23f75485c2cb23d21b814be26f9b7751 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Wed, 15 Jun 2022 18:35:12 +0200 Subject: [PATCH 13/52] core: remove setNormalizeSupportDirection from MinkowskiDiff --- include/hpp/fcl/narrowphase/gjk.h | 9 --------- python/gjk.cc | 4 ++-- test/nesterov_gjk.cpp | 2 -- 3 files changed, 2 insertions(+), 13 deletions(-) diff --git a/include/hpp/fcl/narrowphase/gjk.h b/include/hpp/fcl/narrowphase/gjk.h index c129803c0..8f9ccf77c 100644 --- a/include/hpp/fcl/narrowphase/gjk.h +++ b/include/hpp/fcl/narrowphase/gjk.h @@ -130,15 +130,6 @@ struct HPP_FCL_DLLAPI MinkowskiDiff { getSupportFunc(*this, d, dIsNormalized, supp0, supp1, hint, const_cast(data)); } - - /// @brief Set wether or not to use the normalization heuristic when computing - /// a support point. Only effective if acceleration version of GJK is used. - /// By default, when MinkowskiDiff::set is called, the normalization heuristic - /// is deduced from the shapes. The user can override this behavior with this - /// function. - inline void setNormalizeSupportDirection(bool normalize) { - normalize_support_direction = normalize; - } }; /// @brief class for GJK algorithm diff --git a/python/gjk.cc b/python/gjk.cc index 4dd2c72e7..c9e9f856f 100644 --- a/python/gjk.cc +++ b/python/gjk.cc @@ -80,8 +80,8 @@ void exposeGJK() { .DEF_CLASS_FUNC(MinkowskiDiff, support0) .DEF_CLASS_FUNC(MinkowskiDiff, support1) .DEF_CLASS_FUNC(MinkowskiDiff, support) - .DEF_CLASS_FUNC(MinkowskiDiff, setNormalizeSupportDirection) - .DEF_RW_CLASS_ATTRIB(MinkowskiDiff, inflation); + .DEF_RW_CLASS_ATTRIB(MinkowskiDiff, inflation) + .DEF_RW_CLASS_ATTRIB(MinkowskiDiff, normalize_support_direction); } if (!eigenpy::register_symbolic_link_to_registered_type()) { diff --git a/test/nesterov_gjk.cpp b/test/nesterov_gjk.cpp index 9f3efa52e..a75aeb157 100644 --- a/test/nesterov_gjk.cpp +++ b/test/nesterov_gjk.cpp @@ -74,11 +74,9 @@ BOOST_AUTO_TEST_CASE(set_gjk_variant) { // Checking set solver.gjk_variant = GJKVariant::NesterovAcceleration; gjk.gjk_variant = GJKVariant::NesterovAcceleration; - shape.setNormalizeSupportDirection(true); BOOST_CHECK(solver.gjk_variant == GJKVariant::NesterovAcceleration); BOOST_CHECK(gjk.gjk_variant == GJKVariant::NesterovAcceleration); - BOOST_CHECK(shape.normalize_support_direction == true); } BOOST_AUTO_TEST_CASE(need_nesterov_normalize_support_direction) { From 1749142f3c8410c5e95062888edb8a5714d903f0 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Fri, 17 Jun 2022 18:40:21 +0200 Subject: [PATCH 14/52] core: add missing noalias --- src/narrowphase/gjk.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/narrowphase/gjk.cpp b/src/narrowphase/gjk.cpp index ac7afb07d..b2f27ca12 100644 --- a/src/narrowphase/gjk.cpp +++ b/src/narrowphase/gjk.cpp @@ -538,9 +538,9 @@ void MinkowskiDiff::set(const ShapeBase* shape0, const ShapeBase* shape1, getNormalizeSupportDirectionFromShapes(shape0, shape1, normalize_support_direction); - oR1 = tf0.getRotation().transpose() * tf1.getRotation(); - ot1 = tf0.getRotation().transpose() * - (tf1.getTranslation() - tf0.getTranslation()); + oR1.noalias() = tf0.getRotation().transpose() * tf1.getRotation(); + ot1.noalias() = tf0.getRotation().transpose() * + (tf1.getTranslation() - tf0.getTranslation()); bool identity = (oR1.isIdentity() && ot1.isZero()); From 97971f6f766274b0dbfb1abd6379b9bc853122ab Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Fri, 17 Jun 2022 18:41:13 +0200 Subject: [PATCH 15/52] core: remove useless methods in GJKSolver --- include/hpp/fcl/narrowphase/narrowphase.h | 11 ----------- 1 file changed, 11 deletions(-) diff --git a/include/hpp/fcl/narrowphase/narrowphase.h b/include/hpp/fcl/narrowphase/narrowphase.h index 4196eca95..12b16c14c 100644 --- a/include/hpp/fcl/narrowphase/narrowphase.h +++ b/include/hpp/fcl/narrowphase/narrowphase.h @@ -431,17 +431,6 @@ struct HPP_FCL_DLLAPI GJKSolver { /// @brief Copy constructor GJKSolver(const GJKSolver& other) = default; - // TODO: (enable/set/get)CachedGuess -> use gjk_initial_guess instead - void enableCachedGuess(bool if_enable) const { - enable_cached_guess = if_enable; - } - - HPP_FCL_COMPILER_DIAGNOSTIC_POP - - void setCachedGuess(const Vec3f& guess) const { cached_guess = guess; } - - Vec3f getCachedGuess() const { return cached_guess; } - HPP_FCL_COMPILER_DIAGNOSTIC_PUSH HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS bool operator==(const GJKSolver& other) const { From ecc373ddeb6ac14ea5919227b4902c5bcb59979f Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Fri, 17 Jun 2022 18:42:37 +0200 Subject: [PATCH 16/52] core: move shape_traits in a dedicated file And starts to add feature for inflating the shapes or adding support for inflated support function --- CMakeLists.txt | 1 + .../hpp/fcl/shape/geometric_shapes_traits.h | 140 ++++++++++++++++++ src/narrowphase/gjk.cpp | 48 +----- 3 files changed, 142 insertions(+), 47 deletions(-) create mode 100644 include/hpp/fcl/shape/geometric_shapes_traits.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 8c15e5ea9..afa51909e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -209,6 +209,7 @@ SET(${PROJECT_NAME}_HEADERS include/hpp/fcl/shape/details/convex.hxx include/hpp/fcl/shape/geometric_shape_to_BVH_model.h include/hpp/fcl/shape/geometric_shapes.h + include/hpp/fcl/shape/geometric_shapes_traits.h include/hpp/fcl/shape/geometric_shapes_utility.h include/hpp/fcl/distance_func_matrix.h include/hpp/fcl/collision.h diff --git a/include/hpp/fcl/shape/geometric_shapes_traits.h b/include/hpp/fcl/shape/geometric_shapes_traits.h new file mode 100644 index 000000000..b78d7f268 --- /dev/null +++ b/include/hpp/fcl/shape/geometric_shapes_traits.h @@ -0,0 +1,140 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2015, Open Source Robotics Foundation + * Copyright (c) 2015-2022, CNRS, 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 Open Source Robotics Foundation 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. + */ + +#ifndef HPP_FCL_GEOMETRIC_SHAPES_TRAITS_H +#define HPP_FCL_GEOMETRIC_SHAPES_TRAITS_H + +#include + +namespace hpp { +namespace fcl { + +struct shape_traits_base { + enum { + NeedNormalizedDir = true, + NeedNesterovNormalizeHeuristic = false, + IsInflatable = false, + HasInflatedSupportFunction = false + }; +}; + +template +struct shape_traits : shape_traits_base {}; + +template <> +struct shape_traits : shape_traits_base { + enum { + NeedNormalizedDir = false, + NeedNesterovNormalizeHeuristic = false, + IsInflatable = false, + HasInflatedSupportFunction = false + }; +}; + +template <> +struct shape_traits : shape_traits_base { + enum { + NeedNormalizedDir = false, + NeedNesterovNormalizeHeuristic = false, + IsInflatable = true, + HasInflatedSupportFunction = false + }; +}; + +template <> +struct shape_traits : shape_traits_base { + enum { + NeedNormalizedDir = false, + NeedNesterovNormalizeHeuristic = false, + IsInflatable = true, + HasInflatedSupportFunction = false + }; +}; + +template <> +struct shape_traits : shape_traits_base { + enum { + NeedNormalizedDir = false, + NeedNesterovNormalizeHeuristic = false, + IsInflatable = true, + HasInflatedSupportFunction = false + }; +}; + +template <> +struct shape_traits : shape_traits_base { + enum { + NeedNormalizedDir = false, + NeedNesterovNormalizeHeuristic = false, + IsInflatable = true, + HasInflatedSupportFunction = false + }; +}; + +template <> +struct shape_traits : shape_traits_base { + enum { + NeedNormalizedDir = false, + NeedNesterovNormalizeHeuristic = false, + IsInflatable = true, + HasInflatedSupportFunction = false + }; +}; + +template <> +struct shape_traits : shape_traits_base { + enum { + NeedNormalizedDir = false, + NeedNesterovNormalizeHeuristic = false, + IsInflatable = true, + HasInflatedSupportFunction = false + }; +}; + +template <> +struct shape_traits : shape_traits_base { + enum { + NeedNormalizedDir = false, + NeedNesterovNormalizeHeuristic = true, + IsInflatable = false, + HasInflatedSupportFunction = true + }; +}; + +} // namespace fcl +} // namespace hpp + +#endif // ifndef HPP_FCL_GEOMETRIC_SHAPES_TRAITS_H diff --git a/src/narrowphase/gjk.cpp b/src/narrowphase/gjk.cpp index b2f27ca12..ba4af779a 100644 --- a/src/narrowphase/gjk.cpp +++ b/src/narrowphase/gjk.cpp @@ -38,59 +38,13 @@ #include #include #include +#include namespace hpp { namespace fcl { namespace details { -struct HPP_FCL_LOCAL shape_traits_base { - enum { NeedNormalizedDir = true, NeedNesterovNormalizeHeuristic = false }; -}; - -template -struct HPP_FCL_LOCAL shape_traits : shape_traits_base {}; - -template <> -struct HPP_FCL_LOCAL shape_traits : shape_traits_base { - enum { NeedNormalizedDir = false, NeedNesterovNormalizeHeuristic = false }; -}; - -template <> -struct HPP_FCL_LOCAL shape_traits : shape_traits_base { - enum { NeedNormalizedDir = false, NeedNesterovNormalizeHeuristic = false }; -}; - -template <> -struct HPP_FCL_LOCAL shape_traits : shape_traits_base { - enum { NeedNormalizedDir = false, NeedNesterovNormalizeHeuristic = false }; -}; - -template <> -struct HPP_FCL_LOCAL shape_traits : shape_traits_base { - enum { NeedNormalizedDir = false, NeedNesterovNormalizeHeuristic = false }; -}; - -template <> -struct HPP_FCL_LOCAL shape_traits : shape_traits_base { - enum { NeedNormalizedDir = false, NeedNesterovNormalizeHeuristic = false }; -}; - -template <> -struct HPP_FCL_LOCAL shape_traits : shape_traits_base { - enum { NeedNormalizedDir = false, NeedNesterovNormalizeHeuristic = false }; -}; - -template <> -struct HPP_FCL_LOCAL shape_traits : shape_traits_base { - enum { NeedNormalizedDir = false, NeedNesterovNormalizeHeuristic = false }; -}; - -template <> -struct HPP_FCL_LOCAL shape_traits : shape_traits_base { - enum { NeedNormalizedDir = false, NeedNesterovNormalizeHeuristic = true }; -}; - void getShapeSupport(const TriangleP* triangle, const Vec3f& dir, Vec3f& support, int&, MinkowskiDiff::ShapeData*) { FCL_REAL dota = dir.dot(triangle->a); From 1abbecaee8ec0a120aa3e8bb15d25b6fea5c9af4 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Fri, 17 Jun 2022 18:43:47 +0200 Subject: [PATCH 17/52] shape: rework inflated method Only the supported meshes are now accounted --- include/hpp/fcl/shape/geometric_shapes.h | 174 +++++++++++++---------- test/shape_inflation.cpp | 35 ++--- 2 files changed, 110 insertions(+), 99 deletions(-) diff --git a/include/hpp/fcl/shape/geometric_shapes.h b/include/hpp/fcl/shape/geometric_shapes.h index 86e22f432..d351d26d3 100644 --- a/include/hpp/fcl/shape/geometric_shapes.h +++ b/include/hpp/fcl/shape/geometric_shapes.h @@ -59,14 +59,6 @@ class HPP_FCL_DLLAPI ShapeBase : public CollisionGeometry { virtual ~ShapeBase(){}; - /// \brief Inflate the shape by an amount given by value - /// - /// \param[in] value of the shape inflation. - /// - /// \returns a new inflated shape - virtual std::pair inflated( - const FCL_REAL value) const = 0; - /// @brief Get object type: a geometric shape OBJECT_TYPE getObjectType() const { return OT_GEOM; } }; @@ -92,19 +84,25 @@ class HPP_FCL_DLLAPI TriangleP : public ShapeBase { NODE_TYPE getNodeType() const { return GEOM_TRIANGLE; } - std::pair inflated(const FCL_REAL value) const { - if (value == 0) return std::make_pair(new TriangleP(*this), Transform3f()); - Vec3f AB(b - a), BC(c - b), CA(a - c); - AB.normalize(); - BC.normalize(); - CA.normalize(); - - Vec3f new_a(a + value * Vec3f(-AB + CA).normalized()); - Vec3f new_b(b + value * Vec3f(-BC + AB).normalized()); - Vec3f new_c(c + value * Vec3f(-CA + BC).normalized()); - - return std::make_pair(new TriangleP(new_a, new_b, new_c), Transform3f()); - } + // std::pair inflated(const FCL_REAL value) const { + // if (value == 0) return std::make_pair(new TriangleP(*this), + // Transform3f()); Vec3f AB(b - a), BC(c - b), CA(a - c); AB.normalize(); + // BC.normalize(); + // CA.normalize(); + // + // Vec3f new_a(a + value * Vec3f(-AB + CA).normalized()); + // Vec3f new_b(b + value * Vec3f(-BC + AB).normalized()); + // Vec3f new_c(c + value * Vec3f(-CA + BC).normalized()); + // + // return std::make_pair(new TriangleP(new_a, new_b, new_c), + // Transform3f()); + // } + // + // FCL_REAL minInflationValue() const + // { + // return (std::numeric_limits::max)(); // TODO(jcarpent): + // implement + // } Vec3f a, b, c; @@ -161,14 +159,21 @@ class HPP_FCL_DLLAPI Box : public ShapeBase { return (Vec3f(s[1] + s[2], s[0] + s[2], s[0] + s[1]) / 3).asDiagonal(); } - std::pair inflated(const FCL_REAL value) const { - const FCL_REAL minimal_value = -halfSide.minCoeff(); - if (value <= minimal_value) - HPP_FCL_THROW_PRETTY( - "value (" << value << ") " - << "is two small. It should be at least: " << minimal_value, - std::invalid_argument); - return std::make_pair(new Box(2 * (halfSide + Vec3f::Constant(value))), + FCL_REAL minInflationValue() const { return -halfSide.minCoeff(); } + + /// \brief Inflate the box by an amount given by value + /// + /// \param[in] value of the shape inflation. + /// + /// \returns a new inflated box and the related transform to account for the + /// change of shape frame + std::pair inflated(const FCL_REAL value) const { + if (value <= minInflationValue()) + HPP_FCL_THROW_PRETTY("value (" << value << ") " + << "is two small. It should be at least: " + << minInflationValue(), + std::invalid_argument); + return std::make_pair(Box(2 * (halfSide + Vec3f::Constant(value))), Transform3f()); } @@ -214,13 +219,20 @@ class HPP_FCL_DLLAPI Sphere : public ShapeBase { radius / 3; } - std::pair inflated(const FCL_REAL value) const { - const FCL_REAL minimal_value = -radius; - if (value <= minimal_value) + FCL_REAL minInflationValue() const { return -radius; } + + /// \brief Inflate the sphere by an amount given by value + /// + /// \param[in] value of the shape inflation. + /// + /// \returns a new inflated sphere and the related transform to account for + /// the change of shape frame + std::pair inflated(const FCL_REAL value) const { + if (value <= minInflationValue()) HPP_FCL_THROW_PRETTY( - "value is two small. It should be at least: " << minimal_value, + "value is two small. It should be at least: " << minInflationValue(), std::invalid_argument); - return std::make_pair(new Sphere(radius + value), Transform3f()); + return std::make_pair(Sphere(radius + value), Transform3f()); } private: @@ -274,13 +286,20 @@ class HPP_FCL_DLLAPI Ellipsoid : public ShapeBase { radii[2] / 3; } - std::pair inflated(const FCL_REAL value) const { - const FCL_REAL minimal_value = -radii.minCoeff(); - if (value <= minimal_value) + FCL_REAL minInflationValue() const { return -radii.minCoeff(); } + + /// \brief Inflate the ellipsoid by an amount given by value + /// + /// \param[in] value of the shape inflation. + /// + /// \returns a new inflated ellipsoid and the related transform to account for + /// the change of shape frame + std::pair inflated(const FCL_REAL value) const { + if (value <= minInflationValue()) HPP_FCL_THROW_PRETTY( - "value is two small. It should be at least: " << minimal_value, + "value is two small. It should be at least: " << minInflationValue(), std::invalid_argument); - return std::make_pair(new Ellipsoid(radii + Vec3f::Constant(value)), + return std::make_pair(Ellipsoid(radii + Vec3f::Constant(value)), Transform3f()); } @@ -345,13 +364,20 @@ class HPP_FCL_DLLAPI Capsule : public ShapeBase { return (Matrix3f() << ix, 0, 0, 0, ix, 0, 0, 0, iz).finished(); } - std::pair inflated(const FCL_REAL value) const { - const FCL_REAL minimal_value = -radius; - if (value <= minimal_value) + FCL_REAL minInflationValue() const { return -radius; } + + /// \brief Inflate the capsule by an amount given by value + /// + /// \param[in] value of the shape inflation. + /// + /// \returns a new inflated capsule and the related transform to account for + /// the change of shape frame + std::pair inflated(const FCL_REAL value) const { + if (value <= minInflationValue()) HPP_FCL_THROW_PRETTY( - "value is two small. It should be at least: " << minimal_value, + "value is two small. It should be at least: " << minInflationValue(), std::invalid_argument); - return std::make_pair(new Capsule(radius + value, 2 * halfLength), + return std::make_pair(Capsule(radius + value, 2 * halfLength), Transform3f()); } @@ -411,11 +437,18 @@ class HPP_FCL_DLLAPI Cone : public ShapeBase { Vec3f computeCOM() const { return Vec3f(0, 0, -0.5 * halfLength); } - std::pair inflated(const FCL_REAL value) const { - const FCL_REAL minimal_value = -(std::min)(radius, halfLength); - if (value <= minimal_value) + FCL_REAL minInflationValue() const { return -(std::min)(radius, halfLength); } + + /// \brief Inflate the cone by an amount given by value + /// + /// \param[in] value of the shape inflation. + /// + /// \returns a new inflated cone and the related transform to account for the + /// change of shape frame + std::pair inflated(const FCL_REAL value) const { + if (value <= minInflationValue()) HPP_FCL_THROW_PRETTY( - "value is two small. It should be at least: " << minimal_value, + "value is two small. It should be at least: " << minInflationValue(), std::invalid_argument); // tan(alpha) = 2*halfLength/radius; @@ -428,7 +461,7 @@ class HPP_FCL_DLLAPI Cone : public ShapeBase { const FCL_REAL new_cz = (top_inflation + bottom_inflation) / 2.; const FCL_REAL new_radius = new_lz / tan_alpha; - return std::make_pair(new Cone(new_radius, new_lz), + return std::make_pair(Cone(new_radius, new_lz), Transform3f(Vec3f(0., 0., new_cz))); } @@ -491,14 +524,21 @@ class HPP_FCL_DLLAPI Cylinder : public ShapeBase { return (Matrix3f() << ix, 0, 0, 0, ix, 0, 0, 0, iz).finished(); } - std::pair inflated(const FCL_REAL value) const { - const FCL_REAL minimal_value = -(std::min)(radius, halfLength); - if (value <= minimal_value) + FCL_REAL minInflationValue() const { return -(std::min)(radius, halfLength); } + + /// \brief Inflate the cylinder by an amount given by value + /// + /// \param[in] value of the shape inflation. + /// + /// \returns a new inflated cylinder and the related transform to account for + /// the change of shape frame + std::pair inflated(const FCL_REAL value) const { + if (value <= minInflationValue()) HPP_FCL_THROW_PRETTY( - "value is two small. It should be at least: " << minimal_value, + "value is two small. It should be at least: " << minInflationValue(), std::invalid_argument); - return std::make_pair( - new Cylinder(radius + value, 2 * (halfLength + value)), Transform3f()); + return std::make_pair(Cylinder(radius + value, 2 * (halfLength + value)), + Transform3f()); } private: @@ -596,16 +636,6 @@ class HPP_FCL_DLLAPI ConvexBase : public ShapeBase { /// is guaranteed in the internal of the polytope (as it is convex) Vec3f center; - /// @copydoc ShapeBase::inflated - /// @remarks Inflated should not be applied on Convex objects. - std::pair inflated(const FCL_REAL value) const { - HPP_FCL_UNUSED_VARIABLE(value); - HPP_FCL_THROW_PRETTY( - "ConvexBase::inflated has not been implemented so far.", - std::logic_error); - return std::make_pair(static_cast(NULL), Transform3f()); - } - protected: /// @brief Construct an uninitialized convex object /// Initialization is done with ConvexBase::initialize. @@ -718,13 +748,6 @@ class HPP_FCL_DLLAPI Halfspace : public ShapeBase { /// @brief Plane offset FCL_REAL d; - /// @copydoc ShapeBase::inflated - /// @remarks Inflated method has no effect on the Halfspace - std::pair inflated(const FCL_REAL value) const { - HPP_FCL_UNUSED_VARIABLE(value); - return std::make_pair(clone(), Transform3f()); - } - protected: /// @brief Turn non-unit normal into unit void unitNormalTest(); @@ -786,13 +809,6 @@ class HPP_FCL_DLLAPI Plane : public ShapeBase { /// @brief Plane offset FCL_REAL d; - /// @copydoc ShapeBase::inflated - /// @remarks Inflated method has no effect on the Plane - std::pair inflated(const FCL_REAL value) const { - HPP_FCL_UNUSED_VARIABLE(value); - return std::make_pair(clone(), Transform3f()); - } - protected: /// @brief Turn non-unit normal into unit void unitNormalTest(); diff --git a/test/shape_inflation.cpp b/test/shape_inflation.cpp index 3558e2185..0458a2e57 100644 --- a/test/shape_inflation.cpp +++ b/test/shape_inflation.cpp @@ -110,46 +110,41 @@ void test(const Shape &original_shape, const FCL_REAL inflation, const FCL_REAL inflation = 0.; const auto &inflation_result = original_shape.inflated(inflation); const Transform3f &shift = inflation_result.second; - std::shared_ptr inflated_shape_ptr( - static_cast(inflation_result.first)); + const Shape &inflated_shape = inflation_result.first; - BOOST_CHECK(isApprox(original_shape, *inflated_shape_ptr, tol)); + BOOST_CHECK(isApprox(original_shape, inflated_shape, tol)); BOOST_CHECK(shift.isIdentity(tol)); } // Positive inflation { const auto &inflation_result = original_shape.inflated(inflation); - std::shared_ptr inflated_shape_ptr( - static_cast(inflation_result.first)); + const Shape &inflated_shape = inflation_result.first; const Transform3f &inflation_shift = inflation_result.second; - BOOST_CHECK(!isApprox(original_shape, *inflated_shape_ptr, tol)); + BOOST_CHECK(!isApprox(original_shape, inflated_shape, tol)); - const auto &deflation_result = inflated_shape_ptr->inflated(-inflation); - std::shared_ptr deflated_shape_ptr( - static_cast(deflation_result.first)); + const auto &deflation_result = inflated_shape.inflated(-inflation); + const Shape &deflated_shape = deflation_result.first; const Transform3f &deflation_shift = deflation_result.second; - BOOST_CHECK(isApprox(original_shape, *deflated_shape_ptr, tol)); + BOOST_CHECK(isApprox(original_shape, deflated_shape, tol)); BOOST_CHECK((inflation_shift * deflation_shift).isIdentity(tol)); } // Negative inflation { const auto &inflation_result = original_shape.inflated(-inflation); - std::shared_ptr inflated_shape_ptr( - static_cast(inflation_result.first)); + const Shape &inflated_shape = inflation_result.first; const Transform3f &inflation_shift = inflation_result.second; - BOOST_CHECK(!isApprox(original_shape, *inflated_shape_ptr, tol)); + BOOST_CHECK(!isApprox(original_shape, inflated_shape, tol)); - const auto &deflation_result = inflated_shape_ptr->inflated(+inflation); - std::shared_ptr deflated_shape_ptr( - static_cast(deflation_result.first)); + const auto &deflation_result = inflated_shape.inflated(+inflation); + const Shape &deflated_shape = deflation_result.first; const Transform3f &deflation_shift = deflation_result.second; - BOOST_CHECK(isApprox(original_shape, *deflated_shape_ptr, tol)); + BOOST_CHECK(isApprox(original_shape, deflated_shape, tol)); BOOST_CHECK((inflation_shift * deflation_shift).isIdentity(tol)); } } @@ -190,7 +185,7 @@ BOOST_AUTO_TEST_CASE(test_inflate) { test(cone, 0.01, 1e-8); test_throw(cone, -1.1); - const hpp::fcl::TriangleP triangle(Vec3f::UnitX(), Vec3f::UnitY(), - Vec3f::UnitZ()); - test(triangle, 0.01, 1e-8); + // const hpp::fcl::TriangleP triangle(Vec3f::UnitX(), Vec3f::UnitY(), + // Vec3f::UnitZ()); + // test(triangle, 0.01, 1e-8); } From 1b8699359c837b2588463b585e9a4a835f3546b0 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Sun, 19 Jun 2022 18:08:01 +0200 Subject: [PATCH 18/52] cmake: add missing dependency for Python building tests --- test/python_unit/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/test/python_unit/CMakeLists.txt b/test/python_unit/CMakeLists.txt index 4cd4777a8..6bad8ec8a 100644 --- a/test/python_unit/CMakeLists.txt +++ b/test/python_unit/CMakeLists.txt @@ -4,6 +4,7 @@ SET(${PROJECT_NAME}_PYTHON_TESTS collision ) +ADD_DEPENDENCIES(build_tests hppfcl) FOREACH(TEST ${${PROJECT_NAME}_PYTHON_TESTS}) ADD_PYTHON_UNIT_TEST("py-${TEST}" "test/python_unit/${TEST}.py" "python") ENDFOREACH(TEST ${${PROJECT_NAME}_PYTHON_TESTS}) From f9efa82bf202e782169d3c91a8a37b8b70bc4609 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Sun, 19 Jun 2022 19:50:04 +0200 Subject: [PATCH 19/52] core: add inflate features to Halfspace --- include/hpp/fcl/shape/geometric_shapes.h | 18 ++++++++++++++++++ .../hpp/fcl/shape/geometric_shapes_traits.h | 10 ++++++++++ test/shape_inflation.cpp | 7 +++++++ 3 files changed, 35 insertions(+) diff --git a/include/hpp/fcl/shape/geometric_shapes.h b/include/hpp/fcl/shape/geometric_shapes.h index d351d26d3..d30d96799 100644 --- a/include/hpp/fcl/shape/geometric_shapes.h +++ b/include/hpp/fcl/shape/geometric_shapes.h @@ -742,6 +742,24 @@ class HPP_FCL_DLLAPI Halfspace : public ShapeBase { /// @brief Get node type: a half space NODE_TYPE getNodeType() const { return GEOM_HALFSPACE; } + FCL_REAL minInflationValue() const { + return std::numeric_limits::lowest(); + } + + /// \brief Inflate the cylinder by an amount given by value + /// + /// \param[in] value of the shape inflation. + /// + /// \returns a new inflated cylinder and the related transform to account for + /// the change of shape frame + std::pair inflated(const FCL_REAL value) const { + if (value <= minInflationValue()) + HPP_FCL_THROW_PRETTY( + "value is two small. It should be at least: " << minInflationValue(), + std::invalid_argument); + return std::make_pair(Halfspace(n, d + value), Transform3f()); + } + /// @brief Plane normal Vec3f n; diff --git a/include/hpp/fcl/shape/geometric_shapes_traits.h b/include/hpp/fcl/shape/geometric_shapes_traits.h index b78d7f268..30c5317cd 100644 --- a/include/hpp/fcl/shape/geometric_shapes_traits.h +++ b/include/hpp/fcl/shape/geometric_shapes_traits.h @@ -134,6 +134,16 @@ struct shape_traits : shape_traits_base { }; }; +template <> +struct shape_traits : shape_traits_base { + enum { + NeedNormalizedDir = false, + NeedNesterovNormalizeHeuristic = false, + IsInflatable = true, + HasInflatedSupportFunction = false + }; +}; + } // namespace fcl } // namespace hpp diff --git a/test/shape_inflation.cpp b/test/shape_inflation.cpp index 0458a2e57..e305906bd 100644 --- a/test/shape_inflation.cpp +++ b/test/shape_inflation.cpp @@ -102,6 +102,10 @@ bool isApprox(const TriangleP &s1, const TriangleP &s2, const FCL_REAL tol) { s1.c.isApprox(s2.c, tol); } +bool isApprox(const Halfspace &s1, const Halfspace &s2, const FCL_REAL tol) { + return isApprox(s1.d, s2.d, tol) && s1.n.isApprox(s2.n, tol); +} + template void test(const Shape &original_shape, const FCL_REAL inflation, const FCL_REAL tol = 1e-8) { @@ -185,6 +189,9 @@ BOOST_AUTO_TEST_CASE(test_inflate) { test(cone, 0.01, 1e-8); test_throw(cone, -1.1); + const hpp::fcl::Halfspace halfspace(Vec3f::UnitZ(), 0.); + test(halfspace, 0.01, 1e-8); + // const hpp::fcl::TriangleP triangle(Vec3f::UnitX(), Vec3f::UnitY(), // Vec3f::UnitZ()); // test(triangle, 0.01, 1e-8); From f6bc77ded47e0545d0662de6f243439e968230d1 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Mon, 20 Jun 2022 14:34:04 +0200 Subject: [PATCH 20/52] core: remove useless code --- .../hpp/fcl/internal/traversal_node_setup.h | 32 +------------------ 1 file changed, 1 insertion(+), 31 deletions(-) diff --git a/include/hpp/fcl/internal/traversal_node_setup.h b/include/hpp/fcl/internal/traversal_node_setup.h index 41f79dc67..7d64ee149 100644 --- a/include/hpp/fcl/internal/traversal_node_setup.h +++ b/include/hpp/fcl/internal/traversal_node_setup.h @@ -365,37 +365,7 @@ bool initialize(HeightFieldShapeCollisionTraversalNode& node, HeightField& model1, Transform3f& tf1, const S& model2, const Transform3f& tf2, const GJKSolver* nsolver, CollisionResult& result, bool use_refit = false, - bool refit_bottomup = false) { - if (!tf1.isIdentity()) { - std::vector vertices_transformed(model1.num_vertices); - for (unsigned int i = 0; i < model1.num_vertices; ++i) { - const Vec3f& p = model1.vertices[i]; - Vec3f new_v = tf1.transform(p); - vertices_transformed[i] = new_v; - } - - model1.beginReplaceModel(); - model1.replaceSubModel(vertices_transformed); - model1.endReplaceModel(use_refit, refit_bottomup); - - tf1.setIdentity(); - } - - node.model1 = &model1; - node.tf1 = tf1; - node.model2 = &model2; - node.tf2 = tf2; - node.nsolver = nsolver; - - computeBV(model2, tf2, node.model2_bv); - - node.vertices = model1.vertices; - node.tri_indices = model1.tri_indices; - - node.result = &result; - - return true; -} + bool refit_bottomup = false); /// @brief Initialize traversal node for collision between one mesh and one /// shape From 8f9bd09db804bf9500dba977ca1964a7d38a6a07 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Mon, 20 Jun 2022 17:06:03 +0200 Subject: [PATCH 21/52] traversal: remove useless method --- .../hpp/fcl/internal/traversal_node_base.h | 3 --- .../fcl/internal/traversal_node_bvh_shape.h | 21 --------------- .../hpp/fcl/internal/traversal_node_bvhs.h | 10 ------- .../internal/traversal_node_hfield_shape.h | 26 ------------------- .../hpp/fcl/internal/traversal_node_octree.h | 10 ------- .../hpp/fcl/internal/traversal_node_shapes.h | 3 --- 6 files changed, 73 deletions(-) diff --git a/include/hpp/fcl/internal/traversal_node_base.h b/include/hpp/fcl/internal/traversal_node_base.h index 173fb58a1..a3c56f8d4 100644 --- a/include/hpp/fcl/internal/traversal_node_base.h +++ b/include/hpp/fcl/internal/traversal_node_base.h @@ -108,9 +108,6 @@ class CollisionTraversalNodeBase : public TraversalNodeBase { virtual ~CollisionTraversalNodeBase() {} - /// @brief BV test between b1 and b2 - virtual bool BVDisjoints(unsigned int b1, unsigned int b2) const = 0; - /// BV test between b1 and b2 /// @param b1, b2 Bounding volumes to test, /// @retval sqrDistLowerBound square of a lower bound of the minimal diff --git a/include/hpp/fcl/internal/traversal_node_bvh_shape.h b/include/hpp/fcl/internal/traversal_node_bvh_shape.h index 7d0b1cd52..dae9a3783 100644 --- a/include/hpp/fcl/internal/traversal_node_bvh_shape.h +++ b/include/hpp/fcl/internal/traversal_node_bvh_shape.h @@ -152,16 +152,6 @@ class MeshShapeCollisionTraversalNode nsolver = NULL; } - /// @brief BV culling test in one BVTT node - bool BVDisjoints(unsigned int b1, unsigned int /*b2*/) const { - if (this->enable_statistics) this->num_bv_tests++; - if (RTIsIdentity) - return !this->model1->getBV(b1).bv.overlap(this->model2_bv); - else - return !overlap(this->tf1.getRotation(), this->tf1.getTranslation(), - this->model2_bv, this->model1->getBV(b1).bv); - } - /// test between BV b1 and shape /// @param b1 BV to test, /// @retval sqrDistLowerBound square of a lower bound of the minimal @@ -263,17 +253,6 @@ class ShapeMeshCollisionTraversalNode nsolver = NULL; } - /// BV test between b1 and b2 - /// @param b2 Bounding volumes to test, - bool BVDisjoints(unsigned int /*b1*/, unsigned int b2) const { - if (this->enable_statistics) this->num_bv_tests++; - if (RTIsIdentity) - return !this->model2->getBV(b2).bv.overlap(this->model1_bv); - else - return !overlap(this->tf2.getRotation(), this->tf2.getTranslation(), - this->model1_bv, this->model2->getBV(b2).bv); - } - /// BV test between b1 and b2 /// @param b2 Bounding volumes to test, /// @retval sqrDistLowerBound square of a lower bound of the minimal diff --git a/include/hpp/fcl/internal/traversal_node_bvhs.h b/include/hpp/fcl/internal/traversal_node_bvhs.h index 9bcd9555a..652df4c90 100644 --- a/include/hpp/fcl/internal/traversal_node_bvhs.h +++ b/include/hpp/fcl/internal/traversal_node_bvhs.h @@ -146,16 +146,6 @@ class MeshCollisionTraversalNode : public BVHCollisionTraversalNode { tri_indices2 = NULL; } - /// @brief BV culling test in one BVTT node - bool BVDisjoints(unsigned int b1, unsigned int b2) const { - if (this->enable_statistics) this->num_bv_tests++; - if (RTIsIdentity) - return !this->model1->getBV(b1).overlap(this->model2->getBV(b2)); - else - return !overlap(RT._R(), RT._T(), this->model1->getBV(b1).bv, - this->model2->getBV(b2).bv); - } - /// BV test between b1 and b2 /// @param b1, b2 Bounding volumes to test, /// @retval sqrDistLowerBound square of a lower bound of the minimal diff --git a/include/hpp/fcl/internal/traversal_node_hfield_shape.h b/include/hpp/fcl/internal/traversal_node_hfield_shape.h index 5236a9fd7..d8311b078 100644 --- a/include/hpp/fcl/internal/traversal_node_hfield_shape.h +++ b/include/hpp/fcl/internal/traversal_node_hfield_shape.h @@ -220,21 +220,6 @@ class HeightFieldShapeCollisionTraversalNode return static_cast(model1->getBV(b).rightChild()); } - /// @brief BV culling test in one BVTT node - bool BVDisjoints(unsigned int b1, unsigned int /*b2*/) const { - std::cout << "\t BVDisjoints - 2" << std::endl; - if (this->enable_statistics) this->num_bv_tests++; - if (RTIsIdentity) { - std::cout << "RTIsIdentity" << std::endl; - assert(false && "must never happened"); - return !this->model1->getBV(b1).bv.overlap(this->model2_bv); - } else { - std::cout << "\t call !overlap(" << std::endl; - return !overlap(this->tf1.getRotation(), this->tf1.getTranslation(), - this->model2_bv, this->model1->getBV(b1).bv); - } - } - /// test between BV b1 and shape /// @param b1 BV to test, /// @retval sqrDistLowerBound square of a lower bound of the minimal @@ -408,17 +393,6 @@ class ShapeHeightFieldCollisionTraversalNode return model2->getBV(b).rightChild(); } - /// BV test between b1 and b2 - /// @param b2 Bounding volumes to test, - bool BVDisjoints(unsigned int /*b1*/, unsigned int b2) const { - if (this->enable_statistics) this->num_bv_tests++; - if (RTIsIdentity) - return !this->model2->getBV(b2).bv.overlap(this->model1_bv); - else - return !overlap(this->tf2.getRotation(), this->tf2.getTranslation(), - this->model1_bv, this->model2->getBV(b2).bv); - } - /// BV test between b1 and b2 /// @param b2 Bounding volumes to test, /// @retval sqrDistLowerBound square of a lower bound of the minimal diff --git a/include/hpp/fcl/internal/traversal_node_octree.h b/include/hpp/fcl/internal/traversal_node_octree.h index d9ed85e98..7aea1bfa9 100644 --- a/include/hpp/fcl/internal/traversal_node_octree.h +++ b/include/hpp/fcl/internal/traversal_node_octree.h @@ -708,8 +708,6 @@ class HPP_FCL_DLLAPI OcTreeCollisionTraversalNode otsolver = NULL; } - bool BVDisjoints(unsigned int, unsigned) const { return false; } - bool BVDisjoints(unsigned, unsigned, FCL_REAL&) const { return false; } void leafCollides(unsigned, unsigned, FCL_REAL& sqrDistLowerBound) const { @@ -739,8 +737,6 @@ class HPP_FCL_DLLAPI ShapeOcTreeCollisionTraversalNode otsolver = NULL; } - bool BVDisjoints(unsigned int, unsigned int) const { return false; } - bool BVDisjoints(unsigned int, unsigned int, FCL_REAL&) const { return false; } @@ -774,8 +770,6 @@ class HPP_FCL_DLLAPI OcTreeShapeCollisionTraversalNode otsolver = NULL; } - bool BVDisjoints(unsigned int, unsigned int) const { return false; } - bool BVDisjoints(unsigned int, unsigned int, fcl::FCL_REAL&) const { return false; } @@ -808,8 +802,6 @@ class HPP_FCL_DLLAPI MeshOcTreeCollisionTraversalNode otsolver = NULL; } - bool BVDisjoints(unsigned int, unsigned int) const { return false; } - bool BVDisjoints(unsigned int, unsigned int, FCL_REAL&) const { return false; } @@ -842,8 +834,6 @@ class HPP_FCL_DLLAPI OcTreeMeshCollisionTraversalNode otsolver = NULL; } - bool BVDisjoints(unsigned int, unsigned int) const { return false; } - bool BVDisjoints(unsigned int, unsigned int, FCL_REAL&) const { return false; } diff --git a/include/hpp/fcl/internal/traversal_node_shapes.h b/include/hpp/fcl/internal/traversal_node_shapes.h index 9c54bc434..99f1b671e 100644 --- a/include/hpp/fcl/internal/traversal_node_shapes.h +++ b/include/hpp/fcl/internal/traversal_node_shapes.h @@ -65,9 +65,6 @@ class HPP_FCL_DLLAPI ShapeCollisionTraversalNode nsolver = NULL; } - /// @brief BV culling test in one BVTT node - bool BVDisjoints(int, int) const { return false; } - /// @brief BV culling test in one BVTT node bool BVDisjoints(int, int, FCL_REAL&) const { throw std::runtime_error("Not implemented"); From a5a6af6d0e5a60ad867b8fecbbab73b758156166 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Mon, 20 Jun 2022 17:15:20 +0200 Subject: [PATCH 22/52] gjk: remove useless noalias() --- include/hpp/fcl/narrowphase/gjk.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/hpp/fcl/narrowphase/gjk.h b/include/hpp/fcl/narrowphase/gjk.h index 8f9ccf77c..8480e3884 100644 --- a/include/hpp/fcl/narrowphase/gjk.h +++ b/include/hpp/fcl/narrowphase/gjk.h @@ -207,7 +207,7 @@ struct HPP_FCL_DLLAPI GJK { inline void getSupport(const Vec3f& d, bool dIsNormalized, SimplexV& sv, support_func_guess_t& hint) const { shape->support(d, dIsNormalized, sv.w0, sv.w1, hint); - sv.w.noalias() = sv.w0 - sv.w1; + sv.w = sv.w0 - sv.w1; } /// @brief whether the simplex enclose the origin From 8581667ab77532116932e3fa6da7ba8ea2e75bdb Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Mon, 20 Jun 2022 17:16:46 +0200 Subject: [PATCH 23/52] gjk: add shape_deflation field to MinkowskiDiff --- include/hpp/fcl/narrowphase/gjk.h | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/include/hpp/fcl/narrowphase/gjk.h b/include/hpp/fcl/narrowphase/gjk.h index 8480e3884..d1e1b6c78 100644 --- a/include/hpp/fcl/narrowphase/gjk.h +++ b/include/hpp/fcl/narrowphase/gjk.h @@ -57,6 +57,8 @@ Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, bool dirIsNormalized, /// /// @note The Minkowski difference is expressed in the frame of the first shape. struct HPP_FCL_DLLAPI MinkowskiDiff { + typedef Eigen::Array Array2d; + /// @brief points to two shapes const ShapeBase* shapes[2]; @@ -78,7 +80,13 @@ struct HPP_FCL_DLLAPI MinkowskiDiff { /// @brief The radius of the sphere swepted volume. /// The 2 values correspond to the inflation of shape 0 and shape 1. - Eigen::Array inflation; + /// The 2 values correspond to the inflation of shape 0 and shape 1/ + /// These inflation values are used for Sphere and Capsule. + Array2d inflation; + + /// @brief Deflation values associated to each support function. + /// The values are zero by default. + Array2d shape_deflation; /// @brief Number of points in a Convex object from which using a logarithmic /// support function is faster than a linear one. @@ -100,6 +108,8 @@ struct HPP_FCL_DLLAPI MinkowskiDiff { MinkowskiDiff() : linear_log_convex_threshold(32), + : shape_deflation(0, 0), + linear_log_convex_threshold(32), normalize_support_direction(false), getSupportFunc(NULL) {} From d53ac30b5158973e9776684bcf5ac519efcf27e5 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Mon, 20 Jun 2022 17:17:30 +0200 Subject: [PATCH 24/52] gjk: handle shape_deflation in getShapeSupport functions --- include/hpp/fcl/narrowphase/gjk.h | 10 +-- src/narrowphase/gjk.cpp | 134 ++++++++++++++++++++++++------ 2 files changed, 114 insertions(+), 30 deletions(-) diff --git a/include/hpp/fcl/narrowphase/gjk.h b/include/hpp/fcl/narrowphase/gjk.h index d1e1b6c78..5418acd41 100644 --- a/include/hpp/fcl/narrowphase/gjk.h +++ b/include/hpp/fcl/narrowphase/gjk.h @@ -51,7 +51,7 @@ namespace details { /// @brief the support function for shape /// \param hint use to initialize the search when shape is a ConvexBase object. Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, bool dirIsNormalized, - int& hint); + int& hint, const FCL_REAL& shape_deflation); /// @brief Minkowski difference class of two shapes /// @@ -79,7 +79,6 @@ struct HPP_FCL_DLLAPI MinkowskiDiff { Vec3f ot1; /// @brief The radius of the sphere swepted volume. - /// The 2 values correspond to the inflation of shape 0 and shape 1. /// The 2 values correspond to the inflation of shape 0 and shape 1/ /// These inflation values are used for Sphere and Capsule. Array2d inflation; @@ -107,7 +106,6 @@ struct HPP_FCL_DLLAPI MinkowskiDiff { GetSupportFunction getSupportFunc; MinkowskiDiff() - : linear_log_convex_threshold(32), : shape_deflation(0, 0), linear_log_convex_threshold(32), normalize_support_direction(false), @@ -123,13 +121,13 @@ struct HPP_FCL_DLLAPI MinkowskiDiff { /// @brief support function for shape0 inline Vec3f support0(const Vec3f& d, bool dIsNormalized, int& hint) const { - return getSupport(shapes[0], d, dIsNormalized, hint); + return getSupport(shapes[0], d, dIsNormalized, hint, shape_deflation[0]); } /// @brief support function for shape1 inline Vec3f support1(const Vec3f& d, bool dIsNormalized, int& hint) const { - return oR1 * - getSupport(shapes[1], oR1.transpose() * d, dIsNormalized, hint) + + return oR1 * getSupport(shapes[1], oR1.transpose() * d, dIsNormalized, hint, + shape_deflation[1]) + ot1; } diff --git a/src/narrowphase/gjk.cpp b/src/narrowphase/gjk.cpp index ba4af779a..9b5fdd056 100644 --- a/src/narrowphase/gjk.cpp +++ b/src/narrowphase/gjk.cpp @@ -46,7 +46,11 @@ namespace fcl { namespace details { void getShapeSupport(const TriangleP* triangle, const Vec3f& dir, - Vec3f& support, int&, MinkowskiDiff::ShapeData*) { + Vec3f& support, int&, MinkowskiDiff::ShapeData*, + const FCL_REAL& shape_deflation) { + HPP_FCL_UNUSED_VARIABLE(shape_deflation); + assert(shape_deflation == 0 && "shape_deflation should be equal to zero"); + FCL_REAL dota = dir.dot(triangle->a); FCL_REAL dotb = dir.dot(triangle->b); FCL_REAL dotc = dir.dot(triangle->c); @@ -64,7 +68,11 @@ void getShapeSupport(const TriangleP* triangle, const Vec3f& dir, } inline void getShapeSupport(const Box* box, const Vec3f& dir, Vec3f& support, - int&, MinkowskiDiff::ShapeData*) { + int&, MinkowskiDiff::ShapeData*, + const FCL_REAL& shape_deflation) { + HPP_FCL_UNUSED_VARIABLE(shape_deflation); + assert(shape_deflation == 0 && "shape_deflation should be equal to zero"); + const FCL_REAL inflate = (dir.array() == 0).any() ? 1.00000001 : 1.; support.noalias() = (dir.array() > 0) @@ -72,12 +80,20 @@ inline void getShapeSupport(const Box* box, const Vec3f& dir, Vec3f& support, } inline void getShapeSupport(const Sphere*, const Vec3f& /*dir*/, Vec3f& support, - int&, MinkowskiDiff::ShapeData*) { + int&, MinkowskiDiff::ShapeData*, + const FCL_REAL& shape_deflation) { + HPP_FCL_UNUSED_VARIABLE(shape_deflation); + assert(shape_deflation == 0 && "shape_deflation should be equal to zero"); + support.setZero(); } inline void getShapeSupport(const Ellipsoid* ellipsoid, const Vec3f& dir, - Vec3f& support, int&, MinkowskiDiff::ShapeData*) { + Vec3f& support, int&, MinkowskiDiff::ShapeData*, + const FCL_REAL& shape_deflation) { + HPP_FCL_UNUSED_VARIABLE(shape_deflation); + assert(shape_deflation == 0 && "shape_deflation should be equal to zero"); + FCL_REAL a2 = ellipsoid->radii[0] * ellipsoid->radii[0]; FCL_REAL b2 = ellipsoid->radii[1] * ellipsoid->radii[1]; FCL_REAL c2 = ellipsoid->radii[2] * ellipsoid->radii[2]; @@ -90,7 +106,11 @@ inline void getShapeSupport(const Ellipsoid* ellipsoid, const Vec3f& dir, } inline void getShapeSupport(const Capsule* capsule, const Vec3f& dir, - Vec3f& support, int&, MinkowskiDiff::ShapeData*) { + Vec3f& support, int&, MinkowskiDiff::ShapeData*, + const FCL_REAL& shape_deflation) { + HPP_FCL_UNUSED_VARIABLE(shape_deflation); + assert(shape_deflation == 0 && "shape_deflation should be equal to zero"); + support.head<2>().setZero(); if (dir[2] > 0) support[2] = capsule->halfLength; @@ -99,7 +119,11 @@ inline void getShapeSupport(const Capsule* capsule, const Vec3f& dir, } void getShapeSupport(const Cone* cone, const Vec3f& dir, Vec3f& support, int&, - MinkowskiDiff::ShapeData*) { + MinkowskiDiff::ShapeData*, + const FCL_REAL& shape_deflation) { + HPP_FCL_UNUSED_VARIABLE(shape_deflation); + assert(shape_deflation == 0 && "shape_deflation should be equal to zero"); + // The cone radius is, for -h < z < h, (h - z) * r / (2*h) static const FCL_REAL inflate = 1.00001; FCL_REAL h = cone->halfLength; @@ -137,7 +161,11 @@ void getShapeSupport(const Cone* cone, const Vec3f& dir, Vec3f& support, int&, } void getShapeSupport(const Cylinder* cylinder, const Vec3f& dir, Vec3f& support, - int&, MinkowskiDiff::ShapeData*) { + int&, MinkowskiDiff::ShapeData*, + const FCL_REAL& shape_deflation) { + HPP_FCL_UNUSED_VARIABLE(shape_deflation); + assert(shape_deflation == 0 && "shape_deflation should be equal to zero"); + // The inflation makes the object look strictly convex to GJK and EPA. This // helps solving particular cases (e.g. a cylinder with itself at the same // position...) @@ -166,9 +194,52 @@ void getShapeSupport(const Cylinder* cylinder, const Vec3f& dir, Vec3f& support, struct SmallConvex : ShapeBase {}; struct LargeConvex : ShapeBase {}; +inline void correctConvexBaseSupport(const ConvexBase* convex, Vec3f& support, + const int hint, + const FCL_REAL shape_deflation) { + const Vec3f* pts = convex->points; + const ConvexBase::Neighbors* nn = convex->neighbors; + + const ConvexBase::Neighbors& n = nn[hint]; + unsigned char num_neighbors = n.count(); + + if (num_neighbors == 0) + HPP_FCL_THROW_PRETTY("Point " + << hint + << " has zero neighbor.\n" + "The convex geometry seems to be degenerated.", + std::invalid_argument); + + // step 1: Compute average neighborhood directions (pointing towards the + // interior of the convex object). + const Vec3f first_direction = (pts[n[0]] - support).normalized(); + Vec3f average_dir = first_direction; + for (int in = 1; in < num_neighbors; ++in) { + const unsigned int ip = n[in]; + average_dir += (pts[ip] - support).normalized(); + } + + average_dir.normalize(); + + // step 2: Compute the right penetration + const FCL_REAL cos_alpha = average_dir.dot(first_direction); + const FCL_REAL sin_acos_cos_alpha = std::sqrt(1. - cos_alpha * cos_alpha); + if (sin_acos_cos_alpha <= Eigen::NumTraits::epsilon()) + HPP_FCL_THROW_PRETTY( + "Degenerated neighborhood for the current convex geometry.\n" + << "sin_acos_cos_alpha: " << sin_acos_cos_alpha << "\n", + std::invalid_argument); + const FCL_REAL penetration_depth = -shape_deflation / sin_acos_cos_alpha; + + // step3: Correct the support + support += penetration_depth * average_dir; +} + void getShapeSupportLog(const ConvexBase* convex, const Vec3f& dir, Vec3f& support, int& hint, - MinkowskiDiff::ShapeData* data) { + MinkowskiDiff::ShapeData* data, + const FCL_REAL& shape_deflation) { + assert(shape_deflation <= 0 && "shape_deflation should be negative"); assert(data != NULL); const Vec3f* pts = convex->points; @@ -205,11 +276,16 @@ void getShapeSupportLog(const ConvexBase* convex, const Vec3f& dir, } support = pts[hint]; + + if (shape_deflation < 0) { + correctConvexBaseSupport(convex, support, hint, shape_deflation); + } } void getShapeSupportLinear(const ConvexBase* convex, const Vec3f& dir, - Vec3f& support, int& hint, - MinkowskiDiff::ShapeData*) { + Vec3f& support, int& hint, MinkowskiDiff::ShapeData*, + const FCL_REAL& shape_deflation) { + assert(shape_deflation <= 0 && "shape_deflation should be negative"); const Vec3f* pts = convex->points; hint = 0; @@ -222,31 +298,38 @@ void getShapeSupportLinear(const ConvexBase* convex, const Vec3f& dir, } } support = pts[hint]; + + if (shape_deflation < 0) { + correctConvexBaseSupport(convex, support, hint, shape_deflation); + } } void getShapeSupport(const ConvexBase* convex, const Vec3f& dir, Vec3f& support, - int& hint, MinkowskiDiff::ShapeData*) { + int& hint, MinkowskiDiff::ShapeData*, + const FCL_REAL& shape_deflation) { // TODO add benchmark to set a proper value for switching between linear and // logarithmic. if (convex->num_points > 32) { MinkowskiDiff::ShapeData data; - getShapeSupportLog(convex, dir, support, hint, &data); + getShapeSupportLog(convex, dir, support, hint, &data, shape_deflation); } else - getShapeSupportLinear(convex, dir, support, hint, NULL); + getShapeSupportLinear(convex, dir, support, hint, NULL, shape_deflation); } inline void getShapeSupport(const SmallConvex* convex, const Vec3f& dir, Vec3f& support, int& hint, - MinkowskiDiff::ShapeData* data) { + MinkowskiDiff::ShapeData* data, + const FCL_REAL& shape_deflation) { getShapeSupportLinear(reinterpret_cast(convex), dir, - support, hint, data); + support, hint, data, shape_deflation); } inline void getShapeSupport(const LargeConvex* convex, const Vec3f& dir, Vec3f& support, int& hint, - MinkowskiDiff::ShapeData* data) { + MinkowskiDiff::ShapeData* data, + const FCL_REAL& shape_deflation) { getShapeSupportLog(reinterpret_cast(convex), dir, support, - hint, data); + hint, data, shape_deflation); } #define CALL_GET_SHAPE_SUPPORT(ShapeType) \ @@ -255,10 +338,10 @@ inline void getShapeSupport(const LargeConvex* convex, const Vec3f& dir, (shape_traits::NeedNormalizedDir && !dirIsNormalized) \ ? dir.normalized() \ : dir, \ - support, hint, NULL) + support, hint, NULL, shape_deflation) Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, bool dirIsNormalized, - int& hint) { + int& hint, const FCL_REAL& shape_deflation) { Vec3f support; switch (shape->getNodeType()) { case GEOM_TRIANGLE: @@ -301,12 +384,15 @@ template void getSupportTpl(const Shape0* s0, const Shape1* s1, const Matrix3f& oR1, const Vec3f& ot1, const Vec3f& dir, Vec3f& support0, Vec3f& support1, support_func_guess_t& hint, - MinkowskiDiff::ShapeData data[2]) { - getShapeSupport(s0, dir, support0, hint[0], &(data[0])); + MinkowskiDiff::ShapeData data[2], + const MinkowskiDiff::Array2d& shape_deflation) { + getShapeSupport(s0, dir, support0, hint[0], &(data[0]), shape_deflation[0]); if (TransformIsIdentity) - getShapeSupport(s1, -dir, support1, hint[1], &(data[1])); + getShapeSupport(s1, -dir, support1, hint[1], &(data[1]), + shape_deflation[1]); else { - getShapeSupport(s1, -oR1.transpose() * dir, support1, hint[1], &(data[1])); + getShapeSupport(s1, -oR1.transpose() * dir, support1, hint[1], &(data[1]), + shape_deflation[1]); support1 = oR1 * support1 + ot1; } } @@ -334,7 +420,7 @@ void getSupportFuncTpl(const MinkowskiDiff& md, const Vec3f& dir, static_cast(md.shapes[0]), static_cast(md.shapes[1]), md.oR1, md.ot1, (NeedNormalizedDir && !dirIsNormalized) ? dir.normalized() : dir, - support0, support1, hint, data); + support0, support1, hint, data, md.shape_deflation); } template From f4068cd1241e1feb32801e7af4d3b40a4065ddaf Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Mon, 20 Jun 2022 17:18:36 +0200 Subject: [PATCH 25/52] gjk: add shape_deflation field to GJKSolver --- include/hpp/fcl/narrowphase/narrowphase.h | 22 ++++++++++++++++++++++ 1 file changed, 22 insertions(+) diff --git a/include/hpp/fcl/narrowphase/narrowphase.h b/include/hpp/fcl/narrowphase/narrowphase.h index 12b16c14c..186fa16dd 100644 --- a/include/hpp/fcl/narrowphase/narrowphase.h +++ b/include/hpp/fcl/narrowphase/narrowphase.h @@ -51,6 +51,8 @@ namespace fcl { /// @brief collision and distance solver based on GJK algorithm implemented in /// fcl (rewritten the code from the GJK in bullet) struct HPP_FCL_DLLAPI GJKSolver { + typedef Eigen::Array Array2d; + /// @brief initialize GJK template void initialize_gjk(details::GJK& gjk, const details::MinkowskiDiff& shape, @@ -103,6 +105,7 @@ struct HPP_FCL_DLLAPI GJKSolver { Vec3f* normal) const { details::MinkowskiDiff shape; shape.set(&s1, &s2, tf1, tf2); + shape.shape_deflation = shape_deflation; Vec3f guess; support_func_guess_t support_hint; @@ -346,6 +349,7 @@ struct HPP_FCL_DLLAPI GJKSolver { gjk_variant = GJKVariant::DefaultGJK; gjk_convergence_criterion = GJKConvergenceCriterion::VDB; gjk_convergence_criterion_type = GJKConvergenceCriterionType::Relative; + shape_deflation.setZero(); } /// @brief Constructor from a DistanceRequest @@ -356,6 +360,7 @@ struct HPP_FCL_DLLAPI GJKSolver { cached_guess = Vec3f(1, 0, 0); support_func_cached_guess = support_func_guess_t::Zero(); distance_upper_bound = (std::numeric_limits::max)(); + shape_deflation.setZero(); // EPS settings epa_max_face_num = 128; @@ -393,6 +398,8 @@ struct HPP_FCL_DLLAPI GJKSolver { GJKSolver(const CollisionRequest& request) { cached_guess = Vec3f(1, 0, 0); support_func_cached_guess = support_func_guess_t::Zero(); + distance_upper_bound = (std::numeric_limits::max)(); + shape_deflation.setZero(); // EPS settings epa_max_face_num = 128; @@ -455,6 +462,15 @@ struct HPP_FCL_DLLAPI GJKSolver { bool operator!=(const GJKSolver& other) const { return !(*this == other); } + void resetShapeDeflation() const { shape_deflation.setZero(); } + + void setShapeDeflation(const FCL_REAL shape1_deflation, + const FCL_REAL shape2_deflation) const { + shape_deflation << shape1_deflation, shape2_deflation; + assert((shape_deflation <= 0).all() && + "The deflation values should be negative."); + } + /// @brief maximum number of simplex face used in EPA algorithm unsigned int epa_max_face_num; @@ -502,6 +518,12 @@ struct HPP_FCL_DLLAPI GJKSolver { /// the two shapes have a distance greather than distance_upper_bound. mutable FCL_REAL distance_upper_bound; + /// @brief Inflation values provided to the support functions associated to + /// each shape. + /// @note These inflation values are used to accound for negative + /// security_margin + mutable Array2d shape_deflation; + public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; From b6701dc67e0e3652ed4f073ecf382d338e18ee1c Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Mon, 20 Jun 2022 18:44:10 +0200 Subject: [PATCH 26/52] cor: fix some throw --- include/hpp/fcl/BV/kDOP.h | 4 ++-- .../hpp/fcl/broadphase/detail/simple_hash_table-inl.h | 3 ++- include/hpp/fcl/internal/traversal.h | 4 ++-- include/hpp/fcl/internal/traversal_node_shapes.h | 2 +- include/hpp/fcl/mesh_loader/assimp.h | 4 +--- include/hpp/fcl/narrowphase/narrowphase.h | 10 ++++++---- include/hpp/fcl/shape/details/convex.hxx | 2 +- 7 files changed, 15 insertions(+), 14 deletions(-) diff --git a/include/hpp/fcl/BV/kDOP.h b/include/hpp/fcl/BV/kDOP.h index e8e6ba328..89eab71d0 100644 --- a/include/hpp/fcl/BV/kDOP.h +++ b/include/hpp/fcl/BV/kDOP.h @@ -175,14 +175,14 @@ class HPP_FCL_DLLAPI KDOP { template bool overlap(const Matrix3f& /*R0*/, const Vec3f& /*T0*/, const KDOP& /*b1*/, const KDOP& /*b2*/) { - throw std::logic_error("not implemented"); + HPP_FCL_THROW_PRETTY("not implemented", std::logic_error); } template bool overlap(const Matrix3f& /*R0*/, const Vec3f& /*T0*/, const KDOP& /*b1*/, const KDOP& /*b2*/, const CollisionRequest& /*request*/, FCL_REAL& /*sqrDistLowerBound*/) { - throw std::logic_error("not implemented"); + HPP_FCL_THROW_PRETTY("not implemented", std::logic_error); } /// @brief translate the KDOP BV diff --git a/include/hpp/fcl/broadphase/detail/simple_hash_table-inl.h b/include/hpp/fcl/broadphase/detail/simple_hash_table-inl.h index a078c321d..de51472a2 100644 --- a/include/hpp/fcl/broadphase/detail/simple_hash_table-inl.h +++ b/include/hpp/fcl/broadphase/detail/simple_hash_table-inl.h @@ -56,7 +56,8 @@ SimpleHashTable::SimpleHashTable(const HashFnc& h) : h_(h) { template void SimpleHashTable::init(size_t size) { if (size == 0) { - throw std::logic_error("SimpleHashTable must have non-zero size."); + HPP_FCL_THROW_PRETTY("SimpleHashTable must have non-zero size.", + std::logic_error); } table_.resize(size); diff --git a/include/hpp/fcl/internal/traversal.h b/include/hpp/fcl/internal/traversal.h index dbf032bb6..e999d8c43 100644 --- a/include/hpp/fcl/internal/traversal.h +++ b/include/hpp/fcl/internal/traversal.h @@ -59,10 +59,10 @@ struct HPP_FCL_DLLAPI RelativeTransformation { template <> struct HPP_FCL_DLLAPI RelativeTransformation { static const Matrix3f& _R() { - throw std::logic_error("should never reach this point"); + HPP_FCL_THROW_PRETTY("should never reach this point", std::logic_error); } static const Vec3f& _T() { - throw std::logic_error("should never reach this point"); + HPP_FCL_THROW_PRETTY("should never reach this point", std::logic_error); } }; } // namespace details diff --git a/include/hpp/fcl/internal/traversal_node_shapes.h b/include/hpp/fcl/internal/traversal_node_shapes.h index 99f1b671e..35577af44 100644 --- a/include/hpp/fcl/internal/traversal_node_shapes.h +++ b/include/hpp/fcl/internal/traversal_node_shapes.h @@ -67,7 +67,7 @@ class HPP_FCL_DLLAPI ShapeCollisionTraversalNode /// @brief BV culling test in one BVTT node bool BVDisjoints(int, int, FCL_REAL&) const { - throw std::runtime_error("Not implemented"); + HPP_FCL_THROW_PRETTY("Not implemented", std::runtime_error); } /// @brief Intersection testing between leaves (two shapes) diff --git a/include/hpp/fcl/mesh_loader/assimp.h b/include/hpp/fcl/mesh_loader/assimp.h index e78d18d05..115933740 100644 --- a/include/hpp/fcl/mesh_loader/assimp.h +++ b/include/hpp/fcl/mesh_loader/assimp.h @@ -96,9 +96,7 @@ inline void meshFromAssimpScene( int res = mesh->beginModel(); if (res != fcl::BVH_OK) { - std::ostringstream error; - error << "fcl BVHReturnCode = " << res; - throw std::runtime_error(error.str()); + HPP_FCL_THROW_PRETTY("fcl BVHReturnCode = " << res, std::runtime_error); } buildMesh(scale, scene, (unsigned)mesh->num_vertices, tv); diff --git a/include/hpp/fcl/narrowphase/narrowphase.h b/include/hpp/fcl/narrowphase/narrowphase.h index 186fa16dd..12cff8aa8 100644 --- a/include/hpp/fcl/narrowphase/narrowphase.h +++ b/include/hpp/fcl/narrowphase/narrowphase.h @@ -69,9 +69,11 @@ struct HPP_FCL_DLLAPI GJKSolver { break; case GJKInitialGuess::BoundingVolumeGuess: if (s1.aabb_radius < 0 || s2.aabb_radius < 0) { - throw std::logic_error( - "computeLocalAABB must have been called on the shapes when using " - "GJKInitialGuess::BoundingVolumeGuess."); + HPP_FCL_THROW_PRETTY( + "computeLocalAABB must have been called on the shapes before " + "using " + "GJKInitialGuess::BoundingVolumeGuess.", + std::logic_error); } guess.noalias() = s1.aabb_center - (shape.oR1 * s2.aabb_center + shape.ot1); @@ -79,7 +81,7 @@ struct HPP_FCL_DLLAPI GJKSolver { support_func_cached_guess; // we could also put it to (0, 0) break; default: - throw std::logic_error("Wrong initial guess for GJK."); + HPP_FCL_THROW_PRETTY("Wrong initial guess for GJK.", std::logic_error); } // TODO: use gjk_initial_guess instead HPP_FCL_COMPILER_DIAGNOSTIC_PUSH diff --git a/include/hpp/fcl/shape/details/convex.hxx b/include/hpp/fcl/shape/details/convex.hxx index 8edac251b..c660b858a 100644 --- a/include/hpp/fcl/shape/details/convex.hxx +++ b/include/hpp/fcl/shape/details/convex.hxx @@ -235,7 +235,7 @@ void Convex::fillNeighbors() { for (unsigned int i = 0; i < num_points; ++i) { Neighbors& n = neighbors[i]; if (nneighbors[i].size() >= (std::numeric_limits::max)()) - throw std::logic_error("Too many neighbors."); + HPP_FCL_THROW_PRETTY("Too many neighbors.", std::logic_error); n.count_ = (unsigned char)nneighbors[i].size(); n.n_ = p_nneighbors; p_nneighbors = From ae338f2bad823fe8da68633b615d064cc9bb4635 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Tue, 21 Jun 2022 13:49:30 +0200 Subject: [PATCH 27/52] all: fix include --- include/hpp/fcl/BV/kDOP.h | 2 +- include/hpp/fcl/broadphase/detail/simple_hash_table.h | 2 +- include/hpp/fcl/broadphase/detail/sparse_hash_table.h | 1 - include/hpp/fcl/fwd.hh | 1 + test/gjk_convergence_criterion.cpp | 1 - 5 files changed, 3 insertions(+), 4 deletions(-) diff --git a/include/hpp/fcl/BV/kDOP.h b/include/hpp/fcl/BV/kDOP.h index 89eab71d0..92d82ba4f 100644 --- a/include/hpp/fcl/BV/kDOP.h +++ b/include/hpp/fcl/BV/kDOP.h @@ -38,7 +38,7 @@ #ifndef HPP_FCL_KDOP_H #define HPP_FCL_KDOP_H -#include +#include #include namespace hpp { diff --git a/include/hpp/fcl/broadphase/detail/simple_hash_table.h b/include/hpp/fcl/broadphase/detail/simple_hash_table.h index e136ddeaa..ed1f0fcd8 100644 --- a/include/hpp/fcl/broadphase/detail/simple_hash_table.h +++ b/include/hpp/fcl/broadphase/detail/simple_hash_table.h @@ -38,10 +38,10 @@ #ifndef HPP_FCL_BROADPHASE_SIMPLEHASHTABLE_H #define HPP_FCL_BROADPHASE_SIMPLEHASHTABLE_H -#include #include #include #include + namespace hpp { namespace fcl { diff --git a/include/hpp/fcl/broadphase/detail/sparse_hash_table.h b/include/hpp/fcl/broadphase/detail/sparse_hash_table.h index f0ca992b3..4d8030087 100644 --- a/include/hpp/fcl/broadphase/detail/sparse_hash_table.h +++ b/include/hpp/fcl/broadphase/detail/sparse_hash_table.h @@ -38,7 +38,6 @@ #ifndef HPP_FCL_BROADPHASE_SPARSEHASHTABLE_H #define HPP_FCL_BROADPHASE_SPARSEHASHTABLE_H -#include #include #include #include diff --git a/include/hpp/fcl/fwd.hh b/include/hpp/fcl/fwd.hh index b1822e18d..0585ce3b4 100644 --- a/include/hpp/fcl/fwd.hh +++ b/include/hpp/fcl/fwd.hh @@ -40,6 +40,7 @@ #include #include +#include #include #include diff --git a/test/gjk_convergence_criterion.cpp b/test/gjk_convergence_criterion.cpp index f1d4d5c8a..92e8c48fe 100644 --- a/test/gjk_convergence_criterion.cpp +++ b/test/gjk_convergence_criterion.cpp @@ -36,7 +36,6 @@ #include "hpp/fcl/data_types.h" #include -#include #define BOOST_TEST_MODULE FCL_NESTEROV_GJK #include From d0829c6dca75760333f6c1f7b6a86128560d805b Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Wed, 22 Jun 2022 22:54:55 +0200 Subject: [PATCH 28/52] hfield: remove useless specilaizations --- .../internal/traversal_node_hfield_shape.h | 253 ------------------ 1 file changed, 253 deletions(-) diff --git a/include/hpp/fcl/internal/traversal_node_hfield_shape.h b/include/hpp/fcl/internal/traversal_node_hfield_shape.h index d8311b078..a67aa5d80 100644 --- a/include/hpp/fcl/internal/traversal_node_hfield_shape.h +++ b/include/hpp/fcl/internal/traversal_node_hfield_shape.h @@ -345,176 +345,6 @@ class HeightFieldShapeCollisionTraversalNode const S* model2; BV model2_bv; - mutable int num_bv_tests; - mutable int num_leaf_tests; - mutable FCL_REAL query_time_seconds; -}; - -/// @brief Traversal node for collision between shape and mesh -template -class ShapeHeightFieldCollisionTraversalNode - : public CollisionTraversalNodeBase { - public: - typedef CollisionTraversalNodeBase Base; - - enum { - Options = _Options, - RTIsIdentity = _Options & RelativeTransformationIsIdentity - }; - - ShapeHeightFieldCollisionTraversalNode(const CollisionRequest& request) - : CollisionTraversalNodeBase(request) { - model1 = NULL; - model2 = NULL; - - num_bv_tests = 0; - num_leaf_tests = 0; - query_time_seconds = 0.0; - - nsolver = NULL; - } - - /// @brief Alway extend the second model, which is a BVH model - bool firstOverSecond(unsigned int, unsigned int) const { return false; } - - /// @brief Whether the BV node in the second BVH tree is leaf - bool isSecondNodeLeaf(unsigned int b) const { - return model2->getBV(b).isLeaf(); - } - - /// @brief Obtain the left child of BV node in the second BVH - int getSecondLeftChild(unsigned int b) const { - return model2->getBV(b).leftChild(); - } - - /// @brief Obtain the right child of BV node in the second BVH - int getSecondRightChild(unsigned int b) const { - return model2->getBV(b).rightChild(); - } - - /// BV test between b1 and b2 - /// @param b2 Bounding volumes to test, - /// @retval sqrDistLowerBound square of a lower bound of the minimal - /// distance between bounding volumes. - bool BVDisjoints(unsigned int /*b1*/, unsigned int b2, - FCL_REAL& sqrDistLowerBound) const { - if (this->enable_statistics) this->num_bv_tests++; - bool disjoint; - if (RTIsIdentity) - disjoint = !this->model2->getBV(b2).bv.overlap(this->model1_bv, - sqrDistLowerBound); - else - 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 - bool shapeDistance(const S& shape, const Transform3f& tf1, - const Convex& convex1, - const Convex& convex2, const Transform3f& tf2, - FCL_REAL& distance, Vec3f& c1, Vec3f& c2, - Vec3f& normal) const { - const Transform3f Id; - Vec3f contact2_1, contact2_2, normal2; - FCL_REAL distance2; - bool collision1, collision2; - if (RTIsIdentity) - collision1 = !nsolver->shapeDistance(shape, tf1, convex1, Id, distance, - c1, c2, normal); - else - collision1 = !nsolver->shapeDistance(shape, tf1, convex1, tf2, distance, - c1, c2, normal); - - if (RTIsIdentity) - collision2 = !nsolver->shapeDistance(shape, tf1, convex2, Id, distance2, - c1, c2, normal); - else - collision2 = !nsolver->shapeDistance(shape, tf1, convex2, tf2, distance2, - contact2_1, contact2_2, normal2); - - if (collision1 && collision2) { - if (distance > distance2) // switch values - { - distance = distance2; - c1 = contact2_1; - c2 = contact2_2; - normal = normal2; - } - return true; - } else if (collision1) { - return true; - } else if (collision2) { - distance = distance2; - c1 = contact2_1; - c2 = contact2_2; - normal = normal2; - return true; - } - - return false; - } - - /// @brief Intersection testing between leaves (one shape and one triangle) - void leafCollides(unsigned int /*b1*/, unsigned int b2, - FCL_REAL& sqrDistLowerBound) const { - if (this->enable_statistics) this->num_leaf_tests++; - const HFNode& node = this->model2->getBV(b2); - - // typedef Convex ConvexQuadrilateral; - // const ConvexQuadrilateral convex = - // details::buildConvexQuadrilateral(node,*this->model2); - - typedef Convex ConvexTriangle; - ConvexTriangle convex1, convex2; - details::buildConvexTriangles(node, *this->model1, convex1, convex2); - - FCL_REAL distance; - Vec3f normal; - Vec3f c1, c2; // closest points - - bool collision = - 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, - -distance)); - assert(this->result->isCollision()); - return; - } - } 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, 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; - - const S* model1; - const HeightField* model2; - BV model1_bv; mutable int num_bv_tests; mutable int num_leaf_tests; @@ -612,89 +442,6 @@ class HeightFieldShapeDistanceTraversalNode : public DistanceTraversalNodeBase { mutable FCL_REAL query_time_seconds; }; -/// @brief Traversal node for distance computation between shape and mesh -template -class ShapeHeightFieldDistanceTraversalNode : public DistanceTraversalNodeBase { - public: - typedef DistanceTraversalNodeBase Base; - - enum { - Options = _Options, - RTIsIdentity = _Options & RelativeTransformationIsIdentity - }; - - ShapeHeightFieldDistanceTraversalNode() { - model1 = NULL; - model2 = NULL; - - num_leaf_tests = 0; - query_time_seconds = 0.0; - - rel_err = 0; - abs_err = 0; - nsolver = NULL; - } - - /// @brief Alway extend the second model, which is a BVH model - bool firstOverSecond(unsigned int, unsigned int) const { return false; } - - /// @brief Whether the BV node in the second BVH tree is leaf - bool isSecondNodeLeaf(unsigned int b) const { - return model2->getBV(b).isLeaf(); - } - - /// @brief Obtain the left child of BV node in the second BVH - int getSecondLeftChild(unsigned int b) const { - return model2->getBV(b).leftChild(); - } - - /// @brief Obtain the right child of BV node in the second BVH - int getSecondRightChild(unsigned int b) const { - return model2->getBV(b).rightChild(); - } - - /// @brief Distance testing between leaves (one triangle and one shape) - void leafComputeDistance(unsigned int /*b1*/, unsigned int b2) const { - if (this->enable_statistics) this->num_leaf_tests++; - - const BVNode& node = this->model2->getBV(b2); - - typedef Convex ConvexQuadrilateral; - const ConvexQuadrilateral convex = - details::buildConvexQuadrilateral(node, *this->model2); - - FCL_REAL d; - Vec3f closest_p1, closest_p2, normal; - - nsolver->shapeDistance(*(this->model1), this->tf1, convex, this->tf2, d, - closest_p1, closest_p2, normal); - - this->result->update(d, this->model1, this->model2, DistanceResult::NONE, - b2, closest_p1, closest_p2, normal); - } - - /// @brief Whether the traversal process can stop early - bool canStop(FCL_REAL c) const { - if ((c >= this->result->min_distance - abs_err) && - (c * (1 + rel_err) >= this->result->min_distance)) - return true; - return false; - } - - FCL_REAL rel_err; - FCL_REAL abs_err; - - const GJKSolver* nsolver; - - const S* model1; - const HeightField* model2; - BV model1_bv; - - mutable int num_leaf_tests; - mutable FCL_REAL query_time_seconds; -}; - /// @} } // namespace fcl From fd2eeaa212d0f7e21f1b61ea4af1d3d1f4322f92 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Thu, 23 Jun 2022 17:56:09 +0200 Subject: [PATCH 29/52] hfield: add max_heifght to the height field node information --- include/hpp/fcl/hfield.h | 31 +++++++++++++++----------- include/hpp/fcl/serialization/hfield.h | 1 + 2 files changed, 19 insertions(+), 13 deletions(-) diff --git a/include/hpp/fcl/hfield.h b/include/hpp/fcl/hfield.h index 44a50f0f0..6e04e79db 100644 --- a/include/hpp/fcl/hfield.h +++ b/include/hpp/fcl/hfield.h @@ -60,15 +60,17 @@ struct HPP_FCL_DLLAPI HFNodeBase { Eigen::DenseIndex x_id, x_size; Eigen::DenseIndex y_id, y_size; + + FCL_REAL max_height; /// @brief Default constructor - HFNodeBase() : first_child(0), x_id(-1), x_size(0), y_id(-1), y_size(0) {} + HFNodeBase() : first_child(0), x_id(-1), x_size(0), y_id(-1), y_size(0), max_height(std::numeric_limits::lowest()) {} /// @brief Comparison operator bool operator==(const HFNodeBase& other) const { return first_child == other.first_child && x_id == other.x_id && x_size == other.x_size && y_id == other.y_id && - y_size == other.y_size; + y_size == other.y_size && max_height == other.max_height; } /// @brief Difference operator @@ -352,6 +354,8 @@ class HPP_FCL_DLLAPI HeightField : public CollisionGeometry { max_height = (std::max)(max_left_height, max_right_height); } + + bv_node.max_height = max_height; const Vec3f pointA(x_grid[bv_node.x_id], y_grid[bv_node.y_id], min_height); const Vec3f pointB(x_grid[bv_node.x_id + bv_node.x_size], @@ -372,14 +376,14 @@ class HPP_FCL_DLLAPI HeightField : public CollisionGeometry { "x_size or y_size are not of correct value"); assert(bv_id < bvs.size() && "bv_id exceeds the vector dimension"); - HFNode& bvnode = bvs[bv_id]; + HFNode& bv_node = bvs[bv_id]; FCL_REAL max_height; if (x_size == 1 && y_size == 1) // don't build any BV for the current child node { max_height = heights.block<2, 2>(y_id, x_id).maxCoeff(); } else { - bvnode.first_child = num_bvs; + bv_node.first_child = num_bvs; num_bvs += 2; FCL_REAL max_left_height = min_height, max_right_height = min_height; @@ -387,27 +391,28 @@ class HPP_FCL_DLLAPI HeightField : public CollisionGeometry { { Eigen::DenseIndex x_size_half = x_size / 2; if (x_size == 1) x_size_half = 1; - max_left_height = recursiveBuildTree(bvnode.leftChild(), x_id, + max_left_height = recursiveBuildTree(bv_node.leftChild(), x_id, x_size_half, y_id, y_size); max_right_height = - recursiveBuildTree(bvnode.rightChild(), x_id + x_size_half, + recursiveBuildTree(bv_node.rightChild(), x_id + x_size_half, x_size - x_size_half, y_id, y_size); } else // splitting along the Y axis { Eigen::DenseIndex y_size_half = y_size / 2; if (y_size == 1) y_size_half = 1; - max_left_height = recursiveBuildTree(bvnode.leftChild(), x_id, x_size, + max_left_height = recursiveBuildTree(bv_node.leftChild(), x_id, x_size, y_id, y_size_half); max_right_height = - recursiveBuildTree(bvnode.rightChild(), x_id, x_size, + recursiveBuildTree(bv_node.rightChild(), x_id, x_size, y_id + y_size_half, y_size - y_size_half); } max_height = (std::max)(max_left_height, max_right_height); } + bv_node.max_height = max_height; // max_height = std::max(max_height,min_height); const Vec3f pointA(x_grid[x_id], y_grid[y_id], min_height); @@ -416,11 +421,11 @@ class HPP_FCL_DLLAPI HeightField : public CollisionGeometry { const Vec3f pointB(x_grid[x_id + x_size], y_grid[y_id + y_size], max_height); - details::UpdateBoundingVolume::run(pointA, pointB, bvnode.bv); - bvnode.x_id = x_id; - bvnode.y_id = y_id; - bvnode.x_size = x_size; - bvnode.y_size = y_size; + details::UpdateBoundingVolume::run(pointA, pointB, bv_node.bv); + bv_node.x_id = x_id; + bv_node.y_id = y_id; + bv_node.x_size = x_size; + bv_node.y_size = y_size; return max_height; } diff --git a/include/hpp/fcl/serialization/hfield.h b/include/hpp/fcl/serialization/hfield.h index c88f63abd..7ae2433ef 100644 --- a/include/hpp/fcl/serialization/hfield.h +++ b/include/hpp/fcl/serialization/hfield.h @@ -21,6 +21,7 @@ void serialize(Archive &ar, hpp::fcl::HFNodeBase &node, ar &make_nvp("x_size", node.x_size); ar &make_nvp("y_id", node.y_id); ar &make_nvp("y_size", node.y_size); + ar &make_nvp("max_height", node.max_height); } template From f5436ee44386f0b7a8647d30f54bfe9a60e4471b Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Thu, 23 Jun 2022 23:16:07 +0200 Subject: [PATCH 30/52] BV: update overlap to account for negative security margin --- src/BV/AABB.cpp | 14 ++--- src/BV/OBB.cpp | 13 +++-- src/BV/RSS.cpp | 9 ++- test/security_margin.cpp | 116 +++++++++++++++++++++++++++++++++++++++ 4 files changed, 137 insertions(+), 15 deletions(-) diff --git a/src/BV/AABB.cpp b/src/BV/AABB.cpp index b6ed66cbc..6a13c9e90 100644 --- a/src/BV/AABB.cpp +++ b/src/BV/AABB.cpp @@ -49,15 +49,13 @@ AABB::AABB() bool AABB::overlap(const AABB& other, const CollisionRequest& request, FCL_REAL& sqrDistLowerBound) const { - const FCL_REAL breakDistance(request.break_distance + - request.security_margin); - const FCL_REAL breakDistance2 = breakDistance * breakDistance; + const FCL_REAL break_distance_squared = request.break_distance * request.break_distance; + + sqrDistLowerBound = (min_ - other.max_ - Vec3f::Constant(request.security_margin)).array().max(0).matrix().squaredNorm(); + if (sqrDistLowerBound > break_distance_squared) return false; - sqrDistLowerBound = (min_ - other.max_).array().max(0).matrix().squaredNorm(); - if (sqrDistLowerBound > breakDistance2) return false; - - sqrDistLowerBound = (other.min_ - max_).array().max(0).matrix().squaredNorm(); - if (sqrDistLowerBound > breakDistance2) return false; + sqrDistLowerBound = (other.min_ - max_ - Vec3f::Constant(request.security_margin)).array().max(0).matrix().squaredNorm(); + if (sqrDistLowerBound > break_distance_squared) return false; return true; } diff --git a/src/BV/OBB.cpp b/src/BV/OBB.cpp index 4c3d159aa..73f9a1e9e 100644 --- a/src/BV/OBB.cpp +++ b/src/BV/OBB.cpp @@ -343,14 +343,19 @@ struct HPP_FCL_LOCAL obbDisjoint_check_Ai_cross_Bi { // This function tests whether bounding boxes should be broken down. // bool obbDisjointAndLowerBoundDistance(const Matrix3f& B, const Vec3f& T, - const Vec3f& a, const Vec3f& b, + const Vec3f& a_, const Vec3f& b_, const CollisionRequest& request, FCL_REAL& squaredLowerBoundDistance) { - const FCL_REAL breakDistance(request.break_distance + - request.security_margin); - const FCL_REAL breakDistance2 = breakDistance * breakDistance; + + assert(request.security_margin > -2*(std::min)(a_.minCoeff(),b_.minCoeff()) - 10*Eigen::NumTraits::epsilon() + && "A negative security margin could not be lower than the OBB extent."); +// const FCL_REAL breakDistance(request.break_distance + +// request.security_margin); + const FCL_REAL breakDistance2 = request.break_distance * request.break_distance; Matrix3f Bf(B.cwiseAbs()); + const Vec3f a((a_ + Vec3f::Constant(request.security_margin/2)).array().max(0)); + const Vec3f b((b_ + Vec3f::Constant(request.security_margin/2)).array().max(0)); // Corner of b axis aligned bounding box the closest to the origin squaredLowerBoundDistance = internal::obbDisjoint_check_A_axis(T, a, b, Bf); diff --git a/src/BV/RSS.cpp b/src/BV/RSS.cpp index b6d91cf77..07467e6e9 100644 --- a/src/BV/RSS.cpp +++ b/src/BV/RSS.cpp @@ -37,8 +37,11 @@ #include #include -#include #include +#include + +#include + namespace hpp { namespace fcl { @@ -740,7 +743,7 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, } bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2, - const CollisionRequest& /*request*/, FCL_REAL& sqrDistLowerBound) { + const CollisionRequest& request, FCL_REAL& sqrDistLowerBound) { // ROb2 = R0 . b2 // where b2 = [ b2.axis [0] | b2.axis [1] | b2.axis [2] ] @@ -751,7 +754,7 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2, Matrix3f R(b1.axes.transpose() * R0 * b2.axes); FCL_REAL dist = - rectDistance(R, T, b1.length, b2.length) - b1.radius - b2.radius; + rectDistance(R, T, b1.length, b2.length) - b1.radius - b2.radius - request.security_margin; if (dist <= 0) return true; sqrDistLowerBound = dist * dist; return false; diff --git a/test/security_margin.cpp b/test/security_margin.cpp index 1647a4b7d..552e3c443 100644 --- a/test/security_margin.cpp +++ b/test/security_margin.cpp @@ -55,6 +55,122 @@ using hpp::fcl::DistanceResult; using hpp::fcl::Transform3f; using hpp::fcl::Vec3f; +#define MATH_SQUARED(x) (x*x) + +BOOST_AUTO_TEST_CASE(aabb_aabb) { + 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, 1, 1)); + hpp::fcl::Box s1(1, 1, 1); + hpp::fcl::Box s2(1, 1, 1); + const double tol = 1e-8; + + AABB bv1, bv2; + computeBV(s1, Transform3f(), bv1); + computeBV(s2, Transform3f(), bv2); + + // No security margin - collision + { + CollisionRequest collisionRequest(CONTACT, 1); + AABB bv2_transformed; + computeBV(s2, tf2_collision, bv2_transformed); + FCL_REAL sqrDistLowerBound; + bool res = bv1.overlap(bv2_transformed, collisionRequest, sqrDistLowerBound); + BOOST_CHECK(res); + BOOST_CHECK_CLOSE(sqrDistLowerBound, 0, tol); + } + + // No security margin - no collision + { + CollisionRequest collisionRequest(CONTACT, 1); + const double distance = 0.01; + Transform3f tf2_no_collision( + Vec3f(tf2_collision.getTranslation() + Vec3f(0, 0, distance))); + AABB bv2_transformed; + computeBV(s2, tf2_no_collision, bv2_transformed); + FCL_REAL sqrDistLowerBound; + bool res = bv1.overlap(bv2_transformed, collisionRequest, sqrDistLowerBound); + BOOST_CHECK(!res); + BOOST_CHECK_CLOSE(sqrDistLowerBound, MATH_SQUARED(distance), tol); + } + + // Security margin - collision + { + CollisionRequest collisionRequest(CONTACT, 1); + const double distance = 0.01; + collisionRequest.security_margin = distance; + Transform3f tf2_no_collision( + Vec3f(tf2_collision.getTranslation() + Vec3f(0, 0, distance))); + AABB bv2_transformed; + computeBV(s2, tf2_no_collision, bv2_transformed); + FCL_REAL sqrDistLowerBound; + bool res = bv1.overlap(bv2_transformed, collisionRequest, sqrDistLowerBound); + BOOST_CHECK(res); + BOOST_CHECK_SMALL(sqrDistLowerBound, tol); + } + + // Negative security margin - collion because the two boxes are in contact + { + CollisionRequest collisionRequest(CONTACT, 1); + const double distance = -0.01; + collisionRequest.security_margin = distance; + const Transform3f tf2( + Vec3f(tf2_collision.getTranslation() + Vec3f(0, distance, distance))); + AABB bv2_transformed; + computeBV(s2, tf2, bv2_transformed); + FCL_REAL sqrDistLowerBound; + bool res = bv1.overlap(bv2_transformed, collisionRequest, sqrDistLowerBound); + BOOST_CHECK(res); + BOOST_CHECK_SMALL(sqrDistLowerBound, tol); + } + + // Negative security margin - no collision + { + CollisionRequest collisionRequest(CONTACT, 1); + const double distance = -0.01; + collisionRequest.security_margin = distance; + AABB bv2_transformed; + computeBV(s2, tf2_collision, bv2_transformed); + FCL_REAL sqrDistLowerBound; + bool res = bv1.overlap(bv2_transformed, collisionRequest, sqrDistLowerBound); + BOOST_CHECK(!res); + BOOST_CHECK_CLOSE(sqrDistLowerBound, MATH_SQUARED((std::sqrt(2)*collisionRequest.security_margin)), tol); + } +} + +BOOST_AUTO_TEST_CASE(aabb_aabb_degenerated_cases) { + 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, 0)); + hpp::fcl::Box s1(1, 1, 1); + hpp::fcl::Box s2(1, 1, 1); + const double tol = 1e-8; + + AABB bv1, bv2; + computeBV(s1, Transform3f(), bv1); + computeBV(s2, Transform3f(), bv2); + + // The two AABB are collocated + { + CollisionRequest collisionRequest(CONTACT, 1); + const double distance = -2.; + collisionRequest.security_margin = distance; + AABB bv2_transformed; + computeBV(s2, tf2_collision, bv2_transformed); + FCL_REAL sqrDistLowerBound; + bool res = bv1.overlap(bv2_transformed, collisionRequest, sqrDistLowerBound); + BOOST_CHECK(!res); + } + + const AABB bv3; + BOOST_CHECK(!bv1.overlap(bv3)); + +} + BOOST_AUTO_TEST_CASE(sphere_sphere) { CollisionGeometryPtr_t s1(new hpp::fcl::Sphere(1)); CollisionGeometryPtr_t s2(new hpp::fcl::Sphere(2)); From c15be21b39ca7b6c148ef049ab425da6b1f783b2 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Thu, 23 Jun 2022 23:17:07 +0200 Subject: [PATCH 31/52] collision: refactor to account for negative security margin --- include/hpp/fcl/internal/shape_shape_func.h | 347 +++++++++++++++++++- src/collision_func_matrix.cpp | 174 +++++++--- 2 files changed, 472 insertions(+), 49 deletions(-) diff --git a/include/hpp/fcl/internal/shape_shape_func.h b/include/hpp/fcl/internal/shape_shape_func.h index 0e2d8051f..3ad225d14 100644 --- a/include/hpp/fcl/internal/shape_shape_func.h +++ b/include/hpp/fcl/internal/shape_shape_func.h @@ -40,10 +40,13 @@ /// @cond INTERNAL #include +#include #include +#include namespace hpp { namespace fcl { + template HPP_FCL_DLLAPI FCL_REAL ShapeShapeDistance(const CollisionGeometry* o1, const Transform3f& tf1, @@ -54,13 +57,349 @@ HPP_FCL_DLLAPI FCL_REAL ShapeShapeDistance(const CollisionGeometry* o1, DistanceResult& result); template +struct ShapeShapeCollider +{ + static std::size_t run(const CollisionGeometry* o1, + const Transform3f& tf1, + const CollisionGeometry* o2, + const Transform3f& tf2, const GJKSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result) { + if (request.isSatisfied(result)) return result.numContacts(); + + DistanceResult distanceResult; + DistanceRequest distanceRequest(request.enable_contact); + FCL_REAL distance = ShapeShapeDistance( + o1, tf1, o2, tf2, nsolver, distanceRequest, distanceResult); + + size_t num_contacts = 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, + (distance <= 0 ? distanceResult.normal : (p2 - p1).normalized()), + -distance); + + result.addContact(contact); + } + num_contacts = result.numContacts(); + } + + return num_contacts; + } +}; + +/// @cond DEV +namespace details +{ + +// Forward declaration +template::IsInflatable, bool shape1_has_inflated_support_function = shape_traits::HasInflatedSupportFunction, bool shape2_is_inflatable = shape_traits::IsInflatable, bool shape2_has_inflated_support_function = shape_traits::HasInflatedSupportFunction> +struct shape_shape_collide_negative_security_margin; + +// Handle case where both shapes are inflatable +template +struct shape_shape_collide_negative_security_margin +{ + static std::size_t run(const ShapeType1 & o1, + const Transform3f& tf1, + const ShapeType2 & o2, + const Transform3f& tf2, + const GJKSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result) + { + const FCL_REAL min_inflation1 = o1.minInflationValue(); + const FCL_REAL min_inflation2 = o2.minInflationValue(); + const FCL_REAL min_inflation = min_inflation1 + min_inflation2; + const FCL_REAL security_margin = request.security_margin; + + if(security_margin < min_inflation) + HPP_FCL_THROW_PRETTY("The request security margin: " + << security_margin + << " is below the minimal security margin authorised by the pair of two shapes" + << "(" << std::string(get_node_type_name(o1.getNodeType())) + << "," << std::string(get_node_type_name(o2.getNodeType())) + << "): " + << min_inflation + << ".\n Please consider increasing the requested security margin.", + std::invalid_argument); + + if(security_margin > min_inflation1) + { + const auto & deflated_result = o1.inflated(security_margin); + const ShapeType1 & deflated_o1 = deflated_result.first; + const Transform3f deflated_tf1 = tf1 * deflated_result.second; + CollisionRequest deflated_request(request); + deflated_request.security_margin = 0; + return ShapeShapeCollider::run(&deflated_o1, deflated_tf1, &o2, tf2, nsolver, deflated_request, result); + } + else if(security_margin > min_inflation2) + { + const auto & deflated_result = o2.inflated(security_margin); + const ShapeType2 & deflated_o2 = deflated_result.first; + const Transform3f deflated_tf2 = tf2 * deflated_result.second; + CollisionRequest deflated_request(request); + deflated_request.security_margin = 0; + return ShapeShapeCollider::run(&o1, tf1, &deflated_o2, deflated_tf2, nsolver, deflated_request, result); + } + else // deflate both + { + const FCL_REAL half_security_margin = 0.5 * security_margin; + const auto & deflated_result1 = o1.inflated(half_security_margin); + const ShapeType1 & deflated_o1 = deflated_result1.first; + const Transform3f deflated_tf1 = tf1 * deflated_result1.second; + const auto & deflated_result2 = o2.inflated(half_security_margin); + const ShapeType2 & deflated_o2 = deflated_result2.first; + const Transform3f deflated_tf2 = tf2 * deflated_result2.second; + CollisionRequest deflated_request(request); + deflated_request.security_margin = 0; + return ShapeShapeCollider::run(&deflated_o1, deflated_tf1, &deflated_o2, deflated_tf2, nsolver, deflated_request, result); + } + } +}; + +// Handle case where only the first shape is inflatable +template +struct shape_shape_collide_negative_security_margin +{ + static std::size_t run(const ShapeType1 & o1, + const Transform3f& tf1, + const ShapeType2 & o2, + const Transform3f& tf2, + const GJKSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result) + { + const FCL_REAL min_inflation = o1.minInflationValue(); + const FCL_REAL security_margin = request.security_margin; + + if(security_margin < min_inflation) + HPP_FCL_THROW_PRETTY("The request security margin: " + << security_margin + << " is below the minimal security margin authorised by the pair of two shapes" + << "(" << std::string(get_node_type_name(o1.getNodeType())) + << "," << std::string(get_node_type_name(o2.getNodeType())) + << "): " + << min_inflation + << ".\n Please consider increasing the requested security margin.", + std::invalid_argument); + + const auto & deflated_result = o1.inflated(security_margin); + const ShapeType1 & deflated_o1 = deflated_result.first; + const Transform3f deflated_tf1 = tf1 * deflated_result.second; + CollisionRequest deflated_request(request); + deflated_request.security_margin = 0; + return ShapeShapeCollider::run(&deflated_o1, deflated_tf1, &o2, tf2, nsolver, deflated_request, result); + } +}; + +// Handle case where only the second shape is inflatable +template +struct shape_shape_collide_negative_security_margin +{ + static std::size_t run(const ShapeType1 & o1, + const Transform3f& tf1, + const ShapeType2 & o2, + const Transform3f& tf2, + const GJKSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result) + { + const std::size_t res = shape_shape_collide_negative_security_margin::run(o2, tf2, o1, tf1, nsolver, request, result); + result.swapObjects(); + return res; + } +}; + +// Handle case where the first shape is inflatable and the second shape has an inflated support function +template +struct shape_shape_collide_negative_security_margin +{ + static std::size_t run(const ShapeType1 & o1, + const Transform3f& tf1, + const ShapeType2 & o2, + const Transform3f& tf2, + const GJKSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result) + { + const FCL_REAL min_inflation = o1.minInflationValue(); + const FCL_REAL security_margin = request.security_margin; + + if(min_inflation < security_margin) + { + const auto & deflated_result = o1.inflated(security_margin); + const ShapeType1 & deflated_o1 = deflated_result.first; + const Transform3f deflated_tf1 = tf1 * deflated_result.second; + CollisionRequest deflated_request(request); + deflated_request.security_margin = 0; + return ShapeShapeCollider::run(&deflated_o1, deflated_tf1, &o2, tf2, nsolver, deflated_request, result); + } + else + { + const FCL_REAL half_security_margin = 0.5 * security_margin; + const auto & deflated_result = o1.inflated(half_security_margin); + const ShapeType1 & deflated_o1 = deflated_result.first; + const Transform3f deflated_tf1 = tf1 * deflated_result.second; + CollisionRequest deflated_request(request); + deflated_request.security_margin = 0; + assert(nsolver != NULL && "The GJK solver is not defined."); + nsolver->setShapeDeflation(0, half_security_margin); + + std::size_t res = ShapeShapeCollider::run(&deflated_o1, deflated_tf1, &o2, tf2, nsolver, deflated_request, result); + + nsolver->resetShapeDeflation(); + return res; + } + } +}; + +// Handle case where the second shape is inflatable and the first shape has an inflated support function +template +struct shape_shape_collide_negative_security_margin +{ + static std::size_t run(const ShapeType1 & o1, + const Transform3f& tf1, + const ShapeType2 & o2, + const Transform3f& tf2, + const GJKSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result) + { + const std::size_t res = shape_shape_collide_negative_security_margin::run(o2, tf2, o1, tf1, nsolver, request, result); + result.swapObjects(); + return res; + } +}; + +// Handle case where the two shapes have inflated support function +template +struct shape_shape_collide_negative_security_margin +{ + static std::size_t run(const ShapeType1 & o1, + const Transform3f& tf1, + const ShapeType2 & o2, + const Transform3f& tf2, + const GJKSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result) + { + const FCL_REAL security_margin = request.security_margin; + const FCL_REAL half_security_margin = 0.5 * security_margin; + CollisionRequest deflated_request(request); + deflated_request.security_margin = 0; + + assert(nsolver != NULL && "The GJK solver is not defined."); + nsolver->setShapeDeflation(half_security_margin, half_security_margin); + + std::size_t res = ShapeShapeCollider::run(&o1, tf1, &o2, tf2, nsolver, deflated_request, result); + + nsolver->resetShapeDeflation(); + return res; + } +}; + +// Handle case where only the first shape has inflated support function +template +struct shape_shape_collide_negative_security_margin +{ + static std::size_t run(const ShapeType1 & o1, + const Transform3f& tf1, + const ShapeType2 & o2, + const Transform3f& tf2, + const GJKSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result) + { + const FCL_REAL security_margin = request.security_margin; + CollisionRequest deflated_request(request); + deflated_request.security_margin = 0; + + assert(nsolver != NULL && "The GJK solver is not defined."); + nsolver->setShapeDeflation(security_margin, 0); + + std::size_t res = ShapeShapeCollider::run(&o1, tf1, &o2, tf2, nsolver, deflated_request, result); + + nsolver->resetShapeDeflation(); + return res; + } +}; + +// Handle case where only the second shape has inflated support function +template +struct shape_shape_collide_negative_security_margin +{ + static std::size_t run(const ShapeType1 & o1, + const Transform3f& tf1, + const ShapeType2 & o2, + const Transform3f& tf2, + const GJKSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result) + { + const std::size_t res = shape_shape_collide_negative_security_margin::run(o2, tf2, o1, tf1, nsolver, request, result); + result.swapObjects(); + return res; + } +}; + +// Handle case where the shapes info are not provided. +template +struct shape_shape_collide_negative_security_margin +{ + static std::size_t run(const ShapeType1 & o1, + const Transform3f& /*tf1*/, + const ShapeType2 & o2, + const Transform3f& /*tf2*/, + const GJKSolver* /*nsolver*/, + const CollisionRequest& /*request*/, + CollisionResult& /*result*/) + { + HPP_FCL_THROW_PRETTY("Negative security margin between node type " + << std::string(get_node_type_name(o1.getNodeType())) + << " and node type " + << std::string(get_node_type_name(o2.getNodeType())) + << " is not supported.", + std::invalid_argument); + return 0; + } +}; +} // namespace details + +/// \endcond + +template HPP_FCL_DLLAPI std::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver* nsolver, const CollisionRequest& request, - CollisionResult& result); + CollisionResult& result) +{ + if(request.security_margin < 0) + { + const ShapeType1 & shape1 = static_cast(*o1); + const ShapeType2 & shape2 = static_cast(*o2); + + return details::shape_shape_collide_negative_security_margin::run(shape1, tf1, shape2, tf2, nsolver, request, result); + } + else + { + return ShapeShapeCollider::run(o1, tf1, o2, tf2, nsolver, request, result); + } +} #define SHAPE_SHAPE_DISTANCE_SPECIALIZATION(T1, T2) \ template <> \ @@ -98,11 +437,13 @@ SHAPE_SHAPE_DISTANCE_SPECIALIZATION(TriangleP, Halfspace); #define SHAPE_SHAPE_COLLIDE_SPECIALIZATION(T1, T2) \ template <> \ - HPP_FCL_DLLAPI std::size_t ShapeShapeCollide( \ +struct ShapeShapeCollider { \ + static HPP_FCL_DLLAPI std::size_t run( \ const CollisionGeometry* o1, const Transform3f& tf1, \ const CollisionGeometry* o2, const Transform3f& tf2, \ const GJKSolver* nsolver, const CollisionRequest& request, \ - CollisionResult& result) + CollisionResult& result); \ +} SHAPE_SHAPE_COLLIDE_SPECIALIZATION(Sphere, Sphere); diff --git a/src/collision_func_matrix.cpp b/src/collision_func_matrix.cpp index c033110ac..e8424f3ed 100644 --- a/src/collision_func_matrix.cpp +++ b/src/collision_func_matrix.cpp @@ -41,6 +41,7 @@ #include <../src/collision_node.h> #include #include +#include #include <../src/traits_traversal.h> namespace hpp { @@ -55,6 +56,9 @@ std::size_t OctreeCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionRequest& request, CollisionResult& result) { if (request.isSatisfied(result)) return result.numContacts(); + + if(request.security_margin < 0) + HPP_FCL_THROW_PRETTY("Negative security margin are not handled yet for Octree", std::invalid_argument); typename TraversalTraitsCollision::CollisionTraversal_t node( request); @@ -70,46 +74,6 @@ std::size_t OctreeCollide(const CollisionGeometry* o1, const Transform3f& tf1, #endif -template -std::size_t ShapeShapeCollide(const CollisionGeometry* o1, - const Transform3f& tf1, - const CollisionGeometry* o2, - const Transform3f& tf2, const GJKSolver* nsolver, - const CollisionRequest& request, - CollisionResult& result) { - if (request.isSatisfied(result)) return result.numContacts(); - - DistanceResult distanceResult; - DistanceRequest distanceRequest(request.enable_contact); - FCL_REAL distance = ShapeShapeDistance( - o1, tf1, o2, tf2, nsolver, distanceRequest, distanceResult); - - size_t num_contacts = 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, - (distance <= 0 ? distanceResult.normal : (p2 - p1).normalized()), - -distance); - - result.addContact(contact); - } - num_contacts = result.numContacts(); - } - - return num_contacts; -} - namespace details { template struct bvh_shape_traits { @@ -141,6 +105,9 @@ struct HPP_FCL_LOCAL BVHShapeCollider { const CollisionRequest& request, CollisionResult& result) { if (request.isSatisfied(result)) return result.numContacts(); + + if(request.security_margin < 0) + HPP_FCL_THROW_PRETTY("Negative security margin are not handled yet for BVHModel", std::invalid_argument); if (_Options & RelativeTransformationIsIdentity) return aligned(o1, tf1, o2, tf2, nsolver, request, result); @@ -189,6 +156,112 @@ struct HPP_FCL_LOCAL BVHShapeCollider { } }; +/// @cond DEV +namespace details +{ + +// Forward declaration +template::IsInflatable, bool shape_has_inflated_support_function = shape_traits::HasInflatedSupportFunction> +struct height_field_shape_collide_negative_security_margin; + +template +struct height_field_shape_collide_negative_security_margin +{ + typedef HeightField HF; + + static std::size_t run(const HF & o1, + const Transform3f& tf1, + const ShapeType & o2, + const Transform3f& tf2, + const GJKSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result) + { + const FCL_REAL min_deflation = o2.minInflationValue(); + const FCL_REAL security_margin = request.security_margin; + +// if(security_margin < min_deflation) +// HPP_FCL_THROW_PRETTY("The request security margin: " +// << security_margin +// << " is below the minimal security margin authorised by the pair of two shapes" +// << "(" << std::string(get_node_type_name(o1.getNodeType())) +// << "," << std::string(get_node_type_name(o2.getNodeType())) +// << "): " +// << min_deflation +// << ".\n Please consider increasing the requested security margin.", +// std::invalid_argument); + + const FCL_REAL deflation_value2 = (std::max)(0.5*min_deflation,security_margin); + const auto & deflated_result = o2.inflated(deflation_value2); + const ShapeType & deflated_o2 = deflated_result.first; + const Transform3f deflated_tf2 = tf2 * deflated_result.second; + + CollisionRequest deflated_request(request); + + deflated_request.security_margin = security_margin - deflation_value2; // We already account for the deflation of the shape. + + HeightFieldShapeCollisionTraversalNode node(deflated_request); + node.shape_inflation[0] = deflated_request.security_margin; + + nsolver->setShapeDeflation(0.,0.); + + initialize(node, o1, tf1, deflated_o2, deflated_tf2, nsolver, result); + fcl::collide(&node, deflated_request, result); + return result.numContacts(); + } +}; + +template +struct height_field_shape_collide_negative_security_margin +{ + typedef HeightField HF; + + static std::size_t run(const HF & o1, + const Transform3f& tf1, + const ShapeType & o2, + const Transform3f& tf2, + const GJKSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result) + { + HeightFieldShapeCollisionTraversalNode node(request); + node.shape_inflation[0] = 0.5 * request.security_margin; + + nsolver->setShapeDeflation(0.,0.5 * request.security_margin); + + initialize(node, o1, tf1, o2, tf2, nsolver, result); + fcl::collide(&node, request, result); + return result.numContacts(); + } +}; + +template +struct height_field_shape_collide_negative_security_margin +{ + typedef HeightField HF; + + static std::size_t run(const HF & o1, + const Transform3f& /*tf1*/, + const ShapeType & o2, + const Transform3f& /*tf2*/, + const GJKSolver* /*nsolver*/, + const CollisionRequest& /*request*/, + CollisionResult& /*result*/) + { + HPP_FCL_THROW_PRETTY("Negative security margin between node type " + << std::string(get_node_type_name(o1.getNodeType())) + << " and node type " + << std::string(get_node_type_name(o2.getNodeType())) + << " is not supported.", + std::invalid_argument); + return 0; + } + +}; +} + +/// @endcond + /// @brief Collider functor for HeightField data structure /// \tparam _Options takes two values. /// - RelativeTransformationIsIdentity if object 1 should be moved @@ -205,14 +278,23 @@ struct HPP_FCL_LOCAL HeightFieldShapeCollider { const CollisionRequest& request, CollisionResult& result) { if (request.isSatisfied(result)) return result.numContacts(); + + const HF & height_field = static_cast(*o1); + const Shape & shape = static_cast(*o2); + + if(request.security_margin < 0) + { + return details::height_field_shape_collide_negative_security_margin::run(height_field, tf1, shape, tf2, nsolver, request, result); + } + else + { + HeightFieldShapeCollisionTraversalNode node(request); - HeightFieldShapeCollisionTraversalNode node(request); - const HF* obj1 = static_cast(o1); - const Shape* obj2 = static_cast(o2); + initialize(node, height_field, tf1, shape, tf2, nsolver, result); + fcl::collide(&node, request, result); + return result.numContacts(); + } - initialize(node, *obj1, tf1, *obj2, tf2, nsolver, result); - fcl::collide(&node, request, result); - return result.numContacts(); } }; From 3b7fe103b3612dd672a24003a7fbf10dababe45d Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Thu, 23 Jun 2022 23:17:50 +0200 Subject: [PATCH 32/52] collision: handle specific case for negative security margin --- src/distance/convex_halfspace.cpp | 14 +++- src/distance/sphere_sphere.cpp | 5 +- src/distance/triangle_halfspace.cpp | 14 +++- src/narrowphase/details.h | 5 +- test/security_margin.cpp | 112 +++++++++++++++++++++++++--- 5 files changed, 125 insertions(+), 25 deletions(-) diff --git a/src/distance/convex_halfspace.cpp b/src/distance/convex_halfspace.cpp index 897386d52..2276c26ff 100644 --- a/src/distance/convex_halfspace.cpp +++ b/src/distance/convex_halfspace.cpp @@ -45,13 +45,16 @@ namespace fcl { template <> FCL_REAL ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver* nsolver, const DistanceRequest&, DistanceResult& result) { + assert(nsolver->shape_deflation[0] <= 0 && "Shape deflation for the ConvexBase object should be negative"); + assert(nsolver->shape_deflation[1] == 0 && "Shape deflation for the HalfSpace object should be zero"); + const ConvexBase& s1 = static_cast(*o1); const Halfspace& s2 = static_cast(*o2); details::halfspaceDistance(s2, tf2, s1, tf1, result.min_distance, result.nearest_points[1], result.nearest_points[0], - result.normal); + result.normal, nsolver->shape_deflation[0]); result.o1 = o1; result.o2 = o2; result.b1 = -1; @@ -63,13 +66,16 @@ FCL_REAL ShapeShapeDistance( template <> FCL_REAL ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver* nsolver, const DistanceRequest&, DistanceResult& result) { + assert(nsolver->shape_deflation[0] == 0 && "Shape deflation for the HalfSpace object should be zero"); + assert(nsolver->shape_deflation[1] <= 0 && "Shape deflation for the ConvexBase object should be negative"); + const Halfspace& s1 = static_cast(*o1); const ConvexBase& s2 = static_cast(*o2); details::halfspaceDistance(s1, tf1, s2, tf2, result.min_distance, result.nearest_points[0], result.nearest_points[1], - result.normal); + result.normal, nsolver->shape_deflation[1]); result.o1 = o1; result.o2 = o2; result.b1 = -1; diff --git a/src/distance/sphere_sphere.cpp b/src/distance/sphere_sphere.cpp index ed1f044cc..e4181d3db 100644 --- a/src/distance/sphere_sphere.cpp +++ b/src/distance/sphere_sphere.cpp @@ -93,8 +93,7 @@ FCL_REAL ShapeShapeDistance( return result.min_distance; } -template <> -std::size_t ShapeShapeCollide( +std::size_t ShapeShapeCollider::run( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, const CollisionRequest& request, CollisionResult& result) { @@ -130,7 +129,7 @@ std::size_t ShapeShapeCollide( return 1; } return 0; -} + } } // namespace fcl } // namespace hpp diff --git a/src/distance/triangle_halfspace.cpp b/src/distance/triangle_halfspace.cpp index 13514371e..bb72ca7c9 100644 --- a/src/distance/triangle_halfspace.cpp +++ b/src/distance/triangle_halfspace.cpp @@ -45,13 +45,16 @@ namespace fcl { template <> FCL_REAL ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver* nsolver, const DistanceRequest&, DistanceResult& result) { +assert(nsolver->shape_deflation[0] == 0 && "Shape deflation for the TriangleP object should be zero"); +assert(nsolver->shape_deflation[1] == 0 && "Shape deflation for the HalfSpace object should be zero"); + const TriangleP& s1 = static_cast(*o1); const Halfspace& s2 = static_cast(*o2); details::halfspaceDistance(s2, tf2, s1, tf1, result.min_distance, result.nearest_points[1], result.nearest_points[0], - result.normal); + result.normal, nsolver->shape_deflation[0]); result.o1 = o1; result.o2 = o2; result.b1 = -1; @@ -63,13 +66,16 @@ FCL_REAL ShapeShapeDistance( template <> FCL_REAL ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver* nsolver, const DistanceRequest&, DistanceResult& result) { + assert(nsolver->shape_deflation[0] == 0 && "Shape deflation for the HalfSpace object should be zero"); + assert(nsolver->shape_deflation[1] == 0 && "Shape deflation for the TriangleP object should be zero"); + const Halfspace& s1 = static_cast(*o1); const TriangleP& s2 = static_cast(*o2); details::halfspaceDistance(s1, tf1, s2, tf2, result.min_distance, result.nearest_points[0], result.nearest_points[1], - result.normal); + result.normal, nsolver->shape_deflation[1]); result.o1 = o1; result.o2 = o2; result.b1 = -1; diff --git a/src/narrowphase/details.h b/src/narrowphase/details.h index bcc3b218d..3bc4bb333 100644 --- a/src/narrowphase/details.h +++ b/src/narrowphase/details.h @@ -1619,6 +1619,7 @@ inline bool convexHalfspaceIntersect( const ConvexBase& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) { + // No need to account for the shape_deflation in the case of Convex,Halfspace as Halfspace objects have an infinite value possible for deflation. Halfspace new_s2 = transform(s2, tf2); Vec3f v; @@ -1808,11 +1809,11 @@ inline bool halfspaceIntersect(const Halfspace& s1, const Transform3f& tf1, inline bool halfspaceDistance(const Halfspace& h, const Transform3f& tf1, const ShapeBase& s, const Transform3f& tf2, FCL_REAL& dist, Vec3f& p1, Vec3f& p2, - Vec3f& normal) { + Vec3f& normal, const FCL_REAL shape_deflation) { Vec3f n_w = tf1.getRotation() * h.n; Vec3f n_2(tf2.getRotation().transpose() * n_w); int hint = 0; - p2 = getSupport(&s, -n_2, true, hint); + p2 = getSupport(&s, -n_2, true, hint, shape_deflation); p2 = tf2.transform(p2); dist = (p2 - tf1.getTranslation()).dot(n_w) - h.d; diff --git a/test/security_margin.cpp b/test/security_margin.cpp index 552e3c443..bb4e98863 100644 --- a/test/security_margin.cpp +++ b/test/security_margin.cpp @@ -42,6 +42,8 @@ #include #include #include +#include +#include #include "utility.h" @@ -202,7 +204,7 @@ BOOST_AUTO_TEST_CASE(sphere_sphere) { BOOST_CHECK_CLOSE(collisionResult.distance_lower_bound, distance, 1e-8); } - // Positive security margin + // Positive security margin - collision { CollisionRequest collisionRequest(CONTACT, 1); CollisionResult collisionResult; @@ -229,8 +231,7 @@ BOOST_AUTO_TEST_CASE(sphere_sphere) { 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); + BOOST_CHECK_SMALL(-collisionResult.getContact(0).penetration_depth, 1e-8); } // Negative security margin - no collision @@ -241,7 +242,7 @@ BOOST_AUTO_TEST_CASE(sphere_sphere) { 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_CHECK_CLOSE(collisionResult.distance_lower_bound, -collisionRequest.security_margin, 1e-8); } } @@ -303,8 +304,7 @@ BOOST_AUTO_TEST_CASE(capsule_capsule) { 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); + BOOST_CHECK_SMALL(-collisionResult.getContact(0).penetration_depth, 1e-8); } // Negative security margin - no collision @@ -324,7 +324,7 @@ BOOST_AUTO_TEST_CASE(box_box) { CollisionGeometryPtr_t b2(new hpp::fcl::Box(1, 1, 1)); const Transform3f tf1; - const Transform3f tf2_collision(Vec3f(0, 0, 1)); + const Transform3f tf2_collision(Vec3f(0, 1, 1)); const double tol = 1e-3; @@ -367,7 +367,7 @@ BOOST_AUTO_TEST_CASE(box_box) { BOOST_CHECK_SMALL(-collisionResult.getContact(0).penetration_depth, 1e-8); } - // Positive security margin - no collision + // Negative security margin - no collision { CollisionRequest collisionRequest(CONTACT, 1); collisionRequest.security_margin = -0.01; @@ -376,7 +376,7 @@ BOOST_AUTO_TEST_CASE(box_box) { collisionResult); BOOST_CHECK(!collisionResult.isCollision()); BOOST_CHECK_CLOSE(collisionResult.distance_lower_bound, - -collisionRequest.security_margin, tol); + (collisionRequest.security_margin * tf2_collision.getTranslation()).norm(), tol); } // Negative security margin - collision @@ -386,12 +386,100 @@ BOOST_AUTO_TEST_CASE(box_box) { CollisionResult collisionResult; const Transform3f tf2((tf2_collision.getTranslation() + - Vec3f(0, 0, collisionRequest.security_margin)) + Vec3f(0, collisionRequest.security_margin, 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); + BOOST_CHECK_SMALL(-collisionResult.getContact(0).penetration_depth, tol); + } +} + +template +void test_shape_shape(const ShapeType1 & shape1, const Transform3f & tf1, const ShapeType2 & shape2, const Transform3f & tf2_collision, const FCL_REAL tol) +{ + // No security margin - collision + { + CollisionRequest collisionRequest(CONTACT, 1); + CollisionResult collisionResult; + collide(&shape1, tf1, &shape2, 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(&shape1, tf1, &shape2, 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(&shape1, tf1, &shape2, 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); + } + + // Negative security margin - no collision + { + CollisionRequest collisionRequest(CONTACT, 1); + collisionRequest.security_margin = -0.01; + CollisionResult collisionResult; + collide(&shape1, tf1, &shape2, tf2_collision, collisionRequest, + collisionResult); + BOOST_CHECK(!collisionResult.isCollision()); + BOOST_CHECK_CLOSE(collisionResult.distance_lower_bound, + (collisionRequest.security_margin * tf2_collision.getTranslation()).norm(), tol); } + + // Negative security margin - collision + { + CollisionRequest collisionRequest(CONTACT, 1); + collisionRequest.security_margin = -0.01; + CollisionResult collisionResult; + + const Transform3f tf2((tf2_collision.getTranslation() + + Vec3f(0, collisionRequest.security_margin, collisionRequest.security_margin)) + .eval()); + collide(&shape1, tf1, &shape2, tf2, collisionRequest, collisionResult); + BOOST_CHECK(collisionResult.isCollision()); + BOOST_CHECK_SMALL(collisionResult.distance_lower_bound, tol); + BOOST_CHECK_SMALL(-collisionResult.getContact(0).penetration_depth, tol); + } +} + +BOOST_AUTO_TEST_CASE(sphere_box) { + Box* box_ptr = new hpp::fcl::Box(1, 1, 1); + CollisionGeometryPtr_t b1(box_ptr); + BVHModel box_bvh_model = BVHModel(); + generateBVHModel(box_bvh_model, *box_ptr, Transform3f()); + box_bvh_model.buildConvexRepresentation(false); + ConvexBase & box_convex = *box_bvh_model.convex.get(); + CollisionGeometryPtr_t s2(new hpp::fcl::Sphere(0.5)); + + const Transform3f tf1; + const Transform3f tf2_collision(Vec3f(0, 0, 1)); + + const double tol = 1e-6; + + test_shape_shape(*b1.get(), tf1, *s2.get(), tf2_collision, tol); + test_shape_shape(box_convex, tf1, *s2.get(), tf2_collision, tol); + } From 46652000547f7eceace6fcb76c9daff447ea2521 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Thu, 23 Jun 2022 23:18:23 +0200 Subject: [PATCH 33/52] hfield: handle negative security margin --- .../internal/traversal_node_hfield_shape.h | 134 ++++++++++++++--- test/hfields.cpp | 141 ++++++++++++++++++ 2 files changed, 258 insertions(+), 17 deletions(-) diff --git a/include/hpp/fcl/internal/traversal_node_hfield_shape.h b/include/hpp/fcl/internal/traversal_node_hfield_shape.h index a67aa5d80..0e4857992 100644 --- a/include/hpp/fcl/internal/traversal_node_hfield_shape.h +++ b/include/hpp/fcl/internal/traversal_node_hfield_shape.h @@ -102,7 +102,8 @@ Convex buildConvexQuadrilateral(const HFNode& node, template void buildConvexTriangles(const HFNode& node, const HeightField& model, Convex& convex1, - Convex& convex2) { + Convex& convex2, + const FCL_REAL deflation) { const MatrixXf& heights = model.getHeights(); const VecXf& x_grid = model.getXGrid(); const VecXf& y_grid = model.getYGrid(); @@ -115,6 +116,8 @@ void buildConvexTriangles(const HFNode& node, const HeightField& model, heights.block<2, 2>(node.y_id, node.x_id); const FCL_REAL max_height = cell.maxCoeff(); + assert(deflation <= 0 && "deflation should be negative"); + assert(max_height > min_height && "max_height is lower than min_height"); // Check whether the geometry // is degenerated @@ -131,9 +134,15 @@ void buildConvexTriangles(const HFNode& node, const HeightField& model, pts[6] = Vec3f(x1, y1, cell(1, 1)); pts[7] = Vec3f(x1, y0, cell(0, 1)); + const Vec3f normal_upper_triangular_face = ((pts[7] - pts[4]).cross(pts[5] - pts[4])).normalized(); + const FCL_REAL cos_alpha = normal_upper_triangular_face.dot(Vec3f::UnitZ()); + pts[4][2] += deflation / cos_alpha; + pts[5][2] += deflation / cos_alpha; + pts[7][2] += deflation / cos_alpha; + Triangle* triangles = new Triangle[8]; - triangles[0].set(0, 1, 3); - triangles[1].set(4, 5, 7); + triangles[0].set(0, 1, 3); // bottom + triangles[1].set(4, 5, 7); // top triangles[2].set(0, 1, 4); triangles[3].set(4, 1, 5); triangles[4].set(1, 7, 3); @@ -160,9 +169,15 @@ void buildConvexTriangles(const HFNode& node, const HeightField& model, pts[6] = Vec3f(x1, y1, cell(1, 1)); pts[7] = Vec3f(x1, y0, cell(0, 1)); + const Vec3f normal_upper_triangular_face = ((pts[7] - pts[5]).cross(pts[6] - pts[5])).normalized(); + const FCL_REAL cos_alpha = normal_upper_triangular_face.dot(Vec3f::UnitZ()); + pts[5][2] += deflation / cos_alpha; + pts[6][2] += deflation / cos_alpha; + pts[7][2] += deflation / cos_alpha; + Triangle* triangles = new Triangle[8]; - triangles[0].set(3, 2, 1); - triangles[1].set(5, 6, 7); + triangles[0].set(3, 2, 1); // top + triangles[1].set(5, 6, 7); // bottom triangles[2].set(1, 2, 5); triangles[3].set(5, 2, 6); triangles[4].set(1, 3, 7); @@ -187,6 +202,7 @@ class HeightFieldShapeCollisionTraversalNode : public CollisionTraversalNodeBase { public: typedef CollisionTraversalNodeBase Base; + typedef Eigen::Array Array2d; enum { Options = _Options, @@ -203,6 +219,7 @@ class HeightFieldShapeCollisionTraversalNode query_time_seconds = 0.0; nsolver = NULL; + shape_inflation.setZero(); } /// @brief Whether the BV node in the first BVH tree is leaf @@ -228,19 +245,52 @@ class HeightFieldShapeCollisionTraversalNode bool BVDisjoints(unsigned int b1, unsigned int /*b2*/, FCL_REAL& sqrDistLowerBound) const { if (this->enable_statistics) this->num_bv_tests++; + bool disjoint; - if (RTIsIdentity) { - assert(false && "must never happened"); - disjoint = !this->model1->getBV(b1).bv.overlap( - this->model2_bv, this->request, sqrDistLowerBound); - } else { - disjoint = !overlap(this->tf1.getRotation(), this->tf1.getTranslation(), - this->model2_bv, this->model1->getBV(b1).bv, - this->request, sqrDistLowerBound); + const FCL_REAL deflation = shape_inflation[0]; + + if(deflation < 0) + { + typedef typename HeightField::Node Node; + const HeightField & hfield = *this->model1; + const Node & node = hfield.getBV(b1); + const VecXf& x_grid = hfield.getXGrid(); + const VecXf& y_grid = hfield.getYGrid(); + const Vec3f pointA(x_grid[node.x_id], y_grid[node.y_id], hfield.getMinHeight()); + const Vec3f pointB(x_grid[node.x_id + node.x_size], y_grid[node.y_id + node.y_size], + node.max_height - deflation); + + BV bv_deflated; + details::UpdateBoundingVolume::run(pointA, pointB, bv_deflated); + + if (RTIsIdentity) { + assert(false && "must never happened"); + disjoint = !bv_deflated.overlap( + this->model2_bv, this->request, sqrDistLowerBound); + } else { + disjoint = !overlap(this->tf1.getRotation(), this->tf1.getTranslation(), + bv_deflated, this->model2_bv, + this->request, sqrDistLowerBound); + } + } + else + { + if (RTIsIdentity) { + assert(false && "must never happened"); + disjoint = !this->model1->getBV(b1).bv.overlap( + this->model2_bv, this->request, sqrDistLowerBound); + } else { + disjoint = !overlap(this->tf1.getRotation(), this->tf1.getTranslation(), + this->model1->getBV(b1).bv, this->model2_bv, + this->request, sqrDistLowerBound); + } } + + if (disjoint) internal::updateDistanceLowerBoundFromBV(this->request, *this->result, sqrDistLowerBound); + assert(!disjoint || sqrDistLowerBound > 0); return disjoint; } @@ -290,6 +340,49 @@ class HeightFieldShapeCollisionTraversalNode return false; } + template + bool shapeCollision(const Convex& convex1, + const Convex& convex2, const Transform3f& tf1, + const S& shape, const Transform3f& tf2, FCL_REAL& distance_lower_bound, + Vec3f& contact_point, Vec3f& normal) const { + const Transform3f Id; + Vec3f contact_point2, normal2; + FCL_REAL distance_lower_bound2; + bool collision1, collision2; + if (RTIsIdentity) + collision1 = nsolver->shapeIntersect(convex1, Id, shape, tf2, distance_lower_bound, + true, &contact_point, &normal); + else + collision1 = nsolver->shapeIntersect(convex1, tf1, shape, tf2, distance_lower_bound, + true, &contact_point, &normal); + + if (RTIsIdentity) + collision2 = nsolver->shapeIntersect(convex2, Id, shape, tf2, distance_lower_bound2, + true, &contact_point2, &normal2); + else + collision2 = nsolver->shapeIntersect(convex2, tf1, shape, tf2, distance_lower_bound2, + true, &contact_point2, &normal2); + + if (collision1 && collision2) { + if (distance_lower_bound > distance_lower_bound2) // switch values + { + distance_lower_bound = distance_lower_bound2; + contact_point = contact_point2; + normal = normal2; + } + return true; + } else if (collision1) { + return true; + } else if (collision2) { + distance_lower_bound = distance_lower_bound2; + contact_point = contact_point2; + normal = normal2; + return true; + } + + return false; + } + /// @brief Intersection testing between leaves (one Convex and one shape) void leafCollides(unsigned int b1, unsigned int /*b2*/, FCL_REAL& sqrDistLowerBound) const { @@ -305,14 +398,18 @@ class HeightFieldShapeCollisionTraversalNode typedef Convex ConvexTriangle; ConvexTriangle convex1, convex2; - details::buildConvexTriangles(node, *this->model1, convex1, convex2); + details::buildConvexTriangles(node, *this->model1, convex1, convex2, shape_inflation[0]); FCL_REAL distance; +// Vec3f contact_point, normal; Vec3f c1, c2, normal; bool collision = - this->shapeDistance(convex1, convex2, this->tf1, *(this->model2), - this->tf2, distance, c1, c2, normal); + this->shapeDistance(convex1, convex2, this->tf1, *(this->model2), this->tf2, + distance, c1, c2, normal); + +// this->shapeCollision(convex1, convex2, this->tf1, *(this->model2), this->tf2, +// distance, contact_point, normal); FCL_REAL distToCollision = distance - this->request.security_margin; if (collision) { @@ -333,6 +430,8 @@ class HeightFieldShapeCollisionTraversalNode } else sqrDistLowerBound = distToCollision * distToCollision; +// const Vec3f c1 = contact_point - distance * 0.5 * normal; +// const Vec3f c2 = contact_point + distance * 0.5 * normal; internal::updateDistanceLowerBoundFromLeaf(this->request, *this->result, distToCollision, c1, c2); @@ -345,6 +444,7 @@ class HeightFieldShapeCollisionTraversalNode const S* model2; BV model2_bv; + Array2d shape_inflation; mutable int num_bv_tests; mutable int num_leaf_tests; @@ -397,7 +497,7 @@ class HeightFieldShapeDistanceTraversalNode : public DistanceTraversalNodeBase { /// @brief BV culling test in one BVTT node FCL_REAL BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const { - return model1->getBV(b1).bv.distance(model2_bv); + return model1->getBV(b1).bv.distance(model2_bv); //TODO(jcarpent): tf1 is not taken into account here. } /// @brief Distance testing between leaves (one triangle and one shape) diff --git a/test/hfields.cpp b/test/hfields.cpp index b16873570..d41197f63 100644 --- a/test/hfields.cpp +++ b/test/hfields.cpp @@ -268,6 +268,147 @@ BOOST_AUTO_TEST_CASE(building_constant_hfields) { test_constant_hfields(100, 100, min_altitude, max_altitude); } +template +void test_negative_security_margin(const Eigen::DenseIndex nx, + const Eigen::DenseIndex ny, + const FCL_REAL min_altitude, + const FCL_REAL max_altitude) { + const FCL_REAL x_dim = 1., y_dim = 2.; + const MatrixXf heights = MatrixXf::Constant(ny, nx, max_altitude); + + HeightField hfield(x_dim, y_dim, heights, min_altitude); + + // Build equivalent object + const Box equivalent_box(x_dim, y_dim, max_altitude - min_altitude); + const Transform3f box_placement( + Matrix3f::Identity(), Vec3f(0., 0., (max_altitude + min_altitude) / 2.)); + + // Test collision + const Sphere sphere(1.); + static const Transform3f IdTransform; + + const Box box(Vec3f::Ones()); + + Transform3f M_sphere, M_box; + + // No collision case + { + const FCL_REAL eps_no_collision = +0.1 * (max_altitude - min_altitude); + M_sphere.setTranslation( + Vec3f(0., 0., max_altitude + sphere.radius + eps_no_collision)); + M_box.setTranslation( + Vec3f(0., 0., max_altitude + box.halfSide[2] + eps_no_collision)); + CollisionRequest request; + + CollisionResult result; + collide(&hfield, IdTransform, &sphere, M_sphere, request, result); + + BOOST_CHECK(!result.isCollision()); + + CollisionResult result_check_sphere; + collide(&equivalent_box, IdTransform * box_placement, &sphere, M_sphere, + request, result_check_sphere); + + BOOST_CHECK(!result_check_sphere.isCollision()); + + CollisionResult result_check_box; + collide(&equivalent_box, IdTransform * box_placement, &box, M_box, request, + result_check_box); + + BOOST_CHECK(!result_check_box.isCollision()); + } + + // Collision case - positive security_margin + { + const FCL_REAL eps_no_collision = +0.1 * (max_altitude - min_altitude); + M_sphere.setTranslation( + Vec3f(0., 0., max_altitude + sphere.radius + eps_no_collision)); + M_box.setTranslation( + Vec3f(0., 0., max_altitude + box.halfSide[2] + eps_no_collision)); + CollisionRequest request; + request.security_margin = eps_no_collision + 1e-6; + + CollisionResult result; + collide(&hfield, IdTransform, &sphere, M_sphere, request, result); + + BOOST_CHECK(result.isCollision()); + + CollisionResult result_check_sphere; + collide(&equivalent_box, IdTransform * box_placement, &sphere, M_sphere, + request, result_check_sphere); + + BOOST_CHECK(result_check_sphere.isCollision()); + + CollisionResult result_check_box; + collide(&equivalent_box, IdTransform * box_placement, &box, M_box, request, + result_check_box); + + BOOST_CHECK(result_check_box.isCollision()); + } + + // Collision case + { + const FCL_REAL eps_no_collision = -0.1 * (max_altitude - min_altitude); + M_sphere.setTranslation( + Vec3f(0., 0., max_altitude + sphere.radius + eps_no_collision)); + M_box.setTranslation( + Vec3f(0., 0., max_altitude + box.halfSide[2] + eps_no_collision)); + CollisionRequest request; + + CollisionResult result; + collide(&hfield, IdTransform, &sphere, M_sphere, request, result); + + BOOST_CHECK(result.isCollision()); + + CollisionResult result_check_sphere; + collide(&equivalent_box, IdTransform * box_placement, &sphere, M_sphere, + request, result_check_sphere); + + BOOST_CHECK(result_check_sphere.isCollision()); + + CollisionResult result_check_box; + collide(&equivalent_box, IdTransform * box_placement, &box, M_box, request, + result_check_box); + + BOOST_CHECK(result_check_box.isCollision()); + } + + // No collision case - negative security_margin + { + const FCL_REAL eps_no_collision = -0.1 * (max_altitude - min_altitude); + M_sphere.setTranslation( + Vec3f(0., 0., max_altitude + sphere.radius + eps_no_collision)); + M_box.setTranslation( + Vec3f(0., 0., max_altitude + box.halfSide[2] + eps_no_collision)); + CollisionRequest request; + request.security_margin = eps_no_collision - 1e-4; + + CollisionResult result; + collide(&hfield, IdTransform, &sphere, M_sphere, request, result); + + BOOST_CHECK(!result.isCollision()); + + CollisionResult result_check_sphere; + collide(&equivalent_box, IdTransform * box_placement, &sphere, M_sphere, + request, result_check_sphere); + + BOOST_CHECK(!result_check_sphere.isCollision()); + + CollisionResult result_check_box; + collide(&equivalent_box, IdTransform * box_placement, &box, M_box, request, + result_check_box); + + BOOST_CHECK(!result_check_box.isCollision()); + } +} + +BOOST_AUTO_TEST_CASE(negative_security_margin) { + const FCL_REAL max_altitude = 1., min_altitude = 0.; + + test_negative_security_margin(100, 100, min_altitude, max_altitude); + test_negative_security_margin(100, 100, min_altitude, max_altitude); +} + BOOST_AUTO_TEST_CASE(hfield_with_square_hole) { const Eigen::DenseIndex nx = 100, ny = 100; From af828ef42a646f9bade12d99f9b9e52361d1fd89 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 23 Jun 2022 21:37:21 +0000 Subject: [PATCH 34/52] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- include/hpp/fcl/hfield.h | 12 +- include/hpp/fcl/internal/shape_shape_func.h | 402 +++++++++--------- .../internal/traversal_node_hfield_shape.h | 155 +++---- src/BV/AABB.cpp | 19 +- src/BV/OBB.cpp | 20 +- src/BV/RSS.cpp | 4 +- src/collision_func_matrix.cpp | 171 ++++---- src/distance/convex_halfspace.cpp | 24 +- src/distance/sphere_sphere.cpp | 2 +- src/distance/triangle_halfspace.cpp | 24 +- src/narrowphase/details.h | 3 +- test/hfields.cpp | 14 +- test/security_margin.cpp | 78 ++-- 13 files changed, 493 insertions(+), 435 deletions(-) diff --git a/include/hpp/fcl/hfield.h b/include/hpp/fcl/hfield.h index 6e04e79db..8611faf14 100644 --- a/include/hpp/fcl/hfield.h +++ b/include/hpp/fcl/hfield.h @@ -60,11 +60,17 @@ struct HPP_FCL_DLLAPI HFNodeBase { Eigen::DenseIndex x_id, x_size; Eigen::DenseIndex y_id, y_size; - + FCL_REAL max_height; /// @brief Default constructor - HFNodeBase() : first_child(0), x_id(-1), x_size(0), y_id(-1), y_size(0), max_height(std::numeric_limits::lowest()) {} + HFNodeBase() + : first_child(0), + x_id(-1), + x_size(0), + y_id(-1), + y_size(0), + max_height(std::numeric_limits::lowest()) {} /// @brief Comparison operator bool operator==(const HFNodeBase& other) const { @@ -354,7 +360,7 @@ class HPP_FCL_DLLAPI HeightField : public CollisionGeometry { max_height = (std::max)(max_left_height, max_right_height); } - + bv_node.max_height = max_height; const Vec3f pointA(x_grid[bv_node.x_id], y_grid[bv_node.y_id], min_height); diff --git a/include/hpp/fcl/internal/shape_shape_func.h b/include/hpp/fcl/internal/shape_shape_func.h index 3ad225d14..de87eb6c2 100644 --- a/include/hpp/fcl/internal/shape_shape_func.h +++ b/include/hpp/fcl/internal/shape_shape_func.h @@ -57,26 +57,24 @@ HPP_FCL_DLLAPI FCL_REAL ShapeShapeDistance(const CollisionGeometry* o1, DistanceResult& result); template -struct ShapeShapeCollider -{ - static std::size_t run(const CollisionGeometry* o1, - const Transform3f& tf1, - const CollisionGeometry* o2, - const Transform3f& tf2, const GJKSolver* nsolver, +struct ShapeShapeCollider { + static std::size_t run(const CollisionGeometry* o1, const Transform3f& tf1, + const CollisionGeometry* o2, const Transform3f& tf2, + const GJKSolver* nsolver, const CollisionRequest& request, CollisionResult& result) { if (request.isSatisfied(result)) return result.numContacts(); - + DistanceResult distanceResult; DistanceRequest distanceRequest(request.enable_contact); FCL_REAL distance = ShapeShapeDistance( - o1, tf1, o2, tf2, nsolver, distanceRequest, distanceResult); - + o1, tf1, o2, tf2, nsolver, distanceRequest, distanceResult); + size_t num_contacts = 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 && @@ -84,298 +82,301 @@ struct ShapeShapeCollider 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, - (distance <= 0 ? distanceResult.normal : (p2 - p1).normalized()), - -distance); - + o1, o2, distanceResult.b1, distanceResult.b2, (p1 + p2) / 2, + (distance <= 0 ? distanceResult.normal : (p2 - p1).normalized()), + -distance); + result.addContact(contact); } num_contacts = result.numContacts(); } - + return num_contacts; } }; /// @cond DEV -namespace details -{ +namespace details { // Forward declaration -template::IsInflatable, bool shape1_has_inflated_support_function = shape_traits::HasInflatedSupportFunction, bool shape2_is_inflatable = shape_traits::IsInflatable, bool shape2_has_inflated_support_function = shape_traits::HasInflatedSupportFunction> +template ::IsInflatable, + bool shape1_has_inflated_support_function = + shape_traits::HasInflatedSupportFunction, + bool shape2_is_inflatable = shape_traits::IsInflatable, + bool shape2_has_inflated_support_function = + shape_traits::HasInflatedSupportFunction> struct shape_shape_collide_negative_security_margin; // Handle case where both shapes are inflatable -template -struct shape_shape_collide_negative_security_margin -{ - static std::size_t run(const ShapeType1 & o1, - const Transform3f& tf1, - const ShapeType2 & o2, - const Transform3f& tf2, +template +struct shape_shape_collide_negative_security_margin { + static std::size_t run(const ShapeType1& o1, const Transform3f& tf1, + const ShapeType2& o2, const Transform3f& tf2, const GJKSolver* nsolver, const CollisionRequest& request, - CollisionResult& result) - { + CollisionResult& result) { const FCL_REAL min_inflation1 = o1.minInflationValue(); const FCL_REAL min_inflation2 = o2.minInflationValue(); const FCL_REAL min_inflation = min_inflation1 + min_inflation2; const FCL_REAL security_margin = request.security_margin; - - if(security_margin < min_inflation) - HPP_FCL_THROW_PRETTY("The request security margin: " - << security_margin - << " is below the minimal security margin authorised by the pair of two shapes" - << "(" << std::string(get_node_type_name(o1.getNodeType())) - << "," << std::string(get_node_type_name(o2.getNodeType())) - << "): " - << min_inflation - << ".\n Please consider increasing the requested security margin.", - std::invalid_argument); - - if(security_margin > min_inflation1) - { - const auto & deflated_result = o1.inflated(security_margin); - const ShapeType1 & deflated_o1 = deflated_result.first; + + if (security_margin < min_inflation) + HPP_FCL_THROW_PRETTY( + "The request security margin: " + << security_margin + << " is below the minimal security margin authorised by the pair " + "of two shapes" + << "(" << std::string(get_node_type_name(o1.getNodeType())) << "," + << std::string(get_node_type_name(o2.getNodeType())) + << "): " << min_inflation + << ".\n Please consider increasing the requested security " + "margin.", + std::invalid_argument); + + if (security_margin > min_inflation1) { + const auto& deflated_result = o1.inflated(security_margin); + const ShapeType1& deflated_o1 = deflated_result.first; const Transform3f deflated_tf1 = tf1 * deflated_result.second; CollisionRequest deflated_request(request); deflated_request.security_margin = 0; - return ShapeShapeCollider::run(&deflated_o1, deflated_tf1, &o2, tf2, nsolver, deflated_request, result); - } - else if(security_margin > min_inflation2) - { - const auto & deflated_result = o2.inflated(security_margin); - const ShapeType2 & deflated_o2 = deflated_result.first; + return ShapeShapeCollider::run( + &deflated_o1, deflated_tf1, &o2, tf2, nsolver, deflated_request, + result); + } else if (security_margin > min_inflation2) { + const auto& deflated_result = o2.inflated(security_margin); + const ShapeType2& deflated_o2 = deflated_result.first; const Transform3f deflated_tf2 = tf2 * deflated_result.second; CollisionRequest deflated_request(request); deflated_request.security_margin = 0; - return ShapeShapeCollider::run(&o1, tf1, &deflated_o2, deflated_tf2, nsolver, deflated_request, result); - } - else // deflate both + return ShapeShapeCollider::run( + &o1, tf1, &deflated_o2, deflated_tf2, nsolver, deflated_request, + result); + } else // deflate both { const FCL_REAL half_security_margin = 0.5 * security_margin; - const auto & deflated_result1 = o1.inflated(half_security_margin); - const ShapeType1 & deflated_o1 = deflated_result1.first; + const auto& deflated_result1 = o1.inflated(half_security_margin); + const ShapeType1& deflated_o1 = deflated_result1.first; const Transform3f deflated_tf1 = tf1 * deflated_result1.second; - const auto & deflated_result2 = o2.inflated(half_security_margin); - const ShapeType2 & deflated_o2 = deflated_result2.first; + const auto& deflated_result2 = o2.inflated(half_security_margin); + const ShapeType2& deflated_o2 = deflated_result2.first; const Transform3f deflated_tf2 = tf2 * deflated_result2.second; CollisionRequest deflated_request(request); deflated_request.security_margin = 0; - return ShapeShapeCollider::run(&deflated_o1, deflated_tf1, &deflated_o2, deflated_tf2, nsolver, deflated_request, result); + return ShapeShapeCollider::run( + &deflated_o1, deflated_tf1, &deflated_o2, deflated_tf2, nsolver, + deflated_request, result); } } }; // Handle case where only the first shape is inflatable -template -struct shape_shape_collide_negative_security_margin -{ - static std::size_t run(const ShapeType1 & o1, - const Transform3f& tf1, - const ShapeType2 & o2, - const Transform3f& tf2, +template +struct shape_shape_collide_negative_security_margin { + static std::size_t run(const ShapeType1& o1, const Transform3f& tf1, + const ShapeType2& o2, const Transform3f& tf2, const GJKSolver* nsolver, const CollisionRequest& request, - CollisionResult& result) - { + CollisionResult& result) { const FCL_REAL min_inflation = o1.minInflationValue(); const FCL_REAL security_margin = request.security_margin; - - if(security_margin < min_inflation) - HPP_FCL_THROW_PRETTY("The request security margin: " - << security_margin - << " is below the minimal security margin authorised by the pair of two shapes" - << "(" << std::string(get_node_type_name(o1.getNodeType())) - << "," << std::string(get_node_type_name(o2.getNodeType())) - << "): " - << min_inflation - << ".\n Please consider increasing the requested security margin.", - std::invalid_argument); - - const auto & deflated_result = o1.inflated(security_margin); - const ShapeType1 & deflated_o1 = deflated_result.first; + + if (security_margin < min_inflation) + HPP_FCL_THROW_PRETTY( + "The request security margin: " + << security_margin + << " is below the minimal security margin authorised by the pair " + "of two shapes" + << "(" << std::string(get_node_type_name(o1.getNodeType())) << "," + << std::string(get_node_type_name(o2.getNodeType())) + << "): " << min_inflation + << ".\n Please consider increasing the requested security " + "margin.", + std::invalid_argument); + + const auto& deflated_result = o1.inflated(security_margin); + const ShapeType1& deflated_o1 = deflated_result.first; const Transform3f deflated_tf1 = tf1 * deflated_result.second; CollisionRequest deflated_request(request); deflated_request.security_margin = 0; - return ShapeShapeCollider::run(&deflated_o1, deflated_tf1, &o2, tf2, nsolver, deflated_request, result); + return ShapeShapeCollider::run( + &deflated_o1, deflated_tf1, &o2, tf2, nsolver, deflated_request, + result); } }; // Handle case where only the second shape is inflatable -template -struct shape_shape_collide_negative_security_margin -{ - static std::size_t run(const ShapeType1 & o1, - const Transform3f& tf1, - const ShapeType2 & o2, - const Transform3f& tf2, +template +struct shape_shape_collide_negative_security_margin { + static std::size_t run(const ShapeType1& o1, const Transform3f& tf1, + const ShapeType2& o2, const Transform3f& tf2, const GJKSolver* nsolver, const CollisionRequest& request, - CollisionResult& result) - { - const std::size_t res = shape_shape_collide_negative_security_margin::run(o2, tf2, o1, tf1, nsolver, request, result); + CollisionResult& result) { + const std::size_t res = shape_shape_collide_negative_security_margin< + ShapeType2, ShapeType1, true, false, false, false>::run(o2, tf2, o1, + tf1, nsolver, + request, + result); result.swapObjects(); return res; } }; -// Handle case where the first shape is inflatable and the second shape has an inflated support function -template -struct shape_shape_collide_negative_security_margin -{ - static std::size_t run(const ShapeType1 & o1, - const Transform3f& tf1, - const ShapeType2 & o2, - const Transform3f& tf2, +// Handle case where the first shape is inflatable and the second shape has an +// inflated support function +template +struct shape_shape_collide_negative_security_margin { + static std::size_t run(const ShapeType1& o1, const Transform3f& tf1, + const ShapeType2& o2, const Transform3f& tf2, const GJKSolver* nsolver, const CollisionRequest& request, - CollisionResult& result) - { + CollisionResult& result) { const FCL_REAL min_inflation = o1.minInflationValue(); const FCL_REAL security_margin = request.security_margin; - - if(min_inflation < security_margin) - { - const auto & deflated_result = o1.inflated(security_margin); - const ShapeType1 & deflated_o1 = deflated_result.first; + + if (min_inflation < security_margin) { + const auto& deflated_result = o1.inflated(security_margin); + const ShapeType1& deflated_o1 = deflated_result.first; const Transform3f deflated_tf1 = tf1 * deflated_result.second; CollisionRequest deflated_request(request); deflated_request.security_margin = 0; - return ShapeShapeCollider::run(&deflated_o1, deflated_tf1, &o2, tf2, nsolver, deflated_request, result); - } - else - { + return ShapeShapeCollider::run( + &deflated_o1, deflated_tf1, &o2, tf2, nsolver, deflated_request, + result); + } else { const FCL_REAL half_security_margin = 0.5 * security_margin; - const auto & deflated_result = o1.inflated(half_security_margin); - const ShapeType1 & deflated_o1 = deflated_result.first; + const auto& deflated_result = o1.inflated(half_security_margin); + const ShapeType1& deflated_o1 = deflated_result.first; const Transform3f deflated_tf1 = tf1 * deflated_result.second; CollisionRequest deflated_request(request); deflated_request.security_margin = 0; assert(nsolver != NULL && "The GJK solver is not defined."); nsolver->setShapeDeflation(0, half_security_margin); - - std::size_t res = ShapeShapeCollider::run(&deflated_o1, deflated_tf1, &o2, tf2, nsolver, deflated_request, result); - + + std::size_t res = ShapeShapeCollider::run( + &deflated_o1, deflated_tf1, &o2, tf2, nsolver, deflated_request, + result); + nsolver->resetShapeDeflation(); return res; } } }; -// Handle case where the second shape is inflatable and the first shape has an inflated support function -template -struct shape_shape_collide_negative_security_margin -{ - static std::size_t run(const ShapeType1 & o1, - const Transform3f& tf1, - const ShapeType2 & o2, - const Transform3f& tf2, +// Handle case where the second shape is inflatable and the first shape has an +// inflated support function +template +struct shape_shape_collide_negative_security_margin { + static std::size_t run(const ShapeType1& o1, const Transform3f& tf1, + const ShapeType2& o2, const Transform3f& tf2, const GJKSolver* nsolver, const CollisionRequest& request, - CollisionResult& result) - { - const std::size_t res = shape_shape_collide_negative_security_margin::run(o2, tf2, o1, tf1, nsolver, request, result); + CollisionResult& result) { + const std::size_t res = shape_shape_collide_negative_security_margin< + ShapeType2, ShapeType1, true, false, false, true>::run(o2, tf2, o1, tf1, + nsolver, request, + result); result.swapObjects(); return res; } }; // Handle case where the two shapes have inflated support function -template -struct shape_shape_collide_negative_security_margin -{ - static std::size_t run(const ShapeType1 & o1, - const Transform3f& tf1, - const ShapeType2 & o2, - const Transform3f& tf2, +template +struct shape_shape_collide_negative_security_margin { + static std::size_t run(const ShapeType1& o1, const Transform3f& tf1, + const ShapeType2& o2, const Transform3f& tf2, const GJKSolver* nsolver, const CollisionRequest& request, - CollisionResult& result) - { + CollisionResult& result) { const FCL_REAL security_margin = request.security_margin; const FCL_REAL half_security_margin = 0.5 * security_margin; CollisionRequest deflated_request(request); deflated_request.security_margin = 0; - + assert(nsolver != NULL && "The GJK solver is not defined."); nsolver->setShapeDeflation(half_security_margin, half_security_margin); - - std::size_t res = ShapeShapeCollider::run(&o1, tf1, &o2, tf2, nsolver, deflated_request, result); - + + std::size_t res = ShapeShapeCollider::run( + &o1, tf1, &o2, tf2, nsolver, deflated_request, result); + nsolver->resetShapeDeflation(); return res; } }; // Handle case where only the first shape has inflated support function -template -struct shape_shape_collide_negative_security_margin -{ - static std::size_t run(const ShapeType1 & o1, - const Transform3f& tf1, - const ShapeType2 & o2, - const Transform3f& tf2, +template +struct shape_shape_collide_negative_security_margin { + static std::size_t run(const ShapeType1& o1, const Transform3f& tf1, + const ShapeType2& o2, const Transform3f& tf2, const GJKSolver* nsolver, const CollisionRequest& request, - CollisionResult& result) - { + CollisionResult& result) { const FCL_REAL security_margin = request.security_margin; CollisionRequest deflated_request(request); deflated_request.security_margin = 0; - + assert(nsolver != NULL && "The GJK solver is not defined."); nsolver->setShapeDeflation(security_margin, 0); - - std::size_t res = ShapeShapeCollider::run(&o1, tf1, &o2, tf2, nsolver, deflated_request, result); - + + std::size_t res = ShapeShapeCollider::run( + &o1, tf1, &o2, tf2, nsolver, deflated_request, result); + nsolver->resetShapeDeflation(); return res; } }; // Handle case where only the second shape has inflated support function -template -struct shape_shape_collide_negative_security_margin -{ - static std::size_t run(const ShapeType1 & o1, - const Transform3f& tf1, - const ShapeType2 & o2, - const Transform3f& tf2, +template +struct shape_shape_collide_negative_security_margin { + static std::size_t run(const ShapeType1& o1, const Transform3f& tf1, + const ShapeType2& o2, const Transform3f& tf2, const GJKSolver* nsolver, const CollisionRequest& request, - CollisionResult& result) - { - const std::size_t res = shape_shape_collide_negative_security_margin::run(o2, tf2, o1, tf1, nsolver, request, result); + CollisionResult& result) { + const std::size_t res = shape_shape_collide_negative_security_margin< + ShapeType2, ShapeType1, false, true, false, false>::run(o2, tf2, o1, + tf1, nsolver, + request, + result); result.swapObjects(); return res; } }; // Handle case where the shapes info are not provided. -template -struct shape_shape_collide_negative_security_margin -{ - static std::size_t run(const ShapeType1 & o1, - const Transform3f& /*tf1*/, - const ShapeType2 & o2, - const Transform3f& /*tf2*/, +template +struct shape_shape_collide_negative_security_margin< + ShapeType1, ShapeType2, false, false, false, false> { + static std::size_t run(const ShapeType1& o1, const Transform3f& /*tf1*/, + const ShapeType2& o2, const Transform3f& /*tf2*/, const GJKSolver* /*nsolver*/, const CollisionRequest& /*request*/, - CollisionResult& /*result*/) - { - HPP_FCL_THROW_PRETTY("Negative security margin between node type " - << std::string(get_node_type_name(o1.getNodeType())) - << " and node type " - << std::string(get_node_type_name(o2.getNodeType())) - << " is not supported.", - std::invalid_argument); + CollisionResult& /*result*/) { + HPP_FCL_THROW_PRETTY( + "Negative security margin between node type " + << std::string(get_node_type_name(o1.getNodeType())) + << " and node type " + << std::string(get_node_type_name(o2.getNodeType())) + << " is not supported.", + std::invalid_argument); return 0; } }; -} // namespace details +} // namespace details /// \endcond @@ -386,18 +387,17 @@ HPP_FCL_DLLAPI std::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf2, const GJKSolver* nsolver, const CollisionRequest& request, - CollisionResult& result) -{ - if(request.security_margin < 0) - { - const ShapeType1 & shape1 = static_cast(*o1); - const ShapeType2 & shape2 = static_cast(*o2); - - return details::shape_shape_collide_negative_security_margin::run(shape1, tf1, shape2, tf2, nsolver, request, result); - } - else - { - return ShapeShapeCollider::run(o1, tf1, o2, tf2, nsolver, request, result); + CollisionResult& result) { + if (request.security_margin < 0) { + const ShapeType1& shape1 = static_cast(*o1); + const ShapeType2& shape2 = static_cast(*o2); + + return details::shape_shape_collide_negative_security_margin< + ShapeType1, ShapeType2>::run(shape1, tf1, shape2, tf2, nsolver, request, + result); + } else { + return ShapeShapeCollider::run( + o1, tf1, o2, tf2, nsolver, request, result); } } @@ -435,15 +435,17 @@ SHAPE_SHAPE_DISTANCE_SPECIALIZATION(TriangleP, Halfspace); #undef SHAPE_SHAPE_DISTANCE_SPECIALIZATION -#define SHAPE_SHAPE_COLLIDE_SPECIALIZATION(T1, T2) \ - template <> \ -struct ShapeShapeCollider { \ - static HPP_FCL_DLLAPI std::size_t run( \ - const CollisionGeometry* o1, const Transform3f& tf1, \ - const CollisionGeometry* o2, const Transform3f& tf2, \ - const GJKSolver* nsolver, const CollisionRequest& request, \ - CollisionResult& result); \ -} +#define SHAPE_SHAPE_COLLIDE_SPECIALIZATION(T1, T2) \ + template <> \ + struct ShapeShapeCollider { \ + static HPP_FCL_DLLAPI std::size_t run(const CollisionGeometry* o1, \ + const Transform3f& tf1, \ + const CollisionGeometry* o2, \ + const Transform3f& tf2, \ + const GJKSolver* nsolver, \ + const CollisionRequest& request, \ + CollisionResult& result); \ + } SHAPE_SHAPE_COLLIDE_SPECIALIZATION(Sphere, Sphere); diff --git a/include/hpp/fcl/internal/traversal_node_hfield_shape.h b/include/hpp/fcl/internal/traversal_node_hfield_shape.h index 0e4857992..077068ca7 100644 --- a/include/hpp/fcl/internal/traversal_node_hfield_shape.h +++ b/include/hpp/fcl/internal/traversal_node_hfield_shape.h @@ -101,8 +101,7 @@ Convex buildConvexQuadrilateral(const HFNode& node, template void buildConvexTriangles(const HFNode& node, const HeightField& model, - Convex& convex1, - Convex& convex2, + Convex& convex1, Convex& convex2, const FCL_REAL deflation) { const MatrixXf& heights = model.getHeights(); const VecXf& x_grid = model.getXGrid(); @@ -134,15 +133,16 @@ void buildConvexTriangles(const HFNode& node, const HeightField& model, pts[6] = Vec3f(x1, y1, cell(1, 1)); pts[7] = Vec3f(x1, y0, cell(0, 1)); - const Vec3f normal_upper_triangular_face = ((pts[7] - pts[4]).cross(pts[5] - pts[4])).normalized(); + const Vec3f normal_upper_triangular_face = + ((pts[7] - pts[4]).cross(pts[5] - pts[4])).normalized(); const FCL_REAL cos_alpha = normal_upper_triangular_face.dot(Vec3f::UnitZ()); pts[4][2] += deflation / cos_alpha; pts[5][2] += deflation / cos_alpha; pts[7][2] += deflation / cos_alpha; Triangle* triangles = new Triangle[8]; - triangles[0].set(0, 1, 3); // bottom - triangles[1].set(4, 5, 7); // top + triangles[0].set(0, 1, 3); // bottom + triangles[1].set(4, 5, 7); // top triangles[2].set(0, 1, 4); triangles[3].set(4, 1, 5); triangles[4].set(1, 7, 3); @@ -169,15 +169,16 @@ void buildConvexTriangles(const HFNode& node, const HeightField& model, pts[6] = Vec3f(x1, y1, cell(1, 1)); pts[7] = Vec3f(x1, y0, cell(0, 1)); - const Vec3f normal_upper_triangular_face = ((pts[7] - pts[5]).cross(pts[6] - pts[5])).normalized(); + const Vec3f normal_upper_triangular_face = + ((pts[7] - pts[5]).cross(pts[6] - pts[5])).normalized(); const FCL_REAL cos_alpha = normal_upper_triangular_face.dot(Vec3f::UnitZ()); pts[5][2] += deflation / cos_alpha; pts[6][2] += deflation / cos_alpha; pts[7][2] += deflation / cos_alpha; Triangle* triangles = new Triangle[8]; - triangles[0].set(3, 2, 1); // top - triangles[1].set(5, 6, 7); // bottom + triangles[0].set(3, 2, 1); // top + triangles[1].set(5, 6, 7); // bottom triangles[2].set(1, 2, 5); triangles[3].set(5, 2, 6); triangles[4].set(1, 3, 7); @@ -202,7 +203,7 @@ class HeightFieldShapeCollisionTraversalNode : public CollisionTraversalNodeBase { public: typedef CollisionTraversalNodeBase Base; - typedef Eigen::Array Array2d; + typedef Eigen::Array Array2d; enum { Options = _Options, @@ -249,15 +250,16 @@ class HeightFieldShapeCollisionTraversalNode bool disjoint; const FCL_REAL deflation = shape_inflation[0]; - if(deflation < 0) - { + if (deflation < 0) { typedef typename HeightField::Node Node; - const HeightField & hfield = *this->model1; - const Node & node = hfield.getBV(b1); + const HeightField& hfield = *this->model1; + const Node& node = hfield.getBV(b1); const VecXf& x_grid = hfield.getXGrid(); const VecXf& y_grid = hfield.getYGrid(); - const Vec3f pointA(x_grid[node.x_id], y_grid[node.y_id], hfield.getMinHeight()); - const Vec3f pointB(x_grid[node.x_id + node.x_size], y_grid[node.y_id + node.y_size], + const Vec3f pointA(x_grid[node.x_id], y_grid[node.y_id], + hfield.getMinHeight()); + const Vec3f pointB(x_grid[node.x_id + node.x_size], + y_grid[node.y_id + node.y_size], node.max_height - deflation); BV bv_deflated; @@ -265,20 +267,18 @@ class HeightFieldShapeCollisionTraversalNode if (RTIsIdentity) { assert(false && "must never happened"); - disjoint = !bv_deflated.overlap( - this->model2_bv, this->request, sqrDistLowerBound); + disjoint = !bv_deflated.overlap(this->model2_bv, this->request, + sqrDistLowerBound); } else { disjoint = !overlap(this->tf1.getRotation(), this->tf1.getTranslation(), - bv_deflated, this->model2_bv, - this->request, sqrDistLowerBound); + bv_deflated, this->model2_bv, this->request, + sqrDistLowerBound); } - } - else - { + } else { if (RTIsIdentity) { assert(false && "must never happened"); disjoint = !this->model1->getBV(b1).bv.overlap( - this->model2_bv, this->request, sqrDistLowerBound); + this->model2_bv, this->request, sqrDistLowerBound); } else { disjoint = !overlap(this->tf1.getRotation(), this->tf1.getTranslation(), this->model1->getBV(b1).bv, this->model2_bv, @@ -286,7 +286,6 @@ class HeightFieldShapeCollisionTraversalNode } } - if (disjoint) internal::updateDistanceLowerBoundFromBV(this->request, *this->result, sqrDistLowerBound); @@ -340,48 +339,53 @@ class HeightFieldShapeCollisionTraversalNode return false; } - template - bool shapeCollision(const Convex& convex1, - const Convex& convex2, const Transform3f& tf1, - const S& shape, const Transform3f& tf2, FCL_REAL& distance_lower_bound, - Vec3f& contact_point, Vec3f& normal) const { - const Transform3f Id; - Vec3f contact_point2, normal2; - FCL_REAL distance_lower_bound2; - bool collision1, collision2; - if (RTIsIdentity) - collision1 = nsolver->shapeIntersect(convex1, Id, shape, tf2, distance_lower_bound, - true, &contact_point, &normal); - else - collision1 = nsolver->shapeIntersect(convex1, tf1, shape, tf2, distance_lower_bound, - true, &contact_point, &normal); - - if (RTIsIdentity) - collision2 = nsolver->shapeIntersect(convex2, Id, shape, tf2, distance_lower_bound2, - true, &contact_point2, &normal2); - else - collision2 = nsolver->shapeIntersect(convex2, tf1, shape, tf2, distance_lower_bound2, - true, &contact_point2, &normal2); - - if (collision1 && collision2) { - if (distance_lower_bound > distance_lower_bound2) // switch values - { - distance_lower_bound = distance_lower_bound2; - contact_point = contact_point2; - normal = normal2; - } - return true; - } else if (collision1) { - return true; - } else if (collision2) { - distance_lower_bound = distance_lower_bound2; - contact_point = contact_point2; - normal = normal2; - return true; - } - - return false; + template + bool shapeCollision(const Convex& convex1, + const Convex& convex2, const Transform3f& tf1, + const S& shape, const Transform3f& tf2, + FCL_REAL& distance_lower_bound, Vec3f& contact_point, + Vec3f& normal) const { + const Transform3f Id; + Vec3f contact_point2, normal2; + FCL_REAL distance_lower_bound2; + bool collision1, collision2; + if (RTIsIdentity) + collision1 = + nsolver->shapeIntersect(convex1, Id, shape, tf2, distance_lower_bound, + true, &contact_point, &normal); + else + collision1 = nsolver->shapeIntersect(convex1, tf1, shape, tf2, + distance_lower_bound, true, + &contact_point, &normal); + + if (RTIsIdentity) + collision2 = nsolver->shapeIntersect(convex2, Id, shape, tf2, + distance_lower_bound2, true, + &contact_point2, &normal2); + else + collision2 = nsolver->shapeIntersect(convex2, tf1, shape, tf2, + distance_lower_bound2, true, + &contact_point2, &normal2); + + if (collision1 && collision2) { + if (distance_lower_bound > distance_lower_bound2) // switch values + { + distance_lower_bound = distance_lower_bound2; + contact_point = contact_point2; + normal = normal2; } + return true; + } else if (collision1) { + return true; + } else if (collision2) { + distance_lower_bound = distance_lower_bound2; + contact_point = contact_point2; + normal = normal2; + return true; + } + + return false; + } /// @brief Intersection testing between leaves (one Convex and one shape) void leafCollides(unsigned int b1, unsigned int /*b2*/, @@ -398,18 +402,20 @@ class HeightFieldShapeCollisionTraversalNode typedef Convex ConvexTriangle; ConvexTriangle convex1, convex2; - details::buildConvexTriangles(node, *this->model1, convex1, convex2, shape_inflation[0]); + details::buildConvexTriangles(node, *this->model1, convex1, convex2, + shape_inflation[0]); FCL_REAL distance; -// Vec3f contact_point, normal; + // Vec3f contact_point, normal; Vec3f c1, c2, normal; bool collision = - this->shapeDistance(convex1, convex2, this->tf1, *(this->model2), this->tf2, - distance, c1, c2, normal); + this->shapeDistance(convex1, convex2, this->tf1, *(this->model2), + this->tf2, distance, c1, c2, normal); -// this->shapeCollision(convex1, convex2, this->tf1, *(this->model2), this->tf2, -// distance, contact_point, normal); + // this->shapeCollision(convex1, convex2, this->tf1, *(this->model2), + // this->tf2, + // distance, contact_point, normal); FCL_REAL distToCollision = distance - this->request.security_margin; if (collision) { @@ -430,8 +436,8 @@ class HeightFieldShapeCollisionTraversalNode } else sqrDistLowerBound = distToCollision * distToCollision; -// const Vec3f c1 = contact_point - distance * 0.5 * normal; -// const Vec3f c2 = contact_point + distance * 0.5 * normal; + // const Vec3f c1 = contact_point - distance * 0.5 * normal; + // const Vec3f c2 = contact_point + distance * 0.5 * normal; internal::updateDistanceLowerBoundFromLeaf(this->request, *this->result, distToCollision, c1, c2); @@ -497,7 +503,8 @@ class HeightFieldShapeDistanceTraversalNode : public DistanceTraversalNodeBase { /// @brief BV culling test in one BVTT node FCL_REAL BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const { - return model1->getBV(b1).bv.distance(model2_bv); //TODO(jcarpent): tf1 is not taken into account here. + return model1->getBV(b1).bv.distance( + model2_bv); // TODO(jcarpent): tf1 is not taken into account here. } /// @brief Distance testing between leaves (one triangle and one shape) diff --git a/src/BV/AABB.cpp b/src/BV/AABB.cpp index 6a13c9e90..3fdc7036c 100644 --- a/src/BV/AABB.cpp +++ b/src/BV/AABB.cpp @@ -49,12 +49,23 @@ AABB::AABB() bool AABB::overlap(const AABB& other, const CollisionRequest& request, FCL_REAL& sqrDistLowerBound) const { - const FCL_REAL break_distance_squared = request.break_distance * request.break_distance; - - sqrDistLowerBound = (min_ - other.max_ - Vec3f::Constant(request.security_margin)).array().max(0).matrix().squaredNorm(); + const FCL_REAL break_distance_squared = + request.break_distance * request.break_distance; + + sqrDistLowerBound = + (min_ - other.max_ - Vec3f::Constant(request.security_margin)) + .array() + .max(0) + .matrix() + .squaredNorm(); if (sqrDistLowerBound > break_distance_squared) return false; - sqrDistLowerBound = (other.min_ - max_ - Vec3f::Constant(request.security_margin)).array().max(0).matrix().squaredNorm(); + sqrDistLowerBound = + (other.min_ - max_ - Vec3f::Constant(request.security_margin)) + .array() + .max(0) + .matrix() + .squaredNorm(); if (sqrDistLowerBound > break_distance_squared) return false; return true; diff --git a/src/BV/OBB.cpp b/src/BV/OBB.cpp index 73f9a1e9e..f56e51cba 100644 --- a/src/BV/OBB.cpp +++ b/src/BV/OBB.cpp @@ -346,16 +346,20 @@ bool obbDisjointAndLowerBoundDistance(const Matrix3f& B, const Vec3f& T, const Vec3f& a_, const Vec3f& b_, const CollisionRequest& request, FCL_REAL& squaredLowerBoundDistance) { - - assert(request.security_margin > -2*(std::min)(a_.minCoeff(),b_.minCoeff()) - 10*Eigen::NumTraits::epsilon() - && "A negative security margin could not be lower than the OBB extent."); -// const FCL_REAL breakDistance(request.break_distance + -// request.security_margin); - const FCL_REAL breakDistance2 = request.break_distance * request.break_distance; + assert(request.security_margin > + -2 * (std::min)(a_.minCoeff(), b_.minCoeff()) - + 10 * Eigen::NumTraits::epsilon() && + "A negative security margin could not be lower than the OBB extent."); + // const FCL_REAL breakDistance(request.break_distance + + // request.security_margin); + const FCL_REAL breakDistance2 = + request.break_distance * request.break_distance; Matrix3f Bf(B.cwiseAbs()); - const Vec3f a((a_ + Vec3f::Constant(request.security_margin/2)).array().max(0)); - const Vec3f b((b_ + Vec3f::Constant(request.security_margin/2)).array().max(0)); + const Vec3f a( + (a_ + Vec3f::Constant(request.security_margin / 2)).array().max(0)); + const Vec3f b( + (b_ + Vec3f::Constant(request.security_margin / 2)).array().max(0)); // Corner of b axis aligned bounding box the closest to the origin squaredLowerBoundDistance = internal::obbDisjoint_check_A_axis(T, a, b, Bf); diff --git a/src/BV/RSS.cpp b/src/BV/RSS.cpp index 07467e6e9..0144c591d 100644 --- a/src/BV/RSS.cpp +++ b/src/BV/RSS.cpp @@ -753,8 +753,8 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2, Vec3f T(b1.axes.transpose() * Ttemp); Matrix3f R(b1.axes.transpose() * R0 * b2.axes); - FCL_REAL dist = - rectDistance(R, T, b1.length, b2.length) - b1.radius - b2.radius - request.security_margin; + FCL_REAL dist = rectDistance(R, T, b1.length, b2.length) - b1.radius - + b2.radius - request.security_margin; if (dist <= 0) return true; sqrDistLowerBound = dist * dist; return false; diff --git a/src/collision_func_matrix.cpp b/src/collision_func_matrix.cpp index e8424f3ed..a225b384f 100644 --- a/src/collision_func_matrix.cpp +++ b/src/collision_func_matrix.cpp @@ -56,9 +56,11 @@ std::size_t OctreeCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionRequest& request, CollisionResult& result) { if (request.isSatisfied(result)) return result.numContacts(); - - if(request.security_margin < 0) - HPP_FCL_THROW_PRETTY("Negative security margin are not handled yet for Octree", std::invalid_argument); + + if (request.security_margin < 0) + HPP_FCL_THROW_PRETTY( + "Negative security margin are not handled yet for Octree", + std::invalid_argument); typename TraversalTraitsCollision::CollisionTraversal_t node( request); @@ -105,9 +107,11 @@ struct HPP_FCL_LOCAL BVHShapeCollider { const CollisionRequest& request, CollisionResult& result) { if (request.isSatisfied(result)) return result.numContacts(); - - if(request.security_margin < 0) - HPP_FCL_THROW_PRETTY("Negative security margin are not handled yet for BVHModel", std::invalid_argument); + + if (request.security_margin < 0) + HPP_FCL_THROW_PRETTY( + "Negative security margin are not handled yet for BVHModel", + std::invalid_argument); if (_Options & RelativeTransformationIsIdentity) return aligned(o1, tf1, o2, tf2, nsolver, request, result); @@ -157,108 +161,109 @@ struct HPP_FCL_LOCAL BVHShapeCollider { }; /// @cond DEV -namespace details -{ +namespace details { // Forward declaration -template::IsInflatable, bool shape_has_inflated_support_function = shape_traits::HasInflatedSupportFunction> +template ::IsInflatable, + bool shape_has_inflated_support_function = + shape_traits::HasInflatedSupportFunction> struct height_field_shape_collide_negative_security_margin; -template -struct height_field_shape_collide_negative_security_margin -{ +template +struct height_field_shape_collide_negative_security_margin { typedef HeightField HF; - - static std::size_t run(const HF & o1, - const Transform3f& tf1, - const ShapeType & o2, - const Transform3f& tf2, + + static std::size_t run(const HF& o1, const Transform3f& tf1, + const ShapeType& o2, const Transform3f& tf2, const GJKSolver* nsolver, const CollisionRequest& request, - CollisionResult& result) - { + CollisionResult& result) { const FCL_REAL min_deflation = o2.minInflationValue(); const FCL_REAL security_margin = request.security_margin; - -// if(security_margin < min_deflation) -// HPP_FCL_THROW_PRETTY("The request security margin: " -// << security_margin -// << " is below the minimal security margin authorised by the pair of two shapes" -// << "(" << std::string(get_node_type_name(o1.getNodeType())) -// << "," << std::string(get_node_type_name(o2.getNodeType())) -// << "): " -// << min_deflation -// << ".\n Please consider increasing the requested security margin.", -// std::invalid_argument); - - const FCL_REAL deflation_value2 = (std::max)(0.5*min_deflation,security_margin); - const auto & deflated_result = o2.inflated(deflation_value2); - const ShapeType & deflated_o2 = deflated_result.first; + + // if(security_margin < min_deflation) + // HPP_FCL_THROW_PRETTY("The request security margin: " + // << security_margin + // << " is below the minimal security margin + // authorised by the pair of two shapes" + // << "(" << + // std::string(get_node_type_name(o1.getNodeType())) + // << "," << + // std::string(get_node_type_name(o2.getNodeType())) + // << "): " + // << min_deflation + // << ".\n Please consider increasing the + // requested security margin.", + // std::invalid_argument); + + const FCL_REAL deflation_value2 = + (std::max)(0.5 * min_deflation, security_margin); + const auto& deflated_result = o2.inflated(deflation_value2); + const ShapeType& deflated_o2 = deflated_result.first; const Transform3f deflated_tf2 = tf2 * deflated_result.second; - + CollisionRequest deflated_request(request); - - deflated_request.security_margin = security_margin - deflation_value2; // We already account for the deflation of the shape. - - HeightFieldShapeCollisionTraversalNode node(deflated_request); + + deflated_request.security_margin = + security_margin - + deflation_value2; // We already account for the deflation of the shape. + + HeightFieldShapeCollisionTraversalNode node( + deflated_request); node.shape_inflation[0] = deflated_request.security_margin; - - nsolver->setShapeDeflation(0.,0.); - + + nsolver->setShapeDeflation(0., 0.); + initialize(node, o1, tf1, deflated_o2, deflated_tf2, nsolver, result); fcl::collide(&node, deflated_request, result); return result.numContacts(); } }; -template -struct height_field_shape_collide_negative_security_margin -{ +template +struct height_field_shape_collide_negative_security_margin { typedef HeightField HF; - - static std::size_t run(const HF & o1, - const Transform3f& tf1, - const ShapeType & o2, - const Transform3f& tf2, + + static std::size_t run(const HF& o1, const Transform3f& tf1, + const ShapeType& o2, const Transform3f& tf2, const GJKSolver* nsolver, const CollisionRequest& request, - CollisionResult& result) - { + CollisionResult& result) { HeightFieldShapeCollisionTraversalNode node(request); node.shape_inflation[0] = 0.5 * request.security_margin; - - nsolver->setShapeDeflation(0.,0.5 * request.security_margin); - + + nsolver->setShapeDeflation(0., 0.5 * request.security_margin); + initialize(node, o1, tf1, o2, tf2, nsolver, result); fcl::collide(&node, request, result); return result.numContacts(); } }; -template -struct height_field_shape_collide_negative_security_margin -{ +template +struct height_field_shape_collide_negative_security_margin { typedef HeightField HF; - - static std::size_t run(const HF & o1, - const Transform3f& /*tf1*/, - const ShapeType & o2, - const Transform3f& /*tf2*/, + + static std::size_t run(const HF& o1, const Transform3f& /*tf1*/, + const ShapeType& o2, const Transform3f& /*tf2*/, const GJKSolver* /*nsolver*/, const CollisionRequest& /*request*/, - CollisionResult& /*result*/) - { - HPP_FCL_THROW_PRETTY("Negative security margin between node type " - << std::string(get_node_type_name(o1.getNodeType())) - << " and node type " - << std::string(get_node_type_name(o2.getNodeType())) - << " is not supported.", - std::invalid_argument); + CollisionResult& /*result*/) { + HPP_FCL_THROW_PRETTY( + "Negative security margin between node type " + << std::string(get_node_type_name(o1.getNodeType())) + << " and node type " + << std::string(get_node_type_name(o2.getNodeType())) + << " is not supported.", + std::invalid_argument); return 0; } - }; -} +} // namespace details /// @endcond @@ -278,23 +283,21 @@ struct HPP_FCL_LOCAL HeightFieldShapeCollider { const CollisionRequest& request, CollisionResult& result) { if (request.isSatisfied(result)) return result.numContacts(); - - const HF & height_field = static_cast(*o1); - const Shape & shape = static_cast(*o2); - - if(request.security_margin < 0) - { - return details::height_field_shape_collide_negative_security_margin::run(height_field, tf1, shape, tf2, nsolver, request, result); - } - else - { + + const HF& height_field = static_cast(*o1); + const Shape& shape = static_cast(*o2); + + if (request.security_margin < 0) { + return details::height_field_shape_collide_negative_security_margin< + BV, Shape>::run(height_field, tf1, shape, tf2, nsolver, request, + result); + } else { HeightFieldShapeCollisionTraversalNode node(request); initialize(node, height_field, tf1, shape, tf2, nsolver, result); fcl::collide(&node, request, result); return result.numContacts(); } - } }; diff --git a/src/distance/convex_halfspace.cpp b/src/distance/convex_halfspace.cpp index 2276c26ff..01fd2c9c6 100644 --- a/src/distance/convex_halfspace.cpp +++ b/src/distance/convex_halfspace.cpp @@ -45,11 +45,13 @@ namespace fcl { template <> FCL_REAL ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver* nsolver, - const DistanceRequest&, DistanceResult& result) { - assert(nsolver->shape_deflation[0] <= 0 && "Shape deflation for the ConvexBase object should be negative"); - assert(nsolver->shape_deflation[1] == 0 && "Shape deflation for the HalfSpace object should be zero"); - + const CollisionGeometry* o2, const Transform3f& tf2, + const GJKSolver* nsolver, const DistanceRequest&, DistanceResult& result) { + assert(nsolver->shape_deflation[0] <= 0 && + "Shape deflation for the ConvexBase object should be negative"); + assert(nsolver->shape_deflation[1] == 0 && + "Shape deflation for the HalfSpace object should be zero"); + const ConvexBase& s1 = static_cast(*o1); const Halfspace& s2 = static_cast(*o2); details::halfspaceDistance(s2, tf2, s1, tf1, result.min_distance, @@ -66,11 +68,13 @@ FCL_REAL ShapeShapeDistance( template <> FCL_REAL ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver* nsolver, - const DistanceRequest&, DistanceResult& result) { - assert(nsolver->shape_deflation[0] == 0 && "Shape deflation for the HalfSpace object should be zero"); - assert(nsolver->shape_deflation[1] <= 0 && "Shape deflation for the ConvexBase object should be negative"); - + const CollisionGeometry* o2, const Transform3f& tf2, + const GJKSolver* nsolver, const DistanceRequest&, DistanceResult& result) { + assert(nsolver->shape_deflation[0] == 0 && + "Shape deflation for the HalfSpace object should be zero"); + assert(nsolver->shape_deflation[1] <= 0 && + "Shape deflation for the ConvexBase object should be negative"); + const Halfspace& s1 = static_cast(*o1); const ConvexBase& s2 = static_cast(*o2); details::halfspaceDistance(s1, tf1, s2, tf2, result.min_distance, diff --git a/src/distance/sphere_sphere.cpp b/src/distance/sphere_sphere.cpp index e4181d3db..13a2b960b 100644 --- a/src/distance/sphere_sphere.cpp +++ b/src/distance/sphere_sphere.cpp @@ -129,7 +129,7 @@ std::size_t ShapeShapeCollider::run( return 1; } return 0; - } +} } // namespace fcl } // namespace hpp diff --git a/src/distance/triangle_halfspace.cpp b/src/distance/triangle_halfspace.cpp index bb72ca7c9..c2d6e30f6 100644 --- a/src/distance/triangle_halfspace.cpp +++ b/src/distance/triangle_halfspace.cpp @@ -45,11 +45,13 @@ namespace fcl { template <> FCL_REAL ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver* nsolver, - const DistanceRequest&, DistanceResult& result) { -assert(nsolver->shape_deflation[0] == 0 && "Shape deflation for the TriangleP object should be zero"); -assert(nsolver->shape_deflation[1] == 0 && "Shape deflation for the HalfSpace object should be zero"); - + const CollisionGeometry* o2, const Transform3f& tf2, + const GJKSolver* nsolver, const DistanceRequest&, DistanceResult& result) { + assert(nsolver->shape_deflation[0] == 0 && + "Shape deflation for the TriangleP object should be zero"); + assert(nsolver->shape_deflation[1] == 0 && + "Shape deflation for the HalfSpace object should be zero"); + const TriangleP& s1 = static_cast(*o1); const Halfspace& s2 = static_cast(*o2); details::halfspaceDistance(s2, tf2, s1, tf1, result.min_distance, @@ -66,11 +68,13 @@ assert(nsolver->shape_deflation[1] == 0 && "Shape deflation for the HalfSpace ob template <> FCL_REAL ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver* nsolver, - const DistanceRequest&, DistanceResult& result) { - assert(nsolver->shape_deflation[0] == 0 && "Shape deflation for the HalfSpace object should be zero"); - assert(nsolver->shape_deflation[1] == 0 && "Shape deflation for the TriangleP object should be zero"); - + const CollisionGeometry* o2, const Transform3f& tf2, + const GJKSolver* nsolver, const DistanceRequest&, DistanceResult& result) { + assert(nsolver->shape_deflation[0] == 0 && + "Shape deflation for the HalfSpace object should be zero"); + assert(nsolver->shape_deflation[1] == 0 && + "Shape deflation for the TriangleP object should be zero"); + const Halfspace& s1 = static_cast(*o1); const TriangleP& s2 = static_cast(*o2); details::halfspaceDistance(s1, tf1, s2, tf2, result.min_distance, diff --git a/src/narrowphase/details.h b/src/narrowphase/details.h index 3bc4bb333..e34834a9f 100644 --- a/src/narrowphase/details.h +++ b/src/narrowphase/details.h @@ -1619,7 +1619,8 @@ inline bool convexHalfspaceIntersect( const ConvexBase& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) { - // No need to account for the shape_deflation in the case of Convex,Halfspace as Halfspace objects have an infinite value possible for deflation. + // No need to account for the shape_deflation in the case of Convex,Halfspace + // as Halfspace objects have an infinite value possible for deflation. Halfspace new_s2 = transform(s2, tf2); Vec3f v; diff --git a/test/hfields.cpp b/test/hfields.cpp index d41197f63..ced67c488 100644 --- a/test/hfields.cpp +++ b/test/hfields.cpp @@ -268,7 +268,7 @@ BOOST_AUTO_TEST_CASE(building_constant_hfields) { test_constant_hfields(100, 100, min_altitude, max_altitude); } -template +template void test_negative_security_margin(const Eigen::DenseIndex nx, const Eigen::DenseIndex ny, const FCL_REAL min_altitude, @@ -277,7 +277,7 @@ void test_negative_security_margin(const Eigen::DenseIndex nx, const MatrixXf heights = MatrixXf::Constant(ny, nx, max_altitude); HeightField hfield(x_dim, y_dim, heights, min_altitude); - + // Build equivalent object const Box equivalent_box(x_dim, y_dim, max_altitude - min_altitude); const Transform3f box_placement( @@ -290,7 +290,7 @@ void test_negative_security_margin(const Eigen::DenseIndex nx, const Box box(Vec3f::Ones()); Transform3f M_sphere, M_box; - + // No collision case { const FCL_REAL eps_no_collision = +0.1 * (max_altitude - min_altitude); @@ -317,7 +317,7 @@ void test_negative_security_margin(const Eigen::DenseIndex nx, BOOST_CHECK(!result_check_box.isCollision()); } - + // Collision case - positive security_margin { const FCL_REAL eps_no_collision = +0.1 * (max_altitude - min_altitude); @@ -345,7 +345,7 @@ void test_negative_security_margin(const Eigen::DenseIndex nx, BOOST_CHECK(result_check_box.isCollision()); } - + // Collision case { const FCL_REAL eps_no_collision = -0.1 * (max_altitude - min_altitude); @@ -372,7 +372,7 @@ void test_negative_security_margin(const Eigen::DenseIndex nx, BOOST_CHECK(result_check_box.isCollision()); } - + // No collision case - negative security_margin { const FCL_REAL eps_no_collision = -0.1 * (max_altitude - min_altitude); @@ -404,7 +404,7 @@ void test_negative_security_margin(const Eigen::DenseIndex nx, BOOST_AUTO_TEST_CASE(negative_security_margin) { const FCL_REAL max_altitude = 1., min_altitude = 0.; - + test_negative_security_margin(100, 100, min_altitude, max_altitude); test_negative_security_margin(100, 100, min_altitude, max_altitude); } diff --git a/test/security_margin.cpp b/test/security_margin.cpp index bb4e98863..47bfa2847 100644 --- a/test/security_margin.cpp +++ b/test/security_margin.cpp @@ -57,7 +57,7 @@ using hpp::fcl::DistanceResult; using hpp::fcl::Transform3f; using hpp::fcl::Vec3f; -#define MATH_SQUARED(x) (x*x) +#define MATH_SQUARED(x) (x * x) BOOST_AUTO_TEST_CASE(aabb_aabb) { CollisionGeometryPtr_t b1(new hpp::fcl::Box(1, 1, 1)); @@ -68,22 +68,23 @@ BOOST_AUTO_TEST_CASE(aabb_aabb) { hpp::fcl::Box s1(1, 1, 1); hpp::fcl::Box s2(1, 1, 1); const double tol = 1e-8; - + AABB bv1, bv2; computeBV(s1, Transform3f(), bv1); computeBV(s2, Transform3f(), bv2); - + // No security margin - collision { CollisionRequest collisionRequest(CONTACT, 1); AABB bv2_transformed; computeBV(s2, tf2_collision, bv2_transformed); FCL_REAL sqrDistLowerBound; - bool res = bv1.overlap(bv2_transformed, collisionRequest, sqrDistLowerBound); + bool res = + bv1.overlap(bv2_transformed, collisionRequest, sqrDistLowerBound); BOOST_CHECK(res); BOOST_CHECK_CLOSE(sqrDistLowerBound, 0, tol); } - + // No security margin - no collision { CollisionRequest collisionRequest(CONTACT, 1); @@ -93,11 +94,12 @@ BOOST_AUTO_TEST_CASE(aabb_aabb) { AABB bv2_transformed; computeBV(s2, tf2_no_collision, bv2_transformed); FCL_REAL sqrDistLowerBound; - bool res = bv1.overlap(bv2_transformed, collisionRequest, sqrDistLowerBound); + bool res = + bv1.overlap(bv2_transformed, collisionRequest, sqrDistLowerBound); BOOST_CHECK(!res); BOOST_CHECK_CLOSE(sqrDistLowerBound, MATH_SQUARED(distance), tol); } - + // Security margin - collision { CollisionRequest collisionRequest(CONTACT, 1); @@ -108,11 +110,12 @@ BOOST_AUTO_TEST_CASE(aabb_aabb) { AABB bv2_transformed; computeBV(s2, tf2_no_collision, bv2_transformed); FCL_REAL sqrDistLowerBound; - bool res = bv1.overlap(bv2_transformed, collisionRequest, sqrDistLowerBound); + bool res = + bv1.overlap(bv2_transformed, collisionRequest, sqrDistLowerBound); BOOST_CHECK(res); BOOST_CHECK_SMALL(sqrDistLowerBound, tol); } - + // Negative security margin - collion because the two boxes are in contact { CollisionRequest collisionRequest(CONTACT, 1); @@ -123,11 +126,12 @@ BOOST_AUTO_TEST_CASE(aabb_aabb) { AABB bv2_transformed; computeBV(s2, tf2, bv2_transformed); FCL_REAL sqrDistLowerBound; - bool res = bv1.overlap(bv2_transformed, collisionRequest, sqrDistLowerBound); + bool res = + bv1.overlap(bv2_transformed, collisionRequest, sqrDistLowerBound); BOOST_CHECK(res); BOOST_CHECK_SMALL(sqrDistLowerBound, tol); } - + // Negative security margin - no collision { CollisionRequest collisionRequest(CONTACT, 1); @@ -136,9 +140,12 @@ BOOST_AUTO_TEST_CASE(aabb_aabb) { AABB bv2_transformed; computeBV(s2, tf2_collision, bv2_transformed); FCL_REAL sqrDistLowerBound; - bool res = bv1.overlap(bv2_transformed, collisionRequest, sqrDistLowerBound); + bool res = + bv1.overlap(bv2_transformed, collisionRequest, sqrDistLowerBound); BOOST_CHECK(!res); - BOOST_CHECK_CLOSE(sqrDistLowerBound, MATH_SQUARED((std::sqrt(2)*collisionRequest.security_margin)), tol); + BOOST_CHECK_CLOSE( + sqrDistLowerBound, + MATH_SQUARED((std::sqrt(2) * collisionRequest.security_margin)), tol); } } @@ -151,11 +158,11 @@ BOOST_AUTO_TEST_CASE(aabb_aabb_degenerated_cases) { hpp::fcl::Box s1(1, 1, 1); hpp::fcl::Box s2(1, 1, 1); const double tol = 1e-8; - + AABB bv1, bv2; computeBV(s1, Transform3f(), bv1); computeBV(s2, Transform3f(), bv2); - + // The two AABB are collocated { CollisionRequest collisionRequest(CONTACT, 1); @@ -164,13 +171,13 @@ BOOST_AUTO_TEST_CASE(aabb_aabb_degenerated_cases) { AABB bv2_transformed; computeBV(s2, tf2_collision, bv2_transformed); FCL_REAL sqrDistLowerBound; - bool res = bv1.overlap(bv2_transformed, collisionRequest, sqrDistLowerBound); + bool res = + bv1.overlap(bv2_transformed, collisionRequest, sqrDistLowerBound); BOOST_CHECK(!res); } - + const AABB bv3; BOOST_CHECK(!bv1.overlap(bv3)); - } BOOST_AUTO_TEST_CASE(sphere_sphere) { @@ -242,7 +249,8 @@ BOOST_AUTO_TEST_CASE(sphere_sphere) { collide(s1.get(), tf1, s2.get(), tf2_collision, collisionRequest, collisionResult); BOOST_CHECK(!collisionResult.isCollision()); - BOOST_CHECK_CLOSE(collisionResult.distance_lower_bound, -collisionRequest.security_margin, 1e-8); + BOOST_CHECK_CLOSE(collisionResult.distance_lower_bound, + -collisionRequest.security_margin, 1e-8); } } @@ -375,8 +383,11 @@ BOOST_AUTO_TEST_CASE(box_box) { collide(b1.get(), tf1, b2.get(), tf2_collision, collisionRequest, collisionResult); BOOST_CHECK(!collisionResult.isCollision()); - BOOST_CHECK_CLOSE(collisionResult.distance_lower_bound, - (collisionRequest.security_margin * tf2_collision.getTranslation()).norm(), tol); + BOOST_CHECK_CLOSE( + collisionResult.distance_lower_bound, + (collisionRequest.security_margin * tf2_collision.getTranslation()) + .norm(), + tol); } // Negative security margin - collision @@ -386,7 +397,8 @@ BOOST_AUTO_TEST_CASE(box_box) { CollisionResult collisionResult; const Transform3f tf2((tf2_collision.getTranslation() + - Vec3f(0, collisionRequest.security_margin, collisionRequest.security_margin)) + Vec3f(0, collisionRequest.security_margin, + collisionRequest.security_margin)) .eval()); collide(b1.get(), tf1, b2.get(), tf2, collisionRequest, collisionResult); BOOST_CHECK(collisionResult.isCollision()); @@ -395,9 +407,10 @@ BOOST_AUTO_TEST_CASE(box_box) { } } -template -void test_shape_shape(const ShapeType1 & shape1, const Transform3f & tf1, const ShapeType2 & shape2, const Transform3f & tf2_collision, const FCL_REAL tol) -{ +template +void test_shape_shape(const ShapeType1& shape1, const Transform3f& tf1, + const ShapeType2& shape2, + const Transform3f& tf2_collision, const FCL_REAL tol) { // No security margin - collision { CollisionRequest collisionRequest(CONTACT, 1); @@ -445,8 +458,11 @@ void test_shape_shape(const ShapeType1 & shape1, const Transform3f & tf1, const collide(&shape1, tf1, &shape2, tf2_collision, collisionRequest, collisionResult); BOOST_CHECK(!collisionResult.isCollision()); - BOOST_CHECK_CLOSE(collisionResult.distance_lower_bound, - (collisionRequest.security_margin * tf2_collision.getTranslation()).norm(), tol); + BOOST_CHECK_CLOSE( + collisionResult.distance_lower_bound, + (collisionRequest.security_margin * tf2_collision.getTranslation()) + .norm(), + tol); } // Negative security margin - collision @@ -456,7 +472,8 @@ void test_shape_shape(const ShapeType1 & shape1, const Transform3f & tf1, const CollisionResult collisionResult; const Transform3f tf2((tf2_collision.getTranslation() + - Vec3f(0, collisionRequest.security_margin, collisionRequest.security_margin)) + Vec3f(0, collisionRequest.security_margin, + collisionRequest.security_margin)) .eval()); collide(&shape1, tf1, &shape2, tf2, collisionRequest, collisionResult); BOOST_CHECK(collisionResult.isCollision()); @@ -471,15 +488,14 @@ BOOST_AUTO_TEST_CASE(sphere_box) { BVHModel box_bvh_model = BVHModel(); generateBVHModel(box_bvh_model, *box_ptr, Transform3f()); box_bvh_model.buildConvexRepresentation(false); - ConvexBase & box_convex = *box_bvh_model.convex.get(); + ConvexBase& box_convex = *box_bvh_model.convex.get(); CollisionGeometryPtr_t s2(new hpp::fcl::Sphere(0.5)); const Transform3f tf1; const Transform3f tf2_collision(Vec3f(0, 0, 1)); const double tol = 1e-6; - + test_shape_shape(*b1.get(), tf1, *s2.get(), tf2_collision, tol); test_shape_shape(box_convex, tf1, *s2.get(), tf2_collision, tol); - } From bf7c11513e9f63ed242391eb275422e8add32181 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Fri, 24 Jun 2022 00:19:28 +0200 Subject: [PATCH 35/52] core: remove useless HPP_FCL_DLLAPI was causing the compilation errors `definition of dllimport function not allowed template` --- include/hpp/fcl/internal/shape_shape_func.h | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/include/hpp/fcl/internal/shape_shape_func.h b/include/hpp/fcl/internal/shape_shape_func.h index de87eb6c2..b96c5c706 100644 --- a/include/hpp/fcl/internal/shape_shape_func.h +++ b/include/hpp/fcl/internal/shape_shape_func.h @@ -381,13 +381,13 @@ struct shape_shape_collide_negative_security_margin< /// \endcond template -HPP_FCL_DLLAPI std::size_t ShapeShapeCollide(const CollisionGeometry* o1, - const Transform3f& tf1, - const CollisionGeometry* o2, - const Transform3f& tf2, - const GJKSolver* nsolver, - const CollisionRequest& request, - CollisionResult& result) { +std::size_t ShapeShapeCollide(const CollisionGeometry* o1, + const Transform3f& tf1, + const CollisionGeometry* o2, + const Transform3f& tf2, + const GJKSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result) { if (request.security_margin < 0) { const ShapeType1& shape1 = static_cast(*o1); const ShapeType2& shape2 = static_cast(*o2); From b581294cdb530132c154567a4f784d24cc335a54 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 23 Jun 2022 22:19:41 +0000 Subject: [PATCH 36/52] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- include/hpp/fcl/internal/shape_shape_func.h | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/include/hpp/fcl/internal/shape_shape_func.h b/include/hpp/fcl/internal/shape_shape_func.h index b96c5c706..2ebb1098e 100644 --- a/include/hpp/fcl/internal/shape_shape_func.h +++ b/include/hpp/fcl/internal/shape_shape_func.h @@ -384,8 +384,7 @@ template std::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, - const Transform3f& tf2, - const GJKSolver* nsolver, + const Transform3f& tf2, const GJKSolver* nsolver, const CollisionRequest& request, CollisionResult& result) { if (request.security_margin < 0) { From 9dc35fc988975305c8639ce72f9b7fd439061a7f Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Fri, 24 Jun 2022 01:19:50 +0200 Subject: [PATCH 37/52] hfield: handle isotropic penetration --- .../internal/traversal_node_hfield_shape.h | 41 +++++++------------ 1 file changed, 15 insertions(+), 26 deletions(-) diff --git a/include/hpp/fcl/internal/traversal_node_hfield_shape.h b/include/hpp/fcl/internal/traversal_node_hfield_shape.h index 077068ca7..409d5ea92 100644 --- a/include/hpp/fcl/internal/traversal_node_hfield_shape.h +++ b/include/hpp/fcl/internal/traversal_node_hfield_shape.h @@ -107,15 +107,17 @@ void buildConvexTriangles(const HFNode& node, const HeightField& model, const VecXf& x_grid = model.getXGrid(); const VecXf& y_grid = model.getYGrid(); - const FCL_REAL min_height = model.getMinHeight(); + assert(deflation <= 0 && "deflation should be negative"); - const FCL_REAL x0 = x_grid[node.x_id], x1 = x_grid[node.x_id + 1], - y0 = y_grid[node.y_id], y1 = y_grid[node.y_id + 1]; + const FCL_REAL min_height = model.getMinHeight() - deflation; + + const FCL_REAL x0 = x_grid[node.x_id] - deflation, + x1 = x_grid[node.x_id + 1] + deflation, + y0 = y_grid[node.y_id] - deflation, + y1 = y_grid[node.y_id + 1] + deflation; + const FCL_REAL max_height = node.max_height + deflation; const Eigen::Block cell = heights.block<2, 2>(node.y_id, node.x_id); - const FCL_REAL max_height = cell.maxCoeff(); - - assert(deflation <= 0 && "deflation should be negative"); assert(max_height > min_height && "max_height is lower than min_height"); // Check whether the geometry @@ -133,13 +135,6 @@ void buildConvexTriangles(const HFNode& node, const HeightField& model, pts[6] = Vec3f(x1, y1, cell(1, 1)); pts[7] = Vec3f(x1, y0, cell(0, 1)); - const Vec3f normal_upper_triangular_face = - ((pts[7] - pts[4]).cross(pts[5] - pts[4])).normalized(); - const FCL_REAL cos_alpha = normal_upper_triangular_face.dot(Vec3f::UnitZ()); - pts[4][2] += deflation / cos_alpha; - pts[5][2] += deflation / cos_alpha; - pts[7][2] += deflation / cos_alpha; - Triangle* triangles = new Triangle[8]; triangles[0].set(0, 1, 3); // bottom triangles[1].set(4, 5, 7); // top @@ -160,14 +155,7 @@ void buildConvexTriangles(const HFNode& node, const HeightField& model, { Vec3f* pts = new Vec3f[8]; - pts[0] = Vec3f(x0, y0, min_height); - pts[1] = Vec3f(x0, y1, min_height); - pts[2] = Vec3f(x1, y1, min_height); - pts[3] = Vec3f(x1, y0, min_height); - pts[4] = Vec3f(x0, y0, cell(0, 0)); - pts[5] = Vec3f(x0, y1, cell(1, 0)); - pts[6] = Vec3f(x1, y1, cell(1, 1)); - pts[7] = Vec3f(x1, y0, cell(0, 1)); + memcpy(pts, convex1.points, 8 * sizeof(Vec3f)); const Vec3f normal_upper_triangular_face = ((pts[7] - pts[5]).cross(pts[6] - pts[5])).normalized(); @@ -256,11 +244,12 @@ class HeightFieldShapeCollisionTraversalNode const Node& node = hfield.getBV(b1); const VecXf& x_grid = hfield.getXGrid(); const VecXf& y_grid = hfield.getYGrid(); - const Vec3f pointA(x_grid[node.x_id], y_grid[node.y_id], - hfield.getMinHeight()); - const Vec3f pointB(x_grid[node.x_id + node.x_size], - y_grid[node.y_id + node.y_size], - node.max_height - deflation); + const Vec3f pointA(x_grid[node.x_id] - deflation, + y_grid[node.y_id] - deflation, + hfield.getMinHeight() - deflation); + const Vec3f pointB(x_grid[node.x_id + node.x_size] + deflation, + y_grid[node.y_id + node.y_size] + deflation, + node.max_height + deflation); BV bv_deflated; details::UpdateBoundingVolume::run(pointA, pointB, bv_deflated); From 123c5162c72f969b45d60c1498201e143dfe61da Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Fri, 24 Jun 2022 01:21:41 +0200 Subject: [PATCH 38/52] hfields: fix request handling --- src/collision_func_matrix.cpp | 35 ++++++++++++----------------------- 1 file changed, 12 insertions(+), 23 deletions(-) diff --git a/src/collision_func_matrix.cpp b/src/collision_func_matrix.cpp index a225b384f..63d6e7ff0 100644 --- a/src/collision_func_matrix.cpp +++ b/src/collision_func_matrix.cpp @@ -183,36 +183,20 @@ struct height_field_shape_collide_negative_security_margin node( deflated_request); - node.shape_inflation[0] = deflated_request.security_margin; + node.shape_inflation[0] = 0; nsolver->setShapeDeflation(0., 0.); @@ -232,13 +216,18 @@ struct height_field_shape_collide_negative_security_margin node(request); + CollisionRequest deflated_request(request); + deflated_request.security_margin = 0; + + HeightFieldShapeCollisionTraversalNode node( + deflated_request); + // The deflation is splitted between the two objects. node.shape_inflation[0] = 0.5 * request.security_margin; nsolver->setShapeDeflation(0., 0.5 * request.security_margin); initialize(node, o1, tf1, o2, tf2, nsolver, result); - fcl::collide(&node, request, result); + fcl::collide(&node, deflated_request, result); return result.numContacts(); } }; From ec1333efca83607b8880178ad99a1cd1663a691f Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Fri, 24 Jun 2022 01:36:56 +0200 Subject: [PATCH 39/52] core: update throw message --- include/hpp/fcl/shape/geometric_shapes.h | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) diff --git a/include/hpp/fcl/shape/geometric_shapes.h b/include/hpp/fcl/shape/geometric_shapes.h index d30d96799..5dd9a57ac 100644 --- a/include/hpp/fcl/shape/geometric_shapes.h +++ b/include/hpp/fcl/shape/geometric_shapes.h @@ -230,7 +230,8 @@ class HPP_FCL_DLLAPI Sphere : public ShapeBase { std::pair inflated(const FCL_REAL value) const { if (value <= minInflationValue()) HPP_FCL_THROW_PRETTY( - "value is two small. It should be at least: " << minInflationValue(), + "value (" << value << ") is two small. It should be at least: " + << minInflationValue(), std::invalid_argument); return std::make_pair(Sphere(radius + value), Transform3f()); } @@ -297,7 +298,8 @@ class HPP_FCL_DLLAPI Ellipsoid : public ShapeBase { std::pair inflated(const FCL_REAL value) const { if (value <= minInflationValue()) HPP_FCL_THROW_PRETTY( - "value is two small. It should be at least: " << minInflationValue(), + "value (" << value << ") is two small. It should be at least: " + << minInflationValue(), std::invalid_argument); return std::make_pair(Ellipsoid(radii + Vec3f::Constant(value)), Transform3f()); @@ -375,7 +377,8 @@ class HPP_FCL_DLLAPI Capsule : public ShapeBase { std::pair inflated(const FCL_REAL value) const { if (value <= minInflationValue()) HPP_FCL_THROW_PRETTY( - "value is two small. It should be at least: " << minInflationValue(), + "value (" << value << ") is two small. It should be at least: " + << minInflationValue(), std::invalid_argument); return std::make_pair(Capsule(radius + value, 2 * halfLength), Transform3f()); @@ -448,7 +451,8 @@ class HPP_FCL_DLLAPI Cone : public ShapeBase { std::pair inflated(const FCL_REAL value) const { if (value <= minInflationValue()) HPP_FCL_THROW_PRETTY( - "value is two small. It should be at least: " << minInflationValue(), + "value (" << value << ") is two small. It should be at least: " + << minInflationValue(), std::invalid_argument); // tan(alpha) = 2*halfLength/radius; @@ -535,7 +539,8 @@ class HPP_FCL_DLLAPI Cylinder : public ShapeBase { std::pair inflated(const FCL_REAL value) const { if (value <= minInflationValue()) HPP_FCL_THROW_PRETTY( - "value is two small. It should be at least: " << minInflationValue(), + "value (" << value << ") is two small. It should be at least: " + << minInflationValue(), std::invalid_argument); return std::make_pair(Cylinder(radius + value, 2 * (halfLength + value)), Transform3f()); @@ -755,7 +760,8 @@ class HPP_FCL_DLLAPI Halfspace : public ShapeBase { std::pair inflated(const FCL_REAL value) const { if (value <= minInflationValue()) HPP_FCL_THROW_PRETTY( - "value is two small. It should be at least: " << minInflationValue(), + "value (" << value << ") is two small. It should be at least: " + << minInflationValue(), std::invalid_argument); return std::make_pair(Halfspace(n, d + value), Transform3f()); } From 84b075474023c61df246f460a29cd06246839495 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Fri, 24 Jun 2022 01:37:56 +0200 Subject: [PATCH 40/52] hfields: fix bug --- src/collision_func_matrix.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/collision_func_matrix.cpp b/src/collision_func_matrix.cpp index 63d6e7ff0..c41d4ad9c 100644 --- a/src/collision_func_matrix.cpp +++ b/src/collision_func_matrix.cpp @@ -185,7 +185,7 @@ struct height_field_shape_collide_negative_security_margin Date: Fri, 24 Jun 2022 13:33:21 +0200 Subject: [PATCH 41/52] core: fix handling of penetrations --- include/hpp/fcl/internal/shape_shape_func.h | 13 +-- .../internal/traversal_node_hfield_shape.h | 83 +++++-------------- src/collision_func_matrix.cpp | 16 ++-- test/security_margin.cpp | 25 +++--- 4 files changed, 42 insertions(+), 95 deletions(-) diff --git a/include/hpp/fcl/internal/shape_shape_func.h b/include/hpp/fcl/internal/shape_shape_func.h index 2ebb1098e..036b29beb 100644 --- a/include/hpp/fcl/internal/shape_shape_func.h +++ b/include/hpp/fcl/internal/shape_shape_func.h @@ -387,17 +387,8 @@ std::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf2, const GJKSolver* nsolver, const CollisionRequest& request, CollisionResult& result) { - if (request.security_margin < 0) { - const ShapeType1& shape1 = static_cast(*o1); - const ShapeType2& shape2 = static_cast(*o2); - - return details::shape_shape_collide_negative_security_margin< - ShapeType1, ShapeType2>::run(shape1, tf1, shape2, tf2, nsolver, request, - result); - } else { - return ShapeShapeCollider::run( - o1, tf1, o2, tf2, nsolver, request, result); - } + return ShapeShapeCollider::run( + o1, tf1, o2, tf2, nsolver, request, result); } #define SHAPE_SHAPE_DISTANCE_SPECIALIZATION(T1, T2) \ diff --git a/include/hpp/fcl/internal/traversal_node_hfield_shape.h b/include/hpp/fcl/internal/traversal_node_hfield_shape.h index 409d5ea92..8d90863bb 100644 --- a/include/hpp/fcl/internal/traversal_node_hfield_shape.h +++ b/include/hpp/fcl/internal/traversal_node_hfield_shape.h @@ -101,21 +101,17 @@ Convex buildConvexQuadrilateral(const HFNode& node, template void buildConvexTriangles(const HFNode& node, const HeightField& model, - Convex& convex1, Convex& convex2, - const FCL_REAL deflation) { + Convex& convex1, + Convex& convex2) { const MatrixXf& heights = model.getHeights(); const VecXf& x_grid = model.getXGrid(); const VecXf& y_grid = model.getYGrid(); - assert(deflation <= 0 && "deflation should be negative"); - - const FCL_REAL min_height = model.getMinHeight() - deflation; + const FCL_REAL min_height = model.getMinHeight(); - const FCL_REAL x0 = x_grid[node.x_id] - deflation, - x1 = x_grid[node.x_id + 1] + deflation, - y0 = y_grid[node.y_id] - deflation, - y1 = y_grid[node.y_id + 1] + deflation; - const FCL_REAL max_height = node.max_height + deflation; + const FCL_REAL x0 = x_grid[node.x_id], x1 = x_grid[node.x_id + 1], + y0 = y_grid[node.y_id], y1 = y_grid[node.y_id + 1]; + const FCL_REAL max_height = node.max_height; const Eigen::Block cell = heights.block<2, 2>(node.y_id, node.x_id); @@ -157,13 +153,6 @@ void buildConvexTriangles(const HFNode& node, const HeightField& model, Vec3f* pts = new Vec3f[8]; memcpy(pts, convex1.points, 8 * sizeof(Vec3f)); - const Vec3f normal_upper_triangular_face = - ((pts[7] - pts[5]).cross(pts[6] - pts[5])).normalized(); - const FCL_REAL cos_alpha = normal_upper_triangular_face.dot(Vec3f::UnitZ()); - pts[5][2] += deflation / cos_alpha; - pts[6][2] += deflation / cos_alpha; - pts[7][2] += deflation / cos_alpha; - Triangle* triangles = new Triangle[8]; triangles[0].set(3, 2, 1); // top triangles[1].set(5, 6, 7); // bottom @@ -236,43 +225,14 @@ class HeightFieldShapeCollisionTraversalNode if (this->enable_statistics) this->num_bv_tests++; bool disjoint; - const FCL_REAL deflation = shape_inflation[0]; - - if (deflation < 0) { - typedef typename HeightField::Node Node; - const HeightField& hfield = *this->model1; - const Node& node = hfield.getBV(b1); - const VecXf& x_grid = hfield.getXGrid(); - const VecXf& y_grid = hfield.getYGrid(); - const Vec3f pointA(x_grid[node.x_id] - deflation, - y_grid[node.y_id] - deflation, - hfield.getMinHeight() - deflation); - const Vec3f pointB(x_grid[node.x_id + node.x_size] + deflation, - y_grid[node.y_id + node.y_size] + deflation, - node.max_height + deflation); - - BV bv_deflated; - details::UpdateBoundingVolume::run(pointA, pointB, bv_deflated); - - if (RTIsIdentity) { - assert(false && "must never happened"); - disjoint = !bv_deflated.overlap(this->model2_bv, this->request, - sqrDistLowerBound); - } else { - disjoint = !overlap(this->tf1.getRotation(), this->tf1.getTranslation(), - bv_deflated, this->model2_bv, this->request, - sqrDistLowerBound); - } + if (RTIsIdentity) { + assert(false && "must never happened"); + disjoint = !this->model1->getBV(b1).bv.overlap( + this->model2_bv, this->request, sqrDistLowerBound); } else { - if (RTIsIdentity) { - assert(false && "must never happened"); - disjoint = !this->model1->getBV(b1).bv.overlap( - this->model2_bv, this->request, sqrDistLowerBound); - } else { - disjoint = !overlap(this->tf1.getRotation(), this->tf1.getTranslation(), - this->model1->getBV(b1).bv, this->model2_bv, - this->request, sqrDistLowerBound); - } + disjoint = !overlap(this->tf1.getRotation(), this->tf1.getTranslation(), + this->model1->getBV(b1).bv, this->model2_bv, + this->request, sqrDistLowerBound); } if (disjoint) @@ -391,8 +351,7 @@ class HeightFieldShapeCollisionTraversalNode typedef Convex ConvexTriangle; ConvexTriangle convex1, convex2; - details::buildConvexTriangles(node, *this->model1, convex1, convex2, - shape_inflation[0]); + details::buildConvexTriangles(node, *this->model1, convex1, convex2); FCL_REAL distance; // Vec3f contact_point, normal; @@ -407,20 +366,20 @@ class HeightFieldShapeCollisionTraversalNode // distance, contact_point, normal); FCL_REAL distToCollision = distance - this->request.security_margin; - if (collision) { + 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, c1, normal, - -distance)); - assert(this->result->isCollision()); + (int)Contact::NONE, .5 * (c1 + c2), + (c2 - c1).normalized(), -distance)); } - } else if (distToCollision <= this->request.collision_distance_threshold) { + } else if (collision && this->request.security_margin >= 0) { 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)); + (int)Contact::NONE, c1, normal, + -distance)); + assert(this->result->isCollision()); } } else sqrDistLowerBound = distToCollision * distToCollision; diff --git a/src/collision_func_matrix.cpp b/src/collision_func_matrix.cpp index c41d4ad9c..ab3c710a4 100644 --- a/src/collision_func_matrix.cpp +++ b/src/collision_func_matrix.cpp @@ -276,17 +276,11 @@ struct HPP_FCL_LOCAL HeightFieldShapeCollider { const HF& height_field = static_cast(*o1); const Shape& shape = static_cast(*o2); - if (request.security_margin < 0) { - return details::height_field_shape_collide_negative_security_margin< - BV, Shape>::run(height_field, tf1, shape, tf2, nsolver, request, - result); - } else { - HeightFieldShapeCollisionTraversalNode node(request); - - initialize(node, height_field, tf1, shape, tf2, nsolver, result); - fcl::collide(&node, request, result); - return result.numContacts(); - } + HeightFieldShapeCollisionTraversalNode node(request); + + initialize(node, height_field, tf1, shape, tf2, nsolver, result); + fcl::collide(&node, request, result); + return result.numContacts(); } }; diff --git a/test/security_margin.cpp b/test/security_margin.cpp index 47bfa2847..64963b469 100644 --- a/test/security_margin.cpp +++ b/test/security_margin.cpp @@ -238,7 +238,8 @@ BOOST_AUTO_TEST_CASE(sphere_sphere) { collide(s1.get(), tf1, s2.get(), tf2, 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); + BOOST_CHECK_CLOSE(-collisionResult.getContact(0).penetration_depth, + distance, 1e-8); } // Negative security margin - no collision @@ -312,7 +313,8 @@ BOOST_AUTO_TEST_CASE(capsule_capsule) { collide(c1.get(), tf1, c2.get(), tf2, 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); + BOOST_CHECK_CLOSE(-collisionResult.getContact(0).penetration_depth, + distance, 1e-8); } // Negative security margin - no collision @@ -383,17 +385,15 @@ BOOST_AUTO_TEST_CASE(box_box) { collide(b1.get(), tf1, b2.get(), tf2_collision, collisionRequest, collisionResult); BOOST_CHECK(!collisionResult.isCollision()); - BOOST_CHECK_CLOSE( - collisionResult.distance_lower_bound, - (collisionRequest.security_margin * tf2_collision.getTranslation()) - .norm(), - tol); + BOOST_CHECK_CLOSE(collisionResult.distance_lower_bound, + -collisionRequest.security_margin, tol); } // Negative security margin - collision { CollisionRequest collisionRequest(CONTACT, 1); - collisionRequest.security_margin = -0.01; + const FCL_REAL distance = -0.01; + collisionRequest.security_margin = distance; CollisionResult collisionResult; const Transform3f tf2((tf2_collision.getTranslation() + @@ -403,7 +403,8 @@ BOOST_AUTO_TEST_CASE(box_box) { collide(b1.get(), tf1, b2.get(), tf2, collisionRequest, collisionResult); BOOST_CHECK(collisionResult.isCollision()); BOOST_CHECK_SMALL(collisionResult.distance_lower_bound, tol); - BOOST_CHECK_SMALL(-collisionResult.getContact(0).penetration_depth, tol); + BOOST_CHECK_CLOSE(-collisionResult.getContact(0).penetration_depth, + distance, tol); } } @@ -468,7 +469,8 @@ void test_shape_shape(const ShapeType1& shape1, const Transform3f& tf1, // Negative security margin - collision { CollisionRequest collisionRequest(CONTACT, 1); - collisionRequest.security_margin = -0.01; + const FCL_REAL distance = -0.01; + collisionRequest.security_margin = distance; CollisionResult collisionResult; const Transform3f tf2((tf2_collision.getTranslation() + @@ -478,7 +480,8 @@ void test_shape_shape(const ShapeType1& shape1, const Transform3f& tf1, collide(&shape1, tf1, &shape2, tf2, collisionRequest, collisionResult); BOOST_CHECK(collisionResult.isCollision()); BOOST_CHECK_SMALL(collisionResult.distance_lower_bound, tol); - BOOST_CHECK_SMALL(-collisionResult.getContact(0).penetration_depth, tol); + BOOST_CHECK_CLOSE(-collisionResult.getContact(0).penetration_depth, + distance, tol); } } From 753439d1dfd1d6da8f8076ecff64bffb169f8e07 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Fri, 24 Jun 2022 15:05:56 +0200 Subject: [PATCH 42/52] Hfield: add more test with negative security margins --- test/hfields.cpp | 74 +++++++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 73 insertions(+), 1 deletion(-) diff --git a/test/hfields.cpp b/test/hfields.cpp index ced67c488..a8bf94657 100644 --- a/test/hfields.cpp +++ b/test/hfields.cpp @@ -405,7 +405,8 @@ void test_negative_security_margin(const Eigen::DenseIndex nx, BOOST_AUTO_TEST_CASE(negative_security_margin) { const FCL_REAL max_altitude = 1., min_altitude = 0.; - test_negative_security_margin(100, 100, min_altitude, max_altitude); + // test_negative_security_margin(100, 100, min_altitude, + // max_altitude); test_negative_security_margin(100, 100, min_altitude, max_altitude); } @@ -450,3 +451,74 @@ BOOST_AUTO_TEST_CASE(hfield_with_square_hole) { BOOST_CHECK(result.isCollision()); } } + +BOOST_AUTO_TEST_CASE(hfield_with_circular_hole) { + const Eigen::DenseIndex nx = 100, ny = 100; + + typedef OBBRSS BV; + const MatrixXf X = + Eigen::RowVectorXd::LinSpaced(nx, -1., 1.).replicate(ny, 1); + const MatrixXf Y = Eigen::VectorXd::LinSpaced(ny, 1., -1.).replicate(1, nx); + + const FCL_REAL dim_hole = 1; + + const Eigen::Array hole = + (X.array().square() + Y.array().square() <= dim_hole); + + const MatrixXf heights = + MatrixXf::Ones(ny, nx) - hole.cast().matrix(); + + const HeightField hfield(2., 2., heights, -10.); + + BOOST_CHECK(hfield.getXGrid()[0] == -1.); + BOOST_CHECK(hfield.getXGrid()[nx - 1] == +1.); + + BOOST_CHECK(hfield.getYGrid()[0] == +1.); + BOOST_CHECK(hfield.getYGrid()[ny - 1] == -1.); + + Sphere sphere(0.975); + const Transform3f sphere_pos(Vec3f(0., 0., 1.)); + const Transform3f hfield_pos; + + { + CollisionResult result; + CollisionRequest request; + request.security_margin = 0.; + collide(&hfield, hfield_pos, &sphere, sphere_pos, request, result); + + BOOST_CHECK(!result.isCollision()); + } + + { + CollisionResult result; + CollisionRequest request; + request.security_margin = 0.01; + collide(&hfield, hfield_pos, &sphere, sphere_pos, request, result); + + FCL_REAL dist = result.distance_lower_bound + request.security_margin; + std::cout << "dist: " << dist << std::endl; + std::cout << "distance_lower_bound: " << result.distance_lower_bound + << std::endl; + std::cout << "isCollision(): " << result.isCollision() << std::endl; + + BOOST_CHECK(!result.isCollision()); + } + + { + CollisionResult result; + CollisionRequest request; + request.security_margin = 1. - sphere.radius; + collide(&hfield, hfield_pos, &sphere, sphere_pos, request, result); + + BOOST_CHECK(result.isCollision()); + } + + { + CollisionResult result; + CollisionRequest request; + request.security_margin = -0.005; + collide(&hfield, hfield_pos, &sphere, sphere_pos, request, result); + + BOOST_CHECK(!result.isCollision()); + } +} From eecc2cc0a9b30e41ab1a60fc679c68698e9676ce Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Fri, 24 Jun 2022 15:07:57 +0200 Subject: [PATCH 43/52] core: remove useless codes It appears that the proposed solution to handle negative security margin was a bit overkilled --- include/hpp/fcl/internal/shape_shape_func.h | 283 -------------------- src/collision_func_matrix.cpp | 96 ------- 2 files changed, 379 deletions(-) diff --git a/include/hpp/fcl/internal/shape_shape_func.h b/include/hpp/fcl/internal/shape_shape_func.h index 036b29beb..181302ea8 100644 --- a/include/hpp/fcl/internal/shape_shape_func.h +++ b/include/hpp/fcl/internal/shape_shape_func.h @@ -97,289 +97,6 @@ struct ShapeShapeCollider { } }; -/// @cond DEV -namespace details { - -// Forward declaration -template ::IsInflatable, - bool shape1_has_inflated_support_function = - shape_traits::HasInflatedSupportFunction, - bool shape2_is_inflatable = shape_traits::IsInflatable, - bool shape2_has_inflated_support_function = - shape_traits::HasInflatedSupportFunction> -struct shape_shape_collide_negative_security_margin; - -// Handle case where both shapes are inflatable -template -struct shape_shape_collide_negative_security_margin { - static std::size_t run(const ShapeType1& o1, const Transform3f& tf1, - const ShapeType2& o2, const Transform3f& tf2, - const GJKSolver* nsolver, - const CollisionRequest& request, - CollisionResult& result) { - const FCL_REAL min_inflation1 = o1.minInflationValue(); - const FCL_REAL min_inflation2 = o2.minInflationValue(); - const FCL_REAL min_inflation = min_inflation1 + min_inflation2; - const FCL_REAL security_margin = request.security_margin; - - if (security_margin < min_inflation) - HPP_FCL_THROW_PRETTY( - "The request security margin: " - << security_margin - << " is below the minimal security margin authorised by the pair " - "of two shapes" - << "(" << std::string(get_node_type_name(o1.getNodeType())) << "," - << std::string(get_node_type_name(o2.getNodeType())) - << "): " << min_inflation - << ".\n Please consider increasing the requested security " - "margin.", - std::invalid_argument); - - if (security_margin > min_inflation1) { - const auto& deflated_result = o1.inflated(security_margin); - const ShapeType1& deflated_o1 = deflated_result.first; - const Transform3f deflated_tf1 = tf1 * deflated_result.second; - CollisionRequest deflated_request(request); - deflated_request.security_margin = 0; - return ShapeShapeCollider::run( - &deflated_o1, deflated_tf1, &o2, tf2, nsolver, deflated_request, - result); - } else if (security_margin > min_inflation2) { - const auto& deflated_result = o2.inflated(security_margin); - const ShapeType2& deflated_o2 = deflated_result.first; - const Transform3f deflated_tf2 = tf2 * deflated_result.second; - CollisionRequest deflated_request(request); - deflated_request.security_margin = 0; - return ShapeShapeCollider::run( - &o1, tf1, &deflated_o2, deflated_tf2, nsolver, deflated_request, - result); - } else // deflate both - { - const FCL_REAL half_security_margin = 0.5 * security_margin; - const auto& deflated_result1 = o1.inflated(half_security_margin); - const ShapeType1& deflated_o1 = deflated_result1.first; - const Transform3f deflated_tf1 = tf1 * deflated_result1.second; - const auto& deflated_result2 = o2.inflated(half_security_margin); - const ShapeType2& deflated_o2 = deflated_result2.first; - const Transform3f deflated_tf2 = tf2 * deflated_result2.second; - CollisionRequest deflated_request(request); - deflated_request.security_margin = 0; - return ShapeShapeCollider::run( - &deflated_o1, deflated_tf1, &deflated_o2, deflated_tf2, nsolver, - deflated_request, result); - } - } -}; - -// Handle case where only the first shape is inflatable -template -struct shape_shape_collide_negative_security_margin { - static std::size_t run(const ShapeType1& o1, const Transform3f& tf1, - const ShapeType2& o2, const Transform3f& tf2, - const GJKSolver* nsolver, - const CollisionRequest& request, - CollisionResult& result) { - const FCL_REAL min_inflation = o1.minInflationValue(); - const FCL_REAL security_margin = request.security_margin; - - if (security_margin < min_inflation) - HPP_FCL_THROW_PRETTY( - "The request security margin: " - << security_margin - << " is below the minimal security margin authorised by the pair " - "of two shapes" - << "(" << std::string(get_node_type_name(o1.getNodeType())) << "," - << std::string(get_node_type_name(o2.getNodeType())) - << "): " << min_inflation - << ".\n Please consider increasing the requested security " - "margin.", - std::invalid_argument); - - const auto& deflated_result = o1.inflated(security_margin); - const ShapeType1& deflated_o1 = deflated_result.first; - const Transform3f deflated_tf1 = tf1 * deflated_result.second; - CollisionRequest deflated_request(request); - deflated_request.security_margin = 0; - return ShapeShapeCollider::run( - &deflated_o1, deflated_tf1, &o2, tf2, nsolver, deflated_request, - result); - } -}; - -// Handle case where only the second shape is inflatable -template -struct shape_shape_collide_negative_security_margin { - static std::size_t run(const ShapeType1& o1, const Transform3f& tf1, - const ShapeType2& o2, const Transform3f& tf2, - const GJKSolver* nsolver, - const CollisionRequest& request, - CollisionResult& result) { - const std::size_t res = shape_shape_collide_negative_security_margin< - ShapeType2, ShapeType1, true, false, false, false>::run(o2, tf2, o1, - tf1, nsolver, - request, - result); - result.swapObjects(); - return res; - } -}; - -// Handle case where the first shape is inflatable and the second shape has an -// inflated support function -template -struct shape_shape_collide_negative_security_margin { - static std::size_t run(const ShapeType1& o1, const Transform3f& tf1, - const ShapeType2& o2, const Transform3f& tf2, - const GJKSolver* nsolver, - const CollisionRequest& request, - CollisionResult& result) { - const FCL_REAL min_inflation = o1.minInflationValue(); - const FCL_REAL security_margin = request.security_margin; - - if (min_inflation < security_margin) { - const auto& deflated_result = o1.inflated(security_margin); - const ShapeType1& deflated_o1 = deflated_result.first; - const Transform3f deflated_tf1 = tf1 * deflated_result.second; - CollisionRequest deflated_request(request); - deflated_request.security_margin = 0; - return ShapeShapeCollider::run( - &deflated_o1, deflated_tf1, &o2, tf2, nsolver, deflated_request, - result); - } else { - const FCL_REAL half_security_margin = 0.5 * security_margin; - const auto& deflated_result = o1.inflated(half_security_margin); - const ShapeType1& deflated_o1 = deflated_result.first; - const Transform3f deflated_tf1 = tf1 * deflated_result.second; - CollisionRequest deflated_request(request); - deflated_request.security_margin = 0; - assert(nsolver != NULL && "The GJK solver is not defined."); - nsolver->setShapeDeflation(0, half_security_margin); - - std::size_t res = ShapeShapeCollider::run( - &deflated_o1, deflated_tf1, &o2, tf2, nsolver, deflated_request, - result); - - nsolver->resetShapeDeflation(); - return res; - } - } -}; - -// Handle case where the second shape is inflatable and the first shape has an -// inflated support function -template -struct shape_shape_collide_negative_security_margin { - static std::size_t run(const ShapeType1& o1, const Transform3f& tf1, - const ShapeType2& o2, const Transform3f& tf2, - const GJKSolver* nsolver, - const CollisionRequest& request, - CollisionResult& result) { - const std::size_t res = shape_shape_collide_negative_security_margin< - ShapeType2, ShapeType1, true, false, false, true>::run(o2, tf2, o1, tf1, - nsolver, request, - result); - result.swapObjects(); - return res; - } -}; - -// Handle case where the two shapes have inflated support function -template -struct shape_shape_collide_negative_security_margin { - static std::size_t run(const ShapeType1& o1, const Transform3f& tf1, - const ShapeType2& o2, const Transform3f& tf2, - const GJKSolver* nsolver, - const CollisionRequest& request, - CollisionResult& result) { - const FCL_REAL security_margin = request.security_margin; - const FCL_REAL half_security_margin = 0.5 * security_margin; - CollisionRequest deflated_request(request); - deflated_request.security_margin = 0; - - assert(nsolver != NULL && "The GJK solver is not defined."); - nsolver->setShapeDeflation(half_security_margin, half_security_margin); - - std::size_t res = ShapeShapeCollider::run( - &o1, tf1, &o2, tf2, nsolver, deflated_request, result); - - nsolver->resetShapeDeflation(); - return res; - } -}; - -// Handle case where only the first shape has inflated support function -template -struct shape_shape_collide_negative_security_margin { - static std::size_t run(const ShapeType1& o1, const Transform3f& tf1, - const ShapeType2& o2, const Transform3f& tf2, - const GJKSolver* nsolver, - const CollisionRequest& request, - CollisionResult& result) { - const FCL_REAL security_margin = request.security_margin; - CollisionRequest deflated_request(request); - deflated_request.security_margin = 0; - - assert(nsolver != NULL && "The GJK solver is not defined."); - nsolver->setShapeDeflation(security_margin, 0); - - std::size_t res = ShapeShapeCollider::run( - &o1, tf1, &o2, tf2, nsolver, deflated_request, result); - - nsolver->resetShapeDeflation(); - return res; - } -}; - -// Handle case where only the second shape has inflated support function -template -struct shape_shape_collide_negative_security_margin { - static std::size_t run(const ShapeType1& o1, const Transform3f& tf1, - const ShapeType2& o2, const Transform3f& tf2, - const GJKSolver* nsolver, - const CollisionRequest& request, - CollisionResult& result) { - const std::size_t res = shape_shape_collide_negative_security_margin< - ShapeType2, ShapeType1, false, true, false, false>::run(o2, tf2, o1, - tf1, nsolver, - request, - result); - result.swapObjects(); - return res; - } -}; - -// Handle case where the shapes info are not provided. -template -struct shape_shape_collide_negative_security_margin< - ShapeType1, ShapeType2, false, false, false, false> { - static std::size_t run(const ShapeType1& o1, const Transform3f& /*tf1*/, - const ShapeType2& o2, const Transform3f& /*tf2*/, - const GJKSolver* /*nsolver*/, - const CollisionRequest& /*request*/, - CollisionResult& /*result*/) { - HPP_FCL_THROW_PRETTY( - "Negative security margin between node type " - << std::string(get_node_type_name(o1.getNodeType())) - << " and node type " - << std::string(get_node_type_name(o2.getNodeType())) - << " is not supported.", - std::invalid_argument); - return 0; - } -}; -} // namespace details - -/// \endcond - template std::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf1, diff --git a/src/collision_func_matrix.cpp b/src/collision_func_matrix.cpp index ab3c710a4..997c11444 100644 --- a/src/collision_func_matrix.cpp +++ b/src/collision_func_matrix.cpp @@ -160,102 +160,6 @@ struct HPP_FCL_LOCAL BVHShapeCollider { } }; -/// @cond DEV -namespace details { - -// Forward declaration -template ::IsInflatable, - bool shape_has_inflated_support_function = - shape_traits::HasInflatedSupportFunction> -struct height_field_shape_collide_negative_security_margin; - -template -struct height_field_shape_collide_negative_security_margin { - typedef HeightField HF; - - static std::size_t run(const HF& o1, const Transform3f& tf1, - const ShapeType& o2, const Transform3f& tf2, - const GJKSolver* nsolver, - const CollisionRequest& request, - CollisionResult& result) { - const FCL_REAL min_deflation = o2.minInflationValue(); - const FCL_REAL security_margin = request.security_margin; - - if (security_margin < min_deflation) return result.numContacts(); - - const auto& deflated_result = o2.inflated(security_margin); - const ShapeType& deflated_o2 = deflated_result.first; - const Transform3f deflated_tf2 = tf2 * deflated_result.second; - - CollisionRequest deflated_request(request); - - deflated_request.security_margin = - 0; // We already account for the deflation of the shape. - - HeightFieldShapeCollisionTraversalNode node( - deflated_request); - node.shape_inflation[0] = 0; - - nsolver->setShapeDeflation(0., 0.); - - initialize(node, o1, tf1, deflated_o2, deflated_tf2, nsolver, result); - fcl::collide(&node, deflated_request, result); - return result.numContacts(); - } -}; - -template -struct height_field_shape_collide_negative_security_margin { - typedef HeightField HF; - - static std::size_t run(const HF& o1, const Transform3f& tf1, - const ShapeType& o2, const Transform3f& tf2, - const GJKSolver* nsolver, - const CollisionRequest& request, - CollisionResult& result) { - CollisionRequest deflated_request(request); - deflated_request.security_margin = 0; - - HeightFieldShapeCollisionTraversalNode node( - deflated_request); - // The deflation is splitted between the two objects. - node.shape_inflation[0] = 0.5 * request.security_margin; - - nsolver->setShapeDeflation(0., 0.5 * request.security_margin); - - initialize(node, o1, tf1, o2, tf2, nsolver, result); - fcl::collide(&node, deflated_request, result); - return result.numContacts(); - } -}; - -template -struct height_field_shape_collide_negative_security_margin { - typedef HeightField HF; - - static std::size_t run(const HF& o1, const Transform3f& /*tf1*/, - const ShapeType& o2, const Transform3f& /*tf2*/, - const GJKSolver* /*nsolver*/, - const CollisionRequest& /*request*/, - CollisionResult& /*result*/) { - HPP_FCL_THROW_PRETTY( - "Negative security margin between node type " - << std::string(get_node_type_name(o1.getNodeType())) - << " and node type " - << std::string(get_node_type_name(o2.getNodeType())) - << " is not supported.", - std::invalid_argument); - return 0; - } -}; -} // namespace details - -/// @endcond - /// @brief Collider functor for HeightField data structure /// \tparam _Options takes two values. /// - RelativeTransformationIsIdentity if object 1 should be moved From e4ed2f0922f5f5d27f72d58e9999e69f8b5f5e69 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Fri, 24 Jun 2022 16:01:32 +0200 Subject: [PATCH 44/52] hfield: fix test and add todo message --- test/hfields.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/test/hfields.cpp b/test/hfields.cpp index a8bf94657..82a2b2fcd 100644 --- a/test/hfields.cpp +++ b/test/hfields.cpp @@ -455,7 +455,9 @@ BOOST_AUTO_TEST_CASE(hfield_with_square_hole) { BOOST_AUTO_TEST_CASE(hfield_with_circular_hole) { const Eigen::DenseIndex nx = 100, ny = 100; - typedef OBBRSS BV; + // typedef OBBRSS BV; TODO(jcarpent): OBBRSS does not work (compile in Debug + // mode), as the overlap of OBBRSS is not satisfactory yet. + typedef AABB BV; const MatrixXf X = Eigen::RowVectorXd::LinSpaced(nx, -1., 1.).replicate(ny, 1); const MatrixXf Y = Eigen::VectorXd::LinSpaced(ny, 1., -1.).replicate(1, nx); From 27373a21eba588486f6ee1cfd9936a02f8922463 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Fri, 24 Jun 2022 16:44:27 +0200 Subject: [PATCH 45/52] gjk: fix BoundingVolumeGuess --- include/hpp/fcl/narrowphase/narrowphase.h | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/include/hpp/fcl/narrowphase/narrowphase.h b/include/hpp/fcl/narrowphase/narrowphase.h index 12cff8aa8..915d96dee 100644 --- a/include/hpp/fcl/narrowphase/narrowphase.h +++ b/include/hpp/fcl/narrowphase/narrowphase.h @@ -68,7 +68,7 @@ struct HPP_FCL_DLLAPI GJKSolver { support_hint = support_func_cached_guess; break; case GJKInitialGuess::BoundingVolumeGuess: - if (s1.aabb_radius < 0 || s2.aabb_radius < 0) { + if (s1.aabb_local.volume() < 0 || s2.aabb_local.volume() < 0) { HPP_FCL_THROW_PRETTY( "computeLocalAABB must have been called on the shapes before " "using " @@ -77,8 +77,7 @@ struct HPP_FCL_DLLAPI GJKSolver { } guess.noalias() = s1.aabb_center - (shape.oR1 * s2.aabb_center + shape.ot1); - support_hint = - support_func_cached_guess; // we could also put it to (0, 0) + support_hint.setZero(); break; default: HPP_FCL_THROW_PRETTY("Wrong initial guess for GJK.", std::logic_error); From 331c6d641079ad6e5e91e643ce34dbf39c110833 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Fri, 24 Jun 2022 16:55:15 +0200 Subject: [PATCH 46/52] gjk: normalize guess --- include/hpp/fcl/narrowphase/narrowphase.h | 1 + 1 file changed, 1 insertion(+) diff --git a/include/hpp/fcl/narrowphase/narrowphase.h b/include/hpp/fcl/narrowphase/narrowphase.h index 915d96dee..5f1a11442 100644 --- a/include/hpp/fcl/narrowphase/narrowphase.h +++ b/include/hpp/fcl/narrowphase/narrowphase.h @@ -77,6 +77,7 @@ struct HPP_FCL_DLLAPI GJKSolver { } guess.noalias() = s1.aabb_center - (shape.oR1 * s2.aabb_center + shape.ot1); + guess.normalize(); support_hint.setZero(); break; default: From dab4323f1bb33a1fa0181e5d5aca232d1f07103f Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Fri, 24 Jun 2022 17:07:15 +0200 Subject: [PATCH 47/52] debug --- include/hpp/fcl/narrowphase/narrowphase.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/include/hpp/fcl/narrowphase/narrowphase.h b/include/hpp/fcl/narrowphase/narrowphase.h index 5f1a11442..77c1c4fd1 100644 --- a/include/hpp/fcl/narrowphase/narrowphase.h +++ b/include/hpp/fcl/narrowphase/narrowphase.h @@ -41,6 +41,7 @@ #define HPP_FCL_NARROWPHASE_H #include +#include #include #include @@ -75,9 +76,8 @@ struct HPP_FCL_DLLAPI GJKSolver { "GJKInitialGuess::BoundingVolumeGuess.", std::logic_error); } - guess.noalias() = - s1.aabb_center - (shape.oR1 * s2.aabb_center + shape.ot1); - guess.normalize(); + guess.noalias() = s1.aabb_local.center() - + (shape.oR1 * s2.aabb_local.center() + shape.ot1); support_hint.setZero(); break; default: From fdd1f9da476076677399f379b6981eab5d8df186 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Mon, 27 Jun 2022 12:07:00 +0200 Subject: [PATCH 48/52] distance_lower_bounds: do not account for the security margin --- include/hpp/fcl/collision_data.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/hpp/fcl/collision_data.h b/include/hpp/fcl/collision_data.h index ab1d432cf..04830f1be 100644 --- a/include/hpp/fcl/collision_data.h +++ b/include/hpp/fcl/collision_data.h @@ -540,7 +540,7 @@ inline void updateDistanceLowerBoundFromBV(const CollisionRequest& req, 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; + FCL_REAL new_dlb = std::sqrt(sqrDistLowerBound); // - req.security_margin; if (new_dlb < res.distance_lower_bound) res.distance_lower_bound = new_dlb; } From c93b22b4e111bb22a35eafe0e25b15d00178cfb8 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Mon, 27 Jun 2022 12:07:13 +0200 Subject: [PATCH 49/52] test: remove useless print --- test/hfields.cpp | 6 ------ 1 file changed, 6 deletions(-) diff --git a/test/hfields.cpp b/test/hfields.cpp index 82a2b2fcd..bc6d8279a 100644 --- a/test/hfields.cpp +++ b/test/hfields.cpp @@ -497,12 +497,6 @@ BOOST_AUTO_TEST_CASE(hfield_with_circular_hole) { request.security_margin = 0.01; collide(&hfield, hfield_pos, &sphere, sphere_pos, request, result); - FCL_REAL dist = result.distance_lower_bound + request.security_margin; - std::cout << "dist: " << dist << std::endl; - std::cout << "distance_lower_bound: " << result.distance_lower_bound - << std::endl; - std::cout << "isCollision(): " << result.isCollision() << std::endl; - BOOST_CHECK(!result.isCollision()); } From 5dc47e9de2050092733703212f86bec3c23a67ee Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Wed, 29 Jun 2022 11:22:41 +0200 Subject: [PATCH 50/52] core: remove warning --- include/hpp/fcl/collision_data.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/hpp/fcl/collision_data.h b/include/hpp/fcl/collision_data.h index 04830f1be..bf4357365 100644 --- a/include/hpp/fcl/collision_data.h +++ b/include/hpp/fcl/collision_data.h @@ -535,7 +535,7 @@ struct HPP_FCL_DLLAPI DistanceResult : QueryResult { }; namespace internal { -inline void updateDistanceLowerBoundFromBV(const CollisionRequest& req, +inline void updateDistanceLowerBoundFromBV(const CollisionRequest& /*req*/, CollisionResult& res, const FCL_REAL& sqrDistLowerBound) { // BV cannot find negative distance. From 793b5ec7c464f04286494db8f93c8b4af11b1a77 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Wed, 29 Jun 2022 11:30:50 +0200 Subject: [PATCH 51/52] core: remove useless lines They were related to shape deflations --- include/hpp/fcl/narrowphase/gjk.h | 15 +-- include/hpp/fcl/narrowphase/narrowphase.h | 19 --- src/distance/convex_halfspace.cpp | 22 +--- src/distance/triangle_halfspace.cpp | 22 +--- src/narrowphase/details.h | 6 +- src/narrowphase/gjk.cpp | 134 ++++------------------ 6 files changed, 43 insertions(+), 175 deletions(-) diff --git a/include/hpp/fcl/narrowphase/gjk.h b/include/hpp/fcl/narrowphase/gjk.h index 5418acd41..fe6a52c3e 100644 --- a/include/hpp/fcl/narrowphase/gjk.h +++ b/include/hpp/fcl/narrowphase/gjk.h @@ -51,7 +51,7 @@ namespace details { /// @brief the support function for shape /// \param hint use to initialize the search when shape is a ConvexBase object. Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, bool dirIsNormalized, - int& hint, const FCL_REAL& shape_deflation); + int& hint); /// @brief Minkowski difference class of two shapes /// @@ -83,10 +83,6 @@ struct HPP_FCL_DLLAPI MinkowskiDiff { /// These inflation values are used for Sphere and Capsule. Array2d inflation; - /// @brief Deflation values associated to each support function. - /// The values are zero by default. - Array2d shape_deflation; - /// @brief Number of points in a Convex object from which using a logarithmic /// support function is faster than a linear one. /// It defaults to 32. @@ -106,8 +102,7 @@ struct HPP_FCL_DLLAPI MinkowskiDiff { GetSupportFunction getSupportFunc; MinkowskiDiff() - : shape_deflation(0, 0), - linear_log_convex_threshold(32), + : linear_log_convex_threshold(32), normalize_support_direction(false), getSupportFunc(NULL) {} @@ -121,13 +116,13 @@ struct HPP_FCL_DLLAPI MinkowskiDiff { /// @brief support function for shape0 inline Vec3f support0(const Vec3f& d, bool dIsNormalized, int& hint) const { - return getSupport(shapes[0], d, dIsNormalized, hint, shape_deflation[0]); + return getSupport(shapes[0], d, dIsNormalized, hint); } /// @brief support function for shape1 inline Vec3f support1(const Vec3f& d, bool dIsNormalized, int& hint) const { - return oR1 * getSupport(shapes[1], oR1.transpose() * d, dIsNormalized, hint, - shape_deflation[1]) + + return oR1 * + getSupport(shapes[1], oR1.transpose() * d, dIsNormalized, hint) + ot1; } diff --git a/include/hpp/fcl/narrowphase/narrowphase.h b/include/hpp/fcl/narrowphase/narrowphase.h index 77c1c4fd1..f401fbdc1 100644 --- a/include/hpp/fcl/narrowphase/narrowphase.h +++ b/include/hpp/fcl/narrowphase/narrowphase.h @@ -107,7 +107,6 @@ struct HPP_FCL_DLLAPI GJKSolver { Vec3f* normal) const { details::MinkowskiDiff shape; shape.set(&s1, &s2, tf1, tf2); - shape.shape_deflation = shape_deflation; Vec3f guess; support_func_guess_t support_hint; @@ -351,7 +350,6 @@ struct HPP_FCL_DLLAPI GJKSolver { gjk_variant = GJKVariant::DefaultGJK; gjk_convergence_criterion = GJKConvergenceCriterion::VDB; gjk_convergence_criterion_type = GJKConvergenceCriterionType::Relative; - shape_deflation.setZero(); } /// @brief Constructor from a DistanceRequest @@ -362,7 +360,6 @@ struct HPP_FCL_DLLAPI GJKSolver { cached_guess = Vec3f(1, 0, 0); support_func_cached_guess = support_func_guess_t::Zero(); distance_upper_bound = (std::numeric_limits::max)(); - shape_deflation.setZero(); // EPS settings epa_max_face_num = 128; @@ -401,7 +398,6 @@ struct HPP_FCL_DLLAPI GJKSolver { cached_guess = Vec3f(1, 0, 0); support_func_cached_guess = support_func_guess_t::Zero(); distance_upper_bound = (std::numeric_limits::max)(); - shape_deflation.setZero(); // EPS settings epa_max_face_num = 128; @@ -464,15 +460,6 @@ struct HPP_FCL_DLLAPI GJKSolver { bool operator!=(const GJKSolver& other) const { return !(*this == other); } - void resetShapeDeflation() const { shape_deflation.setZero(); } - - void setShapeDeflation(const FCL_REAL shape1_deflation, - const FCL_REAL shape2_deflation) const { - shape_deflation << shape1_deflation, shape2_deflation; - assert((shape_deflation <= 0).all() && - "The deflation values should be negative."); - } - /// @brief maximum number of simplex face used in EPA algorithm unsigned int epa_max_face_num; @@ -520,12 +507,6 @@ struct HPP_FCL_DLLAPI GJKSolver { /// the two shapes have a distance greather than distance_upper_bound. mutable FCL_REAL distance_upper_bound; - /// @brief Inflation values provided to the support functions associated to - /// each shape. - /// @note These inflation values are used to accound for negative - /// security_margin - mutable Array2d shape_deflation; - public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; diff --git a/src/distance/convex_halfspace.cpp b/src/distance/convex_halfspace.cpp index 01fd2c9c6..897386d52 100644 --- a/src/distance/convex_halfspace.cpp +++ b/src/distance/convex_halfspace.cpp @@ -45,18 +45,13 @@ namespace fcl { template <> FCL_REAL ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver* nsolver, const DistanceRequest&, DistanceResult& result) { - assert(nsolver->shape_deflation[0] <= 0 && - "Shape deflation for the ConvexBase object should be negative"); - assert(nsolver->shape_deflation[1] == 0 && - "Shape deflation for the HalfSpace object should be zero"); - + const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const DistanceRequest&, DistanceResult& result) { const ConvexBase& s1 = static_cast(*o1); const Halfspace& s2 = static_cast(*o2); details::halfspaceDistance(s2, tf2, s1, tf1, result.min_distance, result.nearest_points[1], result.nearest_points[0], - result.normal, nsolver->shape_deflation[0]); + result.normal); result.o1 = o1; result.o2 = o2; result.b1 = -1; @@ -68,18 +63,13 @@ FCL_REAL ShapeShapeDistance( template <> FCL_REAL ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver* nsolver, const DistanceRequest&, DistanceResult& result) { - assert(nsolver->shape_deflation[0] == 0 && - "Shape deflation for the HalfSpace object should be zero"); - assert(nsolver->shape_deflation[1] <= 0 && - "Shape deflation for the ConvexBase object should be negative"); - + const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const DistanceRequest&, DistanceResult& result) { const Halfspace& s1 = static_cast(*o1); const ConvexBase& s2 = static_cast(*o2); details::halfspaceDistance(s1, tf1, s2, tf2, result.min_distance, result.nearest_points[0], result.nearest_points[1], - result.normal, nsolver->shape_deflation[1]); + result.normal); result.o1 = o1; result.o2 = o2; result.b1 = -1; diff --git a/src/distance/triangle_halfspace.cpp b/src/distance/triangle_halfspace.cpp index c2d6e30f6..13514371e 100644 --- a/src/distance/triangle_halfspace.cpp +++ b/src/distance/triangle_halfspace.cpp @@ -45,18 +45,13 @@ namespace fcl { template <> FCL_REAL ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver* nsolver, const DistanceRequest&, DistanceResult& result) { - assert(nsolver->shape_deflation[0] == 0 && - "Shape deflation for the TriangleP object should be zero"); - assert(nsolver->shape_deflation[1] == 0 && - "Shape deflation for the HalfSpace object should be zero"); - + const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const DistanceRequest&, DistanceResult& result) { const TriangleP& s1 = static_cast(*o1); const Halfspace& s2 = static_cast(*o2); details::halfspaceDistance(s2, tf2, s1, tf1, result.min_distance, result.nearest_points[1], result.nearest_points[0], - result.normal, nsolver->shape_deflation[0]); + result.normal); result.o1 = o1; result.o2 = o2; result.b1 = -1; @@ -68,18 +63,13 @@ FCL_REAL ShapeShapeDistance( template <> FCL_REAL ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver* nsolver, const DistanceRequest&, DistanceResult& result) { - assert(nsolver->shape_deflation[0] == 0 && - "Shape deflation for the HalfSpace object should be zero"); - assert(nsolver->shape_deflation[1] == 0 && - "Shape deflation for the TriangleP object should be zero"); - + const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const DistanceRequest&, DistanceResult& result) { const Halfspace& s1 = static_cast(*o1); const TriangleP& s2 = static_cast(*o2); details::halfspaceDistance(s1, tf1, s2, tf2, result.min_distance, result.nearest_points[0], result.nearest_points[1], - result.normal, nsolver->shape_deflation[1]); + result.normal); result.o1 = o1; result.o2 = o2; result.b1 = -1; diff --git a/src/narrowphase/details.h b/src/narrowphase/details.h index e34834a9f..bcc3b218d 100644 --- a/src/narrowphase/details.h +++ b/src/narrowphase/details.h @@ -1619,8 +1619,6 @@ inline bool convexHalfspaceIntersect( const ConvexBase& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) { - // No need to account for the shape_deflation in the case of Convex,Halfspace - // as Halfspace objects have an infinite value possible for deflation. Halfspace new_s2 = transform(s2, tf2); Vec3f v; @@ -1810,11 +1808,11 @@ inline bool halfspaceIntersect(const Halfspace& s1, const Transform3f& tf1, inline bool halfspaceDistance(const Halfspace& h, const Transform3f& tf1, const ShapeBase& s, const Transform3f& tf2, FCL_REAL& dist, Vec3f& p1, Vec3f& p2, - Vec3f& normal, const FCL_REAL shape_deflation) { + Vec3f& normal) { Vec3f n_w = tf1.getRotation() * h.n; Vec3f n_2(tf2.getRotation().transpose() * n_w); int hint = 0; - p2 = getSupport(&s, -n_2, true, hint, shape_deflation); + p2 = getSupport(&s, -n_2, true, hint); p2 = tf2.transform(p2); dist = (p2 - tf1.getTranslation()).dot(n_w) - h.d; diff --git a/src/narrowphase/gjk.cpp b/src/narrowphase/gjk.cpp index 9b5fdd056..ba4af779a 100644 --- a/src/narrowphase/gjk.cpp +++ b/src/narrowphase/gjk.cpp @@ -46,11 +46,7 @@ namespace fcl { namespace details { void getShapeSupport(const TriangleP* triangle, const Vec3f& dir, - Vec3f& support, int&, MinkowskiDiff::ShapeData*, - const FCL_REAL& shape_deflation) { - HPP_FCL_UNUSED_VARIABLE(shape_deflation); - assert(shape_deflation == 0 && "shape_deflation should be equal to zero"); - + Vec3f& support, int&, MinkowskiDiff::ShapeData*) { FCL_REAL dota = dir.dot(triangle->a); FCL_REAL dotb = dir.dot(triangle->b); FCL_REAL dotc = dir.dot(triangle->c); @@ -68,11 +64,7 @@ void getShapeSupport(const TriangleP* triangle, const Vec3f& dir, } inline void getShapeSupport(const Box* box, const Vec3f& dir, Vec3f& support, - int&, MinkowskiDiff::ShapeData*, - const FCL_REAL& shape_deflation) { - HPP_FCL_UNUSED_VARIABLE(shape_deflation); - assert(shape_deflation == 0 && "shape_deflation should be equal to zero"); - + int&, MinkowskiDiff::ShapeData*) { const FCL_REAL inflate = (dir.array() == 0).any() ? 1.00000001 : 1.; support.noalias() = (dir.array() > 0) @@ -80,20 +72,12 @@ inline void getShapeSupport(const Box* box, const Vec3f& dir, Vec3f& support, } inline void getShapeSupport(const Sphere*, const Vec3f& /*dir*/, Vec3f& support, - int&, MinkowskiDiff::ShapeData*, - const FCL_REAL& shape_deflation) { - HPP_FCL_UNUSED_VARIABLE(shape_deflation); - assert(shape_deflation == 0 && "shape_deflation should be equal to zero"); - + int&, MinkowskiDiff::ShapeData*) { support.setZero(); } inline void getShapeSupport(const Ellipsoid* ellipsoid, const Vec3f& dir, - Vec3f& support, int&, MinkowskiDiff::ShapeData*, - const FCL_REAL& shape_deflation) { - HPP_FCL_UNUSED_VARIABLE(shape_deflation); - assert(shape_deflation == 0 && "shape_deflation should be equal to zero"); - + Vec3f& support, int&, MinkowskiDiff::ShapeData*) { FCL_REAL a2 = ellipsoid->radii[0] * ellipsoid->radii[0]; FCL_REAL b2 = ellipsoid->radii[1] * ellipsoid->radii[1]; FCL_REAL c2 = ellipsoid->radii[2] * ellipsoid->radii[2]; @@ -106,11 +90,7 @@ inline void getShapeSupport(const Ellipsoid* ellipsoid, const Vec3f& dir, } inline void getShapeSupport(const Capsule* capsule, const Vec3f& dir, - Vec3f& support, int&, MinkowskiDiff::ShapeData*, - const FCL_REAL& shape_deflation) { - HPP_FCL_UNUSED_VARIABLE(shape_deflation); - assert(shape_deflation == 0 && "shape_deflation should be equal to zero"); - + Vec3f& support, int&, MinkowskiDiff::ShapeData*) { support.head<2>().setZero(); if (dir[2] > 0) support[2] = capsule->halfLength; @@ -119,11 +99,7 @@ inline void getShapeSupport(const Capsule* capsule, const Vec3f& dir, } void getShapeSupport(const Cone* cone, const Vec3f& dir, Vec3f& support, int&, - MinkowskiDiff::ShapeData*, - const FCL_REAL& shape_deflation) { - HPP_FCL_UNUSED_VARIABLE(shape_deflation); - assert(shape_deflation == 0 && "shape_deflation should be equal to zero"); - + MinkowskiDiff::ShapeData*) { // The cone radius is, for -h < z < h, (h - z) * r / (2*h) static const FCL_REAL inflate = 1.00001; FCL_REAL h = cone->halfLength; @@ -161,11 +137,7 @@ void getShapeSupport(const Cone* cone, const Vec3f& dir, Vec3f& support, int&, } void getShapeSupport(const Cylinder* cylinder, const Vec3f& dir, Vec3f& support, - int&, MinkowskiDiff::ShapeData*, - const FCL_REAL& shape_deflation) { - HPP_FCL_UNUSED_VARIABLE(shape_deflation); - assert(shape_deflation == 0 && "shape_deflation should be equal to zero"); - + int&, MinkowskiDiff::ShapeData*) { // The inflation makes the object look strictly convex to GJK and EPA. This // helps solving particular cases (e.g. a cylinder with itself at the same // position...) @@ -194,52 +166,9 @@ void getShapeSupport(const Cylinder* cylinder, const Vec3f& dir, Vec3f& support, struct SmallConvex : ShapeBase {}; struct LargeConvex : ShapeBase {}; -inline void correctConvexBaseSupport(const ConvexBase* convex, Vec3f& support, - const int hint, - const FCL_REAL shape_deflation) { - const Vec3f* pts = convex->points; - const ConvexBase::Neighbors* nn = convex->neighbors; - - const ConvexBase::Neighbors& n = nn[hint]; - unsigned char num_neighbors = n.count(); - - if (num_neighbors == 0) - HPP_FCL_THROW_PRETTY("Point " - << hint - << " has zero neighbor.\n" - "The convex geometry seems to be degenerated.", - std::invalid_argument); - - // step 1: Compute average neighborhood directions (pointing towards the - // interior of the convex object). - const Vec3f first_direction = (pts[n[0]] - support).normalized(); - Vec3f average_dir = first_direction; - for (int in = 1; in < num_neighbors; ++in) { - const unsigned int ip = n[in]; - average_dir += (pts[ip] - support).normalized(); - } - - average_dir.normalize(); - - // step 2: Compute the right penetration - const FCL_REAL cos_alpha = average_dir.dot(first_direction); - const FCL_REAL sin_acos_cos_alpha = std::sqrt(1. - cos_alpha * cos_alpha); - if (sin_acos_cos_alpha <= Eigen::NumTraits::epsilon()) - HPP_FCL_THROW_PRETTY( - "Degenerated neighborhood for the current convex geometry.\n" - << "sin_acos_cos_alpha: " << sin_acos_cos_alpha << "\n", - std::invalid_argument); - const FCL_REAL penetration_depth = -shape_deflation / sin_acos_cos_alpha; - - // step3: Correct the support - support += penetration_depth * average_dir; -} - void getShapeSupportLog(const ConvexBase* convex, const Vec3f& dir, Vec3f& support, int& hint, - MinkowskiDiff::ShapeData* data, - const FCL_REAL& shape_deflation) { - assert(shape_deflation <= 0 && "shape_deflation should be negative"); + MinkowskiDiff::ShapeData* data) { assert(data != NULL); const Vec3f* pts = convex->points; @@ -276,16 +205,11 @@ void getShapeSupportLog(const ConvexBase* convex, const Vec3f& dir, } support = pts[hint]; - - if (shape_deflation < 0) { - correctConvexBaseSupport(convex, support, hint, shape_deflation); - } } void getShapeSupportLinear(const ConvexBase* convex, const Vec3f& dir, - Vec3f& support, int& hint, MinkowskiDiff::ShapeData*, - const FCL_REAL& shape_deflation) { - assert(shape_deflation <= 0 && "shape_deflation should be negative"); + Vec3f& support, int& hint, + MinkowskiDiff::ShapeData*) { const Vec3f* pts = convex->points; hint = 0; @@ -298,38 +222,31 @@ void getShapeSupportLinear(const ConvexBase* convex, const Vec3f& dir, } } support = pts[hint]; - - if (shape_deflation < 0) { - correctConvexBaseSupport(convex, support, hint, shape_deflation); - } } void getShapeSupport(const ConvexBase* convex, const Vec3f& dir, Vec3f& support, - int& hint, MinkowskiDiff::ShapeData*, - const FCL_REAL& shape_deflation) { + int& hint, MinkowskiDiff::ShapeData*) { // TODO add benchmark to set a proper value for switching between linear and // logarithmic. if (convex->num_points > 32) { MinkowskiDiff::ShapeData data; - getShapeSupportLog(convex, dir, support, hint, &data, shape_deflation); + getShapeSupportLog(convex, dir, support, hint, &data); } else - getShapeSupportLinear(convex, dir, support, hint, NULL, shape_deflation); + getShapeSupportLinear(convex, dir, support, hint, NULL); } inline void getShapeSupport(const SmallConvex* convex, const Vec3f& dir, Vec3f& support, int& hint, - MinkowskiDiff::ShapeData* data, - const FCL_REAL& shape_deflation) { + MinkowskiDiff::ShapeData* data) { getShapeSupportLinear(reinterpret_cast(convex), dir, - support, hint, data, shape_deflation); + support, hint, data); } inline void getShapeSupport(const LargeConvex* convex, const Vec3f& dir, Vec3f& support, int& hint, - MinkowskiDiff::ShapeData* data, - const FCL_REAL& shape_deflation) { + MinkowskiDiff::ShapeData* data) { getShapeSupportLog(reinterpret_cast(convex), dir, support, - hint, data, shape_deflation); + hint, data); } #define CALL_GET_SHAPE_SUPPORT(ShapeType) \ @@ -338,10 +255,10 @@ inline void getShapeSupport(const LargeConvex* convex, const Vec3f& dir, (shape_traits::NeedNormalizedDir && !dirIsNormalized) \ ? dir.normalized() \ : dir, \ - support, hint, NULL, shape_deflation) + support, hint, NULL) Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, bool dirIsNormalized, - int& hint, const FCL_REAL& shape_deflation) { + int& hint) { Vec3f support; switch (shape->getNodeType()) { case GEOM_TRIANGLE: @@ -384,15 +301,12 @@ template void getSupportTpl(const Shape0* s0, const Shape1* s1, const Matrix3f& oR1, const Vec3f& ot1, const Vec3f& dir, Vec3f& support0, Vec3f& support1, support_func_guess_t& hint, - MinkowskiDiff::ShapeData data[2], - const MinkowskiDiff::Array2d& shape_deflation) { - getShapeSupport(s0, dir, support0, hint[0], &(data[0]), shape_deflation[0]); + MinkowskiDiff::ShapeData data[2]) { + getShapeSupport(s0, dir, support0, hint[0], &(data[0])); if (TransformIsIdentity) - getShapeSupport(s1, -dir, support1, hint[1], &(data[1]), - shape_deflation[1]); + getShapeSupport(s1, -dir, support1, hint[1], &(data[1])); else { - getShapeSupport(s1, -oR1.transpose() * dir, support1, hint[1], &(data[1]), - shape_deflation[1]); + getShapeSupport(s1, -oR1.transpose() * dir, support1, hint[1], &(data[1])); support1 = oR1 * support1 + ot1; } } @@ -420,7 +334,7 @@ void getSupportFuncTpl(const MinkowskiDiff& md, const Vec3f& dir, static_cast(md.shapes[0]), static_cast(md.shapes[1]), md.oR1, md.ot1, (NeedNormalizedDir && !dirIsNormalized) ? dir.normalized() : dir, - support0, support1, hint, data, md.shape_deflation); + support0, support1, hint, data); } template From 0d74ab8c4ca1fe08376bf7220f5da410d84672ea Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Tue, 5 Jul 2022 20:21:41 +0200 Subject: [PATCH 52/52] core: fix operator= for ShapeBase --- include/hpp/fcl/shape/geometric_shapes.h | 2 +- test/bvh_models.cpp | 13 +++++++++++++ 2 files changed, 14 insertions(+), 1 deletion(-) diff --git a/include/hpp/fcl/shape/geometric_shapes.h b/include/hpp/fcl/shape/geometric_shapes.h index 5dd9a57ac..151f286bd 100644 --- a/include/hpp/fcl/shape/geometric_shapes.h +++ b/include/hpp/fcl/shape/geometric_shapes.h @@ -55,7 +55,7 @@ class HPP_FCL_DLLAPI ShapeBase : public CollisionGeometry { /// \brief Copy constructor ShapeBase(const ShapeBase& other) : CollisionGeometry(other) {} - ShapeBase& operator=(const ShapeBase& /*other*/) { return *this; } + ShapeBase& operator=(const ShapeBase& other) = default; virtual ~ShapeBase(){}; diff --git a/test/bvh_models.cpp b/test/bvh_models.cpp index 38bac56cb..bb8403054 100644 --- a/test/bvh_models.cpp +++ b/test/bvh_models.cpp @@ -46,6 +46,7 @@ #include #include #include +#include #include #include #include "utility.h" @@ -370,3 +371,15 @@ BOOST_AUTO_TEST_CASE(load_illformated_mesh) { MeshLoader loader; BOOST_CHECK_NO_THROW(loader.load(filename)); } + +BOOST_AUTO_TEST_CASE(test_convex) { + Box* box_ptr = new hpp::fcl::Box(1, 1, 1); + CollisionGeometryPtr_t b1(box_ptr); + BVHModel box_bvh_model = BVHModel(); + generateBVHModel(box_bvh_model, *box_ptr, Transform3f()); + box_bvh_model.buildConvexRepresentation(false); + + box_bvh_model.convex->computeLocalAABB(); + std::shared_ptr convex_copy(box_bvh_model.convex->clone()); + BOOST_CHECK(*convex_copy.get() == *box_bvh_model.convex.get()); +}