GCC Code Coverage Report


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