GCC Code Coverage Report

Directory: ./
File: include/coal/contact_patch/contact_patch_solver.hxx
Date: 2025-04-01 09:23:31
Exec Total Coverage
Lines: 158 182 86.8%
Branches: 137 310 44.2%
Line Branch Exec Source
1 /*
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2024, INRIA
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of INRIA nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 */
34
35 /** \author Louis Montaut */
36
37 #ifndef COAL_CONTACT_PATCH_SOLVER_HXX
38 #define COAL_CONTACT_PATCH_SOLVER_HXX
39
40 #include "coal/data_types.h"
41 #include "coal/shape/geometric_shapes_traits.h"
42
43 #include "coal/tracy.hh"
44
45 namespace coal {
46
47 // ============================================================================
48 24 inline void ContactPatchSolver::set(const ContactPatchRequest& request) {
49 // Note: it's important for the number of pre-allocated Vec3s in
50 // `m_clipping_sets` to be larger than `request.max_size_patch`
51 // because we don't know in advance how many supports will be discarded to
52 // form the convex-hulls of the shapes supports which will serve as the
53 // input of the Sutherland-Hodgman algorithm.
54 24 size_t num_preallocated_supports = default_num_preallocated_supports;
55
1/2
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
 24 if (num_preallocated_supports < 2 * request.getNumSamplesCurvedShapes()) {
56 24 num_preallocated_supports = 2 * request.getNumSamplesCurvedShapes();
57 }
58
59 // Used for support set computation of shape1 and for the first iterate of the
60 // Sutherland-Hodgman algo.
61 24 this->support_set_shape1.points().reserve(num_preallocated_supports);
62 24 this->support_set_shape1.direction = SupportSetDirection::DEFAULT;
63
64 // Used for computing the next iterate of the Sutherland-Hodgman algo.
65 24 this->support_set_buffer.points().reserve(num_preallocated_supports);
66
67 // Used for support set computation of shape2 and acts as the "clipper" set in
68 // the Sutherland-Hodgman algo.
69 24 this->support_set_shape2.points().reserve(num_preallocated_supports);
70 24 this->support_set_shape2.direction = SupportSetDirection::INVERTED;
71
72 24 this->num_samples_curved_shapes = request.getNumSamplesCurvedShapes();
73 24 this->patch_tolerance = request.getPatchTolerance();
74 24 }
75
76 // ============================================================================
77 template <typename ShapeType1, typename ShapeType2>
78 24 void ContactPatchSolver::computePatch(const ShapeType1& s1,
79 const Transform3s& tf1,
80 const ShapeType2& s2,
81 const Transform3s& tf2,
82 const Contact& contact,
83 ContactPatch& contact_patch) const {
84 COAL_TRACY_ZONE_SCOPED_N("coal::ContactPatchSolver::computePatch");
85 // Note: `ContactPatch` is an alias for `SupportSet`.
86 // Step 1
87 24 constructContactPatchFrameFromContact(contact, contact_patch);
88 24 contact_patch.points().clear();
89 if ((bool)(shape_traits<ShapeType1>::IsStrictlyConvex) ||
90 (bool)(shape_traits<ShapeType2>::IsStrictlyConvex)) {
91 // If a shape is strictly convex, the support set in any direction is
92 // reduced to a single point. Thus, the contact point `contact.pos` is the
93 // only point belonging to the contact patch, and it has already been
94 // computed.
95 // TODO(louis): even for strictly convex shapes, we can sample the support
96 // function around the normal and return a pseudo support set. This would
97 // allow spheres and ellipsoids to have a contact surface, which does make
98 // sense in certain physics simulation cases.
99 // Do the same for strictly convex regions of non-strictly convex shapes
100 // like the ends of capsules.
101 2 contact_patch.addPoint(contact.pos);
102 2 return;
103 }
104
105 // Step 2 - Compute support set of each shape, in the direction of
106 // the contact's normal.
107 // The first shape's support set is called "current"; it will be the first
108 // iterate of the Sutherland-Hodgman algorithm. The second shape's support set
109 // is called "clipper"; it will be used to clip "current". The support set
110 // computation step computes a convex polygon; its vertices are ordered
111 // counter-clockwise. This is important as the Sutherland-Hodgman algorithm
112 // expects points to be ranked counter-clockwise.
113 22 this->reset(s1, tf1, s2, tf2, contact_patch);
114
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 11 times.
 22 assert(this->num_samples_curved_shapes > 3);
115
116 22 this->supportFuncShape1(&s1, this->support_set_shape1, this->support_guess[0],
117 this->supports_data[0],
118 22 this->num_samples_curved_shapes,
119 22 this->patch_tolerance);
120
121 22 this->supportFuncShape2(&s2, this->support_set_shape2, this->support_guess[1],
122 this->supports_data[1],
123 22 this->num_samples_curved_shapes,
124 22 this->patch_tolerance);
125
126 // We can immediatly return if one of the support set has only
127 // one point.
128
3/6
✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 11 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 11 times.
 44 if (this->support_set_shape1.size() <= 1 ||
129 22 this->support_set_shape2.size() <= 1) {
130 ✗ contact_patch.addPoint(contact.pos);
131 ✗ return;
132 }
133
134 // `eps` is be used to check strict positivity of determinants.
135 22 const Scalar eps = Eigen::NumTraits<Scalar>::dummy_precision();
136 using Polygon = SupportSet::Polygon;
137
138
6/6
✓ Branch 1 taken 3 times.
✓ Branch 2 taken 8 times.
✓ Branch 3 taken 1 times.
✓ Branch 4 taken 2 times.
✓ Branch 5 taken 1 times.
✓ Branch 6 taken 10 times.
 28 if ((this->support_set_shape1.size() == 2) &&
139 6 (this->support_set_shape2.size() == 2)) {
140 // Segment-Segment case
141 // We compute the determinant; if it is non-zero, the intersection
142 // has already been computed: it's `Contact::pos`.
143 2 const Polygon& pts1 = this->support_set_shape1.points();
144 2 const Vec2s& a = pts1[0];
145 2 const Vec2s& b = pts1[1];
146
147 2 const Polygon& pts2 = this->support_set_shape2.points();
148 2 const Vec2s& c = pts2[0];
149 2 const Vec2s& d = pts2[1];
150
151 2 const Scalar det =
152
9/18
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
 2 (b(0) - a(0)) * (d(1) - c(1)) >= (b(1) - a(1)) * (d(0) - c(0));
153
1/8
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
 2 if ((std::abs(det) > eps) || ((c - d).squaredNorm() < eps) ||
154
1/8
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 2 ((b - a).squaredNorm() < eps)) {
155
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 contact_patch.addPoint(contact.pos);
156 2 return;
157 }
158
159 ✗ const Vec2s cd = (d - c);
160 ✗ const Scalar l = cd.squaredNorm();
161 ✗ Polygon& patch = contact_patch.points();
162
163 // Project a onto [c, d]
164 ✗ Scalar t1 = (a - c).dot(cd);
165 ✗ t1 = (t1 >= l) ? 1.0 : ((t1 <= 0) ? 0.0 : (t1 / l));
166 ✗ const Vec2s p1 = c + t1 * cd;
167 ✗ patch.emplace_back(p1);
168
169 // Project b onto [c, d]
170 ✗ Scalar t2 = (b - c).dot(cd);
171 ✗ t2 = (t2 >= l) ? 1.0 : ((t2 <= 0) ? 0.0 : (t2 / l));
172 ✗ const Vec2s p2 = c + t2 * cd;
173 ✗ if ((p1 - p2).squaredNorm() >= eps) {
174 ✗ patch.emplace_back(p2);
175 }
176 ✗ return;
177 }
178
179 //
180 // Step 3 - Main loop of the algorithm: use the "clipper" polygon to clip the
181 // "current" polygon. The resulting intersection is the contact patch of the
182 // contact between s1 and s2. "clipper" and "current" are the support sets of
183 // shape1 and shape2 (they can be swapped, i.e. clipper can be assigned to
184 // shape1 and current to shape2, depending on which case we are). Currently,
185 // to clip one polygon with the other, we use the Sutherland-Hodgman
186 // algorithm:
187 // https://en.wikipedia.org/wiki/Sutherland%E2%80%93Hodgman_algorithm
188 // In the general case, Sutherland-Hodgman clips one polygon of size >=3 using
189 // another polygon of size >=3. However, it can be easily extended to handle
190 // the segment-polygon case.
191 //
192 // The maximum size of the output of the Sutherland-Hodgman algorithm is n1 +
193 // n2 where n1 and n2 are the sizes of the first and second polygon.
194 20 const size_t max_result_size =
195 20 this->support_set_shape1.size() + this->support_set_shape2.size();
196
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 20 if (this->added_to_patch.size() < max_result_size) {
197
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 20 this->added_to_patch.assign(max_result_size, false);
198 }
199
200 20 const Polygon* clipper_ptr = nullptr;
201 20 Polygon* current_ptr = nullptr;
202 20 Polygon* previous_ptr = &(this->support_set_buffer.points());
203
204 // Let the clipper set be the one with the most vertices, to make sure it is
205 // at least a triangle.
206
2/2
✓ Branch 2 taken 2 times.
✓ Branch 3 taken 8 times.
 20 if (this->support_set_shape1.size() < this->support_set_shape2.size()) {
207 4 current_ptr = &(this->support_set_shape1.points());
208 4 clipper_ptr = &(this->support_set_shape2.points());
209 } else {
210 16 current_ptr = &(this->support_set_shape2.points());
211 16 clipper_ptr = &(this->support_set_shape1.points());
212 }
213
214 20 const Polygon& clipper = *(clipper_ptr);
215 20 const size_t clipper_size = clipper.size();
216
2/2
✓ Branch 0 taken 33 times.
✓ Branch 1 taken 8 times.
 82 for (size_t i = 0; i < clipper_size; ++i) {
217 // Swap `current` and `previous`.
218 // `previous` tracks the last iteration of the algorithm; `current` is
219 // filled by clipping `current` using `clipper`.
220 66 Polygon* tmp_ptr = previous_ptr;
221 66 previous_ptr = current_ptr;
222 66 current_ptr = tmp_ptr;
223
224 66 const Polygon& previous = *(previous_ptr);
225 66 Polygon& current = *(current_ptr);
226 66 current.clear();
227
228 66 const Vec2s& a = clipper[i];
229 66 const Vec2s& b = clipper[(i + 1) % clipper_size];
230
2/4
✓ Branch 1 taken 33 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 33 times.
✗ Branch 5 not taken.
 66 const Vec2s ab = b - a;
231
232
2/2
✓ Branch 1 taken 15 times.
✓ Branch 2 taken 18 times.
 66 if (previous.size() == 2) {
233 //
234 // Segment-Polygon case
235 //
236 30 const Vec2s& p1 = previous[0];
237 30 const Vec2s& p2 = previous[1];
238
239
2/4
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
 30 const Vec2s ap1 = p1 - a;
240
2/4
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
 30 const Vec2s ap2 = p2 - a;
241
242
4/8
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 15 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 15 times.
✗ Branch 11 not taken.
 30 const Scalar det1 = ab(0) * ap1(1) - ab(1) * ap1(0);
243
4/8
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 15 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 15 times.
✗ Branch 11 not taken.
 30 const Scalar det2 = ab(0) * ap2(1) - ab(1) * ap2(0);
244
245
3/4
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 3 times.
 30 if (det1 < 0 && det2 < 0) {
246 // Both p1 and p2 are outside the clipping polygon, i.e. there is no
247 // intersection. The algorithm can stop.
248 4 break;
249 }
250
251
4/4
✓ Branch 0 taken 12 times.
✓ Branch 1 taken 3 times.
✓ Branch 2 taken 9 times.
✓ Branch 3 taken 3 times.
 30 if (det1 >= 0 && det2 >= 0) {
252 // Both p1 and p2 are inside the clipping polygon, there is nothing to
253 // do; move to the next iteration.
254
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 18 current = previous;
255 26 continue;
256 }
257
258 // Compute the intersection between the line (a, b) and the segment
259 // [p1, p2].
260
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 3 times.
 12 if (det1 >= 0) {
261
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 1 times.
 6 if (det1 > eps) {
262
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 4 const Vec2s p = computeLineSegmentIntersection(a, b, p1, p2);
263
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 4 current.emplace_back(p1);
264
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 4 current.emplace_back(p);
265 4 continue;
266 4 } else {
267 // p1 is the only point of current which is also a point of the
268 // clipper. We can exit.
269
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 current.emplace_back(p1);
270 2 break;
271 }
272 } else {
273
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 1 times.
 6 if (det2 > eps) {
274
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 4 const Vec2s p = computeLineSegmentIntersection(a, b, p1, p2);
275
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 4 current.emplace_back(p2);
276
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 4 current.emplace_back(p);
277 4 continue;
278 4 } else {
279 // p2 is the only point of current which is also a point of the
280 // clipper. We can exit.
281
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 current.emplace_back(p2);
282 2 break;
283 }
284 }
285 } else {
286 //
287 // Polygon-Polygon case.
288 //
289
1/2
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
 36 std::fill(this->added_to_patch.begin(), //
290 this->added_to_patch.end(), //
291 36 false);
292
293 36 const size_t previous_size = previous.size();
294
2/2
✓ Branch 0 taken 70 times.
✓ Branch 1 taken 18 times.
 176 for (size_t j = 0; j < previous_size; ++j) {
295 140 const Vec2s& p1 = previous[j];
296 140 const Vec2s& p2 = previous[(j + 1) % previous_size];
297
298
2/4
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 70 times.
✗ Branch 5 not taken.
 140 const Vec2s ap1 = p1 - a;
299
2/4
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 70 times.
✗ Branch 5 not taken.
 140 const Vec2s ap2 = p2 - a;
300
301
4/8
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 70 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 70 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 70 times.
✗ Branch 11 not taken.
 140 const Scalar det1 = ab(0) * ap1(1) - ab(1) * ap1(0);
302
4/8
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 70 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 70 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 70 times.
✗ Branch 11 not taken.
 140 const Scalar det2 = ab(0) * ap2(1) - ab(1) * ap2(0);
303
304
3/4
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 68 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 2 times.
 140 if (det1 < 0 && det2 < 0) {
305 // No intersection. Continue to next segment of previous.
306 132 continue;
307 }
308
309
4/4
✓ Branch 0 taken 68 times.
✓ Branch 1 taken 2 times.
✓ Branch 2 taken 66 times.
✓ Branch 3 taken 2 times.
 140 if (det1 >= 0 && det2 >= 0) {
310 // Both p1 and p2 are inside the clipping polygon, add p1 to current
311 // (only if it has not already been added).
312
3/4
✓ Branch 1 taken 66 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 64 times.
✓ Branch 5 taken 2 times.
 132 if (!this->added_to_patch[j]) {
313
1/2
✓ Branch 1 taken 64 times.
✗ Branch 2 not taken.
 128 current.emplace_back(p1);
314
1/2
✓ Branch 1 taken 64 times.
✗ Branch 2 not taken.
 128 this->added_to_patch[j] = true;
315 }
316 // Continue to next segment of previous.
317 132 continue;
318 }
319
320
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 2 times.
 8 if (det1 >= 0) {
321
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 2 times.
 4 if (det1 > eps) {
322 ✗ if (!this->added_to_patch[j]) {
323 ✗ current.emplace_back(p1);
324 ✗ this->added_to_patch[j] = true;
325 }
326 ✗ const Vec2s p = computeLineSegmentIntersection(a, b, p1, p2);
327 ✗ current.emplace_back(p);
328 } else {
329 // a, b and p1 are colinear; we add only p1.
330
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 4 if (!this->added_to_patch[j]) {
331
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 4 current.emplace_back(p1);
332
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 4 this->added_to_patch[j] = true;
333 }
334 }
335 } else {
336
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 2 times.
 4 if (det2 > eps) {
337 ✗ const Vec2s p = computeLineSegmentIntersection(a, b, p1, p2);
338 ✗ current.emplace_back(p);
339 } else {
340
2/4
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
 4 if (!this->added_to_patch[(j + 1) % previous.size()]) {
341
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 4 current.emplace_back(p2);
342
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 4 this->added_to_patch[(j + 1) % previous.size()] = true;
343 }
344 }
345 }
346 }
347 }
348 //
349 // End of iteration i of Sutherland-Hodgman.
350
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 18 times.
 36 if (current.size() <= 1) {
351 // No intersection or one point found, the algo can early stop.
352 ✗ break;
353 }
354 }
355
356 // Transfer the result of the Sutherland-Hodgman algorithm to the contact
357 // patch.
358 20 this->getResult(contact, current_ptr, contact_patch);
359 }
360
361 // ============================================================================
362 22 inline void ContactPatchSolver::getResult(
363 const Contact& contact, const ContactPatch::Polygon* result_ptr,
364 ContactPatch& contact_patch) const {
365
2/2
✓ Branch 1 taken 6 times.
✓ Branch 2 taken 16 times.
 22 if (result_ptr->size() <= 1) {
366 6 contact_patch.addPoint(contact.pos);
367 6 return;
368 }
369
370 16 const ContactPatch::Polygon& result = *(result_ptr);
371 16 ContactPatch::Polygon& patch = contact_patch.points();
372 16 patch = result;
373 }
374
375 // ============================================================================
376 template <typename ShapeType1, typename ShapeType2>
377 22 inline void ContactPatchSolver::reset(const ShapeType1& shape1,
378 const Transform3s& tf1,
379 const ShapeType2& shape2,
380 const Transform3s& tf2,
381 const ContactPatch& contact_patch) const {
382 // Reset internal quantities
383 22 this->support_set_shape1.clear();
384 22 this->support_set_shape2.clear();
385 22 this->support_set_buffer.clear();
386
387 // Get the support function of each shape
388 22 const Transform3s& tfc = contact_patch.tf;
389
390 22 this->support_set_shape1.direction = SupportSetDirection::DEFAULT;
391 // Set the reference frame of the support set of the first shape to be the
392 // local frame of shape 1.
393 22 Transform3s& tf1c = this->support_set_shape1.tf;
394
3/6
✓ Branch 4 taken 11 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 11 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 11 times.
✗ Branch 12 not taken.
 22 tf1c.rotation().noalias() = tf1.rotation().transpose() * tfc.rotation();
395
3/6
✓ Branch 2 taken 11 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 11 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 11 times.
✗ Branch 10 not taken.
 44 tf1c.translation().noalias() =
396
1/2
✓ Branch 4 taken 11 times.
✗ Branch 5 not taken.
 44 tf1.rotation().transpose() * (tfc.translation() - tf1.translation());
397 22 this->supportFuncShape1 =
398 22 this->makeSupportSetFunction(&shape1, this->supports_data[0]);
399
400 22 this->support_set_shape2.direction = SupportSetDirection::INVERTED;
401 // Set the reference frame of the support set of the second shape to be the
402 // local frame of shape 2.
403 22 Transform3s& tf2c = this->support_set_shape2.tf;
404
3/6
✓ Branch 4 taken 11 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 11 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 11 times.
✗ Branch 12 not taken.
 22 tf2c.rotation().noalias() = tf2.rotation().transpose() * tfc.rotation();
405
3/6
✓ Branch 2 taken 11 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 11 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 11 times.
✗ Branch 10 not taken.
 44 tf2c.translation().noalias() =
406
1/2
✓ Branch 4 taken 11 times.
✗ Branch 5 not taken.
 44 tf2.rotation().transpose() * (tfc.translation() - tf2.translation());
407 22 this->supportFuncShape2 =
408 22 this->makeSupportSetFunction(&shape2, this->supports_data[1]);
409 }
410
411 // ==========================================================================
412 4 inline Vec2s ContactPatchSolver::computeLineSegmentIntersection(
413 const Vec2s& a, const Vec2s& b, const Vec2s& c, const Vec2s& d) {
414
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 4 const Vec2s ab = b - a;
415
3/6
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
 4 const Vec2s n(-ab(1), ab(0));
416
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 4 const Scalar denominator = n.dot(c - d);
417
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 4 times.
 4 if (std::abs(denominator) < std::numeric_limits<Scalar>::epsilon()) {
418 ✗ return d;
419 }
420
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 4 const Scalar nominator = n.dot(a - d);
421 4 Scalar alpha = nominator / denominator;
422 4 alpha = std::min<Scalar>(1.0, std::max<Scalar>(0.0, alpha));
423
4/8
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
 8 return alpha * c + (1 - alpha) * d;
424 }
425
426 } // namespace coal
427
428 #endif // COAL_CONTACT_PATCH_SOLVER_HXX
429