| Directory: | ./ |
|---|---|
| File: | include/coal/contact_patch/contact_patch_solver.h |
| Date: | 2025-04-13 14:25:19 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 6 | 12 | 50.0% |
| Branches: | 5 | 22 | 22.7% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | /* | ||
| 2 | * Software License Agreement (BSD License) | ||
| 3 | * | ||
| 4 | * Copyright (c) 2024, INRIA | ||
| 5 | * All rights reserved. | ||
| 6 | * Redistribution and use in source and binary forms, with or without | ||
| 7 | * modification, are permitted provided that the following conditions | ||
| 8 | * are met: | ||
| 9 | * | ||
| 10 | * * Redistributions of source code must retain the above copyright | ||
| 11 | * notice, this list of conditions and the following disclaimer. | ||
| 12 | * * Redistributions in binary form must reproduce the above | ||
| 13 | * copyright notice, this list of conditions and the following | ||
| 14 | * disclaimer in the documentation and/or other materials provided | ||
| 15 | * with the distribution. | ||
| 16 | * * Neither the name of INRIA nor the names of its | ||
| 17 | * contributors may be used to endorse or promote products derived | ||
| 18 | * from this software without specific prior written permission. | ||
| 19 | * | ||
| 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS | ||
| 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT | ||
| 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS | ||
| 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE | ||
| 24 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, | ||
| 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, | ||
| 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; | ||
| 27 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER | ||
| 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT | ||
| 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN | ||
| 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE | ||
| 31 | * POSSIBILITY OF SUCH DAMAGE. | ||
| 32 | */ | ||
| 33 | |||
| 34 | /** \author Louis Montaut */ | ||
| 35 | |||
| 36 | #ifndef COAL_CONTACT_PATCH_SOLVER_H | ||
| 37 | #define COAL_CONTACT_PATCH_SOLVER_H | ||
| 38 | |||
| 39 | #include "coal/collision_data.h" | ||
| 40 | #include "coal/logging.h" | ||
| 41 | #include "coal/narrowphase/gjk.h" | ||
| 42 | |||
| 43 | namespace coal { | ||
| 44 | |||
| 45 | /// @brief Solver to compute contact patches, i.e. the intersection between two | ||
| 46 | /// contact surfaces projected onto the shapes' separating plane. | ||
| 47 | /// Otherwise said, a contact patch is simply the intersection between two | ||
| 48 | /// support sets: the support set of shape S1 in direction `n` and the support | ||
| 49 | /// set of shape S2 in direction `-n`, where `n` is the contact normal | ||
| 50 | /// (satisfying the optimality conditions of GJK/EPA). | ||
| 51 | /// @note A contact patch is **not** the support set of the Minkowski Difference | ||
| 52 | /// in the direction of the normal. | ||
| 53 | /// A contact patch is actually the support set of the Minkowski difference in | ||
| 54 | /// the direction of the normal, i.e. the instersection of the shapes support | ||
| 55 | /// sets as mentioned above. | ||
| 56 | /// | ||
| 57 | /// TODO(louis): algo improvement: | ||
| 58 | /// - The clipping algo is currently n1 * n2; it can be done in n1 + n2. | ||
| 59 | struct COAL_DLLAPI ContactPatchSolver { | ||
| 60 | // Note: `ContactPatch` is an alias for `SupportSet`. | ||
| 61 | // The two can be used interchangeably. | ||
| 62 | using ShapeSupportData = details::ShapeSupportData; | ||
| 63 | using SupportSetDirection = SupportSet::PatchDirection; | ||
| 64 | |||
| 65 | /// @brief Support set function for shape si. | ||
| 66 | /// @param[in] shape the shape. | ||
| 67 | /// @param[in/out] support_set a support set of the shape. A support set is | ||
| 68 | /// attached to a frame. All the points of the set computed by this function | ||
| 69 | /// will be expressed in the local frame of the support set. The support set | ||
| 70 | /// is computed in the direction of the positive z-axis if its direction is | ||
| 71 | /// DEFAULT, negative z-axis if its direction is INVERTED. | ||
| 72 | /// @param[in/out] hint for the support computation of ConvexBase shapes. Gets | ||
| 73 | /// updated after calling the function onto ConvexBase shapes. | ||
| 74 | /// @param[in/out] support_data for the support computation of ConvexBase | ||
| 75 | /// shapes. Gets updated with visited vertices after calling the function onto | ||
| 76 | /// ConvexBase shapes. | ||
| 77 | /// @param[in] num_sampled_supports for shapes like cone or cylinders which | ||
| 78 | /// have smooth non-strictly convex sides (their bases are circles), we need | ||
| 79 | /// to know how many supports we sample from these sides. For any other shape, | ||
| 80 | /// this parameter is not used. | ||
| 81 | /// @param[in] tol the "thickness" of the support plane. Any point v which | ||
| 82 | /// satisfies `max_{x in shape}(x.dot(dir)) - v.dot(dir) <= tol` is tol | ||
| 83 | /// distant from the support plane and is added to the support set. | ||
| 84 | typedef void (*SupportSetFunction)(const ShapeBase* shape, | ||
| 85 | SupportSet& support_set, int& hint, | ||
| 86 | ShapeSupportData& support_data, | ||
| 87 | size_t num_sampled_supports, Scalar tol); | ||
| 88 | |||
| 89 | /// @brief Number of vectors to pre-allocate in the `m_clipping_sets` vectors. | ||
| 90 | static constexpr size_t default_num_preallocated_supports = 16; | ||
| 91 | |||
| 92 | /// @brief Number of points sampled for Cone and Cylinder when the normal is | ||
| 93 | /// orthogonal to the shapes' basis. | ||
| 94 | /// See @ref ContactPatchRequest::m_num_samples_curved_shapes for more | ||
| 95 | /// details. | ||
| 96 | size_t num_samples_curved_shapes; | ||
| 97 | |||
| 98 | /// @brief Tolerance below which points are added to the shapes support sets. | ||
| 99 | /// See @ref ContactPatchRequest::m_patch_tolerance for more details. | ||
| 100 | Scalar patch_tolerance; | ||
| 101 | |||
| 102 | /// @brief Support set function for shape s1. | ||
| 103 | mutable SupportSetFunction supportFuncShape1; | ||
| 104 | |||
| 105 | /// @brief Support set function for shape s2. | ||
| 106 | mutable SupportSetFunction supportFuncShape2; | ||
| 107 | |||
| 108 | /// @brief Temporary data to compute the support sets on each shape. | ||
| 109 | mutable std::array<ShapeSupportData, 2> supports_data; | ||
| 110 | |||
| 111 | /// @brief Guess for the support sets computation. | ||
| 112 | mutable support_func_guess_t support_guess; | ||
| 113 | |||
| 114 | /// @brief Holder for support set of shape 1, used for internal computation. | ||
| 115 | /// After `computePatch` has been called, this support set is no longer valid. | ||
| 116 | mutable SupportSet support_set_shape1; | ||
| 117 | |||
| 118 | /// @brief Holder for support set of shape 2, used for internal computation. | ||
| 119 | /// After `computePatch` has been called, this support set is no longer valid. | ||
| 120 | mutable SupportSet support_set_shape2; | ||
| 121 | |||
| 122 | /// @brief Temporary support set used for the Sutherland-Hodgman algorithm. | ||
| 123 | mutable SupportSet support_set_buffer; | ||
| 124 | |||
| 125 | /// @brief Tracks which point of the Sutherland-Hodgman result have been added | ||
| 126 | /// to the contact patch. Only used if the post-processing step occurs, i.e. | ||
| 127 | /// if the result of Sutherland-Hodgman has a size bigger than | ||
| 128 | /// `max_patch_size`. | ||
| 129 | mutable std::vector<bool> added_to_patch; | ||
| 130 | |||
| 131 | /// @brief Default constructor. | ||
| 132 | ✗ | explicit ContactPatchSolver() { | |
| 133 | ✗ | const size_t num_contact_patch = 1; | |
| 134 | ✗ | const size_t preallocated_patch_size = | |
| 135 | ContactPatch::default_preallocated_size; | ||
| 136 | ✗ | const Scalar patch_tolerance = Scalar(1e-3); | |
| 137 | const ContactPatchRequest request(num_contact_patch, | ||
| 138 | ✗ | preallocated_patch_size, patch_tolerance); | |
| 139 | ✗ | this->set(request); | |
| 140 | } | ||
| 141 | |||
| 142 | /// @brief Construct the solver with a `ContactPatchRequest`. | ||
| 143 |
4/8✓ Branch 2 taken 24 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 24 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 24 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 24 times.
✗ Branch 12 not taken.
|
24 | explicit ContactPatchSolver(const ContactPatchRequest& request) { |
| 144 |
1/2✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
|
24 | this->set(request); |
| 145 | 24 | } | |
| 146 | |||
| 147 | /// @brief Set up the solver using a `ContactPatchRequest`. | ||
| 148 | void set(const ContactPatchRequest& request); | ||
| 149 | |||
| 150 | /// @brief Sets the support guess used during support set computation of | ||
| 151 | /// shapes s1 and s2. | ||
| 152 | 24 | void setSupportGuess(const support_func_guess_t guess) const { | |
| 153 | 24 | this->support_guess = guess; | |
| 154 | 24 | } | |
| 155 | |||
| 156 | /// @brief Main API of the solver: compute a contact patch from a contact | ||
| 157 | /// between shapes s1 and s2. | ||
| 158 | /// The contact patch is the (triple) intersection between the separating | ||
| 159 | /// plane passing (by `contact.pos` and supported by `contact.normal`) and the | ||
| 160 | /// shapes s1 and s2. | ||
| 161 | template <typename ShapeType1, typename ShapeType2> | ||
| 162 | void computePatch(const ShapeType1& s1, const Transform3s& tf1, | ||
| 163 | const ShapeType2& s2, const Transform3s& tf2, | ||
| 164 | const Contact& contact, ContactPatch& contact_patch) const; | ||
| 165 | |||
| 166 | /// @brief Reset the internal quantities of the solver. | ||
| 167 | template <typename ShapeType1, typename ShapeType2> | ||
| 168 | void reset(const ShapeType1& shape1, const Transform3s& tf1, | ||
| 169 | const ShapeType2& shape2, const Transform3s& tf2, | ||
| 170 | const ContactPatch& contact_patch) const; | ||
| 171 | |||
| 172 | /// @brief Retrieve result, adds a post-processing step if result has bigger | ||
| 173 | /// size than `this->max_patch_size`. | ||
| 174 | void getResult(const Contact& contact, const ContactPatch::Polygon* result, | ||
| 175 | ContactPatch& contact_patch) const; | ||
| 176 | |||
| 177 | /// @return the intersecting point between line defined by ray (a, b) and | ||
| 178 | /// the segment [c, d]. | ||
| 179 | /// @note we make the following hypothesis: | ||
| 180 | /// 1) c != d (should be when creating initial polytopes) | ||
| 181 | /// 2) (c, d) is not parallel to ray -> if so, we return d. | ||
| 182 | static Vec2s computeLineSegmentIntersection(const Vec2s& a, const Vec2s& b, | ||
| 183 | const Vec2s& c, const Vec2s& d); | ||
| 184 | |||
| 185 | /// @brief Construct support set function for shape. | ||
| 186 | static SupportSetFunction makeSupportSetFunction( | ||
| 187 | const ShapeBase* shape, ShapeSupportData& support_data); | ||
| 188 | |||
| 189 | bool operator==(const ContactPatchSolver& other) const { | ||
| 190 | return this->num_samples_curved_shapes == other.num_samples_curved_shapes && | ||
| 191 | this->patch_tolerance == other.patch_tolerance && | ||
| 192 | this->support_guess == other.support_guess && | ||
| 193 | this->support_set_shape1 == other.support_set_shape1 && | ||
| 194 | this->support_set_shape2 == other.support_set_shape2 && | ||
| 195 | this->support_set_buffer == other.support_set_buffer && | ||
| 196 | this->added_to_patch == other.added_to_patch && | ||
| 197 | this->supportFuncShape1 == other.supportFuncShape1 && | ||
| 198 | this->supportFuncShape2 == other.supportFuncShape2; | ||
| 199 | } | ||
| 200 | |||
| 201 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
| 202 | }; | ||
| 203 | |||
| 204 | } // namespace coal | ||
| 205 | |||
| 206 | #include "coal/contact_patch/contact_patch_solver.hxx" | ||
| 207 | |||
| 208 | #endif // COAL_CONTACT_PATCH_SOLVER_H | ||
| 209 |