Directory: | ./ |
---|---|
File: | include/coal/contact_patch/contact_patch_solver.h |
Date: | 2025-04-01 09:23:31 |
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 |