diff --git a/CHANGELOG.md b/CHANGELOG.md index 25ba38e1f..f460c3e71 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -5,6 +5,7 @@ All notable changes to this project will be documented in this file. The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/). ## [Unreleased] +- New feature: computation of contact surfaces for any pair of primitive shapes (triangle, sphere, ellipsoid, plane, halfspace, cone, capsule, cylinder, convex) ([#574](https://github.com/humanoid-path-planner/hpp-fcl/pull/574)). - Enhance Broadphase DynamicAABBTree to better handle planes and halfspace ([#570](https://github.com/humanoid-path-planner/hpp-fcl/pull/570)) - [#558](https://github.com/humanoid-path-planner/hpp-fcl/pull/558): - [internal] Removed dead code in `narrowphase/details.h` ([#558](https://github.com/humanoid-path-planner/hpp-fcl/pull/558)) diff --git a/CMakeLists.txt b/CMakeLists.txt index ba1bf98e0..7d5031b66 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -236,6 +236,8 @@ SET(${PROJECT_NAME}_HEADERS include/hpp/fcl/narrowphase/narrowphase.h include/hpp/fcl/narrowphase/gjk.h include/hpp/fcl/narrowphase/narrowphase_defaults.h + include/hpp/fcl/narrowphase/minkowski_difference.h + include/hpp/fcl/narrowphase/support_functions.h include/hpp/fcl/shape/convex.h include/hpp/fcl/shape/details/convex.hxx include/hpp/fcl/shape/geometric_shape_to_BVH_model.h @@ -245,6 +247,10 @@ SET(${PROJECT_NAME}_HEADERS include/hpp/fcl/distance_func_matrix.h include/hpp/fcl/collision.h include/hpp/fcl/collision_func_matrix.h + include/hpp/fcl/contact_patch.h + include/hpp/fcl/contact_patch_func_matrix.h + include/hpp/fcl/contact_patch/contact_patch_solver.h + include/hpp/fcl/contact_patch/contact_patch_solver.hxx include/hpp/fcl/distance.h include/hpp/fcl/math/matrix_3f.h include/hpp/fcl/math/vec_3f.h @@ -265,6 +271,7 @@ SET(${PROJECT_NAME}_HEADERS 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/shape_shape_contact_patch_func.h include/hpp/fcl/internal/intersect.h include/hpp/fcl/internal/tools.h include/hpp/fcl/internal/traversal_node_base.h @@ -284,6 +291,7 @@ SET(${PROJECT_NAME}_HEADERS include/hpp/fcl/serialization/BV_splitter.h include/hpp/fcl/serialization/BVH_model.h include/hpp/fcl/serialization/collision_data.h + include/hpp/fcl/serialization/contact_patch.h include/hpp/fcl/serialization/collision_object.h include/hpp/fcl/serialization/convex.h include/hpp/fcl/serialization/eigen.h diff --git a/include/hpp/fcl/collision.h b/include/hpp/fcl/collision.h index 0a28da308..cbc0bd562 100644 --- a/include/hpp/fcl/collision.h +++ b/include/hpp/fcl/collision.h @@ -69,30 +69,6 @@ HPP_FCL_DLLAPI std::size_t collide(const CollisionGeometry* o1, const CollisionRequest& request, CollisionResult& result); -/// @copydoc collide(const CollisionObject*, const CollisionObject*, const -/// CollisionRequest&, CollisionResult&) \note this function update the initial -/// guess of \c request if requested. -/// See QueryRequest::updateGuess -inline std::size_t collide(const CollisionObject* o1, const CollisionObject* o2, - CollisionRequest& request, CollisionResult& result) { - std::size_t res = collide(o1, o2, (const CollisionRequest&)request, result); - request.updateGuess(result); - return res; -} - -/// @copydoc collide(const CollisionObject*, const CollisionObject*, const -/// CollisionRequest&, CollisionResult&) \note this function update the initial -/// guess of \c request if requested. -/// See QueryRequest::updateGuess -inline std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - CollisionRequest& request, CollisionResult& result) { - std::size_t res = - collide(o1, tf1, o2, tf2, (const CollisionRequest&)request, result); - request.updateGuess(result); - return res; -} - /// @brief This class reduces the cost of identifying the geometry pair. /// This is mostly useful for repeated shape-shape queries. /// @@ -109,15 +85,6 @@ class HPP_FCL_DLLAPI ComputeCollision { const CollisionRequest& request, CollisionResult& result) const; - inline std::size_t operator()(const Transform3f& tf1, const Transform3f& tf2, - CollisionRequest& request, - CollisionResult& result) const { - std::size_t res = operator()(tf1, tf2, (const CollisionRequest&)request, - result); - request.updateGuess(result); - return res; - } - bool operator==(const ComputeCollision& other) const { return o1 == other.o1 && o2 == other.o2 && solver == other.solver; } diff --git a/include/hpp/fcl/collision_data.h b/include/hpp/fcl/collision_data.h index c95f78777..2ea0b1c96 100644 --- a/include/hpp/fcl/collision_data.h +++ b/include/hpp/fcl/collision_data.h @@ -3,6 +3,7 @@ * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation + * Copyright (c) 2024, INRIA * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -42,12 +43,14 @@ #include #include #include +#include #include "hpp/fcl/collision_object.h" #include "hpp/fcl/config.hh" #include "hpp/fcl/data_types.h" #include "hpp/fcl/timings.h" #include "hpp/fcl/narrowphase/narrowphase_defaults.h" +#include "hpp/fcl/logging.h" namespace hpp { namespace fcl { @@ -175,10 +178,10 @@ struct HPP_FCL_DLLAPI QueryRequest { bool enable_cached_gjk_guess; /// @brief the gjk initial guess set by user - Vec3f cached_gjk_guess; + mutable Vec3f cached_gjk_guess; /// @brief the support function initial guess set by user - support_func_guess_t cached_support_func_guess; + mutable support_func_guess_t cached_support_func_guess; /// @brief maximum iteration for the GJK algorithm size_t gjk_max_iterations; @@ -240,7 +243,12 @@ struct HPP_FCL_DLLAPI QueryRequest { QueryRequest& operator=(const QueryRequest& other) = default; HPP_FCL_COMPILER_DIAGNOSTIC_POP - void updateGuess(const QueryResult& result); + /// @brief Updates the guess for the internal GJK algorithm in order to + /// warm-start it when reusing this collision request on the same collision + /// pair. + /// @note The option `gjk_initial_guess` must be set to + /// `GJKInitialGuess::CachedGuess` for this to work. + void updateGuess(const QueryResult& result) const; /// @brief whether two QueryRequest are the same or not inline bool operator==(const QueryRequest& other) const { @@ -280,15 +288,11 @@ struct HPP_FCL_DLLAPI QueryResult { cached_support_func_guess(support_func_guess_t::Constant(-1)) {} }; -inline void QueryRequest::updateGuess(const QueryResult& result) { - if (gjk_initial_guess == GJKInitialGuess::CachedGuess) { - cached_gjk_guess = result.cached_gjk_guess; - cached_support_func_guess = result.cached_support_func_guess; - } - // TODO: use gjk_initial_guess instead +inline void QueryRequest::updateGuess(const QueryResult& result) const { HPP_FCL_COMPILER_DIAGNOSTIC_PUSH HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS - if (enable_cached_gjk_guess) { + if (gjk_initial_guess == GJKInitialGuess::CachedGuess || + enable_cached_gjk_guess) { cached_gjk_guess = result.cached_gjk_guess; cached_support_func_guess = result.cached_support_func_guess; } @@ -489,6 +493,463 @@ struct HPP_FCL_DLLAPI CollisionResult : QueryResult { void swapObjects(); }; +/// @brief This structure allows to encode contact patches. +/// A contact patch is defined by a set of points belonging to a subset of a +/// plane passing by `p` and supported by `n`, where `n = Contact::normal` and +/// `p = Contact::pos`. If we denote by P this plane and by S1 and S2 the first +/// and second shape of a collision pair, a contact patch is represented as a +/// polytope which vertices all belong to `P & S1 & S2`, where `&` denotes the +/// set-intersection. Since a contact patch is a subset of a plane supported by +/// `n`, it has a preferred direction. In HPP-FCL, the `Contact::normal` points +/// from S1 to S2. In the same way, a contact patch points by default from S1 +/// to S2. +/// +/// @note For now (April 2024), a `ContactPatch` is a polygon (2D polytope), +/// so the points of the set, forming the convex-hull of the polytope, are +/// stored in a counter-clockwise fashion. +/// @note If needed, the internal algorithms of hpp-fcl can easily be extended +/// to compute a contact volume instead of a contact patch. +struct HPP_FCL_DLLAPI ContactPatch { + public: + using Polygon = std::vector; + + /// @brief Frame of the set, expressed in the world coordinates. + /// The z-axis of the frame's rotation is the contact patch normal. + Transform3f tf; + + /// @brief Direction of ContactPatch. + /// When doing collision detection, the convention of HPP-FCL is that the + /// normal always points from the first to the second shape of the collision + /// pair i.e. from shape1 to shape2 when calling `collide(shape1, shape2)`. + /// The PatchDirection enum allows to identify if the patch points from + /// shape 1 to shape 2 (Default type) or from shape 2 to shape 1 (Inverted + /// type). The Inverted type should only be used for internal HPP-FCL + /// computations (it allows to properly define two separate contact patches in + /// the same frame). + enum PatchDirection { DEFAULT = 0, INVERTED = 1 }; + + /// @brief Direction of this contact patch. + PatchDirection direction; + + /// @brief Penetration depth of the contact patch. This value corresponds to + /// the signed distance `d` between the shapes. + /// @note For each contact point `p` in the patch of normal `n`, `p1 = p + + /// 0.5*d*n` and `p2 = p - 0.5*d*n` define a pair of witness points. `p1` + /// belongs to the surface of the first shape and `p2` belongs to the surface + /// of the second shape. For any pair of witness points, we always have `p1 - + /// p2 = d * n`. The vector `d * n` is called a minimum separation vector: + /// if S1 is translated by it, S1 and S2 are not in collision anymore. + /// @note Although there may exist multiple minimum separation vectors between + /// two shapes, the term "minimum" comes from the fact that it's impossible to + /// find a different separation vector which has a smaller norm than `d * n`. + FCL_REAL penetration_depth; + + /// @brief Default maximum size of the polygon representing the contact patch. + /// Used to pre-allocate memory for the patch. + static constexpr size_t default_preallocated_size = 12; + + protected: + /// @brief Container for the vertices of the set. + Polygon m_points; + + public: + /// @brief Default constructor. + /// Note: the preallocated size does not determine the maximum number of + /// points in the patch, it only serves as preallocation if the maximum size + /// of the patch is known in advance. HPP-FCL will automatically expand/shrink + /// the contact patch if needed. + explicit ContactPatch(size_t preallocated_size = default_preallocated_size) + : tf(Transform3f::Identity()), + direction(PatchDirection::DEFAULT), + penetration_depth(0) { + this->m_points.reserve(preallocated_size); + } + + /// @brief Normal of the contact patch, expressed in the WORLD frame. + Vec3f getNormal() const { + if (this->direction == PatchDirection::INVERTED) { + return -this->tf.rotation().col(2); + } + return this->tf.rotation().col(2); + } + + /// @brief Returns the number of points in the contact patch. + size_t size() const { return this->m_points.size(); } + + /// @brief Add a 3D point to the set, expressed in the world frame. + /// @note This function takes a 3D point and expresses it in the local frame + /// of the set. It then takes only the x and y components of the vector, + /// effectively doing a projection onto the plane to which the set belongs. + /// TODO(louis): if necessary, we can store the offset to the plane (x, y). + void addPoint(const Vec3f& point_3d) { + const Vec3f point = this->tf.inverseTransform(point_3d); + this->m_points.emplace_back(point.template head<2>()); + } + + /// @brief Get the i-th point of the set, expressed in the 3D world frame. + Vec3f getPoint(const size_t i) const { + Vec3f point(0, 0, 0); + point.head<2>() = this->point(i); + point = tf.transform(point); + return point; + } + + /// @brief Get the i-th point of the contact patch, projected back onto the + /// first shape of the collision pair. This point is expressed in the 3D + /// world frame. + Vec3f getPointShape1(const size_t i) const { + Vec3f point = this->getPoint(i); + point -= (this->penetration_depth / 2) * this->getNormal(); + return point; + } + + /// @brief Get the i-th point of the contact patch, projected back onto the + /// first shape of the collision pair. This 3D point is expressed in the world + /// frame. + Vec3f getPointShape2(const size_t i) const { + Vec3f point = this->getPoint(i); + point += (this->penetration_depth / 2) * this->getNormal(); + return point; + } + + /// @brief Getter for the 2D points in the set. + Polygon& points() { return this->m_points; } + + /// @brief Const getter for the 2D points in the set. + const Polygon& points() const { return this->m_points; } + + /// @brief Getter for the i-th 2D point in the set. + Vec2f& point(const size_t i) { + HPP_FCL_ASSERT(this->m_points.size() > 0, "Patch is empty.", + std::logic_error); + if (i < this->m_points.size()) { + return this->m_points[i]; + } + return this->m_points.back(); + } + + /// @brief Const getter for the i-th 2D point in the set. + const Vec2f& point(const size_t i) const { + HPP_FCL_ASSERT(this->m_points.size() > 0, "Patch is empty.", + std::logic_error); + if (i < this->m_points.size()) { + return this->m_points[i]; + } + return this->m_points.back(); + } + + /// @brief Clear the set. + void clear() { + this->m_points.clear(); + this->tf.setIdentity(); + this->penetration_depth = 0; + } + + /// @brief Whether two contact patches are the same or not. + /// @note This compares the two sets terms by terms. + /// However, two contact patches can be identical, but have a different + /// order for their points. Use `isEqual` in this case. + bool operator==(const ContactPatch& other) const { + return this->tf == other.tf && this->direction == other.direction && + this->penetration_depth == other.penetration_depth && + this->points() == other.points(); + } + + /// @brief Whether two contact patches are the same or not. + /// Checks for different order of the points. + bool isSame(const ContactPatch& other, + const FCL_REAL tol = + Eigen::NumTraits::dummy_precision()) const { + // The x and y axis of the set are arbitrary, but the z axis is + // always the normal. The position of the origin of the frame is also + // arbitrary. So we only check if the normals are the same. + if (!this->getNormal().isApprox(other.getNormal(), tol)) { + return false; + } + + if (std::abs(this->penetration_depth - other.penetration_depth) > tol) { + return false; + } + + if (this->direction != other.direction) { + return false; + } + + if (this->size() != other.size()) { + return false; + } + + // Check all points of the contact patch. + for (size_t i = 0; i < this->size(); ++i) { + bool found = false; + const Vec3f pi = this->getPoint(i); + for (size_t j = 0; j < other.size(); ++j) { + const Vec3f other_pj = other.getPoint(j); + if (pi.isApprox(other_pj, tol)) { + found = true; + } + } + if (!found) { + return false; + } + } + + return true; + } +}; + +/// @brief Construct a frame from a `Contact`'s position and normal. +/// Because both `Contact`'s position and normal are expressed in the world +/// frame, this frame is also expressed w.r.t the world frame. +/// The origin of the frame is `contact.pos` and the z-axis of the frame is +/// `contact.normal`. +inline void constructContactPatchFrameFromContact(const Contact& contact, + ContactPatch& contact_patch) { + contact_patch.penetration_depth = contact.penetration_depth; + contact_patch.tf.translation() = contact.pos; + contact_patch.tf.rotation() = + constructOrthonormalBasisFromVector(contact.normal); + contact_patch.direction = ContactPatch::PatchDirection::DEFAULT; +} + +/// @brief Structure used for internal computations. A support set and a +/// contact patch can be represented by the same structure. In fact, a contact +/// patch is the intersection of two support sets, one with +/// `PatchDirection::DEFAULT` and one with `PatchDirection::INVERTED`. +/// @note A support set with `DEFAULT` direction is the support set of a shape +/// in the direction given by `+n`, where `n` is the z-axis of the frame's +/// patch rotation. An `INVERTED` support set is the support set of a shape in +/// the direction `-n`. +using SupportSet = ContactPatch; + +/// @brief Request for a contact patch computation. +struct HPP_FCL_DLLAPI ContactPatchRequest { + /// @brief Maximum number of contact patches that will be computed. + size_t max_num_patch; + + protected: + /// @brief Maximum samples to compute the support sets of curved shapes, + /// i.e. when the normal is perpendicular to the base of a cylinder. For + /// now, only relevant for Cone and Cylinder. In the future this might be + /// extended to Sphere and Ellipsoid. + size_t m_num_samples_curved_shapes; + + /// @brief Tolerance below which points are added to a contact patch. + /// In details, given two shapes S1 and S2, a contact patch is the triple + /// intersection between the separating plane (P) (passing by `Contact::pos` + /// and supported by `Contact::normal`), S1 and S2; i.e. a contact patch is + /// `P & S1 & S2` if we denote `&` the set intersection operator. If a point + /// p1 of S1 is at a distance below `patch_tolerance` from the separating + /// plane, it is taken into account in the computation of the contact patch. + /// Otherwise, it is not used for the computation. + /// @note Needs to be positive. + FCL_REAL m_patch_tolerance; + + public: + /// @brief Default constructor. + /// @param max_num_patch maximum number of contact patches per collision pair. + /// @param max_sub_patch_size maximum size of each sub contact patch. Each + /// contact patch contains an internal representation for an inscribed sub + /// contact patch. This allows physics simulation to always work with a + /// predetermined maximum size for each contact patch. A sub contact patch is + /// simply a subset of the vertices of a contact patch. + /// @param num_samples_curved_shapes for shapes like cones and cylinders, + /// which have smooth basis (circles in this case), we need to sample a + /// certain amount of point of this basis. + /// @param patch_tolerance the tolerance below which a point of a shape is + /// considered to belong to the support set of this shape in the direction of + /// the normal. Said otherwise, `patch_tolerance` determines the "thickness" + /// of the separating plane between shapes of a collision pair. + explicit ContactPatchRequest(size_t max_num_patch = 1, + size_t num_samples_curved_shapes = + ContactPatch::default_preallocated_size, + FCL_REAL patch_tolerance = 1e-3) + : max_num_patch(max_num_patch) { + this->setNumSamplesCurvedShapes(num_samples_curved_shapes); + this->setPatchTolerance(patch_tolerance); + } + + /// @brief Construct a contact patch request from a collision request. + explicit ContactPatchRequest(const CollisionRequest& collision_request, + size_t num_samples_curved_shapes = + ContactPatch::default_preallocated_size, + FCL_REAL patch_tolerance = 1e-3) + : max_num_patch(collision_request.num_max_contacts) { + this->setNumSamplesCurvedShapes(num_samples_curved_shapes); + this->setPatchTolerance(patch_tolerance); + } + + /// @copydoc m_num_samples_curved_shapes + void setNumSamplesCurvedShapes(const size_t num_samples_curved_shapes) { + if (num_samples_curved_shapes < 3) { + HPP_FCL_LOG_WARNING( + "`num_samples_curved_shapes` cannot be lower than 3. Setting it to " + "3 to prevent bugs."); + this->m_num_samples_curved_shapes = 3; + } else { + this->m_num_samples_curved_shapes = num_samples_curved_shapes; + } + } + + /// @copydoc m_num_samples_curved_shapes + size_t getNumSamplesCurvedShapes() const { + return this->m_num_samples_curved_shapes; + } + + /// @copydoc m_patch_tolerance + void setPatchTolerance(const FCL_REAL patch_tolerance) { + if (patch_tolerance < 0) { + HPP_FCL_LOG_WARNING( + "`patch_tolerance` cannot be negative. Setting it to 0 to prevent " + "bugs."); + this->m_patch_tolerance = Eigen::NumTraits::dummy_precision(); + } else { + this->m_patch_tolerance = patch_tolerance; + } + } + + /// @copydoc m_patch_tolerance + FCL_REAL getPatchTolerance() const { return this->m_patch_tolerance; } + + /// @brief Whether two ContactPatchRequest are identical or not. + bool operator==(const ContactPatchRequest& other) const { + return this->max_num_patch == other.max_num_patch && + this->getNumSamplesCurvedShapes() == + other.getNumSamplesCurvedShapes() && + this->getPatchTolerance() == other.getPatchTolerance(); + } +}; + +/// @brief Result for a contact patch computation. +struct HPP_FCL_DLLAPI ContactPatchResult { + using ContactPatchVector = std::vector; + using ContactPatchRef = std::reference_wrapper; + using ContactPatchRefVector = std::vector; + + protected: + /// @brief Data container for the vector of contact patches. + /// @note Contrary to `CollisionResult` or `DistanceResult`, which have a + /// very small memory footprint, contact patches can contain relatively + /// large polytopes. In order to reuse a `ContactPatchResult` while avoiding + /// successive mallocs, we have a data container and a vector which points + /// to the currently active patches in this data container. + ContactPatchVector m_contact_patches_data; + + /// @brief Contact patches in `m_contact_patches_data` can have two + /// statuses: used or unused. This index tracks the first unused patch in + /// the `m_contact_patches_data` vector. + size_t m_id_available_patch; + + /// @brief Vector of contact patches of the result. + ContactPatchRefVector m_contact_patches; + + public: + /// @brief Default constructor. + ContactPatchResult() : m_id_available_patch(0) { + const size_t max_num_patch = 1; + const ContactPatchRequest request(max_num_patch); + this->set(request); + } + + /// @brief Constructor using a `ContactPatchRequest`. + explicit ContactPatchResult(const ContactPatchRequest& request) + : m_id_available_patch(0) { + this->set(request); + }; + + /// @brief Number of contact patches in the result. + size_t numContactPatches() const { return this->m_contact_patches.size(); } + + /// @brief Returns a new unused contact patch from the internal data vector. + ContactPatchRef getUnusedContactPatch() { + if (this->m_id_available_patch >= this->m_contact_patches_data.size()) { + HPP_FCL_LOG_WARNING( + "Trying to get an unused contact patch but all contact patches are " + "used. Increasing size of contact patches vector, at the cost of a " + "copy. You should increase `max_num_patch` in the " + "`ContactPatchRequest`."); + this->m_contact_patches_data.emplace_back( + this->m_contact_patches_data.back()); + this->m_contact_patches_data.back().clear(); + } + ContactPatch& contact_patch = + this->m_contact_patches_data[this->m_id_available_patch]; + contact_patch.clear(); + this->m_contact_patches.emplace_back(contact_patch); + ++(this->m_id_available_patch); + return this->m_contact_patches.back(); + } + + /// @brief Const getter for the i-th contact patch of the result. + const ContactPatch& getContactPatch(const size_t i) const { + if (this->m_contact_patches.empty()) { + HPP_FCL_THROW_PRETTY( + "The number of contact patches is zero. No ContactPatch can be " + "returned.", + std::invalid_argument); + } + if (i < this->m_contact_patches.size()) { + return this->m_contact_patches[i]; + } + return this->m_contact_patches.back(); + } + + /// @brief Clears the contact patch result. + void clear() { + this->m_contact_patches.clear(); + this->m_id_available_patch = 0; + for (ContactPatch& patch : this->m_contact_patches_data) { + patch.clear(); + } + } + + /// @brief Set up a `ContactPatchResult` from a `ContactPatchRequest` + void set(const ContactPatchRequest& request) { + if (this->m_contact_patches_data.size() < request.max_num_patch) { + this->m_contact_patches_data.resize(request.max_num_patch); + } + for (ContactPatch& patch : this->m_contact_patches_data) { + patch.points().reserve(request.getNumSamplesCurvedShapes()); + } + this->clear(); + } + + /// @brief Return true if this `ContactPatchResult` is aligned with the + /// `ContactPatchRequest` given as input. + bool check(const ContactPatchRequest& request) const { + assert(this->m_contact_patches_data.size() >= request.max_num_patch); + if (this->m_contact_patches_data.size() < request.max_num_patch) { + return false; + } + + for (const ContactPatch& patch : this->m_contact_patches_data) { + if (patch.points().capacity() < request.getNumSamplesCurvedShapes()) { + assert(patch.points().capacity() >= + request.getNumSamplesCurvedShapes()); + return false; + } + } + return true; + } + + /// @brief Whether two ContactPatchResult are identical or not. + bool operator==(const ContactPatchResult& other) const { + if (this->numContactPatches() != other.numContactPatches()) { + return false; + } + + for (size_t i = 0; i < this->numContactPatches(); ++i) { + const ContactPatch& patch = this->getContactPatch(i); + const ContactPatch& other_patch = other.getContactPatch(i); + if (!(patch == other_patch)) { + return false; + } + } + + return true; + } +}; + struct DistanceResult; /// @brief request to the distance computation @@ -669,11 +1130,13 @@ struct HPP_FCL_DLLAPI DistanceResult : QueryResult { // TODO: check also that two GeometryObject are indeed equal. if ((o1 != NULL) ^ (other.o1 != NULL)) return false; is_same &= (o1 == other.o1); - // else if (o1 != NULL and other.o1 != NULL) is_same &= *o1 == *other.o1; + // else if (o1 != NULL and other.o1 != NULL) is_same &= *o1 == + // *other.o1; if ((o2 != NULL) ^ (other.o2 != NULL)) return false; is_same &= (o2 == other.o2); - // else if (o2 != NULL and other.o2 != NULL) is_same &= *o2 == *other.o2; + // else if (o2 != NULL and other.o2 != NULL) is_same &= *o2 == + // *other.o2; return is_same; } diff --git a/include/hpp/fcl/contact_patch.h b/include/hpp/fcl/contact_patch.h new file mode 100644 index 000000000..c3a53ed97 --- /dev/null +++ b/include/hpp/fcl/contact_patch.h @@ -0,0 +1,123 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, 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 INRIA 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. + */ + +/** \author Louis Montaut */ + +#ifndef HPP_FCL_CONTACT_PATCH_H +#define HPP_FCL_CONTACT_PATCH_H + +#include "hpp/fcl/data_types.h" +#include "hpp/fcl/collision_data.h" +#include "hpp/fcl/contact_patch/contact_patch_solver.h" +#include "hpp/fcl/contact_patch_func_matrix.h" + +namespace hpp { +namespace fcl { + +/// @brief Main contact patch computation interface. +/// @note Please see @ref ContactPatchRequest and @ref ContactPatchResult for +/// more info on the content of the input/output of this function. Also, please +/// read @ref ContactPatch if you want to fully understand what is meant by +/// "contact patch". +HPP_FCL_DLLAPI void computeContactPatch(const CollisionGeometry* o1, + const Transform3f& tf1, + const CollisionGeometry* o2, + const Transform3f& tf2, + const CollisionResult& collision_result, + const ContactPatchRequest& request, + ContactPatchResult& result); + +/// @copydoc computeContactPatch(const CollisionGeometry*, const Transform3f&, +// const CollisionGeometry*, const Transform3f&, const CollisionResult&, const +// ContactPatchRequest&, ContactPatchResult&); +HPP_FCL_DLLAPI void computeContactPatch(const CollisionObject* o1, + const CollisionObject* o2, + const CollisionResult& collision_result, + const ContactPatchRequest& request, + ContactPatchResult& result); + +/// @brief This class reduces the cost of identifying the geometry pair. +/// This is usefull for repeated shape-shape queries. +/// @note This needs to be called after `collide` or after `ComputeCollision`. +/// +/// \code +/// ComputeContactPatch calc_patch (o1, o2); +/// calc_patch(tf1, tf2, collision_result, patch_request, patch_result); +/// \endcode +class HPP_FCL_DLLAPI ComputeContactPatch { + public: + /// @brief Default constructor from two Collision Geometries. + ComputeContactPatch(const CollisionGeometry* o1, const CollisionGeometry* o2); + + void operator()(const Transform3f& tf1, const Transform3f& tf2, + const CollisionResult& collision_result, + const ContactPatchRequest& request, + ContactPatchResult& result) const; + + bool operator==(const ComputeContactPatch& other) const { + return this->o1 == other.o1 && this->o2 == other.o2 && + this->csolver == other.csolver; + } + + bool operator!=(const ComputeContactPatch& other) const { + return !(*this == other); + } + + virtual ~ComputeContactPatch() = default; + + protected: + // These pointers are made mutable to let the derived classes to update + // their values when updating the collision geometry (e.g. creating a new + // one). This feature should be used carefully to avoid any mis usage (e.g, + // changing the type of the collision geometry should be avoided). + mutable const CollisionGeometry* o1; + mutable const CollisionGeometry* o2; + + mutable ContactPatchSolver csolver; + + ContactPatchFunctionMatrix::ContactPatchFunc func; + + virtual void run(const Transform3f& tf1, const Transform3f& tf2, + const CollisionResult& collision_result, + const ContactPatchRequest& request, + ContactPatchResult& result) const; + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +} // namespace fcl +} // namespace hpp + +#endif diff --git a/include/hpp/fcl/contact_patch/contact_patch_solver.h b/include/hpp/fcl/contact_patch/contact_patch_solver.h new file mode 100644 index 000000000..ed2ae9478 --- /dev/null +++ b/include/hpp/fcl/contact_patch/contact_patch_solver.h @@ -0,0 +1,210 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, 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 INRIA 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. + */ + +/** \author Louis Montaut */ + +#ifndef HPP_FCL_CONTACT_PATCH_SOLVER_H +#define HPP_FCL_CONTACT_PATCH_SOLVER_H + +#include "hpp/fcl/collision_data.h" +#include "hpp/fcl/logging.h" +#include "hpp/fcl/narrowphase/gjk.h" + +namespace hpp { +namespace fcl { + +/// @brief Solver to compute contact patches, i.e. the intersection between two +/// contact surfaces projected onto the shapes' separating plane. +/// Otherwise said, a contact patch is simply the intersection between two +/// support sets: the support set of shape S1 in direction `n` and the support +/// set of shape S2 in direction `-n`, where `n` is the contact normal +/// (satisfying the optimality conditions of GJK/EPA). +/// @note A contact patch is **not** the support set of the Minkowski Difference +/// in the direction of the normal. +/// A contact patch is actually the support set of the Minkowski difference in +/// the direction of the normal, i.e. the instersection of the shapes support +/// sets as mentioned above. +/// +/// TODO(louis): algo improvement: +/// - The clipping algo is currently n1 * n2; it can be done in n1 + n2. +struct HPP_FCL_DLLAPI ContactPatchSolver { + // Note: `ContactPatch` is an alias for `SupportSet`. + // The two can be used interchangeably. + using ShapeSupportData = details::ShapeSupportData; + using SupportSetDirection = SupportSet::PatchDirection; + + /// @brief Support set function for shape si. + /// @param[in] shape the shape. + /// @param[in/out] support_set a support set of the shape. A support set is + /// attached to a frame. All the points of the set computed by this function + /// will be expressed in the local frame of the support set. The support set + /// is computed in the direction of the positive z-axis if its direction is + /// DEFAULT, negative z-axis if its direction is INVERTED. + /// @param[in/out] hint for the support computation of ConvexBase shapes. Gets + /// updated after calling the function onto ConvexBase shapes. + /// @param[in/out] support_data for the support computation of ConvexBase + /// shapes. Gets updated with visited vertices after calling the function onto + /// ConvexBase shapes. + /// @param[in] num_sampled_supports for shapes like cone or cylinders which + /// have smooth non-strictly convex sides (their bases are circles), we need + /// to know how many supports we sample from these sides. For any other shape, + /// this parameter is not used. + /// @param[in] tol the "thickness" of the support plane. Any point v which + /// satisfies `max_{x in shape}(x.dot(dir)) - v.dot(dir) <= tol` is tol + /// distant from the support plane and is added to the support set. + typedef void (*SupportSetFunction)(const ShapeBase* shape, + SupportSet& support_set, int& hint, + ShapeSupportData& support_data, + size_t num_sampled_supports, FCL_REAL tol); + + /// @brief Number of vectors to pre-allocate in the `m_clipping_sets` vectors. + static constexpr size_t default_num_preallocated_supports = 16; + + /// @brief Number of points sampled for Cone and Cylinder when the normal is + /// orthogonal to the shapes' basis. + /// See @ref ContactPatchRequest::m_num_samples_curved_shapes for more + /// details. + size_t num_samples_curved_shapes; + + /// @brief Tolerance below which points are added to the shapes support sets. + /// See @ref ContactPatchRequest::m_patch_tolerance for more details. + FCL_REAL patch_tolerance; + + /// @brief Support set function for shape s1. + mutable SupportSetFunction supportFuncShape1; + + /// @brief Support set function for shape s2. + mutable SupportSetFunction supportFuncShape2; + + /// @brief Temporary data to compute the support sets on each shape. + mutable std::array supports_data; + + /// @brief Guess for the support sets computation. + mutable support_func_guess_t support_guess; + + /// @brief Holder for support set of shape 1, used for internal computation. + /// After `computePatch` has been called, this support set is no longer valid. + mutable SupportSet support_set_shape1; + + /// @brief Holder for support set of shape 2, used for internal computation. + /// After `computePatch` has been called, this support set is no longer valid. + mutable SupportSet support_set_shape2; + + /// @brief Temporary support set used for the Sutherland-Hodgman algorithm. + mutable SupportSet support_set_buffer; + + /// @brief Tracks which point of the Sutherland-Hodgman result have been added + /// to the contact patch. Only used if the post-processing step occurs, i.e. + /// if the result of Sutherland-Hodgman has a size bigger than + /// `max_patch_size`. + mutable std::vector added_to_patch; + + /// @brief Default constructor. + explicit ContactPatchSolver() { + const size_t num_contact_patch = 1; + const size_t preallocated_patch_size = + ContactPatch::default_preallocated_size; + const FCL_REAL patch_tolerance = 1e-3; + const ContactPatchRequest request(num_contact_patch, + preallocated_patch_size, patch_tolerance); + this->set(request); + } + + /// @brief Construct the solver with a `ContactPatchRequest`. + explicit ContactPatchSolver(const ContactPatchRequest& request) { + this->set(request); + } + + /// @brief Set up the solver using a `ContactPatchRequest`. + void set(const ContactPatchRequest& request); + + /// @brief Sets the support guess used during support set computation of + /// shapes s1 and s2. + void setSupportGuess(const support_func_guess_t guess) const { + this->support_guess = guess; + } + + /// @brief Main API of the solver: compute a contact patch from a contact + /// between shapes s1 and s2. + /// The contact patch is the (triple) intersection between the separating + /// plane passing (by `contact.pos` and supported by `contact.normal`) and the + /// shapes s1 and s2. + template + void computePatch(const ShapeType1& s1, const Transform3f& tf1, + const ShapeType2& s2, const Transform3f& tf2, + const Contact& contact, ContactPatch& contact_patch) const; + + /// @brief Reset the internal quantities of the solver. + template + void reset(const ShapeType1& shape1, const Transform3f& tf1, + const ShapeType2& shape2, const Transform3f& tf2, + const ContactPatch& contact_patch) const; + + /// @brief Retrieve result, adds a post-processing step if result has bigger + /// size than `this->max_patch_size`. + void getResult(const Contact& contact, const ContactPatch::Polygon* result, + ContactPatch& contact_patch) const; + + /// @return the intersecting point between line defined by ray (a, b) and + /// the segment [c, d]. + /// @note we make the following hypothesis: + /// 1) c != d (should be when creating initial polytopes) + /// 2) (c, d) is not parallel to ray -> if so, we return d. + static Vec2f computeLineSegmentIntersection(const Vec2f& a, const Vec2f& b, + const Vec2f& c, const Vec2f& d); + + /// @brief Construct support set function for shape. + static SupportSetFunction makeSupportSetFunction( + const ShapeBase* shape, ShapeSupportData& support_data); + + bool operator==(const ContactPatchSolver& other) const { + return this->num_samples_curved_shapes == other.num_samples_curved_shapes && + this->patch_tolerance == other.patch_tolerance && + this->support_guess == other.support_guess && + this->support_set_shape1 == other.support_set_shape1 && + this->support_set_shape2 == other.support_set_shape2 && + this->support_set_buffer == other.support_set_buffer && + this->added_to_patch == other.added_to_patch && + this->supportFuncShape1 == other.supportFuncShape1 && + this->supportFuncShape2 == other.supportFuncShape2; + } + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +} // namespace fcl +} // namespace hpp + +#include "hpp/fcl/contact_patch/contact_patch_solver.hxx" + +#endif // HPP_FCL_CONTACT_PATCH_SOLVER_H diff --git a/include/hpp/fcl/contact_patch/contact_patch_solver.hxx b/include/hpp/fcl/contact_patch/contact_patch_solver.hxx new file mode 100644 index 000000000..7473d0da5 --- /dev/null +++ b/include/hpp/fcl/contact_patch/contact_patch_solver.hxx @@ -0,0 +1,427 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, 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 INRIA 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. + */ + +/** \author Louis Montaut */ + +#ifndef HPP_FCL_CONTACT_PATCH_SOLVER_HXX +#define HPP_FCL_CONTACT_PATCH_SOLVER_HXX + +#include "hpp/fcl/data_types.h" +#include "hpp/fcl/shape/geometric_shapes_traits.h" + +namespace hpp { +namespace fcl { + +// ============================================================================ +inline void ContactPatchSolver::set(const ContactPatchRequest& request) { + // Note: it's important for the number of pre-allocated Vec3f in + // `m_clipping_sets` to be larger than `request.max_size_patch` + // because we don't know in advance how many supports will be discarded to + // form the convex-hulls of the shapes supports which will serve as the + // input of the Sutherland-Hodgman algorithm. + size_t num_preallocated_supports = default_num_preallocated_supports; + if (num_preallocated_supports < 2 * request.getNumSamplesCurvedShapes()) { + num_preallocated_supports = 2 * request.getNumSamplesCurvedShapes(); + } + + // Used for support set computation of shape1 and for the first iterate of the + // Sutherland-Hodgman algo. + this->support_set_shape1.points().reserve(num_preallocated_supports); + this->support_set_shape1.direction = SupportSetDirection::DEFAULT; + + // Used for computing the next iterate of the Sutherland-Hodgman algo. + this->support_set_buffer.points().reserve(num_preallocated_supports); + + // Used for support set computation of shape2 and acts as the "clipper" set in + // the Sutherland-Hodgman algo. + this->support_set_shape2.points().reserve(num_preallocated_supports); + this->support_set_shape2.direction = SupportSetDirection::INVERTED; + + this->num_samples_curved_shapes = request.getNumSamplesCurvedShapes(); + this->patch_tolerance = request.getPatchTolerance(); +} + +// ============================================================================ +template +void ContactPatchSolver::computePatch(const ShapeType1& s1, + const Transform3f& tf1, + const ShapeType2& s2, + const Transform3f& tf2, + const Contact& contact, + ContactPatch& contact_patch) const { + // Note: `ContactPatch` is an alias for `SupportSet`. + // Step 1 + constructContactPatchFrameFromContact(contact, contact_patch); + contact_patch.points().clear(); + if ((bool)(shape_traits::IsStrictlyConvex) || + (bool)(shape_traits::IsStrictlyConvex)) { + // If a shape is strictly convex, the support set in any direction is + // reduced to a single point. Thus, the contact point `contact.pos` is the + // only point belonging to the contact patch, and it has already been + // computed. + // TODO(louis): even for strictly convex shapes, we can sample the support + // function around the normal and return a pseudo support set. This would + // allow spheres and ellipsoids to have a contact surface, which does make + // sense in certain physics simulation cases. + // Do the same for strictly convex regions of non-strictly convex shapes + // like the ends of capsules. + contact_patch.addPoint(contact.pos); + return; + } + + // Step 2 - Compute support set of each shape, in the direction of + // the contact's normal. + // The first shape's support set is called "current"; it will be the first + // iterate of the Sutherland-Hodgman algorithm. The second shape's support set + // is called "clipper"; it will be used to clip "current". The support set + // computation step computes a convex polygon; its vertices are ordered + // counter-clockwise. This is important as the Sutherland-Hodgman algorithm + // expects points to be ranked counter-clockwise. + this->reset(s1, tf1, s2, tf2, contact_patch); + assert(this->num_samples_curved_shapes > 3); + + this->supportFuncShape1(&s1, this->support_set_shape1, this->support_guess[0], + this->supports_data[0], + this->num_samples_curved_shapes, + this->patch_tolerance); + + this->supportFuncShape2(&s2, this->support_set_shape2, this->support_guess[1], + this->supports_data[1], + this->num_samples_curved_shapes, + this->patch_tolerance); + + // We can immediatly return if one of the support set has only + // one point. + if (this->support_set_shape1.size() <= 1 || + this->support_set_shape2.size() <= 1) { + contact_patch.addPoint(contact.pos); + return; + } + + // `eps` is be used to check strict positivity of determinants. + static constexpr FCL_REAL eps = Eigen::NumTraits::dummy_precision(); + using Polygon = SupportSet::Polygon; + + if ((this->support_set_shape1.size() == 2) && + (this->support_set_shape2.size() == 2)) { + // Segment-Segment case + // We compute the determinant; if it is non-zero, the intersection + // has already been computed: it's `Contact::pos`. + const Polygon& pts1 = this->support_set_shape1.points(); + const Vec2f& a = pts1[0]; + const Vec2f& b = pts1[1]; + + const Polygon& pts2 = this->support_set_shape2.points(); + const Vec2f& c = pts2[0]; + const Vec2f& d = pts2[1]; + + const FCL_REAL det = + (b(0) - a(0)) * (d(1) - c(1)) >= (b(1) - a(1)) * (d(0) - c(0)); + if ((std::abs(det) > eps) || ((c - d).squaredNorm() < eps) || + ((b - a).squaredNorm() < eps)) { + contact_patch.addPoint(contact.pos); + return; + } + + const Vec2f cd = (d - c); + const FCL_REAL l = cd.squaredNorm(); + Polygon& patch = contact_patch.points(); + + // Project a onto [c, d] + FCL_REAL t1 = (a - c).dot(cd); + t1 = (t1 >= l) ? 1.0 : ((t1 <= 0) ? 0.0 : (t1 / l)); + const Vec2f p1 = c + t1 * cd; + patch.emplace_back(p1); + + // Project b onto [c, d] + FCL_REAL t2 = (b - c).dot(cd); + t2 = (t2 >= l) ? 1.0 : ((t2 <= 0) ? 0.0 : (t2 / l)); + const Vec2f p2 = c + t2 * cd; + if ((p1 - p2).squaredNorm() >= eps) { + patch.emplace_back(p2); + } + return; + } + + // + // Step 3 - Main loop of the algorithm: use the "clipper" polygon to clip the + // "current" polygon. The resulting intersection is the contact patch of the + // contact between s1 and s2. "clipper" and "current" are the support sets of + // shape1 and shape2 (they can be swapped, i.e. clipper can be assigned to + // shape1 and current to shape2, depending on which case we are). Currently, + // to clip one polygon with the other, we use the Sutherland-Hodgman + // algorithm: + // https://en.wikipedia.org/wiki/Sutherland%E2%80%93Hodgman_algorithm + // In the general case, Sutherland-Hodgman clips one polygon of size >=3 using + // another polygon of size >=3. However, it can be easily extended to handle + // the segment-polygon case. + // + // The maximum size of the output of the Sutherland-Hodgman algorithm is n1 + + // n2 where n1 and n2 are the sizes of the first and second polygon. + const size_t max_result_size = + this->support_set_shape1.size() + this->support_set_shape2.size(); + if (this->added_to_patch.size() < max_result_size) { + this->added_to_patch.assign(max_result_size, false); + } + + const Polygon* clipper_ptr = nullptr; + Polygon* current_ptr = nullptr; + Polygon* previous_ptr = &(this->support_set_buffer.points()); + + // Let the clipper set be the one with the most vertices, to make sure it is + // at least a triangle. + if (this->support_set_shape1.size() < this->support_set_shape2.size()) { + current_ptr = &(this->support_set_shape1.points()); + clipper_ptr = &(this->support_set_shape2.points()); + } else { + current_ptr = &(this->support_set_shape2.points()); + clipper_ptr = &(this->support_set_shape1.points()); + } + + const Polygon& clipper = *(clipper_ptr); + const size_t clipper_size = clipper.size(); + for (size_t i = 0; i < clipper_size; ++i) { + // Swap `current` and `previous`. + // `previous` tracks the last iteration of the algorithm; `current` is + // filled by clipping `current` using `clipper`. + Polygon* tmp_ptr = previous_ptr; + previous_ptr = current_ptr; + current_ptr = tmp_ptr; + + const Polygon& previous = *(previous_ptr); + Polygon& current = *(current_ptr); + current.clear(); + + const Vec2f& a = clipper[i]; + const Vec2f& b = clipper[(i + 1) % clipper_size]; + const Vec2f ab = b - a; + + if (previous.size() == 2) { + // + // Segment-Polygon case + // + const Vec2f& p1 = previous[0]; + const Vec2f& p2 = previous[1]; + + const Vec2f ap1 = p1 - a; + const Vec2f ap2 = p2 - a; + + const FCL_REAL det1 = ab(0) * ap1(1) - ab(1) * ap1(0); + const FCL_REAL det2 = ab(0) * ap2(1) - ab(1) * ap2(0); + + if (det1 < 0 && det2 < 0) { + // Both p1 and p2 are outside the clipping polygon, i.e. there is no + // intersection. The algorithm can stop. + break; + } + + if (det1 >= 0 && det2 >= 0) { + // Both p1 and p2 are inside the clipping polygon, there is nothing to + // do; move to the next iteration. + current = previous; + continue; + } + + // Compute the intersection between the line (a, b) and the segment + // [p1, p2]. + if (det1 >= 0) { + if (det1 > eps) { + const Vec2f p = computeLineSegmentIntersection(a, b, p1, p2); + current.emplace_back(p1); + current.emplace_back(p); + continue; + } else { + // p1 is the only point of current which is also a point of the + // clipper. We can exit. + current.emplace_back(p1); + break; + } + } else { + if (det2 > eps) { + const Vec2f p = computeLineSegmentIntersection(a, b, p1, p2); + current.emplace_back(p2); + current.emplace_back(p); + continue; + } else { + // p2 is the only point of current which is also a point of the + // clipper. We can exit. + current.emplace_back(p2); + break; + } + } + } else { + // + // Polygon-Polygon case. + // + std::fill(this->added_to_patch.begin(), // + this->added_to_patch.end(), // + false); + + const size_t previous_size = previous.size(); + for (size_t j = 0; j < previous_size; ++j) { + const Vec2f& p1 = previous[j]; + const Vec2f& p2 = previous[(j + 1) % previous_size]; + + const Vec2f ap1 = p1 - a; + const Vec2f ap2 = p2 - a; + + const FCL_REAL det1 = ab(0) * ap1(1) - ab(1) * ap1(0); + const FCL_REAL det2 = ab(0) * ap2(1) - ab(1) * ap2(0); + + if (det1 < 0 && det2 < 0) { + // No intersection. Continue to next segment of previous. + continue; + } + + if (det1 >= 0 && det2 >= 0) { + // Both p1 and p2 are inside the clipping polygon, add p1 to current + // (only if it has not already been added). + if (!this->added_to_patch[j]) { + current.emplace_back(p1); + this->added_to_patch[j] = true; + } + // Continue to next segment of previous. + continue; + } + + if (det1 >= 0) { + if (det1 > eps) { + if (!this->added_to_patch[j]) { + current.emplace_back(p1); + this->added_to_patch[j] = true; + } + const Vec2f p = computeLineSegmentIntersection(a, b, p1, p2); + current.emplace_back(p); + } else { + // a, b and p1 are colinear; we add only p1. + if (!this->added_to_patch[j]) { + current.emplace_back(p1); + this->added_to_patch[j] = true; + } + } + } else { + if (det2 > eps) { + const Vec2f p = computeLineSegmentIntersection(a, b, p1, p2); + current.emplace_back(p); + } else { + if (!this->added_to_patch[(j + 1) % previous.size()]) { + current.emplace_back(p2); + this->added_to_patch[(j + 1) % previous.size()] = true; + } + } + } + } + } + // + // End of iteration i of Sutherland-Hodgman. + if (current.size() <= 1) { + // No intersection or one point found, the algo can early stop. + break; + } + } + + // Transfer the result of the Sutherland-Hodgman algorithm to the contact + // patch. + this->getResult(contact, current_ptr, contact_patch); +} + +// ============================================================================ +inline void ContactPatchSolver::getResult( + const Contact& contact, const ContactPatch::Polygon* result_ptr, + ContactPatch& contact_patch) const { + if (result_ptr->size() <= 1) { + contact_patch.addPoint(contact.pos); + return; + } + + const ContactPatch::Polygon& result = *(result_ptr); + ContactPatch::Polygon& patch = contact_patch.points(); + patch = result; +} + +// ============================================================================ +template +inline void ContactPatchSolver::reset(const ShapeType1& shape1, + const Transform3f& tf1, + const ShapeType2& shape2, + const Transform3f& tf2, + const ContactPatch& contact_patch) const { + // Reset internal quantities + this->support_set_shape1.clear(); + this->support_set_shape2.clear(); + this->support_set_buffer.clear(); + + // Get the support function of each shape + const Transform3f& tfc = contact_patch.tf; + + this->support_set_shape1.direction = SupportSetDirection::DEFAULT; + // Set the reference frame of the support set of the first shape to be the + // local frame of shape 1. + Transform3f& tf1c = this->support_set_shape1.tf; + tf1c.rotation().noalias() = tf1.rotation().transpose() * tfc.rotation(); + tf1c.translation().noalias() = + tf1.rotation().transpose() * (tfc.translation() - tf1.translation()); + this->supportFuncShape1 = + this->makeSupportSetFunction(&shape1, this->supports_data[0]); + + this->support_set_shape2.direction = SupportSetDirection::INVERTED; + // Set the reference frame of the support set of the second shape to be the + // local frame of shape 2. + Transform3f& tf2c = this->support_set_shape2.tf; + tf2c.rotation().noalias() = tf2.rotation().transpose() * tfc.rotation(); + tf2c.translation().noalias() = + tf2.rotation().transpose() * (tfc.translation() - tf2.translation()); + this->supportFuncShape2 = + this->makeSupportSetFunction(&shape2, this->supports_data[1]); +} + +// ========================================================================== +inline Vec2f ContactPatchSolver::computeLineSegmentIntersection( + const Vec2f& a, const Vec2f& b, const Vec2f& c, const Vec2f& d) { + const Vec2f ab = b - a; + const Vec2f n(-ab(1), ab(0)); + const FCL_REAL denominator = n.dot(c - d); + if (std::abs(denominator) < std::numeric_limits::epsilon()) { + return d; + } + const FCL_REAL nominator = n.dot(a - d); + FCL_REAL alpha = nominator / denominator; + alpha = std::min(1.0, std::max(0.0, alpha)); + return alpha * c + (1 - alpha) * d; +} + +} // namespace fcl +} // namespace hpp + +#endif // HPP_FCL_CONTACT_PATCH_SOLVER_HXX diff --git a/include/hpp/fcl/contact_patch_func_matrix.h b/include/hpp/fcl/contact_patch_func_matrix.h new file mode 100644 index 000000000..2eebb96af --- /dev/null +++ b/include/hpp/fcl/contact_patch_func_matrix.h @@ -0,0 +1,87 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, 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 INRIA 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. + */ + +/** \author Louis Montaut */ + +#ifndef HPP_FCL_CONTACT_PATCH_FUNC_MATRIX_H +#define HPP_FCL_CONTACT_PATCH_FUNC_MATRIX_H + +#include "hpp/fcl/collision_data.h" +#include "hpp/fcl/contact_patch/contact_patch_solver.h" +#include "hpp/fcl/narrowphase/narrowphase.h" + +namespace hpp { +namespace fcl { + +/// @brief The contact patch matrix stores the functions for contact patches +/// computation between different types of objects and provides a uniform call +/// interface +struct HPP_FCL_DLLAPI ContactPatchFunctionMatrix { + /// @brief the uniform call interface for computing contact patches: we need + /// know + /// 1. two objects o1 and o2 and their configuration in world coordinate tf1 + /// and tf2; + /// 2. the collision result that generated contact patches candidates + /// (`hpp::fcl::Contact`), from which contact patches will be expanded; + /// 3. the narrow phase solver that was used to compute the collision result; + /// 4. the solver for computation of contact patches; + /// 5. the request setting for contact patches (e.g. maximum amoung and size + /// of contact patches); + /// 6. the structure to return contact patches. + /// + /// Note: we pass a GJKSolver, because it allows to reuse internal computation + /// that was made during the narrow phase. It also allows to experiment with + /// different ways to compute contact patches. We could, for example, perturb + /// tf1 and tf2 and make multiple calls to the GJKSolver (although this is not + /// the approach done by default). + typedef void (*ContactPatchFunc)(const CollisionGeometry* o1, + const Transform3f& tf1, + const CollisionGeometry* o2, + const Transform3f& tf2, + const CollisionResult& collision_result, + const ContactPatchSolver* csolver, + const ContactPatchRequest& request, + ContactPatchResult& result); + + /// @brief Each item in the contact patch matrix is a function to handle + /// contact patch computation between objects of type1 and type2. + ContactPatchFunc contact_patch_matrix[NODE_COUNT][NODE_COUNT]; + + ContactPatchFunctionMatrix(); +}; + +} // namespace fcl +} // namespace hpp + +#endif diff --git a/include/hpp/fcl/data_types.h b/include/hpp/fcl/data_types.h index cb8a0a128..bcbb3c9a3 100644 --- a/include/hpp/fcl/data_types.h +++ b/include/hpp/fcl/data_types.h @@ -65,10 +65,12 @@ namespace hpp { namespace fcl { typedef double FCL_REAL; typedef Eigen::Matrix Vec3f; +typedef Eigen::Matrix Vec2f; typedef Eigen::Matrix Vec6f; typedef Eigen::Matrix VecXf; typedef Eigen::Matrix Matrix3f; typedef Eigen::Matrix Matrixx3f; +typedef Eigen::Matrix Matrixx2f; typedef Eigen::Matrix Matrixx3i; typedef Eigen::Matrix MatrixXf; diff --git a/include/hpp/fcl/distance.h b/include/hpp/fcl/distance.h index 0c501a2bd..c0c456494 100644 --- a/include/hpp/fcl/distance.h +++ b/include/hpp/fcl/distance.h @@ -64,30 +64,6 @@ HPP_FCL_DLLAPI FCL_REAL distance(const CollisionGeometry* o1, const DistanceRequest& request, DistanceResult& result); -/// @copydoc distance(const CollisionObject*, const CollisionObject*, const -/// DistanceRequest&, DistanceResult&) \note this function update the initial -/// guess of \c request if requested. -/// See QueryRequest::updateGuess -inline FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2, - DistanceRequest& request, DistanceResult& result) { - FCL_REAL res = distance(o1, o2, (const DistanceRequest&)request, result); - request.updateGuess(result); - return res; -} - -/// @copydoc distance(const CollisionObject*, const CollisionObject*, const -/// DistanceRequest&, DistanceResult&) \note this function update the initial -/// guess of \c request if requested. -/// See QueryRequest::updateGuess -inline FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - DistanceRequest& request, DistanceResult& result) { - FCL_REAL res = - distance(o1, tf1, o2, tf2, (const DistanceRequest&)request, result); - request.updateGuess(result); - return res; -} - /// This class reduces the cost of identifying the geometry pair. /// This is mostly useful for repeated shape-shape queries. /// @@ -103,15 +79,6 @@ class HPP_FCL_DLLAPI ComputeDistance { const DistanceRequest& request, DistanceResult& result) const; - inline FCL_REAL operator()(const Transform3f& tf1, const Transform3f& tf2, - DistanceRequest& request, - DistanceResult& result) const { - FCL_REAL res = operator()(tf1, tf2, (const DistanceRequest&)request, - result); - request.updateGuess(result); - return res; - } - bool operator==(const ComputeDistance& other) const { return o1 == other.o1 && o2 == other.o2 && swap_geoms == other.swap_geoms && solver == other.solver && func == other.func; diff --git a/include/hpp/fcl/internal/shape_shape_contact_patch_func.h b/include/hpp/fcl/internal/shape_shape_contact_patch_func.h new file mode 100644 index 000000000..33c5b9791 --- /dev/null +++ b/include/hpp/fcl/internal/shape_shape_contact_patch_func.h @@ -0,0 +1,266 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, 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. + */ + +/** \author Louis Montaut */ + +#ifndef HPP_FCL_INTERNAL_SHAPE_SHAPE_CONTACT_PATCH_FUNC_H +#define HPP_FCL_INTERNAL_SHAPE_SHAPE_CONTACT_PATCH_FUNC_H + +#include "hpp/fcl/collision_data.h" +#include "hpp/fcl/collision_utility.h" +#include "hpp/fcl/narrowphase/narrowphase.h" +#include "hpp/fcl/contact_patch/contact_patch_solver.h" +#include "hpp/fcl/shape/geometric_shapes_traits.h" + +namespace hpp { +namespace fcl { + +/// @brief Shape-shape contact patch computation. +/// Assumes that `csolver` and the `ContactPatchResult` have already been set up +/// by the `ContactPatchRequest`. +template +struct ComputeShapeShapeContactPatch { + static void run(const CollisionGeometry* o1, const Transform3f& tf1, + const CollisionGeometry* o2, const Transform3f& tf2, + const CollisionResult& collision_result, + const ContactPatchSolver* csolver, + const ContactPatchRequest& request, + ContactPatchResult& result) { + // Note: see specializations for Plane and Halfspace below. + if (!collision_result.isCollision()) { + return; + } + HPP_FCL_ASSERT( + result.check(request), + "The contact patch result and request are incompatible (issue of " + "contact patch size or maximum number of contact patches). Make sure " + "result is initialized with request.", + std::logic_error); + + const ShapeType1& s1 = static_cast(*o1); + const ShapeType2& s2 = static_cast(*o2); + for (size_t i = 0; i < collision_result.numContacts(); ++i) { + if (i >= request.max_num_patch) { + break; + } + csolver->setSupportGuess(collision_result.cached_support_func_guess); + const Contact& contact = collision_result.getContact(i); + ContactPatch& contact_patch = result.getUnusedContactPatch(); + csolver->computePatch(s1, tf1, s2, tf2, contact, contact_patch); + } + } +}; + +/// @brief Computes the contact patch between a Plane/Halfspace and another +/// shape. +/// @tparam InvertShapes set to true if the first shape of the collision pair +/// is s2 and not s1 (if you had to invert (s1, tf1) and (s2, tf2) when calling +/// this function). +template +void computePatchPlaneOrHalfspace(const OtherShapeType& s1, + const Transform3f& tf1, + const PlaneOrHalfspace& s2, + const Transform3f& tf2, + const ContactPatchSolver* csolver, + const Contact& contact, + ContactPatch& contact_patch) { + HPP_FCL_UNUSED_VARIABLE(s2); + HPP_FCL_UNUSED_VARIABLE(tf2); + constructContactPatchFrameFromContact(contact, contact_patch); + if ((bool)(shape_traits::IsStrictlyConvex)) { + // Only one point of contact; it has already been computed. + contact_patch.addPoint(contact.pos); + return; + } + + // We only need to compute the support set in the direction of the normal. + // We need to temporarily express the patch in the local frame of shape1. + SupportSet& support_set = csolver->support_set_shape1; + support_set.tf.rotation().noalias() = + tf1.rotation().transpose() * contact_patch.tf.rotation(); + support_set.tf.translation().noalias() = + tf1.rotation().transpose() * + (contact_patch.tf.translation() - tf1.translation()); + + // Note: for now, taking into account swept-sphere radius does not change + // anything to the support set computations. However it will be used in the + // future if we want to store the offsets to the support plane for each point + // in a support set. + using SupportOptions = details::SupportOptions; + if (InvertShapes) { + support_set.direction = ContactPatch::PatchDirection::INVERTED; + details::getShapeSupportSet( + &s1, support_set, csolver->support_guess[1], csolver->supports_data[1], + csolver->num_samples_curved_shapes, csolver->patch_tolerance); + } else { + support_set.direction = ContactPatch::PatchDirection::DEFAULT; + details::getShapeSupportSet( + &s1, support_set, csolver->support_guess[0], csolver->supports_data[0], + csolver->num_samples_curved_shapes, csolver->patch_tolerance); + } + csolver->getResult(contact, &(support_set.points()), contact_patch); +} + +#define PLANE_OR_HSPACE_AND_OTHER_SHAPE_CONTACT_PATCH(PlaneOrHspace) \ + template \ + struct ComputeShapeShapeContactPatch { \ + static void run(const CollisionGeometry* o1, const Transform3f& tf1, \ + const CollisionGeometry* o2, const Transform3f& tf2, \ + const CollisionResult& collision_result, \ + const ContactPatchSolver* csolver, \ + const ContactPatchRequest& request, \ + ContactPatchResult& result) { \ + if (!collision_result.isCollision()) { \ + return; \ + } \ + HPP_FCL_ASSERT( \ + result.check(request), \ + "The contact patch result and request are incompatible (issue of " \ + "contact patch size or maximum number of contact patches). Make " \ + "sure " \ + "result is initialized with request.", \ + std::logic_error); \ + \ + const OtherShapeType& s1 = static_cast(*o1); \ + const PlaneOrHspace& s2 = static_cast(*o2); \ + for (size_t i = 0; i < collision_result.numContacts(); ++i) { \ + if (i >= request.max_num_patch) { \ + break; \ + } \ + csolver->setSupportGuess(collision_result.cached_support_func_guess); \ + const Contact& contact = collision_result.getContact(i); \ + ContactPatch& contact_patch = result.getUnusedContactPatch(); \ + computePatchPlaneOrHalfspace( \ + s1, tf1, s2, tf2, csolver, contact, contact_patch); \ + } \ + } \ + }; \ + \ + template \ + struct ComputeShapeShapeContactPatch { \ + static void run(const CollisionGeometry* o1, const Transform3f& tf1, \ + const CollisionGeometry* o2, const Transform3f& tf2, \ + const CollisionResult& collision_result, \ + const ContactPatchSolver* csolver, \ + const ContactPatchRequest& request, \ + ContactPatchResult& result) { \ + if (!collision_result.isCollision()) { \ + return; \ + } \ + HPP_FCL_ASSERT( \ + result.check(request), \ + "The contact patch result and request are incompatible (issue of " \ + "contact patch size or maximum number of contact patches). Make " \ + "sure " \ + "result is initialized with request.", \ + std::logic_error); \ + \ + const PlaneOrHspace& s1 = static_cast(*o1); \ + const OtherShapeType& s2 = static_cast(*o2); \ + for (size_t i = 0; i < collision_result.numContacts(); ++i) { \ + if (i >= request.max_num_patch) { \ + break; \ + } \ + csolver->setSupportGuess(collision_result.cached_support_func_guess); \ + const Contact& contact = collision_result.getContact(i); \ + ContactPatch& contact_patch = result.getUnusedContactPatch(); \ + computePatchPlaneOrHalfspace( \ + s2, tf2, s1, tf1, csolver, contact, contact_patch); \ + } \ + } \ + }; + +PLANE_OR_HSPACE_AND_OTHER_SHAPE_CONTACT_PATCH(Plane); +PLANE_OR_HSPACE_AND_OTHER_SHAPE_CONTACT_PATCH(Halfspace); + +#define PLANE_HSPACE_CONTACT_PATCH(PlaneOrHspace1, PlaneOrHspace2) \ + template <> \ + struct ComputeShapeShapeContactPatch { \ + static void run(const CollisionGeometry* o1, const Transform3f& tf1, \ + const CollisionGeometry* o2, const Transform3f& tf2, \ + const CollisionResult& collision_result, \ + const ContactPatchSolver* csolver, \ + const ContactPatchRequest& request, \ + ContactPatchResult& result) { \ + HPP_FCL_UNUSED_VARIABLE(o1); \ + HPP_FCL_UNUSED_VARIABLE(tf1); \ + HPP_FCL_UNUSED_VARIABLE(o2); \ + HPP_FCL_UNUSED_VARIABLE(tf2); \ + HPP_FCL_UNUSED_VARIABLE(csolver); \ + if (!collision_result.isCollision()) { \ + return; \ + } \ + HPP_FCL_ASSERT( \ + result.check(request), \ + "The contact patch result and request are incompatible (issue of " \ + "contact patch size or maximum number of contact patches). Make " \ + "sure " \ + "result is initialized with request.", \ + std::logic_error); \ + \ + for (size_t i = 0; i < collision_result.numContacts(); ++i) { \ + if (i >= request.max_num_patch) { \ + break; \ + } \ + const Contact& contact = collision_result.getContact(i); \ + ContactPatch& contact_patch = result.getUnusedContactPatch(); \ + constructContactPatchFrameFromContact(contact, contact_patch); \ + contact_patch.addPoint(contact.pos); \ + } \ + } \ + }; + +PLANE_HSPACE_CONTACT_PATCH(Plane, Plane); +PLANE_HSPACE_CONTACT_PATCH(Plane, Halfspace); +PLANE_HSPACE_CONTACT_PATCH(Halfspace, Plane); +PLANE_HSPACE_CONTACT_PATCH(Halfspace, Halfspace); + +#undef PLANE_OR_HSPACE_AND_OTHER_SHAPE_CONTACT_PATCH +#undef PLANE_HSPACE_CONTACT_PATCH + +template +void ShapeShapeContactPatch(const CollisionGeometry* o1, const Transform3f& tf1, + const CollisionGeometry* o2, const Transform3f& tf2, + const CollisionResult& collision_result, + const ContactPatchSolver* csolver, + const ContactPatchRequest& request, + ContactPatchResult& result) { + return ComputeShapeShapeContactPatch::run( + o1, tf1, o2, tf2, collision_result, csolver, request, result); +} + +} // namespace fcl +} // namespace hpp + +#endif diff --git a/include/hpp/fcl/internal/shape_shape_func.h b/include/hpp/fcl/internal/shape_shape_func.h index d9487f672..43643712a 100644 --- a/include/hpp/fcl/internal/shape_shape_func.h +++ b/include/hpp/fcl/internal/shape_shape_func.h @@ -59,7 +59,7 @@ struct ShapeShapeDistancer { // Witness points on shape1 and shape2, normal pointing from shape1 to // shape2. Vec3f p1, p2, normal; - FCL_REAL distance = ShapeShapeDistancer::run( + const FCL_REAL distance = ShapeShapeDistancer::run( o1, tf1, o2, tf2, nsolver, request.enable_signed_distance, p1, p2, normal); @@ -145,7 +145,7 @@ struct ShapeShapeCollider { o1, tf1, o2, tf2, nsolver, compute_penetration, p1, p2, normal); size_t num_contacts = 0; - FCL_REAL distToCollision = distance - request.security_margin; + const FCL_REAL distToCollision = distance - request.security_margin; internal::updateDistanceLowerBoundFromLeaf(request, result, distToCollision, p1, p2, normal); @@ -306,6 +306,7 @@ SHAPE_SELF_DISTANCE_SPECIALIZATION(Plane); SHAPE_SELF_DISTANCE_SPECIALIZATION(Halfspace); #undef SHAPE_SHAPE_DISTANCE_SPECIALIZATION +#undef SHAPE_SELF_DISTANCE_SPECIALIZATION } // namespace fcl } // namespace hpp diff --git a/include/hpp/fcl/math/transform.h b/include/hpp/fcl/math/transform.h index 8b6bf0a3d..0ebae40ef 100644 --- a/include/hpp/fcl/math/transform.h +++ b/include/hpp/fcl/math/transform.h @@ -154,6 +154,12 @@ class HPP_FCL_DLLAPI Transform3f { return R * v + T; } + /// @brief transform a given vector by the inverse of the transform + template + inline Vec3f inverseTransform(const Eigen::MatrixBase& v) const { + return R.transpose() * (v - T); + } + /// @brief inverse transform inline Transform3f& inverseInPlace() { R.transposeInPlace(); @@ -211,6 +217,16 @@ inline Quatf fromAxisAngle(const Eigen::MatrixBase& axis, return Quatf(Eigen::AngleAxis(angle, axis)); } +/// @brief Construct othonormal basis from vector. +/// The z-axis is the normalized input vector. +inline Matrix3f constructOrthonormalBasisFromVector(const Vec3f& vec) { + Matrix3f basis = Matrix3f::Zero(); + basis.col(2) = vec.normalized(); + basis.col(1) = -vec.unitOrthogonal(); + basis.col(0) = basis.col(1).cross(vec); + return basis; +} + } // namespace fcl } // namespace hpp diff --git a/include/hpp/fcl/narrowphase/gjk.h b/include/hpp/fcl/narrowphase/gjk.h index 656979ba1..206109ef8 100644 --- a/include/hpp/fcl/narrowphase/gjk.h +++ b/include/hpp/fcl/narrowphase/gjk.h @@ -41,160 +41,13 @@ #include -#include -#include +#include "hpp/fcl/narrowphase/minkowski_difference.h" namespace hpp { namespace fcl { namespace details { -/// @brief Options for the computation of support points. -/// `NoSweptSphere` option is used when the support function is called -/// by GJK or EPA. In this case, the swept sphere radius is not taken into -/// account in the support function. It is used by GJK and EPA after they have -/// converged to correct the solution. -/// `WithSweptSphere` option is used when the support function is called -/// directly by the user. In this case, the swept sphere radius is taken into -/// account in the support function. -enum SupportOptions { - NoSweptSphere = 0, - WithSweptSphere = 0x1, -}; - -/// @brief the support function for shape. -/// @return argmax_{v in shape0} v.dot(dir). -/// @param shape the shape. -/// @param dir support direction. -/// @param hint used to initialize the search when shape is a ConvexBase object. -/// @tparam SupportOptions is a value of the SupportOptions enum. If set to -/// `WithSweptSphere`, the support functions take into account the shapes' swept -/// sphere radii. Please see `MinkowskiDiff::set(const ShapeBase*, const -/// ShapeBase*)` for more details. -template -Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, int& hint); - -/// @brief Minkowski difference class of two shapes -/// -/// @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]; - - struct ShapeData { - std::vector visited; - Vec3f last_dir = Vec3f::Zero(); - }; - - /// @brief Store temporary data for the computation of the support point for - /// each shape. - ShapeData data[2]; - - /// @brief rotation from shape1 to shape0 - /// such that @f$ p_in_0 = oR1 * p_in_1 + ot1 @f$. - Matrix3f oR1; - - /// @brief translation from shape1 to shape0 - /// such that @f$ p_in_0 = oR1 * p_in_1 + ot1 @f$. - Vec3f ot1; - - /// @brief The radii of the sphere swepted around each shape of the Minkowski - /// difference. The 2 values correspond to the swept-sphere radius of shape 0 - /// and shape 1. - Array2d swept_sphere_radius; - - /// @brief Wether or not to use the normalize heuristic in the GJK Nesterov - /// acceleration. This setting is only applied if the Nesterov acceleration in - /// the GJK class is active. - bool normalize_support_direction; - - typedef void (*GetSupportFunction)(const MinkowskiDiff& minkowskiDiff, - const Vec3f& dir, Vec3f& support0, - Vec3f& support1, - support_func_guess_t& hint, - ShapeData data[2]); - GetSupportFunction getSupportFunc; - - MinkowskiDiff() : normalize_support_direction(false), getSupportFunc(NULL) {} - - /// @brief Set the two shapes, assuming the relative transformation between - /// them is identity. - /// @param shape0 the first shape. - /// @param shape1 the second shape. - /// @tparam SupportOptions is a value of the SupportOptions enum. If set to - /// `WithSweptSphere`, the support computation will take into account the - /// swept sphere radius of the shapes. If set to `NoSweptSphere`, where - /// this information is simply stored in the Minkowski's difference - /// `swept_sphere_radius` array. This array is then used to correct the - /// solution found when GJK or EPA have converged. - /// - /// @note In practice, there is no need to take into account the swept-sphere - /// radius in the iterations of GJK/EPA. It is in fact detrimental to the - /// convergence of both algos. This is because it makes corners and edges of - /// shapes look strictly convex to the algorithms, which leads to poor - /// convergence. This swept sphere template parameter is only here for - /// debugging purposes and for specific uses cases where the swept sphere - /// radius is needed in the support function. The rule is simple. When - /// interacting with GJK/EPA, the `SupportOptions` template - /// parameter should **always** be set to `NoSweptSphere` (except for - /// debugging or testing purposes). When working directly with the shapes - /// involved in the collision, and not relying on GJK/EPA, the - /// `SupportOptions` template parameter should be set to `WithSweptSphere`. - /// This is for example the case for specialized collision/distance functions. - template - void set(const ShapeBase* shape0, const ShapeBase* shape1); - - /// @brief Set the two shapes, with a relative transformation. - /// @param shape0 the first shape. - /// @param shape1 the second shape. - /// @param tf0 the transformation of the first shape. - /// @param tf1 the transformation of the second shape. - /// @tparam `SupportOptions` see `set(const ShapeBase*, const - /// ShapeBase*)` for more details. - template - void set(const ShapeBase* shape0, const ShapeBase* shape1, - const Transform3f& tf0, const Transform3f& tf1); - - /// @brief support function for shape0. - /// @return argmax_{v in shape0} v.dot(dir). - /// @param dir support direction. - /// @param hint used to initialize the search when shape is a ConvexBase - /// object. - /// @tparam `SupportOptions` see `set(const ShapeBase*, const - /// ShapeBase*)` for more details. - template - inline Vec3f support0(const Vec3f& dir, int& hint) const { - return getSupport<_SupportOptions>(shapes[0], dir, hint); - } - - /// @brief support function for shape1. - /// @return argmax_{v in shape0} v.dot(dir). - /// @param dir support direction. - /// @param hint used to initialize the search when shape is a ConvexBase - /// object. - /// @tparam `SupportOptions` see `set(const ShapeBase*, const - /// ShapeBase*)` for more details. - template - inline Vec3f support1(const Vec3f& dir, int& hint) const { - // clang-format off - return oR1 * getSupport<_SupportOptions>(shapes[1], oR1.transpose() * dir, hint) + ot1; - // clang-format on - } - - /// @brief Support function for the pair of shapes. This method assumes `set` - /// has already been called. - /// \param hint used to initialize the search when shape is a ConvexBase - /// object. - inline void support(const Vec3f& dir, Vec3f& supp0, Vec3f& supp1, - support_func_guess_t& hint) const { - assert(getSupportFunc != NULL); - getSupportFunc(*this, dir, supp0, supp1, hint, - const_cast(data)); - } -}; - /// @brief class for GJK algorithm /// /// @note The computations are performed in the frame of the first shape. @@ -491,6 +344,7 @@ struct HPP_FCL_DLLAPI EPA { Status status; GJK::Simplex result; Vec3f normal; + support_func_guess_t support_hint; FCL_REAL depth; SimplexFace* closest_face; diff --git a/include/hpp/fcl/narrowphase/minkowski_difference.h b/include/hpp/fcl/narrowphase/minkowski_difference.h new file mode 100644 index 000000000..f6c0f93f4 --- /dev/null +++ b/include/hpp/fcl/narrowphase/minkowski_difference.h @@ -0,0 +1,188 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2015, Open Source Robotics Foundation + * Copyright (c) 2021-2024, 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. + */ + +/** \authors Jia Pan, Florent Lamiraux, Josef Mirabel, Louis Montaut */ + +#ifndef HPP_FCL_MINKOWSKI_DIFFERENCE_H +#define HPP_FCL_MINKOWSKI_DIFFERENCE_H + +#include "hpp/fcl/shape/geometric_shapes.h" +#include "hpp/fcl/math/transform.h" +#include "hpp/fcl/narrowphase/support_functions.h" + +namespace hpp { +namespace fcl { + +namespace details { + +/// @brief Minkowski difference class of two shapes +/// +/// @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]; + + /// @brief Store temporary data for the computation of the support point for + /// each shape. + ShapeSupportData data[2]; + + /// @brief rotation from shape1 to shape0 + /// such that @f$ p_in_0 = oR1 * p_in_1 + ot1 @f$. + Matrix3f oR1; + + /// @brief translation from shape1 to shape0 + /// such that @f$ p_in_0 = oR1 * p_in_1 + ot1 @f$. + Vec3f ot1; + + /// @brief The radii of the sphere swepted around each shape of the Minkowski + /// difference. The 2 values correspond to the swept-sphere radius of shape 0 + /// and shape 1. + Array2d swept_sphere_radius; + + /// @brief Wether or not to use the normalize heuristic in the GJK Nesterov + /// acceleration. This setting is only applied if the Nesterov acceleration in + /// the GJK class is active. + bool normalize_support_direction; + + typedef void (*GetSupportFunction)(const MinkowskiDiff& minkowskiDiff, + const Vec3f& dir, Vec3f& support0, + Vec3f& support1, + support_func_guess_t& hint, + ShapeSupportData data[2]); + GetSupportFunction getSupportFunc; + + MinkowskiDiff() : normalize_support_direction(false), getSupportFunc(NULL) {} + + /// @brief Set the two shapes, assuming the relative transformation between + /// them is identity. + /// Consequently, all support points computed with the MinkowskiDiff + /// will be expressed in the world frame. + /// @param shape0 the first shape. + /// @param shape1 the second shape. + /// @tparam SupportOptions is a value of the SupportOptions enum. If set to + /// `WithSweptSphere`, the support computation will take into account the + /// swept sphere radius of the shapes. If set to `NoSweptSphere`, where + /// this information is simply stored in the Minkowski's difference + /// `swept_sphere_radius` array. This array is then used to correct the + /// solution found when GJK or EPA have converged. + /// + /// @note In practice, there is no need to take into account the swept-sphere + /// radius in the iterations of GJK/EPA. It is in fact detrimental to the + /// convergence of both algos. This is because it makes corners and edges of + /// shapes look strictly convex to the algorithms, which leads to poor + /// convergence. This swept sphere template parameter is only here for + /// debugging purposes and for specific uses cases where the swept sphere + /// radius is needed in the support function. The rule is simple. When + /// interacting with GJK/EPA, the `SupportOptions` template + /// parameter should **always** be set to `NoSweptSphere` (except for + /// debugging or testing purposes). When working directly with the shapes + /// involved in the collision, and not relying on GJK/EPA, the + /// `SupportOptions` template parameter should be set to `WithSweptSphere`. + /// This is for example the case for specialized collision/distance functions. + template + void set(const ShapeBase* shape0, const ShapeBase* shape1); + + /// @brief Set the two shapes, with a relative transformation from shape0 to + /// shape1. Consequently, all support points computed with the MinkowskiDiff + /// will be expressed in the frame of shape0. + /// @param shape0 the first shape. + /// @param shape1 the second shape. + /// @param tf0 the transformation of the first shape. + /// @param tf1 the transformation of the second shape. + /// @tparam `SupportOptions` see `set(const ShapeBase*, const + /// ShapeBase*)` for more details. + template + void set(const ShapeBase* shape0, const ShapeBase* shape1, + const Transform3f& tf0, const Transform3f& tf1); + + /// @brief support function for shape0. + /// The output vector is expressed in the local frame of shape0. + /// @return a point which belongs to the set {argmax_{v in shape0} + /// v.dot(dir)}, i.e a support of shape0 in direction dir. + /// @param dir support direction. + /// @param hint used to initialize the search when shape is a ConvexBase + /// object. + /// @tparam `SupportOptions` see `set(const ShapeBase*, const + /// ShapeBase*)` for more details. + template + inline Vec3f support0(const Vec3f& dir, int& hint) const { + return getSupport<_SupportOptions>(shapes[0], dir, hint); + } + + /// @brief support function for shape1. + /// The output vector is expressed in the local frame of shape0. + /// This is mandatory because in the end we are interested in support points + /// of the Minkowski difference; hence the supports of shape0 and shape1 must + /// live in the same frame. + /// @return a point which belongs to the set {tf * argmax_{v in shape1} + /// v.dot(R^T * dir)}, i.e. the support of shape1 in direction dir (tf is the + /// tranform from shape1 to shape0). + /// @param dir support direction. + /// @param hint used to initialize the search when shape is a ConvexBase. + /// @tparam `SupportOptions` see `set(const ShapeBase*, const + /// ShapeBase*)` for more details. + template + inline Vec3f support1(const Vec3f& dir, int& hint) const { + // clang-format off + return oR1 * getSupport<_SupportOptions>(shapes[1], oR1.transpose() * dir, hint) + ot1; + // clang-format on + } + + /// @brief Support function for the pair of shapes. This method assumes `set` + /// has already been called. + /// @param[in] dir the support direction. + /// @param[out] supp0 support of shape0 in direction dir, expressed in the + /// frame of shape0. + /// @param[out] supp1 support of shape1 in direction -dir, expressed in the + /// frame of shape0. + /// @param[in/out] hint used to initialize the search when shape is a + /// ConvexBase object. + inline void support(const Vec3f& dir, Vec3f& supp0, Vec3f& supp1, + support_func_guess_t& hint) const { + assert(getSupportFunc != NULL); + getSupportFunc(*this, dir, supp0, supp1, hint, + const_cast(data)); + } +}; + +} // namespace details + +} // namespace fcl +} // namespace hpp + +#endif // HPP_FCL_MINKOWSKI_DIFFERENCE_H diff --git a/include/hpp/fcl/narrowphase/narrowphase.h b/include/hpp/fcl/narrowphase/narrowphase.h index 8b63e086f..ff9a5785b 100644 --- a/include/hpp/fcl/narrowphase/narrowphase.h +++ b/include/hpp/fcl/narrowphase/narrowphase.h @@ -50,8 +50,11 @@ namespace hpp { namespace fcl { -/// @brief collision and distance solver based on GJK algorithm implemented in -/// fcl (rewritten the code from the GJK in bullet) +/// @brief collision and distance solver based on the GJK and EPA algorithms. +/// Originally, GJK and EPA were implemented in fcl which itself took +/// inspiration from the code of the GJK in bullet. Since then, both GJK and EPA +/// have been largely modified to be faster and more robust to numerical +/// accuracy and edge cases. struct HPP_FCL_DLLAPI GJKSolver { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW @@ -104,6 +107,11 @@ struct HPP_FCL_DLLAPI GJKSolver { /// @brief Minkowski difference used by GJK and EPA algorithms mutable details::MinkowskiDiff minkowski_difference; + private: + // Used internally for assertion checks. + static constexpr FCL_REAL m_dummy_precision = 1e-6; + + public: HPP_FCL_COMPILER_DIAGNOSTIC_PUSH HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS /// @brief Default constructor for GJK algorithm @@ -207,7 +215,6 @@ struct HPP_FCL_DLLAPI GJKSolver { // --------------------- // GJK settings this->gjk_initial_guess = request.gjk_initial_guess; - // TODO: use gjk_initial_guess instead this->enable_cached_guess = request.enable_cached_gjk_guess; if (this->gjk_initial_guess == GJKInitialGuess::CachedGuess || this->enable_cached_guess) { @@ -347,14 +354,16 @@ struct HPP_FCL_DLLAPI GJKSolver { void getGJKInitialGuess(const S1& s1, const S2& s2, Vec3f& guess, support_func_guess_t& support_hint, const Vec3f& default_guess = Vec3f(1, 0, 0)) const { + // There is no reason not to warm-start the support function, so we always + // do it. + support_hint = this->support_func_cached_guess; + // The following switch takes care of the GJK warm-start. switch (gjk_initial_guess) { case GJKInitialGuess::DefaultGuess: guess = default_guess; - support_hint.setZero(); break; case GJKInitialGuess::CachedGuess: guess = this->cached_guess; - support_hint = this->support_func_cached_guess; break; case GJKInitialGuess::BoundingVolumeGuess: if (s1.aabb_local.volume() < 0 || s2.aabb_local.volume() < 0) { @@ -368,7 +377,6 @@ struct HPP_FCL_DLLAPI GJKSolver { s1.aabb_local.center() - (this->minkowski_difference.oR1 * s2.aabb_local.center() + this->minkowski_difference.ot1); - support_hint.setZero(); break; default: HPP_FCL_THROW_PRETTY("Wrong initial guess for GJK.", std::logic_error); @@ -378,7 +386,6 @@ struct HPP_FCL_DLLAPI GJKSolver { HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS if (this->enable_cached_guess) { guess = this->cached_guess; - support_hint = this->support_func_cached_guess; } HPP_FCL_COMPILER_DIAGNOSTIC_POP } @@ -437,23 +444,13 @@ struct HPP_FCL_DLLAPI GJKSolver { support_hint); this->gjk.evaluate(this->minkowski_difference, guess, support_hint); - HPP_FCL_COMPILER_DIAGNOSTIC_PUSH - HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS - if (this->gjk_initial_guess == GJKInitialGuess::CachedGuess || - this->enable_cached_guess) { - this->cached_guess = this->gjk.getGuessFromSimplex(); - this->support_func_cached_guess = this->gjk.support_hint; - } - HPP_FCL_COMPILER_DIAGNOSTIC_POP - - const FCL_REAL dummy_precision = - 3 * std::sqrt(std::numeric_limits::epsilon()); - HPP_FCL_UNUSED_VARIABLE(dummy_precision); switch (this->gjk.status) { case details::GJK::DidNotRun: HPP_FCL_ASSERT(false, "GJK did not run. It should have!", std::logic_error); + this->cached_guess = Vec3f(1, 0, 0); + this->support_func_cached_guess.setZero(); distance = -(std::numeric_limits::max)(); p1 = p2 = normal = Vec3f::Constant(std::numeric_limits::quiet_NaN()); @@ -471,11 +468,11 @@ struct HPP_FCL_DLLAPI GJKSolver { // The two witness points have no meaning. GJKEarlyStopExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal); - HPP_FCL_ASSERT( - distance >= this->gjk.distance_upper_bound - dummy_precision, - "The distance should be bigger than GJK's " - "`distance_upper_bound`.", - std::logic_error); + HPP_FCL_ASSERT(distance >= this->gjk.distance_upper_bound - + this->m_dummy_precision, + "The distance should be bigger than GJK's " + "`distance_upper_bound`.", + std::logic_error); break; case details::GJK::NoCollision: // @@ -484,7 +481,7 @@ struct HPP_FCL_DLLAPI GJKSolver { // 1e-6). GJKExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal); HPP_FCL_ASSERT(std::abs((p1 - p2).norm() - distance) <= - this->gjk.getTolerance() + dummy_precision, + this->gjk.getTolerance() + this->m_dummy_precision, "The distance found by GJK should coincide with the " "distance between the closest points.", std::logic_error); @@ -494,10 +491,11 @@ struct HPP_FCL_DLLAPI GJKSolver { // their distance is below GJK's tolerance (default 1e-6). case details::GJK::CollisionWithPenetrationInformation: GJKExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal); - HPP_FCL_ASSERT(distance <= this->gjk.getTolerance() + dummy_precision, - "The distance found by GJK should be negative or at " - "least below GJK's tolerance.", - std::logic_error); + HPP_FCL_ASSERT( + distance <= this->gjk.getTolerance() + this->m_dummy_precision, + "The distance found by GJK should be negative or at " + "least below GJK's tolerance.", + std::logic_error); break; case details::GJK::Collision: if (!compute_penetration) { @@ -546,7 +544,7 @@ struct HPP_FCL_DLLAPI GJKSolver { case details::EPA::Valid: case details::EPA::AccuracyReached: HPP_FCL_ASSERT( - -epa.depth <= epa.getTolerance() + dummy_precision, + -epa.depth <= epa.getTolerance() + this->m_dummy_precision, "EPA's penetration distance should be negative (or " "at least below EPA's tolerance).", std::logic_error); @@ -593,6 +591,10 @@ struct HPP_FCL_DLLAPI GJKSolver { Vec3f& p2, Vec3f& normal) const { HPP_FCL_UNUSED_VARIABLE(tf1); + // Cache gjk result for potential future call to this GJKSolver. + this->cached_guess = this->gjk.ray; + this->support_func_cached_guess = this->gjk.support_hint; + distance = this->gjk.distance; p1 = p2 = normal = Vec3f::Constant(std::numeric_limits::quiet_NaN()); @@ -614,14 +616,14 @@ struct HPP_FCL_DLLAPI GJKSolver { // 2. GJK ran out of iterations. // In any case, `gjk.ray`'s norm is bigger than GJK's tolerance and thus // it can safely be normalized. - const FCL_REAL dummy_precision = - 3 * std::sqrt(std::numeric_limits::epsilon()); - HPP_FCL_UNUSED_VARIABLE(dummy_precision); - HPP_FCL_ASSERT( - this->gjk.ray.norm() > this->gjk.getTolerance() - dummy_precision, + this->gjk.ray.norm() > + this->gjk.getTolerance() - this->m_dummy_precision, "The norm of GJK's ray should be bigger than GJK's tolerance.", std::logic_error); + // Cache gjk result for potential future call to this GJKSolver. + this->cached_guess = this->gjk.ray; + this->support_func_cached_guess = this->gjk.support_hint; distance = this->gjk.distance; // TODO: On degenerated case, the closest points may be non-unique. @@ -638,13 +640,15 @@ struct HPP_FCL_DLLAPI GJKSolver { Vec3f& p2, Vec3f& normal) const { HPP_FCL_UNUSED_VARIABLE(tf1); - const FCL_REAL dummy_precision = - 3 * std::sqrt(std::numeric_limits::epsilon()); - HPP_FCL_UNUSED_VARIABLE(dummy_precision); - - HPP_FCL_ASSERT( - this->gjk.distance <= this->gjk.getTolerance() + dummy_precision, - "The distance should be lower than GJK's tolerance.", std::logic_error); + HPP_FCL_ASSERT(this->gjk.distance <= + this->gjk.getTolerance() + this->m_dummy_precision, + "The distance should be lower than GJK's tolerance.", + std::logic_error); + // Because GJK has returned the `Collision` status and EPA has not run, + // we purposefully do not cache the result of GJK, because ray is zero. + // However, we can cache the support function hint. + // this->cached_guess = this->gjk.ray; + this->support_func_cached_guess = this->gjk.support_hint; distance = this->gjk.distance; p1 = p2 = normal = @@ -654,6 +658,10 @@ struct HPP_FCL_DLLAPI GJKSolver { void EPAExtractWitnessPointsAndNormal(const Transform3f& tf1, FCL_REAL& distance, Vec3f& p1, Vec3f& p2, Vec3f& normal) const { + // Cache EPA result for potential future call to this GJKSolver. + // This caching allows to warm-start the next GJK call. + this->cached_guess = -(this->epa.depth * this->epa.normal); + this->support_func_cached_guess = this->epa.support_hint; distance = (std::min)(0., -this->epa.depth); this->epa.getWitnessPointsAndNormal(this->minkowski_difference, p1, p2, normal); @@ -705,6 +713,9 @@ struct HPP_FCL_DLLAPI GJKSolver { void EPAFailedExtractWitnessPointsAndNormal(const Transform3f& tf1, FCL_REAL& distance, Vec3f& p1, Vec3f& p2, Vec3f& normal) const { + this->cached_guess = Vec3f(1, 0, 0); + this->support_func_cached_guess.setZero(); + HPP_FCL_UNUSED_VARIABLE(tf1); distance = -(std::numeric_limits::max)(); p1 = p2 = normal = diff --git a/include/hpp/fcl/narrowphase/support_functions.h b/include/hpp/fcl/narrowphase/support_functions.h new file mode 100644 index 000000000..8a32de58e --- /dev/null +++ b/include/hpp/fcl/narrowphase/support_functions.h @@ -0,0 +1,302 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2015, Open Source Robotics Foundation + * Copyright (c) 2021-2024, 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. + */ + +/** \authors Jia Pan, Florent Lamiraux, Josef Mirabel, Louis Montaut */ + +#ifndef HPP_FCL_SUPPORT_FUNCTIONS_H +#define HPP_FCL_SUPPORT_FUNCTIONS_H + +#include "hpp/fcl/shape/geometric_shapes.h" +#include "hpp/fcl/math/transform.h" +#include "hpp/fcl/collision_data.h" + +namespace hpp { +namespace fcl { + +namespace details { + +/// @brief Options for the computation of support points. +/// `NoSweptSphere` option is used when the support function is called +/// by GJK or EPA. In this case, the swept sphere radius is not taken into +/// account in the support function. It is used by GJK and EPA after they have +/// converged to correct the solution. +/// `WithSweptSphere` option is used when the support function is called +/// directly by the user. In this case, the swept sphere radius is taken into +/// account in the support function. +enum SupportOptions { + NoSweptSphere = 0, + WithSweptSphere = 0x1, +}; + +// ============================================================================ +// ============================ SUPPORT FUNCTIONS ============================= +// ============================================================================ +/// @brief the support function for shape. +/// The output support point is expressed in the local frame of the shape. +/// @return a point which belongs to the set {argmax_{v in shape} v.dot(dir)}. +/// @param shape the shape. +/// @param dir support direction. +/// @param hint used to initialize the search when shape is a ConvexBase object. +/// @tparam SupportOptions is a value of the SupportOptions enum. If set to +/// `WithSweptSphere`, the support functions take into account the shapes' swept +/// sphere radii. Please see `MinkowskiDiff::set(const ShapeBase*, const +/// ShapeBase*)` for more details. +template +Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, int& hint); + +/// @brief Stores temporary data for the computation of support points. +struct HPP_FCL_DLLAPI ShapeSupportData { + // @brief Tracks which points have been visited in a ConvexBase. + std::vector visited; + + // @brief Tracks the last support direction used on this shape; used to + // warm-start the ConvexBase support function. + Vec3f last_dir = Vec3f::Zero(); + + // @brief Temporary set used to compute the convex-hull of a support set. + // Only used for ConvexBase and Box. + SupportSet::Polygon polygon; +}; + +/// @brief Triangle support function. +template +void getShapeSupport(const TriangleP* triangle, const Vec3f& dir, + Vec3f& support, int& /*unused*/, + ShapeSupportData& /*unused*/); + +/// @brief Box support function. +template +void getShapeSupport(const Box* box, const Vec3f& dir, Vec3f& support, + int& /*unused*/, ShapeSupportData& /*unused*/); + +/// @brief Sphere support function. +template +void getShapeSupport(const Sphere* sphere, const Vec3f& dir, Vec3f& support, + int& /*unused*/, ShapeSupportData& /*unused*/); + +/// @brief Ellipsoid support function. +template +void getShapeSupport(const Ellipsoid* ellipsoid, const Vec3f& dir, + Vec3f& support, int& /*unused*/, + ShapeSupportData& /*unused*/); + +/// @brief Capsule support function. +template +void getShapeSupport(const Capsule* capsule, const Vec3f& dir, Vec3f& support, + int& /*unused*/, ShapeSupportData& /*unused*/); + +/// @brief Cone support function. +template +void getShapeSupport(const Cone* cone, const Vec3f& dir, Vec3f& support, + int& /*unused*/, ShapeSupportData& /*unused*/); + +/// @brief Cylinder support function. +template +void getShapeSupport(const Cylinder* cylinder, const Vec3f& dir, Vec3f& support, + int& /*unused*/, ShapeSupportData& /*unused*/); + +/// @brief ConvexBase support function. +/// @note See @ref LargeConvex and SmallConvex to see how to optimize +/// ConvexBase's support computation. +template +void getShapeSupport(const ConvexBase* convex, const Vec3f& dir, Vec3f& support, + int& hint, ShapeSupportData& /*unused*/); + +/// @brief Cast a `ConvexBase` to a `LargeConvex` to use the log version of +/// `getShapeSupport`. This is **much** faster than the linear version of +/// `getShapeSupport` when a `ConvexBase` has more than a few dozen of vertices. +/// @note WARNING: when using a LargeConvex, the neighbors in `ConvexBase` must +/// have been constructed! Otherwise the support function will segfault. +struct LargeConvex : ShapeBase {}; +/// @brief See @ref LargeConvex. +struct SmallConvex : ShapeBase {}; + +/// @brief Support function for large ConvexBase (>32 vertices). +template +void getShapeSupport(const SmallConvex* convex, const Vec3f& dir, + Vec3f& support, int& hint, ShapeSupportData& data); + +/// @brief Support function for small ConvexBase (<32 vertices). +template +void getShapeSupport(const LargeConvex* convex, const Vec3f& dir, + Vec3f& support, int& hint, ShapeSupportData& support_data); + +// ============================================================================ +// ========================== SUPPORT SET FUNCTIONS =========================== +// ============================================================================ +/// @brief Computes the support set for shape. +/// This function assumes the frame of the support set has already been +/// computed and that this frame is expressed w.r.t the local frame of the +/// shape (i.e. the local frame of the shape is the WORLD frame of the support +/// set). The support direction used to compute the support set is the positive +/// z-axis if the support set has the DEFAULT direction; negative z-axis if it +/// has the INVERTED direction. (In short, a shape's support set is has the +/// DEFAULT direction if the shape is the first shape in a collision pair. It +/// has the INVERTED direction if the shape is the second one in the collision +/// pair). +/// @return an approximation of the set {argmax_{v in shape} v.dot(dir)}, where +/// dir is the support set's support direction. +/// The support set is a plane passing by the origin of the support set frame +/// and supported by the direction dir. As a consequence, any point added to the +/// set is automatically projected onto this plane. +/// @param[in] shape the shape. +/// @param[in/out] support_set of shape. +/// @param[in/out] hint used to initialize the search when shape is a ConvexBase +/// object. +/// @param[in] num_sampled_supports is only used for shapes with smooth +/// non-strictly convex bases like cones and cylinders (their bases are +/// circles). In such a case, if the support direction points to their base, we +/// have to choose which points we want to add to the set. This is not needed +/// for boxes or ConvexBase for example. Indeed, because their support sets are +/// always polygons, we can characterize the entire support set with the +/// vertices of the polygon. +/// @param[in] tol given a point v on the shape, if +/// `max_{p in shape}(p.dot(dir)) - v.dot(dir) <= tol`, where dir is the set's +/// support direction, then v is added to the support set. +/// Otherwise said, if a point p of the shape is at a distance `tol` from the +/// support plane, it is added to the set. Thus, `tol` can be seen as the +/// "thickness" of the support plane. +/// @tparam SupportOptions is a value of the SupportOptions enum. If set to +/// `WithSweptSphere`, the support functions take into account the shapes' swept +/// sphere radii. +template +void getSupportSet(const ShapeBase* shape, SupportSet& support_set, int& hint, + size_t num_sampled_supports = 6, FCL_REAL tol = 1e-3); + +/// @brief Same as @ref getSupportSet(const ShapeBase*, const FCL_REAL, +/// SupportSet&, const int) but also constructs the support set frame from +/// `dir`. +/// @note The support direction `dir` is expressed in the local frame of the +/// shape. +/// @note This function automatically deals with the `direction` of the +/// SupportSet. +template +void getSupportSet(const ShapeBase* shape, const Vec3f& dir, + SupportSet& support_set, int& hint, + size_t num_sampled_supports = 6, FCL_REAL tol = 1e-3) { + support_set.tf.rotation() = constructOrthonormalBasisFromVector(dir); + const Vec3f& support_dir = support_set.getNormal(); + const Vec3f support = getSupport<_SupportOptions>(shape, support_dir, hint); + getSupportSet<_SupportOptions>(shape, support_set, hint, num_sampled_supports, + tol); +} + +/// @brief Triangle support set function. +/// Assumes the support set frame has already been computed. +template +void getShapeSupportSet(const TriangleP* triangle, SupportSet& support_set, + int& /*unused*/, ShapeSupportData& /*unused*/, + size_t /*unused*/, FCL_REAL tol = 1e-3); + +/// @brief Box support set function. +/// Assumes the support set frame has already been computed. +template +void getShapeSupportSet(const Box* box, SupportSet& support_set, + int& /*unused*/, ShapeSupportData& support_data, + size_t /*unused*/, FCL_REAL tol = 1e-3); + +/// @brief Sphere support set function. +/// Assumes the support set frame has already been computed. +template +void getShapeSupportSet(const Sphere* sphere, SupportSet& support_set, + int& /*unused*/, ShapeSupportData& /*unused*/, + size_t /*unused*/, FCL_REAL /*unused*/); + +/// @brief Ellipsoid support set function. +/// Assumes the support set frame has already been computed. +template +void getShapeSupportSet(const Ellipsoid* ellipsoid, SupportSet& support_set, + int& /*unused*/, ShapeSupportData& /*unused*/, + size_t /*unused*/, FCL_REAL /*unused*/); + +/// @brief Capsule support set function. +/// Assumes the support set frame has already been computed. +template +void getShapeSupportSet(const Capsule* capsule, SupportSet& support_set, + int& /*unused*/, ShapeSupportData& /*unused*/, + size_t /*unused*/, FCL_REAL tol = 1e-3); + +/// @brief Cone support set function. +/// Assumes the support set frame has already been computed. +template +void getShapeSupportSet(const Cone* cone, SupportSet& support_set, + int& /*unused*/, ShapeSupportData& /*unused*/, + size_t num_sampled_supports = 6, FCL_REAL tol = 1e-3); + +/// @brief Cylinder support set function. +/// Assumes the support set frame has already been computed. +template +void getShapeSupportSet(const Cylinder* cylinder, SupportSet& support_set, + int& /*unused*/, ShapeSupportData& /*unused*/, + size_t num_sampled_supports = 6, FCL_REAL tol = 1e-3); + +/// @brief ConvexBase support set function. +/// Assumes the support set frame has already been computed. +/// @note See @ref LargeConvex and SmallConvex to see how to optimize +/// ConvexBase's support computation. +template +void getShapeSupportSet(const ConvexBase* convex, SupportSet& support_set, + int& hint, ShapeSupportData& support_data, + size_t /*unused*/, FCL_REAL tol = 1e-3); + +/// @brief Support set function for large ConvexBase (>32 vertices). +/// Assumes the support set frame has already been computed. +template +void getShapeSupportSet(const SmallConvex* convex, SupportSet& support_set, + int& /*unused*/, ShapeSupportData& /*unused*/, + size_t /*unused*/, FCL_REAL tol = 1e-3); + +/// @brief Support set function for small ConvexBase (<32 vertices). +/// Assumes the support set frame has already been computed. +template +void getShapeSupportSet(const LargeConvex* convex, SupportSet& support_set, + int& hint, ShapeSupportData& support_data, + size_t /*unused*/, FCL_REAL tol = 1e-3); + +/// @brief Computes the convex-hull of support_set. For now, this function is +/// only needed for Box and ConvexBase. +/// @param[in] cloud data which contains the 2d points of the support set which +/// convex-hull we want to compute. +/// @param[out] 2d points of the the support set's convex-hull. +HPP_FCL_DLLAPI void computeSupportSetConvexHull(SupportSet::Polygon& cloud, + SupportSet::Polygon& cvx_hull); + +} // namespace details + +} // namespace fcl +} // namespace hpp + +#endif // HPP_FCL_SUPPORT_FUNCTIONS_H diff --git a/include/hpp/fcl/serialization/contact_patch.h b/include/hpp/fcl/serialization/contact_patch.h new file mode 100644 index 000000000..09a5cab4a --- /dev/null +++ b/include/hpp/fcl/serialization/contact_patch.h @@ -0,0 +1,87 @@ +// +// Copyright (c) 2024 INRIA +// + +#ifndef HPP_FCL_SERIALIZATION_CONTACT_PATCH_H +#define HPP_FCL_SERIALIZATION_CONTACT_PATCH_H + +#include "hpp/fcl/collision_data.h" +#include "hpp/fcl/serialization/fwd.h" +#include "hpp/fcl/serialization/transform.h" + +namespace boost { +namespace serialization { + +template +void serialize(Archive& ar, hpp::fcl::ContactPatch& contact_patch, + const unsigned int /*version*/) { + using namespace hpp::fcl; + typedef Eigen::Matrix PolygonPoints; + + size_t patch_size = contact_patch.size(); + ar& make_nvp("patch_size", patch_size); + if (patch_size > 0) { + if (Archive::is_loading::value) { + contact_patch.points().resize(patch_size); + } + Eigen::Map points_map( + reinterpret_cast(contact_patch.points().data()), 2, + patch_size); + ar& make_nvp("points", points_map); + } + + ar& make_nvp("penetration_depth", contact_patch.penetration_depth); + ar& make_nvp("direction", contact_patch.direction); + ar& make_nvp("tf", contact_patch.tf); +} + +template +void serialize(Archive& ar, hpp::fcl::ContactPatchRequest& request, + const unsigned int /*version*/) { + using namespace hpp::fcl; + + ar& make_nvp("max_num_patch", request.max_num_patch); + + size_t num_samples_curved_shapes = request.getNumSamplesCurvedShapes(); + FCL_REAL patch_tolerance = request.getPatchTolerance(); + ar& make_nvp("num_samples_curved_shapes", num_samples_curved_shapes); + ar& make_nvp("patch_tolerance", num_samples_curved_shapes); + + if (Archive::is_loading::value) { + request.setNumSamplesCurvedShapes(num_samples_curved_shapes); + request.setPatchTolerance(patch_tolerance); + } +} + +template +void serialize(Archive& ar, hpp::fcl::ContactPatchResult& result, + const unsigned int /*version*/) { + using namespace hpp::fcl; + + size_t num_patches = result.numContactPatches(); + ar& make_nvp("num_patches", num_patches); + + std::vector patches; + patches.resize(num_patches); + if (Archive::is_loading::value) { + ar& make_nvp("patches", patches); + + const ContactPatchRequest request(num_patches); + result.set(request); + for (size_t i = 0; i < num_patches; ++i) { + ContactPatch& patch = result.getUnusedContactPatch(); + patch = patches[i]; + } + } else { + patches.clear(); + for (size_t i = 0; i < num_patches; ++i) { + patches.emplace_back(result.getContactPatch(i)); + } + ar& make_nvp("patches", patches); + } +} + +} // namespace serialization +} // namespace boost + +#endif // HPP_FCL_SERIALIZATION_CONTACT_PATCH_H diff --git a/include/hpp/fcl/shape/geometric_shapes_traits.h b/include/hpp/fcl/shape/geometric_shapes_traits.h index 8d3e677f6..e4997d09a 100644 --- a/include/hpp/fcl/shape/geometric_shapes_traits.h +++ b/include/hpp/fcl/shape/geometric_shapes_traits.h @@ -47,7 +47,8 @@ struct shape_traits_base { NeedNormalizedDir = false, NeedNesterovNormalizeHeuristic = false, IsInflatable = false, - HasInflatedSupportFunction = false + HasInflatedSupportFunction = false, + IsStrictlyConvex = false }; }; @@ -59,8 +60,9 @@ struct shape_traits : shape_traits_base { enum { NeedNormalizedDir = false, NeedNesterovNormalizeHeuristic = false, - IsInflatable = false, - HasInflatedSupportFunction = false + IsInflatable = true, + HasInflatedSupportFunction = false, + IsStrictlyConvex = false }; }; @@ -70,7 +72,8 @@ struct shape_traits : shape_traits_base { NeedNormalizedDir = false, NeedNesterovNormalizeHeuristic = false, IsInflatable = true, - HasInflatedSupportFunction = false + HasInflatedSupportFunction = false, + IsStrictlyConvex = false }; }; @@ -80,7 +83,8 @@ struct shape_traits : shape_traits_base { NeedNormalizedDir = false, NeedNesterovNormalizeHeuristic = false, IsInflatable = true, - HasInflatedSupportFunction = false + HasInflatedSupportFunction = false, + IsStrictlyConvex = true }; }; @@ -90,7 +94,8 @@ struct shape_traits : shape_traits_base { NeedNormalizedDir = false, NeedNesterovNormalizeHeuristic = false, IsInflatable = true, - HasInflatedSupportFunction = false + HasInflatedSupportFunction = false, + IsStrictlyConvex = true }; }; @@ -100,7 +105,8 @@ struct shape_traits : shape_traits_base { NeedNormalizedDir = false, NeedNesterovNormalizeHeuristic = false, IsInflatable = true, - HasInflatedSupportFunction = false + HasInflatedSupportFunction = false, + IsStrictlyConvex = false }; }; @@ -110,7 +116,8 @@ struct shape_traits : shape_traits_base { NeedNormalizedDir = false, NeedNesterovNormalizeHeuristic = false, IsInflatable = true, - HasInflatedSupportFunction = false + HasInflatedSupportFunction = false, + IsStrictlyConvex = false }; }; @@ -120,7 +127,8 @@ struct shape_traits : shape_traits_base { NeedNormalizedDir = false, NeedNesterovNormalizeHeuristic = false, IsInflatable = true, - HasInflatedSupportFunction = false + HasInflatedSupportFunction = false, + IsStrictlyConvex = false }; }; @@ -129,8 +137,9 @@ struct shape_traits : shape_traits_base { enum { NeedNormalizedDir = false, NeedNesterovNormalizeHeuristic = true, - IsInflatable = false, - HasInflatedSupportFunction = true + IsInflatable = true, + HasInflatedSupportFunction = true, + IsStrictlyConvex = false }; }; @@ -140,7 +149,8 @@ struct shape_traits : shape_traits_base { NeedNormalizedDir = false, NeedNesterovNormalizeHeuristic = false, IsInflatable = true, - HasInflatedSupportFunction = false + HasInflatedSupportFunction = false, + IsStrictlyConvex = false }; }; diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index fc3f5c7c2..a505d0dfa 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -106,6 +106,7 @@ SET(${LIBRARY_NAME}_SOURCES math.cc collision-geometries.cc collision.cc + contact_patch.cc distance.cc fcl.cc gjk.cc diff --git a/python/collision.cc b/python/collision.cc index cfaa89d77..5ea8550f5 100644 --- a/python/collision.cc +++ b/python/collision.cc @@ -253,7 +253,7 @@ void exposeCollisionAPI() { "collide", static_cast( + const CollisionRequest&, CollisionResult&)>( &collide)); class_("ComputeCollision", @@ -262,6 +262,6 @@ void exposeCollisionAPI() { const CollisionGeometry*>()) .def("__call__", static_cast(&ComputeCollision::operator())); } diff --git a/python/contact_patch.cc b/python/contact_patch.cc new file mode 100644 index 000000000..66099b3b3 --- /dev/null +++ b/python/contact_patch.cc @@ -0,0 +1,161 @@ +// +// Software License Agreement (BSD License) +// +// Copyright (c) 2024 INRIA +// Author: Louis Montaut +// 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 INRIA 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. + +#include + +#include +#include +#include + +#include "fcl.hh" +#include "deprecation.hh" +#include "serializable.hh" + +#ifdef HPP_FCL_HAS_DOXYGEN_AUTODOC +#include "doxygen_autodoc/functions.h" +#include "doxygen_autodoc/hpp/fcl/collision_data.h" +#endif + +#include "../doc/python/doxygen.hh" +#include "../doc/python/doxygen-boost.hh" + +using namespace boost::python; +using namespace hpp::fcl; +using namespace hpp::fcl::python; + +namespace dv = doxygen::visitor; + +void exposeContactPatchAPI() { + if (!eigenpy::register_symbolic_link_to_registered_type< + ContactPatch::PatchDirection>()) { + enum_("ContactPatchDirection") + .value("DEFAULT", ContactPatch::PatchDirection::DEFAULT) + .value("INVERTED", ContactPatch::PatchDirection::INVERTED) + .export_values(); + } + + if (!eigenpy::register_symbolic_link_to_registered_type()) { + class_( + "ContactPatch", doxygen::class_doc(), + init>((arg("self"), arg("preallocated_size")), + "ContactPatch constructor.")) + .DEF_RW_CLASS_ATTRIB(ContactPatch, tf) + .DEF_RW_CLASS_ATTRIB(ContactPatch, direction) + .DEF_RW_CLASS_ATTRIB(ContactPatch, penetration_depth) + .DEF_CLASS_FUNC(ContactPatch, size) + .DEF_CLASS_FUNC(ContactPatch, getNormal) + .DEF_CLASS_FUNC(ContactPatch, addPoint) + .DEF_CLASS_FUNC(ContactPatch, getPoint) + .DEF_CLASS_FUNC(ContactPatch, getPointShape1) + .DEF_CLASS_FUNC(ContactPatch, getPointShape2) + .DEF_CLASS_FUNC(ContactPatch, clear) + .DEF_CLASS_FUNC(ContactPatch, isSame); + } + + if (!eigenpy::register_symbolic_link_to_registered_type< + std::vector>()) { + class_>("StdVec_ContactPatch") + .def(vector_indexing_suite>()); + } + + if (!eigenpy::register_symbolic_link_to_registered_type< + ContactPatchRequest>()) { + class_( + "ContactPatchRequest", doxygen::class_doc(), + init>( + (arg("self"), arg("max_num_patch"), + arg("num_samples_curved_shapes"), arg("patch_tolerance")), + "ContactPatchRequest constructor.")) + .def(dv::init>()) + .DEF_RW_CLASS_ATTRIB(ContactPatchRequest, max_num_patch) + .DEF_CLASS_FUNC(ContactPatchRequest, getNumSamplesCurvedShapes) + .DEF_CLASS_FUNC(ContactPatchRequest, setNumSamplesCurvedShapes) + .DEF_CLASS_FUNC(ContactPatchRequest, getPatchTolerance) + .DEF_CLASS_FUNC(ContactPatchRequest, setPatchTolerance); + } + + if (!eigenpy::register_symbolic_link_to_registered_type< + std::vector>()) { + class_>("StdVec_ContactPatchRequest") + .def(vector_indexing_suite>()); + } + + if (!eigenpy::register_symbolic_link_to_registered_type< + ContactPatchResult>()) { + class_("ContactPatchResult", + doxygen::class_doc(), + init<>(arg("self"), "Default constructor.")) + .def(dv::init()) + .DEF_CLASS_FUNC(ContactPatchResult, numContactPatches) + .DEF_CLASS_FUNC(ContactPatchResult, getUnusedContactPatch) + .DEF_CLASS_FUNC2(ContactPatchResult, getContactPatch, + return_value_policy()) + .DEF_CLASS_FUNC(ContactPatchResult, clear) + .DEF_CLASS_FUNC(ContactPatchResult, set) + .DEF_CLASS_FUNC(ContactPatchResult, check); + } + + if (!eigenpy::register_symbolic_link_to_registered_type< + std::vector>()) { + class_>("StdVec_ContactPatchResult") + .def(vector_indexing_suite>()); + } + + doxygen::def( + "computeContactPatch", + static_cast(&computeContactPatch)); + doxygen::def( + "computeContactPatch", + static_cast(&computeContactPatch)); + + if (!eigenpy::register_symbolic_link_to_registered_type< + ComputeContactPatch>()) { + class_("ComputeContactPatch", + doxygen::class_doc(), + no_init) + .def(dv::init()) + .def("__call__", + static_cast( + &ComputeContactPatch::operator())); + } +} diff --git a/python/distance.cc b/python/distance.cc index 95bb05ca6..61aebf684 100644 --- a/python/distance.cc +++ b/python/distance.cc @@ -149,7 +149,8 @@ void exposeDistanceAPI() { "distance", static_cast(&distance)); + const DistanceRequest&, DistanceResult&)>( + &distance)); class_("ComputeDistance", doxygen::class_doc(), no_init) @@ -157,6 +158,6 @@ void exposeDistanceAPI() { const CollisionGeometry*>()) .def("__call__", static_cast(&ComputeDistance::operator())); } diff --git a/python/fcl.cc b/python/fcl.cc index a0061d6fe..956ac8715 100644 --- a/python/fcl.cc +++ b/python/fcl.cc @@ -95,6 +95,7 @@ BOOST_PYTHON_MODULE(hppfcl) { exposeCollisionObject(); exposeMeshLoader(); exposeCollisionAPI(); + exposeContactPatchAPI(); exposeDistanceAPI(); exposeGJK(); #ifdef HPP_FCL_HAS_OCTOMAP diff --git a/python/fcl.hh b/python/fcl.hh index 74d26858e..6b06095f9 100644 --- a/python/fcl.hh +++ b/python/fcl.hh @@ -12,6 +12,8 @@ void exposeMeshLoader(); void exposeCollisionAPI(); +void exposeContactPatchAPI(); + void exposeDistanceAPI(); void exposeGJK(); diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 3699973b7..74e7fb304 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -35,6 +35,9 @@ set(LIBRARY_NAME ${PROJECT_NAME}) set(${LIBRARY_NAME}_SOURCES collision.cpp + contact_patch.cpp + contact_patch/contact_patch_solver.cpp + contact_patch_func_matrix.cpp distance_func_matrix.cpp collision_data.cpp collision_node.cpp @@ -59,6 +62,8 @@ set(${LIBRARY_NAME}_SOURCES broadphase/detail/spatial_hash.cpp broadphase/detail/morton.cpp narrowphase/gjk.cpp + narrowphase/minkowski_difference.cpp + narrowphase/support_functions.cpp narrowphase/details.h shape/convex.cpp shape/geometric_shapes.cpp @@ -220,6 +225,9 @@ IF(WIN32) Boost::thread Boost::date_time ) + # There is an issue with MSVC 2017 and Eigen (due to std::aligned_storage). + # See https://github.com/ceres-solver/ceres-solver/issues/481 + target_compile_definitions(${LIBRARY_NAME} PRIVATE _ENABLE_EXTENDED_ALIGNED_STORAGE) ENDIF(WIN32) if (HPP_FCL_TURN_ASSERT_INTO_EXCEPTION) diff --git a/src/collision.cpp b/src/collision.cpp index 21c95935c..9db29dd7b 100644 --- a/src/collision.cpp +++ b/src/collision.cpp @@ -120,11 +120,11 @@ std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1, o1, tf1, o2, tf2, &solver, request, result); } } - if (solver.gjk_initial_guess == GJKInitialGuess::CachedGuess || - solver.enable_cached_guess) { - result.cached_gjk_guess = solver.cached_guess; - result.cached_support_func_guess = solver.support_func_cached_guess; - } + // Cache narrow phase solver result. If the option in the request is selected, + // also store the solver result in the request for the next call. + result.cached_gjk_guess = solver.cached_guess; + result.cached_support_func_guess = solver.support_func_cached_guess; + request.updateGuess(result); return res; } @@ -175,12 +175,11 @@ std::size_t ComputeCollision::run(const Transform3f& tf1, } else { res = func(o1, tf1, o2, tf2, &solver, request, result); } - - if (solver.gjk_initial_guess == GJKInitialGuess::CachedGuess || - solver.enable_cached_guess) { - result.cached_gjk_guess = solver.cached_guess; - result.cached_support_func_guess = solver.support_func_cached_guess; - } + // Cache narrow phase solver result. If the option in the request is selected, + // also store the solver result in the request for the next call. + result.cached_gjk_guess = solver.cached_guess; + result.cached_support_func_guess = solver.support_func_cached_guess; + request.updateGuess(result); return res; } diff --git a/src/collision_data.cpp b/src/collision_data.cpp index 91080fcda..2572014df 100644 --- a/src/collision_data.cpp +++ b/src/collision_data.cpp @@ -3,6 +3,7 @@ * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation + * Copyright (c) 2024, INRIA * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -49,5 +50,4 @@ bool DistanceRequest::isSatisfied(const DistanceResult& result) const { } } // namespace fcl - } // namespace hpp diff --git a/src/contact_patch.cpp b/src/contact_patch.cpp new file mode 100644 index 000000000..1ad16a810 --- /dev/null +++ b/src/contact_patch.cpp @@ -0,0 +1,164 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, 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 INRIA 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. + */ + +/** \author Louis Montaut */ + +#include "hpp/fcl/contact_patch.h" +#include "hpp/fcl/collision_utility.h" + +namespace hpp { +namespace fcl { + +ContactPatchFunctionMatrix& getContactPatchFunctionLookTable() { + static ContactPatchFunctionMatrix table; + return table; +} + +void computeContactPatch(const CollisionGeometry* o1, const Transform3f& tf1, + const CollisionGeometry* o2, const Transform3f& tf2, + const CollisionResult& collision_result, + const ContactPatchRequest& request, + ContactPatchResult& result) { + if (!collision_result.isCollision() || request.max_num_patch == 0) { + // do nothing + return; + } + + OBJECT_TYPE object_type1 = o1->getObjectType(); + OBJECT_TYPE object_type2 = o2->getObjectType(); + NODE_TYPE node_type1 = o1->getNodeType(); + NODE_TYPE node_type2 = o2->getNodeType(); + // TODO(louis): add support for BVH (leaf is a triangle) and Hfield (leaf is a + // convex) + if (object_type1 != OBJECT_TYPE::OT_GEOM || + object_type2 != OBJECT_TYPE::OT_GEOM) { + HPP_FCL_THROW_PRETTY("Computing contact patches between node type " + << std::string(get_node_type_name(node_type1)) + << " and node type " + << std::string(get_node_type_name(node_type2)) + << " is not yet supported. Only primitive shapes " + "are supported for now.", + std::invalid_argument); + return; + } + + const ContactPatchFunctionMatrix& looktable = + getContactPatchFunctionLookTable(); + if (!looktable.contact_patch_matrix[node_type1][node_type2]) { + HPP_FCL_THROW_PRETTY("Contact patch computation between node type " + << std::string(get_node_type_name(node_type1)) + << " and node type " + << std::string(get_node_type_name(node_type2)) + << " is not yet supported.", + std::invalid_argument); + } + + // Before doing any computation, we initialize and clear the input result. + result.set(request); + ContactPatchSolver csolver(request); + return looktable.contact_patch_matrix[node_type1][node_type2]( + o1, tf1, o2, tf2, collision_result, &csolver, request, result); +} + +void computeContactPatch(const CollisionObject* o1, const CollisionObject* o2, + const CollisionResult& collision_result, + const ContactPatchRequest& request, + ContactPatchResult& result) { + return computeContactPatch(o1->collisionGeometryPtr(), o1->getTransform(), + o2->collisionGeometryPtr(), o2->getTransform(), + collision_result, request, result); +} + +ComputeContactPatch::ComputeContactPatch(const CollisionGeometry* o1, + const CollisionGeometry* o2) + : o1(o1), o2(o2) { + const ContactPatchFunctionMatrix& looktable = + getContactPatchFunctionLookTable(); + + OBJECT_TYPE object_type1 = this->o1->getObjectType(); + NODE_TYPE node_type1 = this->o1->getNodeType(); + OBJECT_TYPE object_type2 = this->o2->getObjectType(); + NODE_TYPE node_type2 = this->o2->getNodeType(); + + if (object_type1 != OBJECT_TYPE::OT_GEOM || + object_type2 != OBJECT_TYPE::OT_GEOM) { + HPP_FCL_THROW_PRETTY("Computing contact patches between node type " + << std::string(get_node_type_name(node_type1)) + << " and node type " + << std::string(get_node_type_name(node_type2)) + << " is not yet supported. Only primitive shapes " + "are supported for now.", + std::invalid_argument); + } + + if (!looktable.contact_patch_matrix[node_type1][node_type2]) { + HPP_FCL_THROW_PRETTY("Contact patch computation between node type " + << std::string(get_node_type_name(node_type1)) + << " and node type " + << std::string(get_node_type_name(node_type2)) + << " is not yet supported.", + std::invalid_argument); + } + + this->func = looktable.contact_patch_matrix[node_type1][node_type2]; +} + +void ComputeContactPatch::run(const Transform3f& tf1, const Transform3f& tf2, + const CollisionResult& collision_result, + const ContactPatchRequest& request, + ContactPatchResult& result) const { + if (!collision_result.isCollision() || request.max_num_patch == 0) { + // do nothing + return; + } + + // Before doing any computation, we initialize and clear the input result. + result.set(request); + this->func(this->o1, tf1, this->o2, tf2, collision_result, &(this->csolver), + request, result); +} + +void ComputeContactPatch::operator()(const Transform3f& tf1, + const Transform3f& tf2, + const CollisionResult& collision_result, + const ContactPatchRequest& request, + ContactPatchResult& result) const + +{ + this->csolver.set(request); + this->run(tf1, tf2, collision_result, request, result); +} + +} // namespace fcl +} // namespace hpp diff --git a/src/contact_patch/contact_patch_solver.cpp b/src/contact_patch/contact_patch_solver.cpp new file mode 100644 index 000000000..9593c5cd5 --- /dev/null +++ b/src/contact_patch/contact_patch_solver.cpp @@ -0,0 +1,107 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, 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 INRIA 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. + */ + +/** \authors Louis Montaut */ + +#include "hpp/fcl/contact_patch/contact_patch_solver.h" + +namespace hpp { +namespace fcl { + +namespace details { + +/// @brief Templated shape support set functions. +template +void getShapeSupportSetTpl(const ShapeBase* shape, SupportSet& support_set, + int& hint, ShapeSupportData& support_data, + size_t num_sampled_supports = 6, + FCL_REAL tol = 1e-3) { + const ShapeType* shape_ = static_cast(shape); + getShapeSupportSet<_SupportOptions>(shape_, support_set, hint, support_data, + num_sampled_supports, tol); +} + +} // namespace details + +// ============================================================================ +ContactPatchSolver::SupportSetFunction +ContactPatchSolver::makeSupportSetFunction(const ShapeBase* shape, + ShapeSupportData& support_data) { + // Note: because the swept-sphere radius was already taken into account when + // constructing the contact patch frame, there is actually no need to take the + // swept-sphere radius of shapes into account. The origin of the contact patch + // frame already encodes this information. + using Options = details::SupportOptions; + switch (shape->getNodeType()) { + case GEOM_TRIANGLE: + return details::getShapeSupportSetTpl; + case GEOM_BOX: { + const size_t num_corners_box = 8; + support_data.polygon.reserve(num_corners_box); + return details::getShapeSupportSetTpl; + } + case GEOM_SPHERE: + return details::getShapeSupportSetTpl; + case GEOM_ELLIPSOID: + return details::getShapeSupportSetTpl; + case GEOM_CAPSULE: + return details::getShapeSupportSetTpl; + case GEOM_CONE: + return details::getShapeSupportSetTpl; + case GEOM_CYLINDER: + return details::getShapeSupportSetTpl; + case GEOM_CONVEX: { + const ConvexBase* convex = static_cast(shape); + if (support_data.polygon.capacity() < default_num_preallocated_supports) { + support_data.polygon.reserve(default_num_preallocated_supports); + } + if ((size_t)(convex->num_points) > + ConvexBase::num_vertices_large_convex_threshold) { + support_data.visited.assign(convex->num_points, false); + support_data.last_dir.setZero(); + return details::getShapeSupportSetTpl; + } else { + return details::getShapeSupportSetTpl; + } + } + default: + HPP_FCL_THROW_PRETTY("Unsupported geometric shape.", std::logic_error); + } +} + +} // namespace fcl +} // namespace hpp diff --git a/src/contact_patch_func_matrix.cpp b/src/contact_patch_func_matrix.cpp new file mode 100644 index 000000000..421542af1 --- /dev/null +++ b/src/contact_patch_func_matrix.cpp @@ -0,0 +1,163 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, 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 INRIA 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. + */ + +/** \author Louis Montaut */ + +#include "hpp/fcl/contact_patch_func_matrix.h" +#include "hpp/fcl/shape/geometric_shapes.h" +#include "hpp/fcl/internal/shape_shape_contact_patch_func.h" + +namespace hpp { +namespace fcl { + +ContactPatchFunctionMatrix::ContactPatchFunctionMatrix() { + for (int i = 0; i < NODE_COUNT; ++i) { + for (int j = 0; j < NODE_COUNT; ++j) contact_patch_matrix[i][j] = nullptr; + } + + // clang-format off + contact_patch_matrix[GEOM_BOX][GEOM_BOX] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_BOX][GEOM_SPHERE] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_BOX][GEOM_CAPSULE] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_BOX][GEOM_CONE] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_BOX][GEOM_CYLINDER] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_BOX][GEOM_CONVEX] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_BOX][GEOM_PLANE] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_BOX][GEOM_HALFSPACE] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_BOX][GEOM_ELLIPSOID] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_BOX][GEOM_TRIANGLE] = &ShapeShapeContactPatch; + + contact_patch_matrix[GEOM_SPHERE][GEOM_BOX] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_SPHERE][GEOM_SPHERE] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_SPHERE][GEOM_CAPSULE] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_SPHERE][GEOM_CONE] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_SPHERE][GEOM_CYLINDER] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_SPHERE][GEOM_CONVEX] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_SPHERE][GEOM_PLANE] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_SPHERE][GEOM_HALFSPACE] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_SPHERE][GEOM_ELLIPSOID] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_SPHERE][GEOM_TRIANGLE] = &ShapeShapeContactPatch; + + contact_patch_matrix[GEOM_ELLIPSOID][GEOM_BOX] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_ELLIPSOID][GEOM_SPHERE] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_ELLIPSOID][GEOM_CAPSULE] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_ELLIPSOID][GEOM_CONE] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_ELLIPSOID][GEOM_CYLINDER] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_ELLIPSOID][GEOM_CONVEX] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_ELLIPSOID][GEOM_PLANE] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_ELLIPSOID][GEOM_HALFSPACE] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_ELLIPSOID][GEOM_ELLIPSOID] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_ELLIPSOID][GEOM_TRIANGLE] = &ShapeShapeContactPatch; + + contact_patch_matrix[GEOM_CAPSULE][GEOM_BOX] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_CAPSULE][GEOM_SPHERE] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_CAPSULE][GEOM_CAPSULE] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_CAPSULE][GEOM_CONE] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_CAPSULE][GEOM_CYLINDER] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_CAPSULE][GEOM_CONVEX] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_CAPSULE][GEOM_PLANE] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_CAPSULE][GEOM_HALFSPACE] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_CAPSULE][GEOM_ELLIPSOID] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_CAPSULE][GEOM_TRIANGLE] = &ShapeShapeContactPatch; + + contact_patch_matrix[GEOM_CONE][GEOM_BOX] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_CONE][GEOM_SPHERE] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_CONE][GEOM_CAPSULE] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_CONE][GEOM_CONE] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_CONE][GEOM_CYLINDER] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_CONE][GEOM_CONVEX] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_CONE][GEOM_PLANE] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_CONE][GEOM_HALFSPACE] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_CONE][GEOM_ELLIPSOID] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_CONE][GEOM_TRIANGLE] = &ShapeShapeContactPatch; + + contact_patch_matrix[GEOM_CYLINDER][GEOM_BOX] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_CYLINDER][GEOM_SPHERE] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_CYLINDER][GEOM_CAPSULE] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_CYLINDER][GEOM_CONE] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_CYLINDER][GEOM_CYLINDER] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_CYLINDER][GEOM_CONVEX] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_CYLINDER][GEOM_PLANE] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_CYLINDER][GEOM_HALFSPACE] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_CYLINDER][GEOM_ELLIPSOID] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_CYLINDER][GEOM_TRIANGLE] = &ShapeShapeContactPatch; + + contact_patch_matrix[GEOM_CONVEX][GEOM_BOX] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_CONVEX][GEOM_SPHERE] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_CONVEX][GEOM_CAPSULE] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_CONVEX][GEOM_CONE] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_CONVEX][GEOM_CYLINDER] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_CONVEX][GEOM_CONVEX] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_CONVEX][GEOM_PLANE] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_CONVEX][GEOM_HALFSPACE] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_CONVEX][GEOM_ELLIPSOID] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_CONVEX][GEOM_TRIANGLE] = &ShapeShapeContactPatch; + + contact_patch_matrix[GEOM_PLANE][GEOM_BOX] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_PLANE][GEOM_SPHERE] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_PLANE][GEOM_CAPSULE] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_PLANE][GEOM_CONE] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_PLANE][GEOM_CYLINDER] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_PLANE][GEOM_CONVEX] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_PLANE][GEOM_PLANE] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_PLANE][GEOM_HALFSPACE] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_PLANE][GEOM_ELLIPSOID] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_PLANE][GEOM_TRIANGLE] = &ShapeShapeContactPatch; + + contact_patch_matrix[GEOM_HALFSPACE][GEOM_BOX] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_HALFSPACE][GEOM_SPHERE] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_HALFSPACE][GEOM_CAPSULE] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_HALFSPACE][GEOM_CONE] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_HALFSPACE][GEOM_CYLINDER] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_HALFSPACE][GEOM_CONVEX] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_HALFSPACE][GEOM_PLANE] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_HALFSPACE][GEOM_HALFSPACE] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_HALFSPACE][GEOM_ELLIPSOID] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_HALFSPACE][GEOM_TRIANGLE] = &ShapeShapeContactPatch; + + contact_patch_matrix[GEOM_TRIANGLE][GEOM_BOX] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_TRIANGLE][GEOM_SPHERE] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_TRIANGLE][GEOM_CAPSULE] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_TRIANGLE][GEOM_CONE] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_TRIANGLE][GEOM_CYLINDER] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_TRIANGLE][GEOM_CONVEX] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_TRIANGLE][GEOM_PLANE] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_TRIANGLE][GEOM_HALFSPACE] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_TRIANGLE][GEOM_ELLIPSOID] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_TRIANGLE][GEOM_TRIANGLE] = &ShapeShapeContactPatch; + // clang-format on +} + +} // namespace fcl +} // namespace hpp diff --git a/src/distance.cpp b/src/distance.cpp index 4c0852e04..a234b5ff5 100644 --- a/src/distance.cpp +++ b/src/distance.cpp @@ -100,12 +100,11 @@ FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, o1, tf1, o2, tf2, &solver, request, result); } } - if (solver.gjk_initial_guess == GJKInitialGuess::CachedGuess || - solver.enable_cached_guess) { - result.cached_gjk_guess = solver.cached_guess; - result.cached_support_func_guess = solver.support_func_cached_guess; - } - + // Cache narrow phase solver result. If the option in the request is selected, + // also store the solver result in the request for the next call. + result.cached_gjk_guess = solver.cached_guess; + result.cached_support_func_guess = solver.support_func_cached_guess; + request.updateGuess(result); return res; } @@ -150,7 +149,11 @@ FCL_REAL ComputeDistance::run(const Transform3f& tf1, const Transform3f& tf2, } else { res = func(o1, tf1, o2, tf2, &solver, request, result); } - + // Cache narrow phase solver result. If the option in the request is selected, + // also store the solver result in the request for the next call. + result.cached_gjk_guess = solver.cached_guess; + result.cached_support_func_guess = solver.support_func_cached_guess; + request.updateGuess(result); return res; } @@ -167,12 +170,6 @@ FCL_REAL ComputeDistance::operator()(const Transform3f& tf1, result.timings = timer.elapsed(); } else res = run(tf1, tf2, request, result); - - if (solver.gjk_initial_guess == GJKInitialGuess::CachedGuess || - solver.enable_cached_guess) { - result.cached_gjk_guess = solver.cached_guess; - result.cached_support_func_guess = solver.support_func_cached_guess; - } return res; } diff --git a/src/distance/triangle_triangle.cpp b/src/distance/triangle_triangle.cpp index 3a506fc07..2f7302ebc 100644 --- a/src/distance/triangle_triangle.cpp +++ b/src/distance/triangle_triangle.cpp @@ -79,11 +79,8 @@ FCL_REAL ShapeShapeDistance( details::GJK::Status gjk_status = solver->gjk.evaluate(solver->minkowski_difference, guess, support_hint); - if (solver->gjk_initial_guess == GJKInitialGuess::CachedGuess || - solver->enable_cached_guess) { - solver->cached_guess = solver->gjk.getGuessFromSimplex(); - solver->support_func_cached_guess = solver->gjk.support_hint; - } + solver->cached_guess = solver->gjk.getGuessFromSimplex(); + solver->support_func_cached_guess = solver->gjk.support_hint; // Retrieve witness points and normal solver->gjk.getWitnessPointsAndNormal(solver->minkowski_difference, p1, p2, diff --git a/src/narrowphase/gjk.cpp b/src/narrowphase/gjk.cpp index 84453eb46..3b03a9ff8 100644 --- a/src/narrowphase/gjk.cpp +++ b/src/narrowphase/gjk.cpp @@ -48,617 +48,6 @@ namespace fcl { namespace details { -template -void getShapeSupport(const TriangleP* triangle, const Vec3f& dir, - 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); - if (dota > dotb) { - if (dotc > dota) - support = triangle->c; - else - support = triangle->a; - } else { - if (dotc > dotb) - support = triangle->c; - else - support = triangle->b; - } - - if (_SupportOptions == SupportOptions::WithSweptSphere) { - support += triangle->getSweptSphereRadius() * dir.normalized(); - } -} - -template -inline void getShapeSupport(const Box* box, const Vec3f& dir, Vec3f& support, - int&, MinkowskiDiff::ShapeData*) { - // The inflate value is simply to make the specialized functions with box - // have a preferred side for edge cases. - static const FCL_REAL inflate = (dir.array() == 0).any() ? 1 + 1e-10 : 1.; - static const FCL_REAL dummy_precision = - Eigen::NumTraits::dummy_precision(); - Vec3f support1 = (dir.array() > dummy_precision).select(box->halfSide, 0); - Vec3f support2 = - (dir.array() < -dummy_precision).select(-inflate * box->halfSide, 0); - support.noalias() = support1 + support2; - - if (_SupportOptions == SupportOptions::WithSweptSphere) { - support += box->getSweptSphereRadius() * dir.normalized(); - } -} - -template -inline void getShapeSupport(const Sphere* sphere, const Vec3f& dir, - Vec3f& support, int&, MinkowskiDiff::ShapeData*) { - if (_SupportOptions == SupportOptions::WithSweptSphere) { - support.noalias() = - (sphere->radius + sphere->getSweptSphereRadius()) * dir.normalized(); - } else { - support.setZero(); - } - - HPP_FCL_UNUSED_VARIABLE(sphere); - HPP_FCL_UNUSED_VARIABLE(dir); -} - -template -inline void getShapeSupport(const Ellipsoid* ellipsoid, const Vec3f& dir, - 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]; - - Vec3f v(a2 * dir[0], b2 * dir[1], c2 * dir[2]); - - FCL_REAL d = std::sqrt(v.dot(dir)); - - support = v / d; - - if (_SupportOptions == SupportOptions::WithSweptSphere) { - support += ellipsoid->getSweptSphereRadius() * dir.normalized(); - } -} - -template -inline void getShapeSupport(const Capsule* capsule, const Vec3f& dir, - Vec3f& support, int&, MinkowskiDiff::ShapeData*) { - static const FCL_REAL dummy_precision = - Eigen::NumTraits::dummy_precision(); - support.setZero(); - if (dir[2] > dummy_precision) { - support[2] = capsule->halfLength; - } else if (dir[2] < -dummy_precision) { - support[2] = -capsule->halfLength; - } - - if (_SupportOptions == SupportOptions::WithSweptSphere) { - support += - (capsule->radius + capsule->getSweptSphereRadius()) * dir.normalized(); - } -} - -template -void getShapeSupport(const Cone* cone, const Vec3f& dir, Vec3f& support, int&, - MinkowskiDiff::ShapeData*) { - static const FCL_REAL dummy_precision = - Eigen::NumTraits::dummy_precision(); - - // The cone radius is, for -h < z < h, (h - z) * r / (2*h) - // The inflate value is simply to make the specialized functions with cone - // have a preferred side for edge cases. - static const FCL_REAL inflate = 1 + 1e-10; - FCL_REAL h = cone->halfLength; - FCL_REAL r = cone->radius; - - if (dir.head<2>().isZero(dummy_precision)) { - support.head<2>().setZero(); - if (dir[2] > dummy_precision) { - support[2] = h; - } else { - support[2] = -inflate * h; - } - } else { - FCL_REAL zdist = dir[0] * dir[0] + dir[1] * dir[1]; - FCL_REAL len = zdist + dir[2] * dir[2]; - zdist = std::sqrt(zdist); - - if (dir[2] <= 0) { - FCL_REAL rad = r / zdist; - support.head<2>() = rad * dir.head<2>(); - support[2] = -h; - } else { - len = std::sqrt(len); - FCL_REAL sin_a = r / std::sqrt(r * r + 4 * h * h); - - if (dir[2] > len * sin_a) - support << 0, 0, h; - else { - FCL_REAL rad = r / zdist; - support.head<2>() = rad * dir.head<2>(); - support[2] = -h; - } - } - } - - if (_SupportOptions == SupportOptions::WithSweptSphere) { - support += cone->getSweptSphereRadius() * dir.normalized(); - } -} - -template -void getShapeSupport(const Cylinder* cylinder, const Vec3f& dir, Vec3f& support, - int&, MinkowskiDiff::ShapeData*) { - static const FCL_REAL dummy_precision = - Eigen::NumTraits::dummy_precision(); - - // The inflate value is simply to make the specialized functions with cylinder - // have a preferred side for edge cases. - static const FCL_REAL inflate = 1 + 1e-10; - FCL_REAL half_h = cylinder->halfLength; - FCL_REAL r = cylinder->radius; - - const bool dir_is_aligned_with_z = dir.head<2>().isZero(dummy_precision); - if (dir_is_aligned_with_z) half_h *= inflate; - - if (dir[2] > dummy_precision) { - support[2] = half_h; - } else if (dir[2] < -dummy_precision) { - support[2] = -half_h; - } else { - support[2] = 0; - r *= inflate; - } - - if (dir_is_aligned_with_z) { - support.head<2>().setZero(); - } else { - support.head<2>() = dir.head<2>().normalized() * r; - } - - assert(fabs(support[0] * dir[1] - support[1] * dir[0]) < - sqrt(std::numeric_limits::epsilon())); - - if (_SupportOptions == SupportOptions::WithSweptSphere) { - support += cylinder->getSweptSphereRadius() * dir.normalized(); - } -} - -struct SmallConvex : ShapeBase {}; -struct LargeConvex : ShapeBase {}; - -template -void getShapeSupportLog(const ConvexBase* convex, const Vec3f& dir, - Vec3f& support, int& hint, - MinkowskiDiff::ShapeData* data) { - assert(data != nullptr && "data is null."); - assert(convex->neighbors != nullptr && "Convex has no neighbors."); - - // Use warm start if current support direction is distant from last support - // direction. - const double use_warm_start_threshold = 0.9; - Vec3f dir_normalized = dir.normalized(); - if (!data->last_dir.isZero() && !convex->support_warm_starts.points.empty() && - data->last_dir.dot(dir_normalized) < use_warm_start_threshold) { - // Change hint if last dir is too far from current dir. - FCL_REAL maxdot = convex->support_warm_starts.points[0].dot(dir); - hint = convex->support_warm_starts.indices[0]; - for (size_t i = 1; i < convex->support_warm_starts.points.size(); ++i) { - FCL_REAL dot = convex->support_warm_starts.points[i].dot(dir); - if (dot > maxdot) { - maxdot = dot; - hint = convex->support_warm_starts.indices[i]; - } - } - } - data->last_dir = dir_normalized; - - const std::vector& pts = *(convex->points); - const std::vector& nn = *(convex->neighbors); - - if (hint < 0 || hint >= (int)convex->num_points) hint = 0; - FCL_REAL maxdot = pts[static_cast(hint)].dot(dir); - std::vector& visited = data->visited; - std::fill(visited.begin(), visited.end(), false); - visited[static_cast(hint)] = true; - // when the first face is orthogonal to dir, all the dot products will be - // equal. Yet, the neighbors must be visited. - bool found = true, loose_check = true; - while (found) { - const ConvexBase::Neighbors& n = nn[static_cast(hint)]; - found = false; - for (int in = 0; in < n.count(); ++in) { - const unsigned int ip = n[in]; - if (visited[ip]) continue; - visited[ip] = true; - const FCL_REAL dot = pts[ip].dot(dir); - bool better = false; - if (dot > maxdot) { - better = true; - loose_check = false; - } else if (loose_check && dot == maxdot) - better = true; - if (better) { - maxdot = dot; - hint = static_cast(ip); - found = true; - } - } - } - - support = pts[static_cast(hint)]; - - if (_SupportOptions == SupportOptions::WithSweptSphere) { - support += convex->getSweptSphereRadius() * dir.normalized(); - } -} - -template -void getShapeSupportLinear(const ConvexBase* convex, const Vec3f& dir, - Vec3f& support, int& hint, - MinkowskiDiff::ShapeData*) { - const std::vector& pts = *(convex->points); - - hint = 0; - FCL_REAL maxdot = pts[0].dot(dir); - for (int i = 1; i < (int)convex->num_points; ++i) { - FCL_REAL dot = pts[static_cast(i)].dot(dir); - if (dot > maxdot) { - maxdot = dot; - hint = i; - } - } - - support = pts[static_cast(hint)]; - - if (_SupportOptions == SupportOptions::WithSweptSphere) { - support += convex->getSweptSphereRadius() * dir.normalized(); - } -} - -template -void getShapeSupport(const ConvexBase* convex, const Vec3f& dir, Vec3f& support, - int& hint, MinkowskiDiff::ShapeData*) { - // TODO add benchmark to set a proper value for switching between linear and - // logarithmic. - if (convex->num_points > ConvexBase::num_vertices_large_convex_threshold && - convex->neighbors != nullptr) { - MinkowskiDiff::ShapeData data; - data.visited.assign(convex->num_points, false); - getShapeSupportLog<_SupportOptions>(convex, dir, support, hint, &data); - } else - getShapeSupportLinear<_SupportOptions>(convex, dir, support, hint, nullptr); -} - -template -inline void getShapeSupport(const SmallConvex* convex, const Vec3f& dir, - Vec3f& support, int& hint, - MinkowskiDiff::ShapeData* data) { - getShapeSupportLinear<_SupportOptions>( - reinterpret_cast(convex), dir, support, hint, data); -} - -template -inline void getShapeSupport(const LargeConvex* convex, const Vec3f& dir, - Vec3f& support, int& hint, - MinkowskiDiff::ShapeData* data) { - getShapeSupportLog<_SupportOptions>( - reinterpret_cast(convex), dir, support, hint, data); -} - -#define CALL_GET_SHAPE_SUPPORT(ShapeType) \ - getShapeSupport<_SupportOptions>(static_cast(shape), dir, \ - support, hint, NULL) - -template -Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, int& hint) { - Vec3f support; - switch (shape->getNodeType()) { - case GEOM_TRIANGLE: - CALL_GET_SHAPE_SUPPORT(TriangleP); - break; - case GEOM_BOX: - CALL_GET_SHAPE_SUPPORT(Box); - break; - case GEOM_SPHERE: - CALL_GET_SHAPE_SUPPORT(Sphere); - break; - case GEOM_ELLIPSOID: - CALL_GET_SHAPE_SUPPORT(Ellipsoid); - break; - case GEOM_CAPSULE: - CALL_GET_SHAPE_SUPPORT(Capsule); - break; - case GEOM_CONE: - CALL_GET_SHAPE_SUPPORT(Cone); - break; - case GEOM_CYLINDER: - CALL_GET_SHAPE_SUPPORT(Cylinder); - break; - case GEOM_CONVEX: - CALL_GET_SHAPE_SUPPORT(ConvexBase); - break; - case GEOM_PLANE: - case GEOM_HALFSPACE: - default: - support.setZero(); - ; // nothing - } - - return support; -} - -// Explicit instantiation -// clang-format off -template Vec3f HPP_FCL_DLLAPI getSupport(const ShapeBase*, const Vec3f&, int&); -template Vec3f HPP_FCL_DLLAPI getSupport(const ShapeBase*, const Vec3f&, int&); -// clang-format on - -#undef CALL_GET_SHAPE_SUPPORT - -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]) { - assert(dir.norm() > Eigen::NumTraits::epsilon()); - getShapeSupport<_SupportOptions>(s0, dir, support0, hint[0], &(data[0])); - - if (TransformIsIdentity) - getShapeSupport<_SupportOptions>(s1, -dir, support1, hint[1], &(data[1])); - else { - getShapeSupport<_SupportOptions>(s1, -oR1.transpose() * dir, support1, - hint[1], &(data[1])); - support1 = oR1 * support1 + ot1; - } -} - -template -void getSupportFuncTpl(const MinkowskiDiff& md, const Vec3f& dir, - Vec3f& support0, Vec3f& support1, - support_func_guess_t& hint, - MinkowskiDiff::ShapeData data[2]) { - getSupportTpl( - static_cast(md.shapes[0]), - static_cast(md.shapes[1]), md.oR1, md.ot1, dir, support0, - support1, hint, data); -} - -template -MinkowskiDiff::GetSupportFunction makeGetSupportFunction1( - const ShapeBase* s1, bool identity, - Eigen::Array& swept_sphere_radius, - MinkowskiDiff::ShapeData data[2]) { - if (_SupportOptions == SupportOptions::WithSweptSphere) { - // No need to store the information of swept sphere radius - swept_sphere_radius[1] = 0; - } else { - // We store the information of swept sphere radius. - // GJK and EPA will use this information to correct the solution they find. - swept_sphere_radius[1] = s1->getSweptSphereRadius(); - } - - switch (s1->getNodeType()) { - case GEOM_TRIANGLE: - if (identity) - return getSupportFuncTpl; - else - return getSupportFuncTpl; - case GEOM_BOX: - if (identity) - return getSupportFuncTpl; - else - return getSupportFuncTpl; - case GEOM_SPHERE: - if (_SupportOptions == SupportOptions::NoSweptSphere) { - // Sphere can be considered a swept-sphere point. - swept_sphere_radius[1] += static_cast(s1)->radius; - } - if (identity) - return getSupportFuncTpl; - else - return getSupportFuncTpl; - case GEOM_ELLIPSOID: - if (identity) - return getSupportFuncTpl; - else - return getSupportFuncTpl; - case GEOM_CAPSULE: - if (_SupportOptions == SupportOptions::NoSweptSphere) { - // Sphere can be considered as a swept-sphere segment. - swept_sphere_radius[1] += static_cast(s1)->radius; - } - if (identity) - return getSupportFuncTpl; - else - return getSupportFuncTpl; - case GEOM_CONE: - if (identity) - return getSupportFuncTpl; - else - return getSupportFuncTpl; - case GEOM_CYLINDER: - if (identity) - return getSupportFuncTpl; - else - return getSupportFuncTpl; - case GEOM_CONVEX: { - const ConvexBase* convex1 = static_cast(s1); - if (static_cast(convex1->num_points) > - ConvexBase::num_vertices_large_convex_threshold) { - data[1].visited.assign(convex1->num_points, false); - if (identity) - return getSupportFuncTpl; - else - return getSupportFuncTpl; - } else { - if (identity) - return getSupportFuncTpl; - else - return getSupportFuncTpl; - } - } - default: - HPP_FCL_THROW_PRETTY("Unsupported geometric shape.", std::logic_error); - } -} - -template -MinkowskiDiff::GetSupportFunction makeGetSupportFunction0( - const ShapeBase* s0, const ShapeBase* s1, bool identity, - Eigen::Array& swept_sphere_radius, - MinkowskiDiff::ShapeData data[2]) { - if (_SupportOptions == SupportOptions::WithSweptSphere) { - // No need to store the information of swept sphere radius - swept_sphere_radius[0] = 0; - } else { - // We store the information of swept sphere radius. - // GJK and EPA will use this information to correct the solution they find. - swept_sphere_radius[0] = s0->getSweptSphereRadius(); - } - - switch (s0->getNodeType()) { - case GEOM_TRIANGLE: - return makeGetSupportFunction1( - s1, identity, swept_sphere_radius, data); - break; - case GEOM_BOX: - return makeGetSupportFunction1( - s1, identity, swept_sphere_radius, data); - break; - case GEOM_SPHERE: - if (_SupportOptions == SupportOptions::NoSweptSphere) { - // Sphere can always be considered as a swept-sphere point. - swept_sphere_radius[0] += static_cast(s0)->radius; - } - return makeGetSupportFunction1( - s1, identity, swept_sphere_radius, data); - break; - case GEOM_ELLIPSOID: - return makeGetSupportFunction1( - s1, identity, swept_sphere_radius, data); - break; - case GEOM_CAPSULE: - if (_SupportOptions == SupportOptions::NoSweptSphere) { - // Capsule can always be considered as a swept-sphere segment. - swept_sphere_radius[0] += static_cast(s0)->radius; - } - return makeGetSupportFunction1( - s1, identity, swept_sphere_radius, data); - break; - case GEOM_CONE: - return makeGetSupportFunction1( - s1, identity, swept_sphere_radius, data); - break; - case GEOM_CYLINDER: - return makeGetSupportFunction1( - s1, identity, swept_sphere_radius, data); - break; - case GEOM_CONVEX: { - const ConvexBase* convex0 = static_cast(s0); - if (static_cast(convex0->num_points) > - ConvexBase::num_vertices_large_convex_threshold) { - data[0].visited.assign(convex0->num_points, false); - return makeGetSupportFunction1( - s1, identity, swept_sphere_radius, data); - } else - return makeGetSupportFunction1( - s1, identity, swept_sphere_radius, data); - break; - } - default: - HPP_FCL_THROW_PRETTY("Unsupported geometric shape", std::logic_error); - } -} - -bool getNormalizeSupportDirection(const ShapeBase* shape) { - switch (shape->getNodeType()) { - case GEOM_TRIANGLE: - return (bool)shape_traits::NeedNesterovNormalizeHeuristic; - break; - case GEOM_BOX: - return (bool)shape_traits::NeedNesterovNormalizeHeuristic; - break; - case GEOM_SPHERE: - return (bool)shape_traits::NeedNesterovNormalizeHeuristic; - break; - case GEOM_ELLIPSOID: - return (bool)shape_traits::NeedNesterovNormalizeHeuristic; - break; - case GEOM_CAPSULE: - return (bool)shape_traits::NeedNesterovNormalizeHeuristic; - break; - case GEOM_CONE: - return (bool)shape_traits::NeedNesterovNormalizeHeuristic; - break; - case GEOM_CYLINDER: - return (bool)shape_traits::NeedNesterovNormalizeHeuristic; - break; - case GEOM_CONVEX: - return (bool)shape_traits::NeedNesterovNormalizeHeuristic; - break; - default: - HPP_FCL_THROW_PRETTY("Unsupported geometric shape", std::logic_error); - } -} - -void getNormalizeSupportDirectionFromShapes(const ShapeBase* shape0, - const ShapeBase* shape1, - bool& normalize_support_direction) { - normalize_support_direction = getNormalizeSupportDirection(shape0) && - getNormalizeSupportDirection(shape1); -} - -template -void MinkowskiDiff::set(const ShapeBase* shape0, const ShapeBase* shape1, - const Transform3f& tf0, const Transform3f& tf1) { - shapes[0] = shape0; - shapes[1] = shape1; - getNormalizeSupportDirectionFromShapes(shape0, shape1, - normalize_support_direction); - - oR1.noalias() = tf0.getRotation().transpose() * tf1.getRotation(); - ot1.noalias() = tf0.getRotation().transpose() * - (tf1.getTranslation() - tf0.getTranslation()); - - bool identity = (oR1.isIdentity() && ot1.isZero()); - - getSupportFunc = makeGetSupportFunction0<_SupportOptions>( - shape0, shape1, identity, swept_sphere_radius, data); -} - -template -void MinkowskiDiff::set(const ShapeBase* shape0, const ShapeBase* shape1) { - shapes[0] = shape0; - shapes[1] = shape1; - getNormalizeSupportDirectionFromShapes(shape0, shape1, - normalize_support_direction); - - oR1.setIdentity(); - ot1.setZero(); - - getSupportFunc = makeGetSupportFunction0<_SupportOptions>( - shape0, shape1, true, swept_sphere_radius, data); -} - -// Explicit instantiation -// clang-format off -template void HPP_FCL_DLLAPI MinkowskiDiff::set(const ShapeBase*, const ShapeBase*); -template void HPP_FCL_DLLAPI MinkowskiDiff::set(const ShapeBase*, const ShapeBase*); - -template void HPP_FCL_DLLAPI MinkowskiDiff::set(const ShapeBase*, const ShapeBase*, const Transform3f&, const Transform3f&); -template void HPP_FCL_DLLAPI MinkowskiDiff::set(const ShapeBase*, const ShapeBase*, const Transform3f&, const Transform3f&); - -template Vec3f HPP_FCL_DLLAPI MinkowskiDiff::support0(const Vec3f&, int&) const; -template Vec3f HPP_FCL_DLLAPI MinkowskiDiff::support0(const Vec3f&, int&) const; - -template Vec3f HPP_FCL_DLLAPI MinkowskiDiff::support1(const Vec3f&, int&) const; -template Vec3f HPP_FCL_DLLAPI MinkowskiDiff::support1(const Vec3f&, int&) const; -// clang-format on - void GJK::initialize() { distance_upper_bound = (std::numeric_limits::max)(); gjk_variant = GJKVariant::DefaultGJK; @@ -1632,6 +1021,7 @@ void EPA::reset(size_t max_iterations_, FCL_REAL tolerance_) { fc_store.resize(2 * max_iterations + 4); status = DidNotRun; normal.setZero(); + support_hint.setZero(); depth = 0; closest_face = nullptr; result.reset(); @@ -1765,7 +1155,7 @@ EPA::SimplexFace* EPA::findClosestFace() { EPA::Status EPA::evaluate(GJK& gjk, const Vec3f& guess) { GJK::Simplex& simplex = *gjk.getSimplex(); - support_func_guess_t hint(gjk.support_hint); + support_hint = gjk.support_hint; // TODO(louis): we might want to start with a hexahedron if the // simplex given by GJK is of rank <= 3. @@ -1835,7 +1225,7 @@ EPA::Status EPA::evaluate(GJK& gjk, const Vec3f& guess) { closest_face->pass = ++pass; // At the moment, SimplexF.n is always normalized. This could be revised // in the future... - gjk.getSupport(closest_face->n, w, hint); + gjk.getSupport(closest_face->n, w, support_hint); // Step 2: check for convergence. // ------------------------------ @@ -2087,25 +1477,25 @@ void ConvexBase::buildSupportWarmStart() { ConvexBase::num_support_warm_starts); Vec3f axiis(0, 0, 0); + details::ShapeSupportData support_data; + int support_hint = 0; for (int i = 0; i < 3; ++i) { axiis(i) = 1; { Vec3f support; - int support_index{0}; hpp::fcl::details::getShapeSupport(this, axiis, support, - support_index, nullptr); + support_hint, support_data); this->support_warm_starts.points.emplace_back(support); - this->support_warm_starts.indices.emplace_back(support_index); + this->support_warm_starts.indices.emplace_back(support_hint); } axiis(i) = -1; { Vec3f support; - int support_index{0}; hpp::fcl::details::getShapeSupport(this, axiis, support, - support_index, nullptr); + support_hint, support_data); this->support_warm_starts.points.emplace_back(support); - this->support_warm_starts.indices.emplace_back(support_index); + this->support_warm_starts.indices.emplace_back(support_hint); } axiis(i) = 0; @@ -2119,20 +1509,18 @@ void ConvexBase::buildSupportWarmStart() { for (size_t ei_index = 0; ei_index < 4; ++ei_index) { { Vec3f support; - int support_index{0}; hpp::fcl::details::getShapeSupport(this, eis[ei_index], support, - support_index, nullptr); + support_hint, support_data); this->support_warm_starts.points.emplace_back(support); - this->support_warm_starts.indices.emplace_back(support_index); + this->support_warm_starts.indices.emplace_back(support_hint); } { Vec3f support; - int support_index{0}; hpp::fcl::details::getShapeSupport(this, -eis[ei_index], support, - support_index, nullptr); + support_hint, support_data); this->support_warm_starts.points.emplace_back(support); - this->support_warm_starts.indices.emplace_back(support_index); + this->support_warm_starts.indices.emplace_back(support_hint); } } diff --git a/src/narrowphase/minkowski_difference.cpp b/src/narrowphase/minkowski_difference.cpp new file mode 100644 index 000000000..4db64acd1 --- /dev/null +++ b/src/narrowphase/minkowski_difference.cpp @@ -0,0 +1,325 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2015, Open Source Robotics Foundation + * Copyright (c) 2021-2024, 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. + */ + +/** \authors Jia Pan, Florent Lamiraux, Josef Mirabel, Louis Montaut */ + +#include "hpp/fcl/narrowphase/minkowski_difference.h" +#include "hpp/fcl/shape/geometric_shapes_traits.h" + +namespace hpp { +namespace fcl { +namespace details { + +// ============================================================================ +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, + ShapeSupportData data[2]) { + assert(dir.norm() > Eigen::NumTraits::epsilon()); + getShapeSupport<_SupportOptions>(s0, dir, support0, hint[0], data[0]); + + if (TransformIsIdentity) { + getShapeSupport<_SupportOptions>(s1, -dir, support1, hint[1], data[1]); + } else { + getShapeSupport<_SupportOptions>(s1, -oR1.transpose() * dir, support1, + hint[1], data[1]); + support1 = oR1 * support1 + ot1; + } +} + +// ============================================================================ +template +void getSupportFuncTpl(const MinkowskiDiff& md, const Vec3f& dir, + Vec3f& support0, Vec3f& support1, + support_func_guess_t& hint, ShapeSupportData data[2]) { + getSupportTpl( + static_cast(md.shapes[0]), + static_cast(md.shapes[1]), md.oR1, md.ot1, dir, support0, + support1, hint, data); +} + +// ============================================================================ +template +MinkowskiDiff::GetSupportFunction makeGetSupportFunction1( + const ShapeBase* s1, bool identity, + Eigen::Array& swept_sphere_radius, + ShapeSupportData data[2]) { + if (_SupportOptions == SupportOptions::WithSweptSphere) { + // No need to store the information of swept sphere radius + swept_sphere_radius[1] = 0; + } else { + // We store the information of swept sphere radius. + // GJK and EPA will use this information to correct the solution they find. + swept_sphere_radius[1] = s1->getSweptSphereRadius(); + } + + switch (s1->getNodeType()) { + case GEOM_TRIANGLE: + if (identity) + return getSupportFuncTpl; + else + return getSupportFuncTpl; + case GEOM_BOX: + if (identity) + return getSupportFuncTpl; + else + return getSupportFuncTpl; + case GEOM_SPHERE: + if (_SupportOptions == SupportOptions::NoSweptSphere) { + // Sphere can be considered a swept-sphere point. + swept_sphere_radius[1] += static_cast(s1)->radius; + } + if (identity) + return getSupportFuncTpl; + else + return getSupportFuncTpl; + case GEOM_ELLIPSOID: + if (identity) + return getSupportFuncTpl; + else + return getSupportFuncTpl; + case GEOM_CAPSULE: + if (_SupportOptions == SupportOptions::NoSweptSphere) { + // Sphere can be considered as a swept-sphere segment. + swept_sphere_radius[1] += static_cast(s1)->radius; + } + if (identity) + return getSupportFuncTpl; + else + return getSupportFuncTpl; + case GEOM_CONE: + if (identity) + return getSupportFuncTpl; + else + return getSupportFuncTpl; + case GEOM_CYLINDER: + if (identity) + return getSupportFuncTpl; + else + return getSupportFuncTpl; + case GEOM_CONVEX: { + const ConvexBase* convex1 = static_cast(s1); + if (static_cast(convex1->num_points) > + ConvexBase::num_vertices_large_convex_threshold) { + data[1].visited.assign(convex1->num_points, false); + data[1].last_dir.setZero(); + if (identity) + return getSupportFuncTpl; + else + return getSupportFuncTpl; + } else { + if (identity) + return getSupportFuncTpl; + else + return getSupportFuncTpl; + } + } + default: + HPP_FCL_THROW_PRETTY("Unsupported geometric shape.", std::logic_error); + } +} + +// ============================================================================ +template +MinkowskiDiff::GetSupportFunction makeGetSupportFunction0( + const ShapeBase* s0, const ShapeBase* s1, bool identity, + Eigen::Array& swept_sphere_radius, + ShapeSupportData data[2]) { + if (_SupportOptions == SupportOptions::WithSweptSphere) { + // No need to store the information of swept sphere radius + swept_sphere_radius[0] = 0; + } else { + // We store the information of swept sphere radius. + // GJK and EPA will use this information to correct the solution they find. + swept_sphere_radius[0] = s0->getSweptSphereRadius(); + } + + switch (s0->getNodeType()) { + case GEOM_TRIANGLE: + return makeGetSupportFunction1( + s1, identity, swept_sphere_radius, data); + break; + case GEOM_BOX: + return makeGetSupportFunction1( + s1, identity, swept_sphere_radius, data); + break; + case GEOM_SPHERE: + if (_SupportOptions == SupportOptions::NoSweptSphere) { + // Sphere can always be considered as a swept-sphere point. + swept_sphere_radius[0] += static_cast(s0)->radius; + } + return makeGetSupportFunction1( + s1, identity, swept_sphere_radius, data); + break; + case GEOM_ELLIPSOID: + return makeGetSupportFunction1( + s1, identity, swept_sphere_radius, data); + break; + case GEOM_CAPSULE: + if (_SupportOptions == SupportOptions::NoSweptSphere) { + // Capsule can always be considered as a swept-sphere segment. + swept_sphere_radius[0] += static_cast(s0)->radius; + } + return makeGetSupportFunction1( + s1, identity, swept_sphere_radius, data); + break; + case GEOM_CONE: + return makeGetSupportFunction1( + s1, identity, swept_sphere_radius, data); + break; + case GEOM_CYLINDER: + return makeGetSupportFunction1( + s1, identity, swept_sphere_radius, data); + break; + case GEOM_CONVEX: { + const ConvexBase* convex0 = static_cast(s0); + if (static_cast(convex0->num_points) > + ConvexBase::num_vertices_large_convex_threshold) { + data[0].visited.assign(convex0->num_points, false); + data[0].last_dir.setZero(); + return makeGetSupportFunction1( + s1, identity, swept_sphere_radius, data); + } else + return makeGetSupportFunction1( + s1, identity, swept_sphere_radius, data); + break; + } + default: + HPP_FCL_THROW_PRETTY("Unsupported geometric shape", std::logic_error); + } +} + +// ============================================================================ +bool getNormalizeSupportDirection(const ShapeBase* shape) { + switch (shape->getNodeType()) { + case GEOM_TRIANGLE: + return (bool)shape_traits::NeedNesterovNormalizeHeuristic; + break; + case GEOM_BOX: + return (bool)shape_traits::NeedNesterovNormalizeHeuristic; + break; + case GEOM_SPHERE: + return (bool)shape_traits::NeedNesterovNormalizeHeuristic; + break; + case GEOM_ELLIPSOID: + return (bool)shape_traits::NeedNesterovNormalizeHeuristic; + break; + case GEOM_CAPSULE: + return (bool)shape_traits::NeedNesterovNormalizeHeuristic; + break; + case GEOM_CONE: + return (bool)shape_traits::NeedNesterovNormalizeHeuristic; + break; + case GEOM_CYLINDER: + return (bool)shape_traits::NeedNesterovNormalizeHeuristic; + break; + case GEOM_CONVEX: + return (bool)shape_traits::NeedNesterovNormalizeHeuristic; + break; + default: + HPP_FCL_THROW_PRETTY("Unsupported geometric shape", std::logic_error); + } +} + +// ============================================================================ +void getNormalizeSupportDirectionFromShapes(const ShapeBase* shape0, + const ShapeBase* shape1, + bool& normalize_support_direction) { + normalize_support_direction = getNormalizeSupportDirection(shape0) && + getNormalizeSupportDirection(shape1); +} + +// ============================================================================ +template +void MinkowskiDiff::set(const ShapeBase* shape0, const ShapeBase* shape1, + const Transform3f& tf0, const Transform3f& tf1) { + shapes[0] = shape0; + shapes[1] = shape1; + getNormalizeSupportDirectionFromShapes(shape0, shape1, + normalize_support_direction); + + oR1.noalias() = tf0.getRotation().transpose() * tf1.getRotation(); + ot1.noalias() = tf0.getRotation().transpose() * + (tf1.getTranslation() - tf0.getTranslation()); + + bool identity = (oR1.isIdentity() && ot1.isZero()); + + getSupportFunc = makeGetSupportFunction0<_SupportOptions>( + shape0, shape1, identity, swept_sphere_radius, data); +} +// clang-format off +template void HPP_FCL_DLLAPI MinkowskiDiff::set(const ShapeBase*, const ShapeBase*); + +template void HPP_FCL_DLLAPI MinkowskiDiff::set(const ShapeBase*, const ShapeBase*); +// clang-format on + +// ============================================================================ +template +void MinkowskiDiff::set(const ShapeBase* shape0, const ShapeBase* shape1) { + shapes[0] = shape0; + shapes[1] = shape1; + getNormalizeSupportDirectionFromShapes(shape0, shape1, + normalize_support_direction); + + oR1.setIdentity(); + ot1.setZero(); + + getSupportFunc = makeGetSupportFunction0<_SupportOptions>( + shape0, shape1, true, swept_sphere_radius, data); +} +// clang-format off +template void HPP_FCL_DLLAPI MinkowskiDiff::set(const ShapeBase*, const ShapeBase*, const Transform3f&, const Transform3f&); + +template void HPP_FCL_DLLAPI MinkowskiDiff::set(const ShapeBase*, const ShapeBase*, const Transform3f&, const Transform3f&); +// clang-format on + +// ============================================================================ +// clang-format off +template Vec3f HPP_FCL_DLLAPI MinkowskiDiff::support0(const Vec3f&, int&) const; + +template Vec3f HPP_FCL_DLLAPI MinkowskiDiff::support0(const Vec3f&, int&) const; + +template Vec3f HPP_FCL_DLLAPI MinkowskiDiff::support1(const Vec3f&, int&) const; + +template Vec3f HPP_FCL_DLLAPI MinkowskiDiff::support1(const Vec3f&, int&) const; +// clang-format on + +} // namespace details +} // namespace fcl +} // namespace hpp diff --git a/src/narrowphase/support_functions.cpp b/src/narrowphase/support_functions.cpp new file mode 100644 index 000000000..c215654d9 --- /dev/null +++ b/src/narrowphase/support_functions.cpp @@ -0,0 +1,1070 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2015, Open Source Robotics Foundation + * Copyright (c) 2021-2024, 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. + */ + +/** \authors Jia Pan, Florent Lamiraux, Josef Mirabel, Louis Montaut */ + +#include "hpp/fcl/narrowphase/support_functions.h" + +#include + +namespace hpp { +namespace fcl { +namespace details { + +// ============================================================================ +#define CALL_GET_SHAPE_SUPPORT(ShapeType) \ + getShapeSupport<_SupportOptions>(static_cast(shape), dir, \ + support, hint, support_data) +template +Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, int& hint) { + Vec3f support; + ShapeSupportData support_data; + switch (shape->getNodeType()) { + case GEOM_TRIANGLE: + CALL_GET_SHAPE_SUPPORT(TriangleP); + break; + case GEOM_BOX: + CALL_GET_SHAPE_SUPPORT(Box); + break; + case GEOM_SPHERE: + CALL_GET_SHAPE_SUPPORT(Sphere); + break; + case GEOM_ELLIPSOID: + CALL_GET_SHAPE_SUPPORT(Ellipsoid); + break; + case GEOM_CAPSULE: + CALL_GET_SHAPE_SUPPORT(Capsule); + break; + case GEOM_CONE: + CALL_GET_SHAPE_SUPPORT(Cone); + break; + case GEOM_CYLINDER: + CALL_GET_SHAPE_SUPPORT(Cylinder); + break; + case GEOM_CONVEX: + CALL_GET_SHAPE_SUPPORT(ConvexBase); + break; + case GEOM_PLANE: + case GEOM_HALFSPACE: + default: + support.setZero(); + ; // nothing + } + + return support; +} +#undef CALL_GET_SHAPE_SUPPORT + +// Explicit instantiation +// clang-format off +template HPP_FCL_DLLAPI Vec3f getSupport(const ShapeBase*, const Vec3f&, int&); + +template HPP_FCL_DLLAPI Vec3f getSupport(const ShapeBase*, const Vec3f&, int&); +// clang-format on + +// ============================================================================ +#define getShapeSupportTplInstantiation(ShapeType) \ + template HPP_FCL_DLLAPI void getShapeSupport( \ + const ShapeType* shape_, const Vec3f& dir, Vec3f& support, int& hint, \ + ShapeSupportData& support_data); \ + \ + template HPP_FCL_DLLAPI void \ + getShapeSupport( \ + const ShapeType* shape_, const Vec3f& dir, Vec3f& support, int& hint, \ + ShapeSupportData& support_data); + +// ============================================================================ +template +void getShapeSupport(const TriangleP* triangle, const Vec3f& dir, + Vec3f& support, int& /*unused*/, + ShapeSupportData& /*unused*/) { + FCL_REAL dota = dir.dot(triangle->a); + FCL_REAL dotb = dir.dot(triangle->b); + FCL_REAL dotc = dir.dot(triangle->c); + if (dota > dotb) { + if (dotc > dota) { + support = triangle->c; + } else { + support = triangle->a; + } + } else { + if (dotc > dotb) { + support = triangle->c; + } else { + support = triangle->b; + } + } + + if (_SupportOptions == SupportOptions::WithSweptSphere) { + support += triangle->getSweptSphereRadius() * dir.normalized(); + } +} +getShapeSupportTplInstantiation(TriangleP); + +// ============================================================================ +template +inline void getShapeSupport(const Box* box, const Vec3f& dir, Vec3f& support, + int& /*unused*/, ShapeSupportData& /*unused*/) { + // The inflate value is simply to make the specialized functions with box + // have a preferred side for edge cases. + static const FCL_REAL inflate = (dir.array() == 0).any() ? 1 + 1e-10 : 1.; + static const FCL_REAL dummy_precision = + Eigen::NumTraits::dummy_precision(); + Vec3f support1 = (dir.array() > dummy_precision).select(box->halfSide, 0); + Vec3f support2 = + (dir.array() < -dummy_precision).select(-inflate * box->halfSide, 0); + support.noalias() = support1 + support2; + + if (_SupportOptions == SupportOptions::WithSweptSphere) { + support += box->getSweptSphereRadius() * dir.normalized(); + } +} +getShapeSupportTplInstantiation(Box); + +// ============================================================================ +template +inline void getShapeSupport(const Sphere* sphere, const Vec3f& dir, + Vec3f& support, int& /*unused*/, + ShapeSupportData& /*unused*/) { + if (_SupportOptions == SupportOptions::WithSweptSphere) { + support.noalias() = + (sphere->radius + sphere->getSweptSphereRadius()) * dir.normalized(); + } else { + support.setZero(); + } + + HPP_FCL_UNUSED_VARIABLE(sphere); + HPP_FCL_UNUSED_VARIABLE(dir); +} +getShapeSupportTplInstantiation(Sphere); + +// ============================================================================ +template +inline void getShapeSupport(const Ellipsoid* ellipsoid, const Vec3f& dir, + Vec3f& support, int& /*unused*/, + ShapeSupportData& /*unused*/) { + 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]; + + Vec3f v(a2 * dir[0], b2 * dir[1], c2 * dir[2]); + + FCL_REAL d = std::sqrt(v.dot(dir)); + + support = v / d; + + if (_SupportOptions == SupportOptions::WithSweptSphere) { + support += ellipsoid->getSweptSphereRadius() * dir.normalized(); + } +} +getShapeSupportTplInstantiation(Ellipsoid); + +// ============================================================================ +template +inline void getShapeSupport(const Capsule* capsule, const Vec3f& dir, + Vec3f& support, int& /*unused*/, + ShapeSupportData& /*unused*/) { + static const FCL_REAL dummy_precision = + Eigen::NumTraits::dummy_precision(); + support.setZero(); + if (dir[2] > dummy_precision) { + support[2] = capsule->halfLength; + } else if (dir[2] < -dummy_precision) { + support[2] = -capsule->halfLength; + } + + if (_SupportOptions == SupportOptions::WithSweptSphere) { + support += + (capsule->radius + capsule->getSweptSphereRadius()) * dir.normalized(); + } +} +getShapeSupportTplInstantiation(Capsule); + +// ============================================================================ +template +void getShapeSupport(const Cone* cone, const Vec3f& dir, Vec3f& support, + int& /*unused*/, ShapeSupportData& /*unused*/) { + static const FCL_REAL dummy_precision = + Eigen::NumTraits::dummy_precision(); + + // The cone radius is, for -h < z < h, (h - z) * r / (2*h) + // The inflate value is simply to make the specialized functions with cone + // have a preferred side for edge cases. + static const FCL_REAL inflate = 1 + 1e-10; + FCL_REAL h = cone->halfLength; + FCL_REAL r = cone->radius; + + if (dir.head<2>().isZero(dummy_precision)) { + support.head<2>().setZero(); + if (dir[2] > dummy_precision) { + support[2] = h; + } else { + support[2] = -inflate * h; + } + } else { + FCL_REAL zdist = dir[0] * dir[0] + dir[1] * dir[1]; + FCL_REAL len = zdist + dir[2] * dir[2]; + zdist = std::sqrt(zdist); + + if (dir[2] <= 0) { + FCL_REAL rad = r / zdist; + support.head<2>() = rad * dir.head<2>(); + support[2] = -h; + } else { + len = std::sqrt(len); + FCL_REAL sin_a = r / std::sqrt(r * r + 4 * h * h); + + if (dir[2] > len * sin_a) + support << 0, 0, h; + else { + FCL_REAL rad = r / zdist; + support.head<2>() = rad * dir.head<2>(); + support[2] = -h; + } + } + } + + if (_SupportOptions == SupportOptions::WithSweptSphere) { + support += cone->getSweptSphereRadius() * dir.normalized(); + } +} +getShapeSupportTplInstantiation(Cone); + +// ============================================================================ +template +void getShapeSupport(const Cylinder* cylinder, const Vec3f& dir, Vec3f& support, + int& /*unused*/, ShapeSupportData& /*unused*/) { + static const FCL_REAL dummy_precision = + Eigen::NumTraits::dummy_precision(); + + // The inflate value is simply to make the specialized functions with cylinder + // have a preferred side for edge cases. + static const FCL_REAL inflate = 1 + 1e-10; + FCL_REAL half_h = cylinder->halfLength; + FCL_REAL r = cylinder->radius; + + const bool dir_is_aligned_with_z = dir.head<2>().isZero(dummy_precision); + if (dir_is_aligned_with_z) half_h *= inflate; + + if (dir[2] > dummy_precision) { + support[2] = half_h; + } else if (dir[2] < -dummy_precision) { + support[2] = -half_h; + } else { + support[2] = 0; + r *= inflate; + } + + if (dir_is_aligned_with_z) { + support.head<2>().setZero(); + } else { + support.head<2>() = dir.head<2>().normalized() * r; + } + + assert(fabs(support[0] * dir[1] - support[1] * dir[0]) < + sqrt(std::numeric_limits::epsilon())); + + if (_SupportOptions == SupportOptions::WithSweptSphere) { + support += cylinder->getSweptSphereRadius() * dir.normalized(); + } +} +getShapeSupportTplInstantiation(Cylinder); + +// ============================================================================ +template +void getShapeSupportLog(const ConvexBase* convex, const Vec3f& dir, + Vec3f& support, int& hint, + ShapeSupportData& support_data) { + assert(convex->neighbors != nullptr && "Convex has no neighbors."); + + // Use warm start if current support direction is distant from last support + // direction. + const double use_warm_start_threshold = 0.9; + Vec3f dir_normalized = dir.normalized(); + if (!support_data.last_dir.isZero() && + !convex->support_warm_starts.points.empty() && + support_data.last_dir.dot(dir_normalized) < use_warm_start_threshold) { + // Change hint if last dir is too far from current dir. + FCL_REAL maxdot = convex->support_warm_starts.points[0].dot(dir); + hint = convex->support_warm_starts.indices[0]; + for (size_t i = 1; i < convex->support_warm_starts.points.size(); ++i) { + FCL_REAL dot = convex->support_warm_starts.points[i].dot(dir); + if (dot > maxdot) { + maxdot = dot; + hint = convex->support_warm_starts.indices[i]; + } + } + } + support_data.last_dir = dir_normalized; + + const std::vector& pts = *(convex->points); + const std::vector& nn = *(convex->neighbors); + + if (hint < 0 || hint >= (int)convex->num_points) { + hint = 0; + } + FCL_REAL maxdot = pts[static_cast(hint)].dot(dir); + std::vector& visited = support_data.visited; + if (support_data.visited.size() == convex->num_points) { + std::fill(visited.begin(), visited.end(), false); + } else { + // std::vector::assign not only assigns the values of the vector but also + // resizes the vector. So if `visited` has not been set up yet, this makes + // sure the size convex's points and visited are identical. + support_data.visited.assign(convex->num_points, false); + } + visited[static_cast(hint)] = true; + // When the first face is orthogonal to dir, all the dot products will be + // equal. Yet, the neighbors must be visited. + bool found = true; + bool loose_check = true; + while (found) { + const ConvexBase::Neighbors& n = nn[static_cast(hint)]; + found = false; + for (int in = 0; in < n.count(); ++in) { + const unsigned int ip = n[in]; + if (visited[ip]) continue; + visited[ip] = true; + const FCL_REAL dot = pts[ip].dot(dir); + bool better = false; + if (dot > maxdot) { + better = true; + loose_check = false; + } else if (loose_check && dot == maxdot) + better = true; + if (better) { + maxdot = dot; + hint = static_cast(ip); + found = true; + } + } + } + + support = pts[static_cast(hint)]; + + if (_SupportOptions == SupportOptions::WithSweptSphere) { + support += convex->getSweptSphereRadius() * dir.normalized(); + } +} + +// ============================================================================ +template +void getShapeSupportLinear(const ConvexBase* convex, const Vec3f& dir, + Vec3f& support, int& hint, + ShapeSupportData& /*unused*/) { + const std::vector& pts = *(convex->points); + + hint = 0; + FCL_REAL maxdot = pts[0].dot(dir); + for (int i = 1; i < (int)convex->num_points; ++i) { + FCL_REAL dot = pts[static_cast(i)].dot(dir); + if (dot > maxdot) { + maxdot = dot; + hint = i; + } + } + + support = pts[static_cast(hint)]; + + if (_SupportOptions == SupportOptions::WithSweptSphere) { + support += convex->getSweptSphereRadius() * dir.normalized(); + } +} + +// ============================================================================ +template +void getShapeSupport(const ConvexBase* convex, const Vec3f& dir, Vec3f& support, + int& hint, ShapeSupportData& support_data) { + // TODO add benchmark to set a proper value for switching between linear and + // logarithmic. + if (convex->num_points > ConvexBase::num_vertices_large_convex_threshold && + convex->neighbors != nullptr) { + getShapeSupportLog<_SupportOptions>(convex, dir, support, hint, + support_data); + } else { + getShapeSupportLinear<_SupportOptions>(convex, dir, support, hint, + support_data); + } +} +getShapeSupportTplInstantiation(ConvexBase); + +// ============================================================================ +template +inline void getShapeSupport(const SmallConvex* convex, const Vec3f& dir, + Vec3f& support, int& hint, + ShapeSupportData& support_data) { + getShapeSupportLinear<_SupportOptions>( + reinterpret_cast(convex), dir, support, hint, + support_data); +} +getShapeSupportTplInstantiation(SmallConvex); + +// ============================================================================ +template +inline void getShapeSupport(const LargeConvex* convex, const Vec3f& dir, + Vec3f& support, int& hint, + ShapeSupportData& support_data) { + getShapeSupportLog<_SupportOptions>( + reinterpret_cast(convex), dir, support, hint, + support_data); +} +getShapeSupportTplInstantiation(LargeConvex); + +// ============================================================================ +#define CALL_GET_SHAPE_SUPPORT_SET(ShapeType) \ + getShapeSupportSet<_SupportOptions>(static_cast(shape), \ + support_set, hint, support_data, \ + max_num_supports, tol) +template +void getSupportSet(const ShapeBase* shape, SupportSet& support_set, int& hint, + size_t max_num_supports, FCL_REAL tol) { + ShapeSupportData support_data; + switch (shape->getNodeType()) { + case GEOM_TRIANGLE: + CALL_GET_SHAPE_SUPPORT_SET(TriangleP); + break; + case GEOM_BOX: + CALL_GET_SHAPE_SUPPORT_SET(Box); + break; + case GEOM_SPHERE: + CALL_GET_SHAPE_SUPPORT_SET(Sphere); + break; + case GEOM_ELLIPSOID: + CALL_GET_SHAPE_SUPPORT_SET(Ellipsoid); + break; + case GEOM_CAPSULE: + CALL_GET_SHAPE_SUPPORT_SET(Capsule); + break; + case GEOM_CONE: + CALL_GET_SHAPE_SUPPORT_SET(Cone); + break; + case GEOM_CYLINDER: + CALL_GET_SHAPE_SUPPORT_SET(Cylinder); + break; + case GEOM_CONVEX: + CALL_GET_SHAPE_SUPPORT_SET(ConvexBase); + break; + case GEOM_PLANE: + case GEOM_HALFSPACE: + default:; // nothing + } +} +#undef CALL_GET_SHAPE_SUPPORT + +// Explicit instantiation +// clang-format off +template HPP_FCL_DLLAPI void getSupportSet(const ShapeBase*, SupportSet&, int&, size_t, FCL_REAL); + +template HPP_FCL_DLLAPI void getSupportSet(const ShapeBase*, SupportSet&, int&, size_t, FCL_REAL); +// clang-format on + +// ============================================================================ +#define getShapeSupportSetTplInstantiation(ShapeType) \ + template HPP_FCL_DLLAPI void \ + getShapeSupportSet( \ + const ShapeType* shape_, SupportSet& support_set, int& hint, \ + ShapeSupportData& data, size_t num_sampled_supports, FCL_REAL tol); \ + \ + template HPP_FCL_DLLAPI void \ + getShapeSupportSet( \ + const ShapeType* shape_, SupportSet& support_set, int& hint, \ + ShapeSupportData& data, size_t num_sampled_supports, FCL_REAL tol); + +// ============================================================================ +template +void getShapeSupportSet(const TriangleP* triangle, SupportSet& support_set, + int& hint /*unused*/, + ShapeSupportData& support_data /*unused*/, + size_t /*unused*/, FCL_REAL tol) { + assert(tol > 0); + support_set.clear(); + + Vec3f support; + const Vec3f& support_dir = support_set.getNormal(); + // We simply want to compute the support value, no need to take the + // swept-sphere radius into account. + getShapeSupport(triangle, support_dir, support, + hint, support_data); + const FCL_REAL support_value = support.dot(support_dir); + + if (support_value - support_dir.dot(triangle->a) < tol) { + // Note: at the moment, it's useless to take into account the + // swept-sphere radius, but in the future we might want to store the + // offsets to the plane in `SupportSet`. + if (_SupportOptions == SupportOptions::WithSweptSphere) { + support_set.addPoint(triangle->a + + triangle->getSweptSphereRadius() * support_dir); + } else { + support_set.addPoint(triangle->a); + } + } + if (support_value - support_dir.dot(triangle->b) < tol) { + if (_SupportOptions == SupportOptions::WithSweptSphere) { + support_set.addPoint(triangle->b + + triangle->getSweptSphereRadius() * support_dir); + } else { + support_set.addPoint(triangle->b); + } + } + if (support_value - support_dir.dot(triangle->c) < tol) { + if (_SupportOptions == SupportOptions::WithSweptSphere) { + support_set.addPoint(triangle->c + + triangle->getSweptSphereRadius() * support_dir); + } else { + support_set.addPoint(triangle->c); + } + } +} +getShapeSupportSetTplInstantiation(TriangleP); + +// ============================================================================ +template +void getShapeSupportSet(const Box* box, SupportSet& support_set, + int& hint /*unused*/, ShapeSupportData& support_data, + size_t /*unused*/, FCL_REAL tol) { + assert(tol > 0); + Vec3f support; + const Vec3f& support_dir = support_set.getNormal(); + getShapeSupport(box, support_dir, support, + hint, support_data); + const FCL_REAL support_value = support.dot(support_dir); + + const FCL_REAL x = box->halfSide[0]; + const FCL_REAL y = box->halfSide[1]; + const FCL_REAL z = box->halfSide[2]; + const std::array corners = { + Vec3f(x, y, z), Vec3f(-x, y, z), Vec3f(-x, -y, z), Vec3f(x, -y, z), + Vec3f(x, y, -z), Vec3f(-x, y, -z), Vec3f(-x, -y, -z), Vec3f(x, -y, -z), + }; + + SupportSet::Polygon& polygon = support_data.polygon; + polygon.clear(); + const Transform3f& tf = support_set.tf; + for (const Vec3f& corner : corners) { + const FCL_REAL val = corner.dot(support_dir); + if (support_value - val < tol) { + if (_SupportOptions == SupportOptions::WithSweptSphere) { + const Vec2f p = + tf.inverseTransform(corner + + box->getSweptSphereRadius() * support_dir) + .template head<2>(); + polygon.emplace_back(p); + } else { + const Vec2f p = tf.inverseTransform(corner).template head<2>(); + polygon.emplace_back(p); + } + } + } + computeSupportSetConvexHull(polygon, support_set.points()); +} +getShapeSupportSetTplInstantiation(Box); + +// ============================================================================ +template +void getShapeSupportSet(const Sphere* sphere, SupportSet& support_set, + int& hint /*unused*/, + ShapeSupportData& support_data /*unused*/, + size_t /*unused*/, FCL_REAL /*unused*/) { + support_set.points().clear(); + + Vec3f support; + const Vec3f& support_dir = support_set.getNormal(); + getShapeSupport<_SupportOptions>(sphere, support_dir, support, hint, + support_data); + support_set.addPoint(support); +} +getShapeSupportSetTplInstantiation(Sphere); + +// ============================================================================ +template +void getShapeSupportSet(const Ellipsoid* ellipsoid, SupportSet& support_set, + int& hint, ShapeSupportData& support_data /*unused*/, + size_t /*unused*/, FCL_REAL /*unused*/) { + support_set.points().clear(); + + Vec3f support; + const Vec3f& support_dir = support_set.getNormal(); + getShapeSupport<_SupportOptions>(ellipsoid, support_dir, support, hint, + support_data); + support_set.addPoint(support); +} +getShapeSupportSetTplInstantiation(Ellipsoid); + +// ============================================================================ +template +void getShapeSupportSet(const Capsule* capsule, SupportSet& support_set, + int& hint /*unused*/, + ShapeSupportData& support_data /*unused*/, + size_t /*unused*/, FCL_REAL tol) { + assert(tol > 0); + support_set.points().clear(); + + Vec3f support; + const Vec3f& support_dir = support_set.getNormal(); + getShapeSupport(capsule, support_dir, support, + hint, support_data); + const FCL_REAL support_value = + support_dir.dot(support + capsule->radius * support_dir); + // The support set of a capsule has either 2 points or 1 point. + // The two candidate points lie at the frontier between the cylinder and + // sphere parts of the capsule. + const FCL_REAL h = capsule->halfLength; + const FCL_REAL r = capsule->radius; + const Vec3f p1(r * support_dir[0], r * support_dir[1], h); + const Vec3f p2(r * support_dir[0], r * support_dir[1], -h); + if ((support_value - support_dir.dot(p1) <= tol) && + (support_value - support_dir.dot(p2) <= tol)) { + if (_SupportOptions == SupportOptions::WithSweptSphere) { + const Vec3f ssr_vec = support_dir * capsule->getSweptSphereRadius(); + support_set.addPoint(p1 + ssr_vec); + support_set.addPoint(p2 + ssr_vec); + } else { + support_set.addPoint(p1); + support_set.addPoint(p2); + } + } else { + if (_SupportOptions == SupportOptions::WithSweptSphere) { + const Vec3f ssr_vec = support_dir * capsule->getSweptSphereRadius(); + support_set.addPoint(support + ssr_vec); + } else { + support_set.addPoint(support); + } + } +} +getShapeSupportSetTplInstantiation(Capsule); + +// ============================================================================ +template +void getShapeSupportSet(const Cone* cone, SupportSet& support_set, + int& hint /*unused*/, + ShapeSupportData& support_data /*unused*/, + size_t num_sampled_supports, FCL_REAL tol) { + assert(tol > 0); + support_set.points().clear(); + + Vec3f support; + const Vec3f& support_dir = support_set.getNormal(); + getShapeSupport(cone, support_dir, support, + hint, support_data); + const FCL_REAL support_value = support.dot(support_dir); + + // If the support direction is perpendicular to the cone's basis, there is an + // infinite amount of support points; otherwise there are up to two support + // points (two if direction is perpendicular to the side of the cone and one + // otherwise). + // + // To check this condition, we look at two points on the cone's basis; these + // two points are symmetrical w.r.t the center of the circle. If both these + // points are tol away from the support plane, then all the points of the + // circle are tol away from the support plane. + const FCL_REAL r = cone->radius; + const FCL_REAL z = -cone->halfLength; + const Vec3f p1(r * support_dir[0], r * support_dir[1], z); + const Vec3f p2(-r * support_dir[0], -r * support_dir[1], z); + + if ((support_value - support_dir.dot(p1) <= tol) && + (support_value - support_dir.dot(p2) <= tol)) { + // If this check passed, support direction is considered perpendicular to + // the basis of the cone. We sample `num_sampled_supports` points on the + // base of the cone. We are guaranteed that these points like at a distance + // tol of the support plane. + const FCL_REAL angle_increment = + 2.0 * (FCL_REAL)(EIGEN_PI) / ((FCL_REAL)(num_sampled_supports)); + for (size_t i = 0; i < num_sampled_supports; ++i) { + const FCL_REAL theta = (FCL_REAL)(i)*angle_increment; + Vec3f point_on_cone_base(r * std::cos(theta), r * std::sin(theta), z); + assert(std::abs(support_dir.dot(support - point_on_cone_base)) <= tol); + if (_SupportOptions == SupportOptions::WithSweptSphere) { + point_on_cone_base += cone->getSweptSphereRadius() * support_dir; + } + support_set.addPoint(point_on_cone_base); + } + } else { + // There are two potential supports to add: the tip of the cone and a point + // on the basis of the cone. We compare each of these points to the support + // value. + Vec3f cone_tip(0, 0, cone->halfLength); + if (support_value - support_dir.dot(cone_tip) <= tol) { + if (_SupportOptions == SupportOptions::WithSweptSphere) { + cone_tip += cone->getSweptSphereRadius() * support_dir; + } + support_set.addPoint(cone_tip); + } + + Vec3f point_on_cone_base = Vec3f(cone->radius * support_dir[0], // + cone->radius * support_dir[1], // + z); + if (support_value - support_dir.dot(point_on_cone_base) <= tol) { + if (_SupportOptions == SupportOptions::WithSweptSphere) { + point_on_cone_base += cone->getSweptSphereRadius() * support_dir; + } + support_set.addPoint(point_on_cone_base); + } + } +} +getShapeSupportSetTplInstantiation(Cone); + +// ============================================================================ +template +void getShapeSupportSet(const Cylinder* cylinder, SupportSet& support_set, + int& hint /*unused*/, + ShapeSupportData& support_data /*unused*/, + size_t num_sampled_supports, FCL_REAL tol) { + assert(tol > 0); + support_set.points().clear(); + + Vec3f support; + const Vec3f& support_dir = support_set.getNormal(); + getShapeSupport(cylinder, support_dir, support, + hint, support_data); + const FCL_REAL support_value = support.dot(support_dir); + + // The following is very similar to what is done for Cone's support set + // computation. + const FCL_REAL r = cylinder->radius; + const FCL_REAL z = + support_dir[2] <= 0 ? -cylinder->halfLength : cylinder->halfLength; + const Vec3f p1(r * support_dir[0], r * support_dir[1], z); + const Vec3f p2(-r * support_dir[0], -r * support_dir[1], z); + + if ((support_value - support_dir.dot(p1) <= tol) && + (support_value - support_dir.dot(p2) <= tol)) { + const FCL_REAL angle_increment = + 2.0 * (FCL_REAL)(EIGEN_PI) / ((FCL_REAL)(num_sampled_supports)); + for (size_t i = 0; i < num_sampled_supports; ++i) { + const FCL_REAL theta = (FCL_REAL)(i)*angle_increment; + Vec3f point_on_cone_base(r * std::cos(theta), r * std::sin(theta), z); + assert(std::abs(support_dir.dot(support - point_on_cone_base)) <= tol); + if (_SupportOptions == SupportOptions::WithSweptSphere) { + point_on_cone_base += cylinder->getSweptSphereRadius() * support_dir; + } + support_set.addPoint(point_on_cone_base); + } + } else { + // There are two potential supports to add: one on each circle bases of the + // cylinder. + Vec3f point_on_lower_circle = Vec3f(cylinder->radius * support_dir[0], // + cylinder->radius * support_dir[1], // + -cylinder->halfLength); + if (support_value - support_dir.dot(point_on_lower_circle) <= tol) { + if (_SupportOptions == SupportOptions::WithSweptSphere) { + point_on_lower_circle += cylinder->getSweptSphereRadius() * support_dir; + } + support_set.addPoint(point_on_lower_circle); + } + + Vec3f point_on_upper_circle = Vec3f(cylinder->radius * support_dir[0], // + cylinder->radius * support_dir[1], // + cylinder->halfLength); + if (support_value - support_dir.dot(point_on_upper_circle) <= tol) { + if (_SupportOptions == SupportOptions::WithSweptSphere) { + point_on_upper_circle += cylinder->getSweptSphereRadius() * support_dir; + } + support_set.addPoint(point_on_upper_circle); + } + } +} +getShapeSupportSetTplInstantiation(Cylinder); + +// ============================================================================ +template +void getShapeSupportSetLinear(const ConvexBase* convex, SupportSet& support_set, + int& hint /*unused*/, + ShapeSupportData& support_data, size_t /*unused*/, + FCL_REAL tol) { + assert(tol > 0); + Vec3f support; + const Vec3f& support_dir = support_set.getNormal(); + getShapeSupport(convex, support_dir, support, + hint, support_data); + const FCL_REAL support_value = support_dir.dot(support); + + const std::vector& points = *(convex->points); + SupportSet::Polygon& polygon = support_data.polygon; + polygon.clear(); + const Transform3f& tf = support_set.tf; + for (const Vec3f& point : points) { + const FCL_REAL dot = support_dir.dot(point); + if (support_value - dot <= tol) { + if (_SupportOptions == SupportOptions::WithSweptSphere) { + const Vec2f p = + tf.inverseTransform(point + + convex->getSweptSphereRadius() * support_dir) + .template head<2>(); + polygon.emplace_back(p); + } else { + const Vec2f p = tf.inverseTransform(point).template head<2>(); + polygon.emplace_back(p); + } + } + } + + computeSupportSetConvexHull(polygon, support_set.points()); +} + +// ============================================================================ +template +void convexSupportSetRecurse( + const std::vector& points, + const std::vector& neighbors, + const FCL_REAL swept_sphere_radius, const size_t vertex_idx, + const Vec3f& support_dir, const FCL_REAL support_value, + const Transform3f& tf, std::vector& visited, + SupportSet::Polygon& polygon, FCL_REAL tol) { + HPP_FCL_UNUSED_VARIABLE(swept_sphere_radius); + + if (visited[vertex_idx]) { + return; + } + + visited[vertex_idx] = true; + const Vec3f& point = points[vertex_idx]; + const FCL_REAL val = point.dot(support_dir); + if (support_value - val <= tol) { + if (_SupportOptions == SupportOptions::WithSweptSphere) { + const Vec2f p = + tf.inverseTransform(point + swept_sphere_radius * support_dir) + .template head<2>(); + polygon.emplace_back(p); + + } else { + const Vec2f p = tf.inverseTransform(point).template head<2>(); + polygon.emplace_back(p); + } + + const ConvexBase::Neighbors& point_neighbors = neighbors[vertex_idx]; + for (int i = 0; i < point_neighbors.count(); ++i) { + const size_t neighbor_index = (size_t)(point_neighbors[i]); + convexSupportSetRecurse<_SupportOptions>( + points, neighbors, swept_sphere_radius, neighbor_index, support_dir, + support_value, tf, visited, polygon, tol); + } + } +} + +// ============================================================================ +template +void getShapeSupportSetLog(const ConvexBase* convex, SupportSet& support_set, + int& hint, ShapeSupportData& support_data, + size_t /*unused*/, FCL_REAL tol) { + assert(tol > 0); + Vec3f support; + const Vec3f& support_dir = support_set.getNormal(); + getShapeSupportLog( + convex, support_dir, support, hint, support_data); + const FCL_REAL support_value = support.dot(support_dir); + + const std::vector& points = *(convex->points); + const std::vector& neighbors = *(convex->neighbors); + const FCL_REAL swept_sphere_radius = convex->getSweptSphereRadius(); + std::vector& visited = support_data.visited; + // `visited` is guaranteed to be of right size due to previous call to convex + // log support function. + std::fill(support_data.visited.begin(), support_data.visited.end(), false); + + SupportSet::Polygon& polygon = support_data.polygon; + polygon.clear(); + const Transform3f& tf = support_set.tf; + + const size_t vertex_idx = (size_t)(hint); + convexSupportSetRecurse<_SupportOptions>( + points, neighbors, swept_sphere_radius, vertex_idx, support_dir, + support_value, tf, visited, polygon, tol); + + computeSupportSetConvexHull(polygon, support_set.points()); +} + +// ============================================================================ +template +void getShapeSupportSet(const ConvexBase* convex, SupportSet& support_set, + int& hint, ShapeSupportData& support_data, + size_t num_sampled_supports /*unused*/, FCL_REAL tol) { + if (convex->num_points > ConvexBase::num_vertices_large_convex_threshold && + convex->neighbors != nullptr) { + getShapeSupportSetLog<_SupportOptions>( + convex, support_set, hint, support_data, num_sampled_supports, tol); + } else { + getShapeSupportSetLinear<_SupportOptions>( + convex, support_set, hint, support_data, num_sampled_supports, tol); + } +} +getShapeSupportSetTplInstantiation(ConvexBase); + +// ============================================================================ +template +void getShapeSupportSet(const SmallConvex* convex, SupportSet& support_set, + int& hint /*unused*/, + ShapeSupportData& support_data /*unused*/, + size_t num_sampled_supports /*unused*/, FCL_REAL tol) { + getShapeSupportSetLinear<_SupportOptions>( + reinterpret_cast(convex), support_set, hint, + support_data, num_sampled_supports, tol); +} +getShapeSupportSetTplInstantiation(SmallConvex); + +// ============================================================================ +template +void getShapeSupportSet(const LargeConvex* convex, SupportSet& support_set, + int& hint, ShapeSupportData& support_data, + size_t num_sampled_supports /*unused*/, FCL_REAL tol) { + getShapeSupportSetLog<_SupportOptions>( + reinterpret_cast(convex), support_set, hint, + support_data, num_sampled_supports, tol); +} +getShapeSupportSetTplInstantiation(LargeConvex); + +// ============================================================================ +HPP_FCL_DLLAPI void computeSupportSetConvexHull(SupportSet::Polygon& cloud, + SupportSet::Polygon& cvx_hull) { + cvx_hull.clear(); + + if (cloud.size() <= 2) { + // Point or segment, nothing to do. + for (const Vec2f& point : cloud) { + cvx_hull.emplace_back(point); + } + return; + } + + if (cloud.size() == 3) { + // We have a triangle, we only need to arrange it in a counter clockwise + // fashion. + // + // Put the vector which has the lowest y coordinate first. + if (cloud[0](1) > cloud[1](1)) { + std::swap(cloud[0], cloud[1]); + } + if (cloud[0](1) > cloud[2](1)) { + std::swap(cloud[0], cloud[2]); + } + const Vec2f& a = cloud[0]; + const Vec2f& b = cloud[1]; + const Vec2f& c = cloud[2]; + const FCL_REAL det = + (b(0) - a(0)) * (c(1) - a(1)) - (b(1) - a(1)) * (c(0) - a(0)); + if (det < 0) { + std::swap(cloud[1], cloud[2]); + } + + for (const Vec2f& point : cloud) { + cvx_hull.emplace_back(point); + } + return; + } + + // The following is an implementation of the O(nlog(n)) graham scan + // algorithm, used to compute convex-hulls in 2D. + // See https://en.wikipedia.org/wiki/Graham_scan + // + // Step 1 - Compute first element of the convex-hull by computing the support + // in the direction (0, -1) (take the element of the set which has the lowest + // y coordinate). + size_t support_idx = 0; + FCL_REAL support_val = cloud[0](1); + for (size_t i = 1; i < cloud.size(); ++i) { + const FCL_REAL val = cloud[i](1); + if (val < support_val) { + support_val = val; + support_idx = i; + } + } + std::swap(cloud[0], cloud[support_idx]); + cvx_hull.clear(); + cvx_hull.emplace_back(cloud[0]); + const Vec2f& v = cvx_hull[0]; + + // Step 2 - Sort the rest of the point cloud according to the angle made with + // v. Note: we use stable_sort instead of sort because sort can fail if two + // values are identical. + std::stable_sort( + cloud.begin() + 1, cloud.end(), [&v](const Vec2f& p1, const Vec2f& p2) { + // p1 is "smaller" than p2 if det(p1 - v, p2 - v) >= 0 + const FCL_REAL det = + (p1(0) - v(0)) * (p2(1) - v(1)) - (p1(1) - v(1)) * (p2(0) - v(0)); + if (std::abs(det) <= Eigen::NumTraits::dummy_precision()) { + // If two points are identical or (v, p1, p2) are colinear, p1 is + // "smaller" if it is closer to v. + return ((p1 - v).squaredNorm() <= (p2 - v).squaredNorm()); + } + return det > 0; + }); + + // Step 3 - We iterate over the now ordered point of cloud and add the points + // to the cvx-hull if they successively form "left turns" only. A left turn + // is: considering the last three points of the cvx-hull, if they form a + // right-hand basis (determinant > 0) then they make a left turn. + auto isRightSided = [](const Vec2f& p1, const Vec2f& p2, const Vec2f& p3) { + // Checks if (p2 - p1, p3 - p1) forms a right-sided base based on + // det(p2 - p1, p3 - p1) + const FCL_REAL det = + (p2(0) - p1(0)) * (p3(1) - p1(1)) - (p2(1) - p1(1)) * (p3(0) - p1(0)); + // Note: we set a dummy precision threshold so that identical points or + // colinear pionts are not added to the cvx-hull. + return det > Eigen::NumTraits::dummy_precision(); + }; + + // We initialize the cvx-hull algo by adding the first three + // (distinct) points of the set. + // These three points are guaranteed, due to the previous sorting, + // to form a right sided basis, hence to form a left turn. + size_t cloud_beginning_idx = 1; + while (cvx_hull.size() < 3) { + const Vec2f& vec = cloud[cloud_beginning_idx]; + if ((cvx_hull.back() - vec).squaredNorm() > + Eigen::NumTraits::epsilon()) { + cvx_hull.emplace_back(vec); + } + ++cloud_beginning_idx; + } + // The convex-hull should wrap counter-clockwise, i.e. three successive + // points should always form a right-sided basis. Every time we do a turn + // in the wrong direction, we remove the last point of the convex-hull. + // When we do a turn in the correct direction, we add a point to the + // convex-hull. + for (size_t i = cloud_beginning_idx; i < cloud.size(); ++i) { + const Vec2f& vec = cloud[i]; + while (cvx_hull.size() > 1 && + !isRightSided(cvx_hull[cvx_hull.size() - 2], + cvx_hull[cvx_hull.size() - 1], vec)) { + cvx_hull.pop_back(); + } + cvx_hull.emplace_back(vec); + } +} + +} // namespace details +} // namespace fcl +} // namespace hpp diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 8b0cbe5a0..2891ad789 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -27,6 +27,7 @@ target_link_libraries(utility PUBLIC ${PROJECT_NAME}) add_fcl_test(math math.cpp) add_fcl_test(collision collision.cpp) +add_fcl_test(contact_patch contact_patch.cpp) add_fcl_test(distance distance.cpp) add_fcl_test(swept_sphere_radius swept_sphere_radius.cpp) add_fcl_test(normal_and_nearest_points normal_and_nearest_points.cpp) diff --git a/test/contact_patch.cpp b/test/contact_patch.cpp new file mode 100644 index 000000000..4bebaaea9 --- /dev/null +++ b/test/contact_patch.cpp @@ -0,0 +1,1026 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, 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 INRIA 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. + */ + +/** \author Louis Montaut */ + +#define BOOST_TEST_MODULE FCL_CONTACT_PATCH +#include + +#include + +#include "utility.h" + +using namespace hpp::fcl; + +BOOST_AUTO_TEST_CASE(box_box_no_collision) { + const FCL_REAL halfside = 0.5; + const Box box1(2 * halfside, 2 * halfside, 2 * halfside); + const Box box2(2 * halfside, 2 * halfside, 2 * halfside); + + const Transform3f tf1; + Transform3f tf2; + // set translation to separate the shapes + const FCL_REAL offset = 0.001; + tf2.setTranslation(Vec3f(0, 0, 2 * halfside + offset)); + + const size_t num_max_contact = 1; + const CollisionRequest col_req(CollisionRequestFlag::CONTACT, + num_max_contact); + CollisionResult col_res; + + hpp::fcl::collide(&box1, tf1, &box2, tf2, col_req, col_res); + + BOOST_CHECK(!col_res.isCollision()); + + const ContactPatchRequest patch_req; + ContactPatchResult patch_res(patch_req); + hpp::fcl::computeContactPatch(&box1, tf1, &box2, tf2, col_res, patch_req, + patch_res); + BOOST_CHECK(patch_res.numContactPatches() == 0); +} + +BOOST_AUTO_TEST_CASE(box_sphere) { + const FCL_REAL halfside = 0.5; + const Box box(2 * halfside, 2 * halfside, 2 * halfside); + const Sphere sphere(halfside); + + const Transform3f tf1; + Transform3f tf2; + // set translation to have a collision + const FCL_REAL offset = 0.001; + tf2.setTranslation(Vec3f(0, 0, 2 * halfside - offset)); + + const size_t num_max_contact = 1; + const CollisionRequest col_req(CollisionRequestFlag::CONTACT, + num_max_contact); + CollisionResult col_res; + + hpp::fcl::collide(&box, tf1, &sphere, tf2, col_req, col_res); + + BOOST_CHECK(col_res.isCollision()); + + const ContactPatchRequest patch_req; + ContactPatchResult patch_res(patch_req); + hpp::fcl::computeContactPatch(&box, tf1, &sphere, tf2, col_res, patch_req, + patch_res); + BOOST_CHECK(patch_res.numContactPatches() == 1); + if (patch_res.numContactPatches() > 0 && col_res.isCollision()) { + const Contact& contact = col_res.getContact(0); + const ContactPatch& contact_patch = patch_res.getContactPatch(0); + BOOST_CHECK(contact_patch.size() == 1); + const FCL_REAL tol = 1e-8; + EIGEN_VECTOR_IS_APPROX(contact_patch.getPoint(0), contact.pos, tol); + EIGEN_VECTOR_IS_APPROX(contact_patch.tf.translation(), contact.pos, tol); + EIGEN_VECTOR_IS_APPROX(contact_patch.getNormal(), contact.normal, tol); + BOOST_CHECK(std::abs(contact_patch.penetration_depth - + contact.penetration_depth) < tol); + } +} + +BOOST_AUTO_TEST_CASE(box_box) { + const FCL_REAL halfside = 0.5; + const Box box1(2 * halfside, 2 * halfside, 2 * halfside); + const Box box2(2 * halfside, 2 * halfside, 2 * halfside); + + const Transform3f tf1; + Transform3f tf2; + // set translation to have a collision + const FCL_REAL offset = 0.001; + tf2.setTranslation(Vec3f(0, 0, 2 * halfside - offset)); + + const size_t num_max_contact = 1; + const CollisionRequest col_req(CollisionRequestFlag::CONTACT, + num_max_contact); + CollisionResult col_res; + + hpp::fcl::collide(&box1, tf1, &box2, tf2, col_req, col_res); + + BOOST_CHECK(col_res.isCollision()); + + const ContactPatchRequest patch_req; + ContactPatchResult patch_res1(patch_req); + ContactPatchResult patch_res2(patch_req); + hpp::fcl::computeContactPatch(&box1, tf1, &box2, tf2, col_res, patch_req, + patch_res1); + hpp::fcl::computeContactPatch(&box1, tf1, &box2, tf2, col_res, patch_req, + patch_res2); + BOOST_CHECK(patch_res1.numContactPatches() == 1); + BOOST_CHECK(patch_res2.numContactPatches() == 1); + + if (patch_res1.numContactPatches() > 0 && + patch_res2.numContactPatches() > 0 && col_res.isCollision()) { + const Contact& contact = col_res.getContact(0); + const FCL_REAL tol = 1e-6; + EIGEN_VECTOR_IS_APPROX(contact.normal, Vec3f(0, 0, 1), tol); + + const size_t expected_size = 4; + ContactPatch expected(expected_size); + expected.tf.rotation() = + constructOrthonormalBasisFromVector(contact.normal); + expected.tf.translation() = contact.pos; + expected.penetration_depth = contact.penetration_depth; + const std::array corners = { + Vec3f(halfside, halfside, halfside), + Vec3f(halfside, -halfside, halfside), + Vec3f(-halfside, -halfside, halfside), + Vec3f(-halfside, halfside, halfside), + }; + for (size_t i = 0; i < expected_size; ++i) { + expected.addPoint(corners[i] + + (contact.penetration_depth * contact.normal) / 2); + } + + BOOST_CHECK(patch_res1.getContactPatch(0).isSame(expected, tol)); + BOOST_CHECK(patch_res2.getContactPatch(0).isSame(expected, tol)); + } +} + +BOOST_AUTO_TEST_CASE(halfspace_box) { + const Halfspace hspace(0, 0, 1, 0); + const FCL_REAL halfside = 0.5; + const Box box(2 * halfside, 2 * halfside, 2 * halfside); + + const Transform3f tf1; + Transform3f tf2; + // set translation to have a collision + const FCL_REAL offset = 0.001; + tf2.setTranslation(Vec3f(0, 0, halfside - offset)); + + const size_t num_max_contact = 1; + const CollisionRequest col_req(CollisionRequestFlag::CONTACT, + num_max_contact); + CollisionResult col_res; + + hpp::fcl::collide(&hspace, tf1, &box, tf2, col_req, col_res); + + BOOST_CHECK(col_res.isCollision()); + + const ContactPatchRequest patch_req; + ContactPatchResult patch_res1(patch_req); + ContactPatchResult patch_res2(patch_req); + hpp::fcl::computeContactPatch(&hspace, tf1, &box, tf2, col_res, patch_req, + patch_res1); + hpp::fcl::computeContactPatch(&hspace, tf1, &box, tf2, col_res, patch_req, + patch_res2); + BOOST_CHECK(patch_res1.numContactPatches() == 1); + BOOST_CHECK(patch_res2.numContactPatches() == 1); + + if (patch_res1.numContactPatches() > 0 && + patch_res2.numContactPatches() > 0 && col_res.isCollision()) { + const Contact& contact = col_res.getContact(0); + const FCL_REAL tol = 1e-6; + EIGEN_VECTOR_IS_APPROX(contact.normal, hspace.n, tol); + EIGEN_VECTOR_IS_APPROX(hspace.n, Vec3f(0, 0, 1), tol); + + const size_t expected_size = 4; + ContactPatch expected(expected_size); + expected.tf.rotation() = + constructOrthonormalBasisFromVector(contact.normal); + expected.tf.translation() = contact.pos; + expected.penetration_depth = contact.penetration_depth; + const std::array corners = { + tf2.transform(Vec3f(halfside, halfside, -halfside)), + tf2.transform(Vec3f(halfside, -halfside, -halfside)), + tf2.transform(Vec3f(-halfside, -halfside, -halfside)), + tf2.transform(Vec3f(-halfside, halfside, -halfside)), + }; + for (size_t i = 0; i < expected_size; ++i) { + expected.addPoint(corners[i] - + (contact.penetration_depth * contact.normal) / 2); + } + + BOOST_CHECK(patch_res1.getContactPatch(0).isSame(expected, tol)); + BOOST_CHECK(patch_res2.getContactPatch(0).isSame(expected, tol)); + } +} + +BOOST_AUTO_TEST_CASE(halfspace_capsule) { + const Halfspace hspace(0, 0, 1, 0); + const FCL_REAL radius = 0.25; + const FCL_REAL height = 1.; + const Capsule capsule(radius, height); + + const Transform3f tf1; + Transform3f tf2; + // set translation to have a collision + const FCL_REAL offset = 0.001; + tf2.setTranslation(Vec3f(0, 0, height / 2 - offset)); + + const size_t num_max_contact = 1; + const CollisionRequest col_req(CollisionRequestFlag::CONTACT, + num_max_contact); + CollisionResult col_res; + hpp::fcl::collide(&hspace, tf1, &capsule, tf2, col_req, col_res); + BOOST_CHECK(col_res.isCollision()); + + const ContactPatchRequest patch_req; + BOOST_CHECK(patch_req.getNumSamplesCurvedShapes() == + ContactPatch::default_preallocated_size); + ContactPatchResult patch_res(patch_req); + hpp::fcl::computeContactPatch(&hspace, tf1, &capsule, tf2, col_res, patch_req, + patch_res); + BOOST_CHECK(patch_res.numContactPatches() == 1); + + if (patch_res.numContactPatches() > 0 && col_res.isCollision()) { + const Contact& contact = col_res.getContact(0); + const FCL_REAL tol = 1e-6; + EIGEN_VECTOR_IS_APPROX(contact.normal, hspace.n, tol); + + const size_t expected_size = 1; + ContactPatch expected(expected_size); + expected.tf.rotation() = + constructOrthonormalBasisFromVector(contact.normal); + expected.tf.translation() = contact.pos; + expected.penetration_depth = contact.penetration_depth; + const Vec3f capsule_end(0, 0, -capsule.halfLength); + expected.addPoint(tf2.transform(capsule_end)); + + const ContactPatch& contact_patch = patch_res.getContactPatch(0); + BOOST_CHECK(expected.tf == contact_patch.tf); + BOOST_CHECK(expected.isSame(contact_patch, tol)); + } + + // Rotate capsule 180 degrees around y-axis + // Should only have one contact. + tf2.rotation().col(0) << -1, 0, 0; + tf2.rotation().col(1) << 0, 1, 0; + tf2.rotation().col(2) << 0, 0, -1; + col_res.clear(); + hpp::fcl::collide(&hspace, tf1, &capsule, tf2, col_req, col_res); + BOOST_CHECK(col_res.isCollision()); + patch_res.clear(); + hpp::fcl::computeContactPatch(&hspace, tf1, &capsule, tf2, col_res, patch_req, + patch_res); + BOOST_CHECK(patch_res.numContactPatches() == 1); + if (patch_res.numContactPatches() > 0 && col_res.isCollision()) { + const Contact& contact = col_res.getContact(0); + const FCL_REAL tol = 1e-6; + EIGEN_VECTOR_IS_APPROX(contact.normal, hspace.n, tol); + + const size_t expected_size = 1; + ContactPatch expected(expected_size); + expected.tf.rotation() = + constructOrthonormalBasisFromVector(contact.normal); + expected.tf.translation() = contact.pos; + expected.penetration_depth = contact.penetration_depth; + const Vec3f capsule_end(0, 0, capsule.halfLength); + expected.addPoint(tf2.transform(capsule_end)); + + const ContactPatch& contact_patch = patch_res.getContactPatch(0); + BOOST_CHECK(expected.tf == contact_patch.tf); + BOOST_CHECK(expected.isSame(contact_patch, tol)); + } + + // Rotate cone 90 degrees around y-axis + // Should only have two contacts. + tf2.rotation().col(0) << 0, 0, 1; + tf2.rotation().col(1) << 0, 1, 0; + tf2.rotation().col(2) << -1, 0, 0; + tf2.translation() << 0, 0, capsule.radius - offset; + col_res.clear(); + hpp::fcl::collide(&hspace, tf1, &capsule, tf2, col_req, col_res); + BOOST_CHECK(col_res.isCollision()); + patch_res.clear(); + hpp::fcl::computeContactPatch(&hspace, tf1, &capsule, tf2, col_res, patch_req, + patch_res); + BOOST_CHECK(patch_res.numContactPatches() == 1); + if (patch_res.numContactPatches() > 0 && col_res.isCollision()) { + const Contact& contact = col_res.getContact(0); + const FCL_REAL tol = 1e-6; + EIGEN_VECTOR_IS_APPROX(contact.normal, hspace.n, tol); + + const size_t expected_size = 2; + ContactPatch expected(expected_size); + expected.tf.rotation() = + constructOrthonormalBasisFromVector(contact.normal); + expected.tf.translation() = contact.pos; + expected.penetration_depth = contact.penetration_depth; + const Vec3f p1(-capsule.radius, 0, capsule.halfLength); + const Vec3f p2(-capsule.radius, 0, -capsule.halfLength); + expected.addPoint(tf2.transform(p1)); + expected.addPoint(tf2.transform(p2)); + + const ContactPatch& contact_patch = patch_res.getContactPatch(0); + BOOST_CHECK(expected.tf == contact_patch.tf); + BOOST_CHECK(expected.isSame(contact_patch, tol)); + } +} + +BOOST_AUTO_TEST_CASE(halfspace_cone) { + const Halfspace hspace(0, 0, 1, 0); + const FCL_REAL radius = 0.25; + const FCL_REAL height = 1.; + const Cone cone(radius, height); + + const Transform3f tf1; + Transform3f tf2; + // set translation to have a collision + const FCL_REAL offset = 0.001; + tf2.setTranslation(Vec3f(0, 0, height / 2 - offset)); + + const size_t num_max_contact = 1; + const CollisionRequest col_req(CollisionRequestFlag::CONTACT, + num_max_contact); + CollisionResult col_res; + hpp::fcl::collide(&hspace, tf1, &cone, tf2, col_req, col_res); + BOOST_CHECK(col_res.isCollision()); + + const ContactPatchRequest patch_req; + BOOST_CHECK(patch_req.getNumSamplesCurvedShapes() == + ContactPatch::default_preallocated_size); + ContactPatchResult patch_res(patch_req); + hpp::fcl::computeContactPatch(&hspace, tf1, &cone, tf2, col_res, patch_req, + patch_res); + BOOST_CHECK(patch_res.numContactPatches() == 1); + + if (patch_res.numContactPatches() > 0 && col_res.isCollision()) { + const Contact& contact = col_res.getContact(0); + const FCL_REAL tol = 1e-6; + EIGEN_VECTOR_IS_APPROX(contact.normal, hspace.n, tol); + + const size_t expected_size = ContactPatch::default_preallocated_size; + ContactPatch expected(expected_size); + expected.tf.rotation() = + constructOrthonormalBasisFromVector(contact.normal); + expected.tf.translation() = contact.pos; + expected.penetration_depth = contact.penetration_depth; + std::array points; + const FCL_REAL angle_increment = + 2.0 * (FCL_REAL)(EIGEN_PI) / ((FCL_REAL)(6)); + for (size_t i = 0; i < ContactPatch::default_preallocated_size; ++i) { + const FCL_REAL theta = (FCL_REAL)(i)*angle_increment; + Vec3f point_on_cone_base(std::cos(theta) * cone.radius, + std::sin(theta) * cone.radius, -cone.halfLength); + expected.addPoint(tf2.transform(point_on_cone_base)); + } + + const ContactPatch& contact_patch = patch_res.getContactPatch(0); + BOOST_CHECK(expected.tf == contact_patch.tf); + BOOST_CHECK(expected.isSame(contact_patch, tol)); + } + + // Rotate cone 180 degrees around y-axis + // Should only have one contact, due to cone-tip/halfspace collision. + tf2.rotation().col(0) << -1, 0, 0; + tf2.rotation().col(1) << 0, 1, 0; + tf2.rotation().col(2) << 0, 0, -1; + col_res.clear(); + hpp::fcl::collide(&hspace, tf1, &cone, tf2, col_req, col_res); + BOOST_CHECK(col_res.isCollision()); + patch_res.clear(); + hpp::fcl::computeContactPatch(&hspace, tf1, &cone, tf2, col_res, patch_req, + patch_res); + BOOST_CHECK(patch_res.numContactPatches() == 1); + if (patch_res.numContactPatches() > 0 && col_res.isCollision()) { + const Contact& contact = col_res.getContact(0); + const ContactPatch& contact_patch = patch_res.getContactPatch(0); + BOOST_CHECK(contact_patch.size() == 1); + const FCL_REAL tol = 1e-8; + EIGEN_VECTOR_IS_APPROX(contact_patch.getPoint(0), contact.pos, tol); + EIGEN_VECTOR_IS_APPROX(contact_patch.tf.translation(), contact.pos, tol); + EIGEN_VECTOR_IS_APPROX(contact_patch.getNormal(), contact.normal, tol); + BOOST_CHECK(std::abs(contact_patch.penetration_depth - + contact.penetration_depth) < tol); + + const size_t expected_size = 1; + ContactPatch expected(expected_size); + expected.tf.rotation() = + constructOrthonormalBasisFromVector(contact.normal); + expected.tf.translation() = contact.pos; + expected.penetration_depth = contact.penetration_depth; + const Vec3f cone_tip(0, 0, cone.halfLength); + expected.addPoint(tf2.transform(cone_tip)); + + BOOST_CHECK(contact_patch.isSame(expected, tol)); + } + + // Rotate cone 90 degrees around y-axis + // Should only have one contact, on cone circle basis. + tf2.rotation().col(0) << 0, 0, 1; + tf2.rotation().col(1) << 0, 1, 0; + tf2.rotation().col(2) << -1, 0, 0; + tf2.translation() << 0, 0, cone.radius - offset; + col_res.clear(); + hpp::fcl::collide(&hspace, tf1, &cone, tf2, col_req, col_res); + BOOST_CHECK(col_res.isCollision()); + patch_res.clear(); + hpp::fcl::computeContactPatch(&hspace, tf1, &cone, tf2, col_res, patch_req, + patch_res); + BOOST_CHECK(patch_res.numContactPatches() == 1); + if (patch_res.numContactPatches() > 0 && col_res.isCollision()) { + const Contact& contact = col_res.getContact(0); + const ContactPatch& contact_patch = patch_res.getContactPatch(0); + BOOST_CHECK(contact_patch.size() == 1); + const FCL_REAL tol = 1e-8; + EIGEN_VECTOR_IS_APPROX(contact_patch.getPoint(0), contact.pos, tol); + EIGEN_VECTOR_IS_APPROX(contact_patch.tf.translation(), contact.pos, tol); + EIGEN_VECTOR_IS_APPROX(contact_patch.getNormal(), contact.normal, tol); + BOOST_CHECK(std::abs(contact_patch.penetration_depth - + contact.penetration_depth) < tol); + + const size_t expected_size = 1; + ContactPatch expected(expected_size); + expected.tf.rotation() = + constructOrthonormalBasisFromVector(contact.normal); + expected.tf.translation() = contact.pos; + expected.penetration_depth = contact.penetration_depth; + const Vec3f point_on_circle_basis(-cone.radius, 0, -cone.halfLength); + expected.addPoint(tf2.transform(point_on_circle_basis)); + + BOOST_CHECK(contact_patch.isSame(expected, tol)); + } +} + +BOOST_AUTO_TEST_CASE(halfspace_cylinder) { + const Halfspace hspace(0, 0, 1, 0); + const FCL_REAL radius = 0.25; + const FCL_REAL height = 1.; + const Cylinder cylinder(radius, height); + + const Transform3f tf1; + Transform3f tf2; + // set translation to have a collision + const FCL_REAL offset = 0.001; + tf2.setTranslation(Vec3f(0, 0, height / 2 - offset)); + + const size_t num_max_contact = 1; + const CollisionRequest col_req(CollisionRequestFlag::CONTACT, + num_max_contact); + CollisionResult col_res; + hpp::fcl::collide(&hspace, tf1, &cylinder, tf2, col_req, col_res); + BOOST_CHECK(col_res.isCollision()); + + if (col_res.isCollision()) { + const Contact& contact = col_res.getContact(0); + const size_t expected_size = ContactPatch::default_preallocated_size; + const FCL_REAL tol = 1e-6; + ContactPatch expected(expected_size); + expected.tf.rotation() = + constructOrthonormalBasisFromVector(contact.normal); + expected.tf.translation() = contact.pos; + expected.penetration_depth = contact.penetration_depth; + std::array points; + const FCL_REAL angle_increment = + 2.0 * (FCL_REAL)(EIGEN_PI) / ((FCL_REAL)(6)); + for (size_t i = 0; i < ContactPatch::default_preallocated_size; ++i) { + const FCL_REAL theta = (FCL_REAL)(i)*angle_increment; + Vec3f point_on_cone_base(std::cos(theta) * cylinder.radius, + std::sin(theta) * cylinder.radius, + -cylinder.halfLength); + expected.addPoint(tf2.transform(point_on_cone_base)); + } + + const ContactPatchRequest patch_req; + BOOST_CHECK(patch_req.getNumSamplesCurvedShapes() == + ContactPatch::default_preallocated_size); + ContactPatchResult patch_res(patch_req); + hpp::fcl::computeContactPatch(&hspace, tf1, &cylinder, tf2, col_res, + patch_req, patch_res); + BOOST_CHECK(patch_res.numContactPatches() == 1); + + if (patch_res.numContactPatches() > 0) { + EIGEN_VECTOR_IS_APPROX(contact.normal, hspace.n, tol); + const ContactPatch& contact_patch = patch_res.getContactPatch(0); + BOOST_CHECK(expected.tf == contact_patch.tf); + BOOST_CHECK(expected.isSame(contact_patch, tol)); + } + + // Rotate cylinder 180 degrees around y-axis. + // Should only have the same contact-patch, due to cylinder symmetry. + tf2.rotation().col(0) << -1, 0, 0; + tf2.rotation().col(1) << 0, 1, 0; + tf2.rotation().col(2) << 0, 0, -1; + col_res.clear(); + hpp::fcl::collide(&hspace, tf1, &cylinder, tf2, col_req, col_res); + BOOST_CHECK(col_res.isCollision()); + patch_res.clear(); + hpp::fcl::computeContactPatch(&hspace, tf1, &cylinder, tf2, col_res, + patch_req, patch_res); + BOOST_CHECK(patch_res.numContactPatches() == 1); + if (patch_res.numContactPatches() > 0 && col_res.isCollision()) { + EIGEN_VECTOR_IS_APPROX(contact.normal, hspace.n, tol); + const ContactPatch& contact_patch = patch_res.getContactPatch(0); + BOOST_CHECK(expected.tf == contact_patch.tf); + BOOST_CHECK(expected.isSame(contact_patch, tol)); + } + } + + // Rotate cylinder 90 degrees around y-axis. + // Should have 2 contact points. + tf2.rotation().col(0) << 0, 0, 1; + tf2.rotation().col(1) << 0, 1, 0; + tf2.rotation().col(2) << -1, 0, 0; + tf2.translation() << 0, 0, cylinder.radius - offset; + + col_res.clear(); + hpp::fcl::collide(&hspace, tf1, &cylinder, tf2, col_req, col_res); + BOOST_CHECK(col_res.isCollision()); + + const ContactPatchRequest patch_req; + ContactPatchResult patch_res(patch_req); + hpp::fcl::computeContactPatch(&hspace, tf1, &cylinder, tf2, col_res, + patch_req, patch_res); + BOOST_CHECK(patch_res.numContactPatches() == 1); + if (col_res.isCollision() && patch_res.numContactPatches() > 0) { + const Contact& contact = col_res.getContact(0); + const FCL_REAL tol = 1e-6; + + const size_t expected_size = 2; + ContactPatch expected(expected_size); + expected.tf.rotation() = + constructOrthonormalBasisFromVector(contact.normal); + expected.tf.translation() = contact.pos; + expected.penetration_depth = contact.penetration_depth; + expected.addPoint( + tf2.transform(Vec3f(cylinder.radius, 0, cylinder.halfLength))); + expected.addPoint( + tf2.transform(Vec3f(cylinder.radius, 0, -cylinder.halfLength))); + + const ContactPatch& contact_patch = patch_res.getContactPatch(0); + BOOST_CHECK(expected.isSame(contact_patch, tol)); + } +} + +BOOST_AUTO_TEST_CASE(convex_convex) { + const FCL_REAL halfside = 0.5; + const Convex box1(buildBox(halfside, halfside, halfside)); + const Convex box2(buildBox(halfside, halfside, halfside)); + + const Transform3f tf1; + Transform3f tf2; + // set translation to have a collision + const FCL_REAL offset = 0.001; + tf2.setTranslation(Vec3f(0, 0, 2 * halfside - offset)); + + const size_t num_max_contact = 1; + const CollisionRequest col_req(CollisionRequestFlag::CONTACT, + num_max_contact); + CollisionResult col_res; + + hpp::fcl::collide(&box1, tf1, &box2, tf2, col_req, col_res); + + BOOST_CHECK(col_res.isCollision()); + + const ContactPatchRequest patch_req; + ContactPatchResult patch_res1(patch_req); + ContactPatchResult patch_res2(patch_req); + hpp::fcl::computeContactPatch(&box1, tf1, &box2, tf2, col_res, patch_req, + patch_res1); + hpp::fcl::computeContactPatch(&box1, tf1, &box2, tf2, col_res, patch_req, + patch_res2); + BOOST_CHECK(patch_res1.numContactPatches() == 1); + BOOST_CHECK(patch_res2.numContactPatches() == 1); + + if (patch_res1.numContactPatches() > 0 && + patch_res2.numContactPatches() > 0 && col_res.isCollision()) { + const Contact& contact = col_res.getContact(0); + const FCL_REAL tol = 1e-6; + EIGEN_VECTOR_IS_APPROX(contact.normal, Vec3f(0, 0, 1), tol); + + const size_t expected_size = 4; + ContactPatch expected(expected_size); + expected.tf.rotation() = + constructOrthonormalBasisFromVector(contact.normal); + expected.tf.translation() = contact.pos; + expected.penetration_depth = contact.penetration_depth; + const std::array corners = { + Vec3f(halfside, halfside, halfside), + Vec3f(halfside, -halfside, halfside), + Vec3f(-halfside, -halfside, halfside), + Vec3f(-halfside, halfside, halfside), + }; + for (size_t i = 0; i < expected_size; ++i) { + expected.addPoint(corners[i] + + (contact.penetration_depth * contact.normal) / 2); + } + + BOOST_CHECK(patch_res1.getContactPatch(0).isSame(expected, tol)); + BOOST_CHECK(patch_res2.getContactPatch(0).isSame(expected, tol)); + } +} + +BOOST_AUTO_TEST_CASE(edge_case_segment_segment) { + // This case covers the segment-segment edge case of contact patches. + // Two tetrahedrons make contact on one of their edge. + + const size_t expected_size = 2; + const Vec3f expected_cp1(0, 0.5, 0); + const Vec3f expected_cp2(0, 1, 0); + + const Transform3f tf1; // identity + const Transform3f tf2; // identity + + const size_t num_max_contact = 1; + const CollisionRequest col_req(CollisionRequestFlag::CONTACT, + num_max_contact); + CollisionResult col_res; + const ContactPatchRequest patch_req; + ContactPatchResult patch_res(patch_req); + + { + // Case 1 - Face-Face contact + std::shared_ptr> pts1(new std::vector({ + Vec3f(-1, 0, 0), + Vec3f(0, 0, 0), + Vec3f(0, 1, 0), + Vec3f(-1, -1, -1), + })); + std::shared_ptr> tris1( + new std::vector({Triangle(0, 1, 2), Triangle(0, 2, 3), + Triangle(0, 3, 1), Triangle(2, 1, 3)})); + Convex tetra1(pts1, 4, tris1, 4); + + std::shared_ptr> pts2(new std::vector({ + Vec3f(0, 0.5, 0), + Vec3f(0, 1.5, 0), + Vec3f(1, 0.5, 0), + Vec3f(1, 1, 1), + })); + std::shared_ptr> tris2( + new std::vector({Triangle(0, 1, 2), Triangle(0, 2, 3), + Triangle(0, 3, 1), Triangle(2, 1, 3)})); + Convex tetra2(pts2, 4, tris2, 4); + + col_res.clear(); + hpp::fcl::collide(&tetra1, tf1, &tetra2, tf2, col_req, col_res); + BOOST_CHECK(col_res.isCollision()); + patch_res.clear(); + hpp::fcl::computeContactPatch(&tetra1, tf1, &tetra2, tf2, col_res, + patch_req, patch_res); + BOOST_CHECK(patch_res.numContactPatches() == 1); + + if (patch_res.numContactPatches() > 0) { + const Contact& contact = col_res.getContact(0); + const FCL_REAL tol = 1e-6; + + ContactPatch expected(expected_size); + // GJK/EPA can return any normal which is in the dual cone + // of the cone {(-1, 0, 0)}. + expected.tf.rotation() = + constructOrthonormalBasisFromVector(contact.normal); + expected.tf.translation() = contact.pos; + expected.penetration_depth = contact.penetration_depth; + expected.addPoint(expected_cp1); + expected.addPoint(expected_cp2); + + const ContactPatch& contact_patch = patch_res.getContactPatch(0); + BOOST_CHECK(expected.isSame(contact_patch, tol)); + } + } + + { + // Case 2 - Face-Segment contact + std::shared_ptr> pts1(new std::vector({ + Vec3f(-1, 0, -0.2), + Vec3f(0, 0, 0), + Vec3f(0, 1, 0), + Vec3f(-1, -1, -1), + })); + std::shared_ptr> tris1( + new std::vector({Triangle(0, 1, 2), Triangle(0, 2, 3), + Triangle(0, 3, 1), Triangle(2, 1, 3)})); + Convex tetra1(pts1, 4, tris1, 4); + + std::shared_ptr> pts2(new std::vector({ + Vec3f(0, 0.5, 0), + Vec3f(0, 1.5, 0), + Vec3f(1, 0.5, 0), + Vec3f(1, 1, 1), + })); + std::shared_ptr> tris2( + new std::vector({Triangle(0, 1, 2), Triangle(0, 2, 3), + Triangle(0, 3, 1), Triangle(2, 1, 3)})); + Convex tetra2(pts2, 4, tris2, 4); + + col_res.clear(); + hpp::fcl::collide(&tetra1, tf1, &tetra2, tf2, col_req, col_res); + BOOST_CHECK(col_res.isCollision()); + patch_res.clear(); + hpp::fcl::computeContactPatch(&tetra1, tf1, &tetra2, tf2, col_res, + patch_req, patch_res); + BOOST_CHECK(patch_res.numContactPatches() == 1); + + if (patch_res.numContactPatches() > 0) { + const Contact& contact = col_res.getContact(0); + const FCL_REAL tol = 1e-6; + + ContactPatch expected(expected_size); + expected.tf.rotation() = + constructOrthonormalBasisFromVector(contact.normal); + expected.tf.translation() = contact.pos; + expected.penetration_depth = contact.penetration_depth; + expected.addPoint(expected_cp1); + expected.addPoint(expected_cp2); + + const ContactPatch& contact_patch = patch_res.getContactPatch(0); + BOOST_CHECK(expected.isSame(contact_patch, tol)); + } + } + + { + // Case 3 - Segment-Segment contact + std::shared_ptr> pts1(new std::vector({ + Vec3f(-1, 0, -0.2), + Vec3f(0, 0, 0), + Vec3f(0, 1, 0), + Vec3f(-1, -1, -1), + })); + std::shared_ptr> tris1( + new std::vector({Triangle(0, 1, 2), Triangle(0, 2, 3), + Triangle(0, 3, 1), Triangle(2, 1, 3)})); + Convex tetra1(pts1, 4, tris1, 4); + + std::shared_ptr> pts2(new std::vector({ + Vec3f(0, 0.5, 0), + Vec3f(0, 1.5, 0), + Vec3f(1, 0.5, 0.5), + Vec3f(1, 1, 1), + })); + std::shared_ptr> tris2( + new std::vector({Triangle(0, 1, 2), Triangle(0, 2, 3), + Triangle(0, 3, 1), Triangle(2, 1, 3)})); + Convex tetra2(pts2, 4, tris2, 4); + + col_res.clear(); + hpp::fcl::collide(&tetra1, tf1, &tetra2, tf2, col_req, col_res); + BOOST_CHECK(col_res.isCollision()); + patch_res.clear(); + hpp::fcl::computeContactPatch(&tetra1, tf1, &tetra2, tf2, col_res, + patch_req, patch_res); + BOOST_CHECK(patch_res.numContactPatches() == 1); + + if (patch_res.numContactPatches() > 0) { + const Contact& contact = col_res.getContact(0); + const FCL_REAL tol = 1e-6; + + ContactPatch expected(expected_size); + expected.tf.rotation() = + constructOrthonormalBasisFromVector(contact.normal); + expected.tf.translation() = contact.pos; + expected.penetration_depth = contact.penetration_depth; + expected.addPoint(expected_cp1); + expected.addPoint(expected_cp2); + + const ContactPatch& contact_patch = patch_res.getContactPatch(0); + BOOST_CHECK(expected.isSame(contact_patch, tol)); + } + } +} + +BOOST_AUTO_TEST_CASE(edge_case_vertex_vertex) { + // This case covers the vertex-vertex edge case of contact patches. + // Two tetrahedrons make contact on one of their vertex. + const size_t expected_size = 1; + const Vec3f expected_cp(0, 0, 0); + + const Transform3f tf1; // identity + const Transform3f tf2; // identity + + const size_t num_max_contact = 1; + const CollisionRequest col_req(CollisionRequestFlag::CONTACT, + num_max_contact); + CollisionResult col_res; + const ContactPatchRequest patch_req; + ContactPatchResult patch_res(patch_req); + + { + // Case 1 - Face-Face contact + std::shared_ptr> pts1(new std::vector({ + Vec3f(-1, 0, 0), + Vec3f(0, 0, 0), + Vec3f(0, 1, 0), + Vec3f(-1, -1, -1), + })); + std::shared_ptr> tris1( + new std::vector({Triangle(0, 1, 2), Triangle(0, 2, 3), + Triangle(0, 3, 1), Triangle(2, 1, 3)})); + Convex tetra1(pts1, 4, tris1, 4); + + std::shared_ptr> pts2(new std::vector({ + Vec3f(1, 0, 0), + Vec3f(0, 0, 0), + Vec3f(0, -1, 0), + Vec3f(1, 1, 1), + })); + std::shared_ptr> tris2( + new std::vector({Triangle(0, 1, 2), Triangle(0, 2, 3), + Triangle(0, 3, 1), Triangle(2, 1, 3)})); + Convex tetra2(pts2, 4, tris2, 4); + + col_res.clear(); + hpp::fcl::collide(&tetra1, tf1, &tetra2, tf2, col_req, col_res); + BOOST_CHECK(col_res.isCollision()); + patch_res.clear(); + hpp::fcl::computeContactPatch(&tetra1, tf1, &tetra2, tf2, col_res, + patch_req, patch_res); + BOOST_CHECK(patch_res.numContactPatches() == 1); + + if (patch_res.numContactPatches() > 0) { + const Contact& contact = col_res.getContact(0); + const FCL_REAL tol = 1e-6; + + ContactPatch expected(expected_size); + expected.tf.rotation() = + constructOrthonormalBasisFromVector(contact.normal); + expected.tf.translation() = contact.pos; + expected.penetration_depth = contact.penetration_depth; + expected.addPoint(expected_cp); + + const ContactPatch& contact_patch = patch_res.getContactPatch(0); + BOOST_CHECK(expected.isSame(contact_patch, tol)); + } + } + + { + // Case 2 - Segment-Face contact + std::shared_ptr> pts1(new std::vector({ + Vec3f(-1, 0, -0.5), + Vec3f(0, 0, 0), + Vec3f(0, 1, 0), + Vec3f(-1, -1, -1), + })); + std::shared_ptr> tris1( + new std::vector({Triangle(0, 1, 2), Triangle(0, 2, 3), + Triangle(0, 3, 1), Triangle(2, 1, 3)})); + Convex tetra1(pts1, 4, tris1, 4); + + std::shared_ptr> pts2(new std::vector({ + Vec3f(1, 0, 0), + Vec3f(0, 0, 0), + Vec3f(0, -1, 0), + Vec3f(1, 1, 1), + })); + std::shared_ptr> tris2( + new std::vector({Triangle(0, 1, 2), Triangle(0, 2, 3), + Triangle(0, 3, 1), Triangle(2, 1, 3)})); + Convex tetra2(pts2, 4, tris2, 4); + + col_res.clear(); + hpp::fcl::collide(&tetra1, tf1, &tetra2, tf2, col_req, col_res); + BOOST_CHECK(col_res.isCollision()); + patch_res.clear(); + hpp::fcl::computeContactPatch(&tetra1, tf1, &tetra2, tf2, col_res, + patch_req, patch_res); + BOOST_CHECK(patch_res.numContactPatches() == 1); + + if (patch_res.numContactPatches() > 0) { + const Contact& contact = col_res.getContact(0); + const FCL_REAL tol = 1e-6; + + ContactPatch expected(expected_size); + expected.tf.rotation() = + constructOrthonormalBasisFromVector(contact.normal); + expected.tf.translation() = contact.pos; + expected.penetration_depth = contact.penetration_depth; + expected.addPoint(expected_cp); + + const ContactPatch& contact_patch = patch_res.getContactPatch(0); + BOOST_CHECK(expected.isSame(contact_patch, tol)); + } + } + + { + // Case 2 - Segment-Segment contact + std::shared_ptr> pts1(new std::vector({ + Vec3f(-1, 0, -0.2), + Vec3f(0, 0, 0), + Vec3f(0, 1, 0), + Vec3f(-1, -1, -1), + })); + std::shared_ptr> tris1( + new std::vector({Triangle(0, 1, 2), Triangle(0, 2, 3), + Triangle(0, 3, 1), Triangle(2, 1, 3)})); + Convex tetra1(pts1, 4, tris1, 4); + + std::shared_ptr> pts2(new std::vector({ + Vec3f(1, 0, 0), + Vec3f(0, 0, 0), + Vec3f(0, -1, 0.5), + Vec3f(1, 1, 1), + })); + std::shared_ptr> tris2( + new std::vector({Triangle(0, 1, 2), Triangle(0, 2, 3), + Triangle(0, 3, 1), Triangle(2, 1, 3)})); + Convex tetra2(pts2, 4, tris2, 4); + + col_res.clear(); + hpp::fcl::collide(&tetra1, tf1, &tetra2, tf2, col_req, col_res); + BOOST_CHECK(col_res.isCollision()); + patch_res.clear(); + hpp::fcl::computeContactPatch(&tetra1, tf1, &tetra2, tf2, col_res, + patch_req, patch_res); + BOOST_CHECK(patch_res.numContactPatches() == 1); + + if (patch_res.numContactPatches() > 0) { + const Contact& contact = col_res.getContact(0); + const FCL_REAL tol = 1e-6; + + ContactPatch expected(expected_size); + expected.tf.rotation() = + constructOrthonormalBasisFromVector(contact.normal); + expected.tf.translation() = contact.pos; + expected.penetration_depth = contact.penetration_depth; + expected.addPoint(expected_cp); + + const ContactPatch& contact_patch = patch_res.getContactPatch(0); + BOOST_CHECK(expected.isSame(contact_patch, tol)); + } + } +} + +BOOST_AUTO_TEST_CASE(edge_case_segment_face) { + // This case covers the segment-face edge case of contact patches. + // Two tetrahedrons make contact on one of their segment/face respectively. + const size_t expected_size = 2; + const Vec3f expected_cp1(0, 0, 0); + const Vec3f expected_cp2(-0.5, 0.5, 0); + + const Transform3f tf1; // identity + const Transform3f tf2; // identity + + const size_t num_max_contact = 1; + const CollisionRequest col_req(CollisionRequestFlag::CONTACT, + num_max_contact); + CollisionResult col_res; + const ContactPatchRequest patch_req; + ContactPatchResult patch_res(patch_req); + + { + std::shared_ptr> pts1(new std::vector({ + Vec3f(-1, 0, -0), + Vec3f(0, 0, 0), + Vec3f(0, 1, 0), + Vec3f(-1, -1, -1), + })); + std::shared_ptr> tris1( + new std::vector({Triangle(0, 1, 2), Triangle(0, 2, 3), + Triangle(0, 3, 1), Triangle(2, 1, 3)})); + Convex tetra1(pts1, 4, tris1, 4); + + std::shared_ptr> pts2(new std::vector({ + Vec3f(-0.5, 0.5, 0), + Vec3f(0.5, -0.5, 0), + Vec3f(1, 0.5, 0.5), + Vec3f(1, 1, 1), + })); + std::shared_ptr> tris2( + new std::vector({Triangle(0, 1, 2), Triangle(0, 2, 3), + Triangle(0, 3, 1), Triangle(2, 1, 3)})); + Convex tetra2(pts2, 4, tris2, 4); + + col_res.clear(); + hpp::fcl::collide(&tetra1, tf1, &tetra2, tf2, col_req, col_res); + BOOST_CHECK(col_res.isCollision()); + patch_res.clear(); + hpp::fcl::computeContactPatch(&tetra1, tf1, &tetra2, tf2, col_res, + patch_req, patch_res); + BOOST_CHECK(patch_res.numContactPatches() == 1); + + if (patch_res.numContactPatches() > 0) { + const Contact& contact = col_res.getContact(0); + const FCL_REAL tol = 1e-6; + + ContactPatch expected(expected_size); + expected.tf.rotation() = + constructOrthonormalBasisFromVector(contact.normal); + expected.tf.translation() = contact.pos; + expected.penetration_depth = contact.penetration_depth; + expected.addPoint(expected_cp1); + expected.addPoint(expected_cp2); + + const ContactPatch& contact_patch = patch_res.getContactPatch(0); + BOOST_CHECK(expected.isSame(contact_patch, tol)); + } + } +} diff --git a/test/convex.cpp b/test/convex.cpp index ff789b98f..744902b97 100644 --- a/test/convex.cpp +++ b/test/convex.cpp @@ -45,28 +45,6 @@ using namespace hpp::fcl; -Convex buildBox(FCL_REAL l, FCL_REAL w, FCL_REAL d) { - std::shared_ptr> pts(new std::vector( - {Vec3f(l, w, d), Vec3f(l, w, -d), Vec3f(l, -w, d), Vec3f(l, -w, -d), - Vec3f(-l, w, d), Vec3f(-l, w, -d), Vec3f(-l, -w, d), - Vec3f(-l, -w, -d)})); - - std::shared_ptr> polygons( - new std::vector(6)); - (*polygons)[0].set(0, 2, 3, 1); // x+ side - (*polygons)[1].set(2, 6, 7, 3); // y- side - (*polygons)[2].set(4, 5, 7, 6); // x- side - (*polygons)[3].set(0, 1, 5, 4); // y+ side - (*polygons)[4].set(1, 3, 7, 5); // z- side - (*polygons)[5].set(0, 2, 6, 4); // z+ side - - return Convex(pts, // points - 8, // num points - polygons, - 6 // number of polygons - ); -} - BOOST_AUTO_TEST_CASE(convex) { FCL_REAL l = 1, w = 1, d = 1; Convex box(buildBox(l, w, d)); diff --git a/test/serialization.cpp b/test/serialization.cpp index 0e2e1aef5..c8a0231b0 100644 --- a/test/serialization.cpp +++ b/test/serialization.cpp @@ -37,11 +37,13 @@ #include #include +#include #include #include #include #include +#include #include #include #include @@ -270,6 +272,41 @@ BOOST_AUTO_TEST_CASE(test_collision_data) { distance_result.nearest_points[0].setRandom(); distance_result.nearest_points[1].setRandom(); test_serialization(distance_result); + + { + // Serializing contact patches. + const Halfspace hspace(0, 0, 1, 0); + const FCL_REAL radius = 0.25; + const FCL_REAL height = 1.; + const Cylinder cylinder(radius, height); + + const Transform3f tf1; + Transform3f tf2; + // set translation to have a collision + const FCL_REAL offset = 0.001; + tf2.setTranslation(Vec3f(0, 0, height / 2 - offset)); + + const size_t num_max_contact = 1; + const CollisionRequest col_req(CollisionRequestFlag::CONTACT, + num_max_contact); + CollisionResult col_res; + hpp::fcl::collide(&hspace, tf1, &cylinder, tf2, col_req, col_res); + BOOST_CHECK(col_res.isCollision()); + if (col_res.isCollision()) { + ContactPatchRequest patch_req; + ContactPatchResult patch_res(patch_req); + hpp::fcl::computeContactPatch(&hspace, tf1, &cylinder, tf2, col_res, + patch_req, patch_res); + BOOST_CHECK(patch_res.numContactPatches() == 1); + + // Serialize patch request, result and the patch itself + test_serialization(patch_req); + test_serialization(patch_res); + if (patch_res.numContactPatches() > 0) { + test_serialization(patch_res.getContactPatch(0)); + } + } + } } template diff --git a/test/utility.cpp b/test/utility.cpp index 9a8ea447b..cf20cdd14 100644 --- a/test/utility.cpp +++ b/test/utility.cpp @@ -457,6 +457,28 @@ void generateEnvironmentsMesh(std::vector& env, } } +Convex buildBox(FCL_REAL l, FCL_REAL w, FCL_REAL d) { + std::shared_ptr> pts(new std::vector( + {Vec3f(l, w, d), Vec3f(l, w, -d), Vec3f(l, -w, d), Vec3f(l, -w, -d), + Vec3f(-l, w, d), Vec3f(-l, w, -d), Vec3f(-l, -w, d), + Vec3f(-l, -w, -d)})); + + std::shared_ptr> polygons( + new std::vector(6)); + (*polygons)[0].set(0, 2, 3, 1); // x+ side + (*polygons)[1].set(2, 6, 7, 3); // y- side + (*polygons)[2].set(4, 5, 7, 6); // x- side + (*polygons)[3].set(0, 1, 5, 4); // y+ side + (*polygons)[4].set(1, 3, 7, 5); // z- side + (*polygons)[5].set(0, 2, 6, 4); // z+ side + + return Convex(pts, // points + 8, // num points + polygons, + 6 // number of polygons + ); +} + /// Takes a point and projects it onto the surface of the unit sphere void toSphere(Vec3f& point) { assert(point.norm() > 1e-8); diff --git a/test/utility.h b/test/utility.h index 04e0be5b9..b81723ab9 100644 --- a/test/utility.h +++ b/test/utility.h @@ -199,6 +199,11 @@ void generateEnvironments(std::vector& env, void generateEnvironmentsMesh(std::vector& env, FCL_REAL env_scale, std::size_t n); +/// @brief Constructs a box with halfsides (l, w, d), centered around 0. +/// The box is 2*l wide on the x-axis, 2*w wide on the y-axis and 2*d wide on +/// the z-axis. +Convex buildBox(FCL_REAL l, FCL_REAL w, FCL_REAL d); + /// @brief We give an ellipsoid as input. The output is a 20 faces polytope /// which vertices belong to the original ellipsoid surface. The procedure is /// simple: we construct a icosahedron, see