GCC Code Coverage Report

Directory: ./
File: src/narrowphase/support_functions.cpp
Date: 2025-05-02 10:16:21
Exec Total Coverage
Lines: 308 477 64.6%
Branches: 372 898 41.4%
Line Branch Exec Source
1 /*
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011-2014, Willow Garage, Inc.
5 * Copyright (c) 2014-2015, Open Source Robotics Foundation
6 * Copyright (c) 2021-2024, INRIA
7 * All rights reserved.
8 *
9 * Redistribution and use in source and binary forms, with or without
10 * modification, are permitted provided that the following conditions
11 * are met:
12 *
13 * * Redistributions of source code must retain the above copyright
14 * notice, this list of conditions and the following disclaimer.
15 * * Redistributions in binary form must reproduce the above
16 * copyright notice, this list of conditions and the following
17 * disclaimer in the documentation and/or other materials provided
18 * with the distribution.
19 * * Neither the name of Open Source Robotics Foundation nor the names of its
20 * contributors may be used to endorse or promote products derived
21 * from this software without specific prior written permission.
22 *
23 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34 * POSSIBILITY OF SUCH DAMAGE.
35 */
36
37 /** \authors Jia Pan, Florent Lamiraux, Josef Mirabel, Louis Montaut */
38
39 #include "coal/narrowphase/support_functions.h"
40
41 #include <algorithm>
42
43 namespace coal {
44 namespace details {
45
46 // ============================================================================
47 #define CALL_GET_SHAPE_SUPPORT(ShapeType) \
48 getShapeSupport<_SupportOptions>(static_cast<const ShapeType*>(shape), dir, \
49 support, hint, support_data)
50
51 template <int _SupportOptions>
52 35076 Vec3s getSupport(const ShapeBase* shape, const Vec3s& dir, int& hint) {
53
1/2
✓ Branch 1 taken 17538 times.
✗ Branch 2 not taken.
 35076 Vec3s support;
54
1/2
✓ Branch 1 taken 17538 times.
✗ Branch 2 not taken.
 35076 ShapeSupportData support_data;
55
9/12
✓ Branch 1 taken 17538 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 170 times.
✓ Branch 4 taken 2711 times.
✓ Branch 5 taken 2695 times.
✓ Branch 6 taken 2592 times.
✓ Branch 7 taken 2775 times.
✓ Branch 8 taken 2799 times.
✓ Branch 9 taken 2800 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 996 times.
✗ Branch 12 not taken.
 35076 switch (shape->getNodeType()) {
56 340 case GEOM_TRIANGLE:
57
1/2
✓ Branch 1 taken 170 times.
✗ Branch 2 not taken.
 340 CALL_GET_SHAPE_SUPPORT(TriangleP);
58 340 break;
59 5422 case GEOM_BOX:
60
1/2
✓ Branch 1 taken 2711 times.
✗ Branch 2 not taken.
 5422 CALL_GET_SHAPE_SUPPORT(Box);
61 5422 break;
62 5390 case GEOM_SPHERE:
63
1/2
✓ Branch 1 taken 2695 times.
✗ Branch 2 not taken.
 5390 CALL_GET_SHAPE_SUPPORT(Sphere);
64 5390 break;
65 5184 case GEOM_ELLIPSOID:
66
1/2
✓ Branch 1 taken 2592 times.
✗ Branch 2 not taken.
 5184 CALL_GET_SHAPE_SUPPORT(Ellipsoid);
67 5184 break;
68 5550 case GEOM_CAPSULE:
69
1/2
✓ Branch 1 taken 2775 times.
✗ Branch 2 not taken.
 5550 CALL_GET_SHAPE_SUPPORT(Capsule);
70 5550 break;
71 5598 case GEOM_CONE:
72
1/2
✓ Branch 1 taken 2799 times.
✗ Branch 2 not taken.
 5598 CALL_GET_SHAPE_SUPPORT(Cone);
73 5598 break;
74 5600 case GEOM_CYLINDER:
75
1/2
✓ Branch 1 taken 2800 times.
✗ Branch 2 not taken.
 5600 CALL_GET_SHAPE_SUPPORT(Cylinder);
76 5600 break;
77 ✗ case GEOM_CONVEX16:
78 ✗ CALL_GET_SHAPE_SUPPORT(ConvexBaseTpl<Triangle16::IndexType>);
79 ✗ break;
80 1992 case GEOM_CONVEX32:
81
1/2
✓ Branch 1 taken 996 times.
✗ Branch 2 not taken.
 1992 CALL_GET_SHAPE_SUPPORT(ConvexBaseTpl<Triangle32::IndexType>);
82 1992 break;
83 ✗ case GEOM_PLANE:
84 case GEOM_HALFSPACE:
85 default:
86 ✗ support.setZero();
87 ; // nothing
88 }
89
90 70152 return support;
91 }
92 #undef CALL_GET_SHAPE_SUPPORT
93
94 // Explicit instantiation
95 // clang-format off
96 template COAL_DLLAPI Vec3s getSupport<SupportOptions::NoSweptSphere>(const ShapeBase*, const Vec3s&, int&);
97
98 template COAL_DLLAPI Vec3s getSupport<SupportOptions::WithSweptSphere>(const ShapeBase*, const Vec3s&, int&);
99 // clang-format on
100
101 // ============================================================================
102 #define getShapeSupportTplInstantiation(ShapeType) \
103 template COAL_DLLAPI void getShapeSupport<SupportOptions::NoSweptSphere>( \
104 const ShapeType* shape_, const Vec3s& dir, Vec3s& support, int& hint, \
105 ShapeSupportData& support_data); \
106 \
107 template COAL_DLLAPI void getShapeSupport<SupportOptions::WithSweptSphere>( \
108 const ShapeType* shape_, const Vec3s& dir, Vec3s& support, int& hint, \
109 ShapeSupportData& support_data);
110
111 // ============================================================================
112 template <int _SupportOptions>
113 272242094 void getShapeSupport(const TriangleP* triangle, const Vec3s& dir,
114 Vec3s& support, int& /*unused*/,
115 ShapeSupportData& /*unused*/) {
116 272242094 Scalar dota = dir.dot(triangle->a);
117 272242094 Scalar dotb = dir.dot(triangle->b);
118 272242094 Scalar dotc = dir.dot(triangle->c);
119
2/2
✓ Branch 0 taken 73521453 times.
✓ Branch 1 taken 62599594 times.
 272242094 if (dota > dotb) {
120
2/2
✓ Branch 0 taken 31137824 times.
✓ Branch 1 taken 42383629 times.
 147042906 if (dotc > dota) {
121 62275648 support = triangle->c;
122 } else {
123 84767258 support = triangle->a;
124 }
125 } else {
126
2/2
✓ Branch 0 taken 19651552 times.
✓ Branch 1 taken 42948042 times.
 125199188 if (dotc > dotb) {
127 39303104 support = triangle->c;
128 } else {
129 85896084 support = triangle->b;
130 }
131 }
132
133 if (_SupportOptions == SupportOptions::WithSweptSphere) {
134
2/4
✓ Branch 3 taken 170 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 170 times.
✗ Branch 7 not taken.
 340 support += triangle->getSweptSphereRadius() * dir.normalized();
135 }
136 272242094 }
137 getShapeSupportTplInstantiation(TriangleP);
138
139 // ============================================================================
140 template <int _SupportOptions>
141 8169442 void getShapeSupport(const Box* box, const Vec3s& dir, Vec3s& support,
142 int& /*unused*/, ShapeSupportData& /*unused*/) {
143 // The inflate value is simply to make the specialized functions with box
144 // have a preferred side for edge cases.
145
3/6
✓ Branch 0 taken 27 times.
✓ Branch 1 taken 4084694 times.
✓ Branch 3 taken 27 times.
✗ Branch 4 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 8169496 static const Scalar inflate =
146
5/8
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 27 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 23 times.
✓ Branch 10 taken 4 times.
 108 (dir.array() == 0).any() ? 1 + Scalar(1e-10) : 1;
147 static const Scalar dummy_precision =
148 Eigen::NumTraits<Scalar>::dummy_precision();
149
4/8
✓ Branch 1 taken 4084721 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4084721 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4084721 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4084721 times.
✗ Branch 11 not taken.
 8169442 Vec3s support1 = (dir.array() > dummy_precision).select(box->halfSide, 0);
150
3/6
✓ Branch 1 taken 4084721 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4084721 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4084721 times.
✗ Branch 8 not taken.
 16338884 Vec3s support2 =
151
2/4
✓ Branch 1 taken 4084721 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4084721 times.
✗ Branch 5 not taken.
 16338884 (dir.array() < -dummy_precision).select(-inflate * box->halfSide, 0);
152
3/6
✓ Branch 1 taken 4084721 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4084721 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4084721 times.
✗ Branch 8 not taken.
 8169442 support.noalias() = support1 + support2;
153
154 if (_SupportOptions == SupportOptions::WithSweptSphere) {
155
3/6
✓ Branch 1 taken 36487 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 36487 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 36487 times.
✗ Branch 9 not taken.
 72974 support += box->getSweptSphereRadius() * dir.normalized();
156 }
157 8169442 }
158 getShapeSupportTplInstantiation(Box);
159
160 // ============================================================================
161 template <int _SupportOptions>
162 384924 void getShapeSupport(const Sphere* sphere, const Vec3s& dir, Vec3s& support,
163 int& /*unused*/, ShapeSupportData& /*unused*/) {
164 if (_SupportOptions == SupportOptions::WithSweptSphere) {
165
2/4
✓ Branch 2 taken 2695 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2695 times.
✗ Branch 6 not taken.
 10780 support.noalias() =
166
1/2
✓ Branch 2 taken 2695 times.
✗ Branch 3 not taken.
 10780 (sphere->radius + sphere->getSweptSphereRadius()) * dir.normalized();
167 } else {
168 379534 support.setZero();
169 }
170
171 COAL_UNUSED_VARIABLE(sphere);
172 COAL_UNUSED_VARIABLE(dir);
173 384924 }
174 getShapeSupportTplInstantiation(Sphere);
175
176 // ============================================================================
177 template <int _SupportOptions>
178 2192268 void getShapeSupport(const Ellipsoid* ellipsoid, const Vec3s& dir,
179 Vec3s& support, int& /*unused*/,
180 ShapeSupportData& /*unused*/) {
181
2/4
✓ Branch 1 taken 1096134 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1096134 times.
✗ Branch 5 not taken.
 2192268 Scalar a2 = ellipsoid->radii[0] * ellipsoid->radii[0];
182
2/4
✓ Branch 1 taken 1096134 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1096134 times.
✗ Branch 5 not taken.
 2192268 Scalar b2 = ellipsoid->radii[1] * ellipsoid->radii[1];
183
2/4
✓ Branch 1 taken 1096134 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1096134 times.
✗ Branch 5 not taken.
 2192268 Scalar c2 = ellipsoid->radii[2] * ellipsoid->radii[2];
184
185
4/8
✓ Branch 1 taken 1096134 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1096134 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1096134 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1096134 times.
✗ Branch 11 not taken.
 2192268 Vec3s v(a2 * dir[0], b2 * dir[1], c2 * dir[2]);
186
187
1/2
✓ Branch 1 taken 1096134 times.
✗ Branch 2 not taken.
 2192268 Scalar d = std::sqrt(v.dot(dir));
188
189
2/4
✓ Branch 1 taken 1096134 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1096134 times.
✗ Branch 5 not taken.
 2192268 support = v / d;
190
191 if (_SupportOptions == SupportOptions::WithSweptSphere) {
192
3/6
✓ Branch 1 taken 63356 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 63356 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 63356 times.
✗ Branch 9 not taken.
 126712 support += ellipsoid->getSweptSphereRadius() * dir.normalized();
193 }
194 2192268 }
195 getShapeSupportTplInstantiation(Ellipsoid);
196
197 // ============================================================================
198 template <int _SupportOptions>
199 1053698 void getShapeSupport(const Capsule* capsule, const Vec3s& dir, Vec3s& support,
200 int& /*unused*/, ShapeSupportData& /*unused*/) {
201 static const Scalar dummy_precision =
202 Eigen::NumTraits<Scalar>::dummy_precision();
203 1053698 support.setZero();
204
2/2
✓ Branch 1 taken 287055 times.
✓ Branch 2 taken 239794 times.
 1053698 if (dir[2] > dummy_precision) {
205 574110 support[2] = capsule->halfLength;
206
2/2
✓ Branch 1 taken 180904 times.
✓ Branch 2 taken 58890 times.
 479588 } else if (dir[2] < -dummy_precision) {
207 361808 support[2] = -capsule->halfLength;
208 }
209
210 if (_SupportOptions == SupportOptions::WithSweptSphere) {
211
1/2
✓ Branch 2 taken 31366 times.
✗ Branch 3 not taken.
 125464 support +=
212
1/2
✓ Branch 2 taken 31366 times.
✗ Branch 3 not taken.
 62732 (capsule->radius + capsule->getSweptSphereRadius()) * dir.normalized();
213 }
214 1053698 }
215 getShapeSupportTplInstantiation(Capsule);
216
217 // ============================================================================
218 template <int _SupportOptions>
219 140294 void getShapeSupport(const Cone* cone, const Vec3s& dir, Vec3s& support,
220 int& /*unused*/, ShapeSupportData& /*unused*/) {
221 static const Scalar dummy_precision =
222 Eigen::NumTraits<Scalar>::dummy_precision();
223
224 // The cone radius is, for -h < z < h, (h - z) * r / (2*h)
225 // The inflate value is simply to make the specialized functions with cone
226 // have a preferred side for edge cases.
227 static const Scalar inflate = 1 + Scalar(1e-10);
228 140294 Scalar h = cone->halfLength;
229 140294 Scalar r = cone->radius;
230
231
4/6
✓ Branch 1 taken 70147 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 70147 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 172 times.
✓ Branch 7 taken 69975 times.
 140294 if (dir.head<2>().isZero(dummy_precision)) {
232
2/4
✓ Branch 1 taken 172 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 172 times.
✗ Branch 5 not taken.
 344 support.head<2>().setZero();
233
3/4
✓ Branch 1 taken 172 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 52 times.
✓ Branch 4 taken 120 times.
 344 if (dir[2] > dummy_precision) {
234
1/2
✓ Branch 1 taken 52 times.
✗ Branch 2 not taken.
 104 support[2] = h;
235 } else {
236
1/2
✓ Branch 1 taken 120 times.
✗ Branch 2 not taken.
 240 support[2] = -inflate * h;
237 }
238 } else {
239
4/8
✓ Branch 1 taken 69975 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 69975 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 69975 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 69975 times.
✗ Branch 11 not taken.
 139950 Scalar zdist = dir[0] * dir[0] + dir[1] * dir[1];
240
2/4
✓ Branch 1 taken 69975 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 69975 times.
✗ Branch 5 not taken.
 139950 Scalar len = zdist + dir[2] * dir[2];
241 139950 zdist = std::sqrt(zdist);
242
243
3/4
✓ Branch 1 taken 69975 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 35483 times.
✓ Branch 4 taken 34492 times.
 139950 if (dir[2] <= 0) {
244 70966 Scalar rad = r / zdist;
245
4/8
✓ Branch 1 taken 35483 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 35483 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 35483 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 35483 times.
✗ Branch 11 not taken.
 70966 support.head<2>() = rad * dir.head<2>();
246
1/2
✓ Branch 1 taken 35483 times.
✗ Branch 2 not taken.
 70966 support[2] = -h;
247 } else {
248 68984 len = std::sqrt(len);
249 68984 Scalar sin_a = r / std::sqrt(r * r + 4 * h * h);
250
251
3/4
✓ Branch 1 taken 34492 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 9356 times.
✓ Branch 4 taken 25136 times.
 68984 if (dir[2] > len * sin_a)
252
3/6
✓ Branch 1 taken 9356 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9356 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9356 times.
✗ Branch 8 not taken.
 18712 support << 0, 0, h;
253 else {
254 50272 Scalar rad = r / zdist;
255
4/8
✓ Branch 1 taken 25136 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25136 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 25136 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 25136 times.
✗ Branch 11 not taken.
 50272 support.head<2>() = rad * dir.head<2>();
256
1/2
✓ Branch 1 taken 25136 times.
✗ Branch 2 not taken.
 50272 support[2] = -h;
257 }
258 }
259 }
260
261 if (_SupportOptions == SupportOptions::WithSweptSphere) {
262
3/6
✓ Branch 1 taken 39450 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 39450 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 39450 times.
✗ Branch 9 not taken.
 78900 support += cone->getSweptSphereRadius() * dir.normalized();
263 }
264 140294 }
265 getShapeSupportTplInstantiation(Cone);
266
267 // ============================================================================
268 template <int _SupportOptions>
269 3739390 void getShapeSupport(const Cylinder* cylinder, const Vec3s& dir, Vec3s& support,
270 int& /*unused*/, ShapeSupportData& /*unused*/) {
271 static const Scalar dummy_precision =
272 Eigen::NumTraits<Scalar>::dummy_precision();
273
274 // The inflate value is simply to make the specialized functions with cylinder
275 // have a preferred side for edge cases.
276 static const Scalar inflate = 1 + Scalar(1e-10);
277 3739390 Scalar half_h = cylinder->halfLength;
278 3739390 Scalar r = cylinder->radius;
279
280
2/4
✓ Branch 1 taken 1869695 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1869695 times.
✗ Branch 5 not taken.
 3739390 const bool dir_is_aligned_with_z = dir.head<2>().isZero(dummy_precision);
281
2/2
✓ Branch 0 taken 510 times.
✓ Branch 1 taken 1869185 times.
 3739390 if (dir_is_aligned_with_z) half_h *= inflate;
282
283
3/4
✓ Branch 1 taken 1869695 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 771939 times.
✓ Branch 4 taken 1097756 times.
 3739390 if (dir[2] > dummy_precision) {
284
1/2
✓ Branch 1 taken 771939 times.
✗ Branch 2 not taken.
 1543878 support[2] = half_h;
285
3/4
✓ Branch 1 taken 1097756 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 781777 times.
✓ Branch 4 taken 315979 times.
 2195512 } else if (dir[2] < -dummy_precision) {
286
1/2
✓ Branch 1 taken 781777 times.
✗ Branch 2 not taken.
 1563554 support[2] = -half_h;
287 } else {
288
1/2
✓ Branch 1 taken 315979 times.
✗ Branch 2 not taken.
 631958 support[2] = 0;
289 631958 r *= inflate;
290 }
291
292
2/2
✓ Branch 0 taken 510 times.
✓ Branch 1 taken 1869185 times.
 3739390 if (dir_is_aligned_with_z) {
293
2/4
✓ Branch 1 taken 510 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 510 times.
✗ Branch 5 not taken.
 1020 support.head<2>().setZero();
294 } else {
295
5/10
✓ Branch 1 taken 1869185 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1869185 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1869185 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1869185 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1869185 times.
✗ Branch 14 not taken.
 3738370 support.head<2>() = dir.head<2>().normalized() * r;
296 }
297
298
5/10
✓ Branch 1 taken 1869695 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1869695 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1869695 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1869695 times.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✓ Branch 14 taken 1869695 times.
 3739390 assert(fabs(support[0] * dir[1] - support[1] * dir[0]) <
299 sqrt(std::numeric_limits<Scalar>::epsilon()));
300
301 if (_SupportOptions == SupportOptions::WithSweptSphere) {
302
3/6
✓ Branch 1 taken 27196 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 27196 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 27196 times.
✗ Branch 9 not taken.
 54392 support += cylinder->getSweptSphereRadius() * dir.normalized();
303 }
304 3739390 }
305 getShapeSupportTplInstantiation(Cylinder);
306
307 // ============================================================================
308 template <int _SupportOptions, typename IndexType>
309 ✗ void getShapeSupportLog(const ConvexBaseTpl<IndexType>* convex,
310 const Vec3s& dir, Vec3s& support, int& hint,
311 ShapeSupportData& support_data) {
312 ✗ assert(convex->neighbors != nullptr && "Convex has no neighbors.");
313
314 // Use warm start if current support direction is distant from last support
315 // direction.
316 ✗ const Scalar use_warm_start_threshold = Scalar(0.9);
317 ✗ Vec3s dir_normalized = dir.normalized();
318 ✗ if (!support_data.last_dir.isZero() &&
319 ✗ !convex->support_warm_starts.points.empty() &&
320 ✗ support_data.last_dir.dot(dir_normalized) < use_warm_start_threshold) {
321 // Change hint if last dir is too far from current dir.
322 ✗ Scalar maxdot = convex->support_warm_starts.points[0].dot(dir);
323 ✗ hint = int(convex->support_warm_starts.indices[0]);
324 ✗ for (size_t i = 1; i < convex->support_warm_starts.points.size(); ++i) {
325 ✗ Scalar dot = convex->support_warm_starts.points[i].dot(dir);
326 ✗ if (dot > maxdot) {
327 ✗ maxdot = dot;
328 ✗ hint = int(convex->support_warm_starts.indices[i]);
329 }
330 }
331 }
332 ✗ support_data.last_dir = dir_normalized;
333
334 ✗ const std::vector<Vec3s>& pts = *(convex->points);
335 typedef typename ConvexBaseTpl<IndexType>::Neighbors Neighbors;
336 ✗ const std::vector<Neighbors>& nn = *(convex->neighbors);
337
338 ✗ if (hint < 0 || hint >= (int)convex->num_points) {
339 ✗ hint = 0;
340 }
341 ✗ Scalar maxdot = pts[static_cast<size_t>(hint)].dot(dir);
342 ✗ std::vector<int8_t>& visited = support_data.visited;
343 ✗ if (support_data.visited.size() == convex->num_points) {
344 ✗ std::fill(visited.begin(), visited.end(), false);
345 } else {
346 // std::vector::assign not only assigns the values of the vector but also
347 // resizes the vector. So if `visited` has not been set up yet, this makes
348 // sure the size convex's points and visited are identical.
349 ✗ support_data.visited.assign(convex->num_points, false);
350 }
351 ✗ visited[static_cast<std::size_t>(hint)] = true;
352 // When the first face is orthogonal to dir, all the dot products will be
353 // equal. Yet, the neighbors must be visited.
354 ✗ bool found = true;
355 ✗ bool loose_check = true;
356 ✗ while (found) {
357 ✗ const Neighbors& n = nn[static_cast<size_t>(hint)];
358 ✗ found = false;
359 ✗ IndexType current_vertex_idx = IndexType(hint);
360 ✗ for (IndexType in = 0; in < n.count; ++in) {
361 ✗ const IndexType ip = convex->neighbor(current_vertex_idx, in);
362 ✗ if (visited[ip]) continue;
363 ✗ visited[ip] = true;
364 ✗ const Scalar dot = pts[ip].dot(dir);
365 ✗ bool better = false;
366 ✗ if (dot > maxdot) {
367 ✗ better = true;
368 ✗ loose_check = false;
369 ✗ } else if (loose_check && dot == maxdot)
370 ✗ better = true;
371 ✗ if (better) {
372 ✗ maxdot = dot;
373 ✗ hint = static_cast<int>(ip);
374 ✗ found = true;
375 }
376 }
377 }
378
379 ✗ support = pts[static_cast<size_t>(hint)];
380
381 if (_SupportOptions == SupportOptions::WithSweptSphere) {
382 ✗ support += convex->getSweptSphereRadius() * dir.normalized();
383 }
384 }
385
386 // ============================================================================
387 template <int _SupportOptions, typename IndexType>
388 1918502 void getShapeSupportLinear(const ConvexBaseTpl<IndexType>* convex,
389 const Vec3s& dir, Vec3s& support, int& hint,
390 ShapeSupportData& /*unused*/) {
391 1918502 const std::vector<Vec3s>& pts = *(convex->points);
392
393 1918502 hint = 0;
394 1918502 Scalar maxdot = pts[0].dot(dir);
395
2/2
✓ Branch 0 taken 9353331 times.
✓ Branch 1 taken 959251 times.
 20625164 for (int i = 1; i < (int)convex->num_points; ++i) {
396 18706662 Scalar dot = pts[static_cast<size_t>(i)].dot(dir);
397
2/2
✓ Branch 0 taken 1876494 times.
✓ Branch 1 taken 7476837 times.
 18706662 if (dot > maxdot) {
398 3752988 maxdot = dot;
399 3752988 hint = i;
400 }
401 }
402
403 1918502 support = pts[static_cast<size_t>(hint)];
404
405 if (_SupportOptions == SupportOptions::WithSweptSphere) {
406
2/4
✓ Branch 3 taken 31048 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 31048 times.
✗ Branch 7 not taken.
 62096 support += convex->getSweptSphereRadius() * dir.normalized();
407 }
408 }
409
410 // ============================================================================
411 template <int _SupportOptions, typename IndexType>
412 2028 void getShapeSupport(const ConvexBaseTpl<IndexType>* convex, const Vec3s& dir,
413 Vec3s& support, int& hint,
414 ShapeSupportData& support_data) {
415 // TODO add benchmark to set a proper value for switching between linear and
416 // logarithmic.
417 4056 if (convex->num_points >
418
2/6
✗ Branch 0 not taken.
✓ Branch 1 taken 1014 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 1014 times.
 2028 ConvexBaseTpl<IndexType>::num_vertices_large_convex_threshold &&
419 ✗ convex->neighbors != nullptr) {
420 ✗ getShapeSupportLog<_SupportOptions>(convex, dir, support, hint,
421 support_data);
422 } else {
423 2028 getShapeSupportLinear<_SupportOptions>(convex, dir, support, hint,
424 support_data);
425 }
426 }
427 getShapeSupportTplInstantiation(ConvexBaseTpl<Triangle16::IndexType>);
428 getShapeSupportTplInstantiation(ConvexBaseTpl<Triangle32::IndexType>);
429
430 // ============================================================================
431 template <int _SupportOptions, typename IndexType>
432 1916474 void getShapeSupport(const SmallConvex<IndexType>* convex, const Vec3s& dir,
433 Vec3s& support, int& hint,
434 ShapeSupportData& support_data) {
435 1916474 getShapeSupportLinear<_SupportOptions>(
436 reinterpret_cast<const ConvexBaseTpl<IndexType>*>(convex), dir, support,
437 hint, support_data);
438 }
439 getShapeSupportTplInstantiation(SmallConvex<Triangle16::IndexType>);
440 getShapeSupportTplInstantiation(SmallConvex<Triangle32::IndexType>);
441
442 // ============================================================================
443 template <int _SupportOptions, typename IndexType>
444 ✗ void getShapeSupport(const LargeConvex<IndexType>* convex, const Vec3s& dir,
445 Vec3s& support, int& hint,
446 ShapeSupportData& support_data) {
447 ✗ getShapeSupportLog<_SupportOptions>(
448 reinterpret_cast<const ConvexBaseTpl<IndexType>*>(convex), dir, support,
449 hint, support_data);
450 }
451 getShapeSupportTplInstantiation(LargeConvex<Triangle16::IndexType>);
452 getShapeSupportTplInstantiation(LargeConvex<Triangle32::IndexType>);
453
454 // ============================================================================
455 #define CALL_GET_SHAPE_SUPPORT_SET(ShapeType) \
456 getShapeSupportSet<_SupportOptions>(static_cast<const ShapeType*>(shape), \
457 support_set, hint, support_data, \
458 max_num_supports, tol)
459
460 template <int _SupportOptions>
461 ✗ void getSupportSet(const ShapeBase* shape, SupportSet& support_set, int& hint,
462 size_t max_num_supports, Scalar tol) {
463 ✗ ShapeSupportData support_data;
464 ✗ switch (shape->getNodeType()) {
465 ✗ case GEOM_TRIANGLE:
466 ✗ CALL_GET_SHAPE_SUPPORT_SET(TriangleP);
467 ✗ break;
468 ✗ case GEOM_BOX:
469 ✗ CALL_GET_SHAPE_SUPPORT_SET(Box);
470 ✗ break;
471 ✗ case GEOM_SPHERE:
472 ✗ CALL_GET_SHAPE_SUPPORT_SET(Sphere);
473 ✗ break;
474 ✗ case GEOM_ELLIPSOID:
475 ✗ CALL_GET_SHAPE_SUPPORT_SET(Ellipsoid);
476 ✗ break;
477 ✗ case GEOM_CAPSULE:
478 ✗ CALL_GET_SHAPE_SUPPORT_SET(Capsule);
479 ✗ break;
480 ✗ case GEOM_CONE:
481 ✗ CALL_GET_SHAPE_SUPPORT_SET(Cone);
482 ✗ break;
483 ✗ case GEOM_CYLINDER:
484 ✗ CALL_GET_SHAPE_SUPPORT_SET(Cylinder);
485 ✗ break;
486 ✗ case GEOM_CONVEX16:
487 ✗ CALL_GET_SHAPE_SUPPORT_SET(ConvexBaseTpl<Triangle16::IndexType>);
488 ✗ break;
489 ✗ case GEOM_CONVEX32:
490 ✗ CALL_GET_SHAPE_SUPPORT_SET(ConvexBaseTpl<Triangle32::IndexType>);
491 ✗ break;
492 ✗ case GEOM_PLANE:
493 case GEOM_HALFSPACE:
494 default:; // nothing
495 }
496 }
497 #undef CALL_GET_SHAPE_SUPPORT
498
499 // Explicit instantiation
500 // clang-format off
501 template COAL_DLLAPI void getSupportSet<SupportOptions::NoSweptSphere>(const ShapeBase*, SupportSet&, int&, size_t, Scalar);
502
503 template COAL_DLLAPI void getSupportSet<SupportOptions::WithSweptSphere>(const ShapeBase*, SupportSet&, int&, size_t, Scalar);
504 // clang-format on
505
506 // ============================================================================
507 #define getShapeSupportSetTplInstantiation(ShapeType) \
508 template COAL_DLLAPI void getShapeSupportSet<SupportOptions::NoSweptSphere>( \
509 const ShapeType* shape_, SupportSet& support_set, int& hint, \
510 ShapeSupportData& data, size_t num_sampled_supports, Scalar tol); \
511 \
512 template COAL_DLLAPI void \
513 getShapeSupportSet<SupportOptions::WithSweptSphere>( \
514 const ShapeType* shape_, SupportSet& support_set, int& hint, \
515 ShapeSupportData& data, size_t num_sampled_supports, Scalar tol);
516
517 // ============================================================================
518 template <int _SupportOptions>
519 ✗ void getShapeSupportSet(const TriangleP* triangle, SupportSet& support_set,
520 int& hint /*unused*/,
521 ShapeSupportData& support_data /*unused*/,
522 size_t /*unused*/, Scalar tol) {
523 ✗ assert(tol > 0);
524 ✗ support_set.clear();
525
526 ✗ Vec3s support;
527 ✗ const Vec3s& support_dir = support_set.getNormal();
528 // We simply want to compute the support value, no need to take the
529 // swept-sphere radius into account.
530 ✗ getShapeSupport<SupportOptions::NoSweptSphere>(triangle, support_dir, support,
531 hint, support_data);
532 ✗ const Scalar support_value = support.dot(support_dir);
533
534 ✗ if (support_value - support_dir.dot(triangle->a) < tol) {
535 // Note: at the moment, it's useless to take into account the
536 // swept-sphere radius, but in the future we might want to store the
537 // offsets to the plane in `SupportSet`.
538 if (_SupportOptions == SupportOptions::WithSweptSphere) {
539 ✗ support_set.addPoint(triangle->a +
540 ✗ triangle->getSweptSphereRadius() * support_dir);
541 } else {
542 ✗ support_set.addPoint(triangle->a);
543 }
544 }
545 ✗ if (support_value - support_dir.dot(triangle->b) < tol) {
546 if (_SupportOptions == SupportOptions::WithSweptSphere) {
547 ✗ support_set.addPoint(triangle->b +
548 ✗ triangle->getSweptSphereRadius() * support_dir);
549 } else {
550 ✗ support_set.addPoint(triangle->b);
551 }
552 }
553 ✗ if (support_value - support_dir.dot(triangle->c) < tol) {
554 if (_SupportOptions == SupportOptions::WithSweptSphere) {
555 ✗ support_set.addPoint(triangle->c +
556 ✗ triangle->getSweptSphereRadius() * support_dir);
557 } else {
558 ✗ support_set.addPoint(triangle->c);
559 }
560 }
561 }
562 getShapeSupportSetTplInstantiation(TriangleP);
563
564 // ============================================================================
565 template <int _SupportOptions>
566 12 void getShapeSupportSet(const Box* box, SupportSet& support_set,
567 int& hint /*unused*/, ShapeSupportData& support_data,
568 size_t /*unused*/, Scalar tol) {
569
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 6 times.
 12 assert(tol > 0);
570
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 12 Vec3s support;
571
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 12 const Vec3s& support_dir = support_set.getNormal();
572
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 12 getShapeSupport<SupportOptions::NoSweptSphere>(box, support_dir, support,
573 hint, support_data);
574
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 12 const Scalar support_value = support.dot(support_dir);
575
576
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 12 const Scalar x = box->halfSide[0];
577
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 12 const Scalar y = box->halfSide[1];
578
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 12 const Scalar z = box->halfSide[2];
579
8/16
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 6 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 6 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 6 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 6 times.
✗ Branch 23 not taken.
 12 const std::array<Vec3s, 8> corners = {
580 Vec3s(x, y, z), Vec3s(-x, y, z), Vec3s(-x, -y, z), Vec3s(x, -y, z),
581 Vec3s(x, y, -z), Vec3s(-x, y, -z), Vec3s(-x, -y, -z), Vec3s(x, -y, -z),
582 };
583
584 12 SupportSet::Polygon& polygon = support_data.polygon;
585 12 polygon.clear();
586 12 const Transform3s& tf = support_set.tf;
587
2/2
✓ Branch 2 taken 48 times.
✓ Branch 3 taken 6 times.
 108 for (const Vec3s& corner : corners) {
588
1/2
✓ Branch 1 taken 48 times.
✗ Branch 2 not taken.
 96 const Scalar val = corner.dot(support_dir);
589
2/2
✓ Branch 0 taken 24 times.
✓ Branch 1 taken 24 times.
 96 if (support_value - val < tol) {
590 if (_SupportOptions == SupportOptions::WithSweptSphere) {
591
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 16 const Vec2s p =
592 tf.inverseTransform(corner +
593 16 box->getSweptSphereRadius() * support_dir)
594
4/8
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 8 times.
✗ Branch 11 not taken.
 16 .template head<2>();
595
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 16 polygon.emplace_back(p);
596 } else {
597
3/6
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 16 times.
✗ Branch 8 not taken.
 32 const Vec2s p = tf.inverseTransform(corner).template head<2>();
598
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 polygon.emplace_back(p);
599 }
600 }
601 }
602
1/2
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
 12 computeSupportSetConvexHull(polygon, support_set.points());
603 12 }
604 getShapeSupportSetTplInstantiation(Box);
605
606 // ============================================================================
607 template <int _SupportOptions>
608 ✗ void getShapeSupportSet(const Sphere* sphere, SupportSet& support_set,
609 int& hint /*unused*/,
610 ShapeSupportData& support_data /*unused*/,
611 size_t /*unused*/, Scalar /*unused*/) {
612 ✗ support_set.points().clear();
613
614 ✗ Vec3s support;
615 ✗ const Vec3s& support_dir = support_set.getNormal();
616 ✗ getShapeSupport<_SupportOptions>(sphere, support_dir, support, hint,
617 support_data);
618 ✗ support_set.addPoint(support);
619 }
620 getShapeSupportSetTplInstantiation(Sphere);
621
622 // ============================================================================
623 template <int _SupportOptions>
624 ✗ void getShapeSupportSet(const Ellipsoid* ellipsoid, SupportSet& support_set,
625 int& hint, ShapeSupportData& support_data /*unused*/,
626 size_t /*unused*/, Scalar /*unused*/) {
627 ✗ support_set.points().clear();
628
629 ✗ Vec3s support;
630 ✗ const Vec3s& support_dir = support_set.getNormal();
631 ✗ getShapeSupport<_SupportOptions>(ellipsoid, support_dir, support, hint,
632 support_data);
633 ✗ support_set.addPoint(support);
634 }
635 getShapeSupportSetTplInstantiation(Ellipsoid);
636
637 // ============================================================================
638 template <int _SupportOptions>
639 6 void getShapeSupportSet(const Capsule* capsule, SupportSet& support_set,
640 int& hint /*unused*/,
641 ShapeSupportData& support_data /*unused*/,
642 size_t /*unused*/, Scalar tol) {
643 // clang-format on
644
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 3 times.
 6 assert(tol > 0);
645 6 support_set.points().clear();
646
647
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 6 Vec3s support;
648
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 6 const Vec3s& support_dir = support_set.getNormal();
649
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 6 getShapeSupport<SupportOptions::NoSweptSphere>(capsule, support_dir, support,
650 hint, support_data);
651 const Scalar support_value =
652
3/6
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
 6 support_dir.dot(support + capsule->radius * support_dir);
653 // The support set of a capsule has either 2 points or 1 point.
654 // The two candidate points lie at the frontier between the cylinder and
655 // sphere parts of the capsule.
656 6 const Scalar h = capsule->halfLength;
657 6 const Scalar r = capsule->radius;
658
3/6
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
 6 const Vec3s p1(r * support_dir[0], r * support_dir[1], h);
659
3/6
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
 6 const Vec3s p2(r * support_dir[0], r * support_dir[1], -h);
660
5/6
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
✓ Branch 4 taken 2 times.
✓ Branch 5 taken 1 times.
✓ Branch 6 taken 2 times.
 8 if ((support_value - support_dir.dot(p1) <= tol) &&
661
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 (support_value - support_dir.dot(p2) <= tol)) {
662 if (_SupportOptions == SupportOptions::WithSweptSphere) {
663
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 const Vec3s ssr_vec = support_dir * capsule->getSweptSphereRadius();
664
3/6
✓ 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.
 2 support_set.addPoint(p1 + ssr_vec);
665
3/6
✓ 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.
 2 support_set.addPoint(p2 + ssr_vec);
666 } else {
667 ✗ support_set.addPoint(p1);
668 ✗ support_set.addPoint(p2);
669 }
670 } else {
671 if (_SupportOptions == SupportOptions::WithSweptSphere) {
672
2/4
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
 4 const Vec3s ssr_vec = support_dir * capsule->getSweptSphereRadius();
673
3/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
 4 support_set.addPoint(support + ssr_vec);
674 } else {
675 ✗ support_set.addPoint(support);
676 }
677 }
678 }
679 getShapeSupportSetTplInstantiation(Capsule);
680
681 // ============================================================================
682 template <int _SupportOptions>
683 6 void getShapeSupportSet(const Cone* cone, SupportSet& support_set,
684 int& hint /*unused*/,
685 ShapeSupportData& support_data /*unused*/,
686 size_t num_sampled_supports, Scalar tol) {
687
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 3 times.
 6 assert(tol > 0);
688 6 support_set.points().clear();
689
690
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 6 Vec3s support;
691
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 6 const Vec3s& support_dir = support_set.getNormal();
692
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 6 getShapeSupport<SupportOptions::NoSweptSphere>(cone, support_dir, support,
693 hint, support_data);
694
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 6 const Scalar support_value = support.dot(support_dir);
695
696 // If the support direction is perpendicular to the cone's basis, there is an
697 // infinite amount of support points; otherwise there are up to two support
698 // points (two if direction is perpendicular to the side of the cone and one
699 // otherwise).
700 //
701 // To check this condition, we look at two points on the cone's basis;
702 // these two points are symmetrical w.r.t the center of the circle. If
703 // both these points are tol away from the support plane, then all the
704 // points of the circle are tol away from the support plane.
705 6 const Scalar r = cone->radius;
706 6 const Scalar z = -cone->halfLength;
707
3/6
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
 6 const Vec3s p1(r * support_dir[0], r * support_dir[1], z);
708
3/6
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
 6 const Vec3s p2(-r * support_dir[0], -r * support_dir[1], z);
709
710
5/6
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 2 times.
✓ Branch 4 taken 1 times.
✓ Branch 5 taken 1 times.
✓ Branch 6 taken 2 times.
 10 if ((support_value - support_dir.dot(p1) <= tol) &&
711
3/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
✓ Branch 4 taken 1 times.
 4 (support_value - support_dir.dot(p2) <= tol)) {
712 // If this check passed, support direction is considered perpendicular to
713 // the basis of the cone. We sample `num_sampled_supports` points on the
714 // base of the cone. We are guaranteed that these points like at a distance
715 // tol of the support plane.
716 2 const Scalar angle_increment =
717 2 Scalar(2 * EIGEN_PI) / (Scalar(num_sampled_supports));
718
2/2
✓ Branch 0 taken 12 times.
✓ Branch 1 taken 1 times.
 26 for (size_t i = 0; i < num_sampled_supports; ++i) {
719 24 const Scalar theta = (Scalar)(i)*angle_increment;
720
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 24 Vec3s point_on_cone_base(r * std::cos(theta), r * std::sin(theta), z);
721
3/6
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 12 times.
 24 assert(std::abs(support_dir.dot(support - point_on_cone_base)) <= tol);
722 if (_SupportOptions == SupportOptions::WithSweptSphere) {
723
2/4
✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 12 times.
✗ Branch 6 not taken.
 24 point_on_cone_base += cone->getSweptSphereRadius() * support_dir;
724 }
725
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 24 support_set.addPoint(point_on_cone_base);
726 }
727 } else {
728 // There are two potential supports to add: the tip of the cone and a point
729 // on the basis of the cone. We compare each of these points to the support
730 // value.
731
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 4 Vec3s cone_tip(0, 0, cone->halfLength);
732
3/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
✓ Branch 4 taken 1 times.
 4 if (support_value - support_dir.dot(cone_tip) <= tol) {
733 if (_SupportOptions == SupportOptions::WithSweptSphere) {
734
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 cone_tip += cone->getSweptSphereRadius() * support_dir;
735 }
736
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 support_set.addPoint(cone_tip);
737 }
738
739
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 4 Vec3s point_on_cone_base = Vec3s(cone->radius * support_dir[0], //
740
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 4 cone->radius * support_dir[1], //
741 z);
742
3/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
✓ Branch 4 taken 1 times.
 4 if (support_value - support_dir.dot(point_on_cone_base) <= tol) {
743 if (_SupportOptions == SupportOptions::WithSweptSphere) {
744
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 point_on_cone_base += cone->getSweptSphereRadius() * support_dir;
745 }
746
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 support_set.addPoint(point_on_cone_base);
747 }
748 }
749 }
750 getShapeSupportSetTplInstantiation(Cone);
751
752 // ============================================================================
753 template <int _SupportOptions>
754 8 void getShapeSupportSet(const Cylinder* cylinder, SupportSet& support_set,
755 int& hint /*unused*/,
756 ShapeSupportData& support_data /*unused*/,
757 size_t num_sampled_supports, Scalar tol) {
758
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 4 times.
 8 assert(tol > 0);
759 8 support_set.points().clear();
760
761
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 Vec3s support;
762
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 const Vec3s& support_dir = support_set.getNormal();
763
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 getShapeSupport<SupportOptions::NoSweptSphere>(cylinder, support_dir, support,
764 hint, support_data);
765
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 const Scalar support_value = support.dot(support_dir);
766
767 // The following is very similar to what is done for Cone's support set
768 // computation.
769 8 const Scalar r = cylinder->radius;
770 8 const Scalar z =
771
3/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 3 times.
✓ Branch 4 taken 1 times.
 8 support_dir[2] <= 0 ? -cylinder->halfLength : cylinder->halfLength;
772
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.
 8 const Vec3s p1(r * support_dir[0], r * support_dir[1], z);
773
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.
 8 const Vec3s p2(-r * support_dir[0], -r * support_dir[1], z);
774
775
4/6
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 3 times.
✓ Branch 6 taken 1 times.
 16 if ((support_value - support_dir.dot(p1) <= tol) &&
776
3/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 3 times.
✓ Branch 4 taken 1 times.
 8 (support_value - support_dir.dot(p2) <= tol)) {
777 6 const Scalar angle_increment =
778 6 Scalar(2 * EIGEN_PI) / (Scalar(num_sampled_supports));
779
2/2
✓ Branch 0 taken 36 times.
✓ Branch 1 taken 3 times.
 78 for (size_t i = 0; i < num_sampled_supports; ++i) {
780 72 const Scalar theta = (Scalar)(i)*angle_increment;
781
1/2
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
 72 Vec3s point_on_cone_base(r * std::cos(theta), r * std::sin(theta), z);
782
3/6
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 36 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 36 times.
 72 assert(std::abs(support_dir.dot(support - point_on_cone_base)) <= tol);
783 if (_SupportOptions == SupportOptions::WithSweptSphere) {
784
2/4
✓ Branch 2 taken 36 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 36 times.
✗ Branch 6 not taken.
 72 point_on_cone_base += cylinder->getSweptSphereRadius() * support_dir;
785 }
786
1/2
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
 72 support_set.addPoint(point_on_cone_base);
787 }
788 } else {
789 // There are two potential supports to add: one on each circle bases of the
790 // cylinder.
791
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Vec3s point_on_lower_circle = Vec3s(cylinder->radius * support_dir[0], //
792
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 cylinder->radius * support_dir[1], //
793
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 -cylinder->halfLength);
794
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 if (support_value - support_dir.dot(point_on_lower_circle) <= tol) {
795 if (_SupportOptions == SupportOptions::WithSweptSphere) {
796
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 point_on_lower_circle += cylinder->getSweptSphereRadius() * support_dir;
797 }
798
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 support_set.addPoint(point_on_lower_circle);
799 }
800
801
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Vec3s point_on_upper_circle = Vec3s(cylinder->radius * support_dir[0], //
802
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 cylinder->radius * support_dir[1], //
803
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 cylinder->halfLength);
804
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 if (support_value - support_dir.dot(point_on_upper_circle) <= tol) {
805 if (_SupportOptions == SupportOptions::WithSweptSphere) {
806
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 point_on_upper_circle += cylinder->getSweptSphereRadius() * support_dir;
807 }
808
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 support_set.addPoint(point_on_upper_circle);
809 }
810 }
811 }
812 getShapeSupportSetTplInstantiation(Cylinder);
813
814 // ============================================================================
815 template <int _SupportOptions, typename IndexType>
816 36 void getShapeSupportSetLinear(const ConvexBaseTpl<IndexType>* convex,
817 SupportSet& support_set, int& hint /*unused*/,
818 ShapeSupportData& support_data, size_t /*unused*/,
819 Scalar tol) {
820
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 18 times.
 36 assert(tol > 0);
821
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 36 Vec3s support;
822
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 36 const Vec3s& support_dir = support_set.getNormal();
823
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 36 getShapeSupport<SupportOptions::NoSweptSphere>(convex, support_dir, support,
824 hint, support_data);
825
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 36 const Scalar support_value = support_dir.dot(support);
826
827 36 const std::vector<Vec3s>& points = *(convex->points);
828 36 SupportSet::Polygon& polygon = support_data.polygon;
829 36 polygon.clear();
830 36 const Transform3s& tf = support_set.tf;
831
2/2
✓ Branch 5 taken 88 times.
✓ Branch 6 taken 18 times.
 212 for (const Vec3s& point : points) {
832
1/2
✓ Branch 1 taken 88 times.
✗ Branch 2 not taken.
 176 const Scalar dot = support_dir.dot(point);
833
2/2
✓ Branch 0 taken 52 times.
✓ Branch 1 taken 36 times.
 176 if (support_value - dot <= tol) {
834 if (_SupportOptions == SupportOptions::WithSweptSphere) {
835 ✗ const Vec2s p =
836 tf.inverseTransform(point +
837 ✗ convex->getSweptSphereRadius() * support_dir)
838 ✗ .template head<2>();
839 ✗ polygon.emplace_back(p);
840 } else {
841
3/6
✓ Branch 1 taken 52 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 52 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 52 times.
✗ Branch 8 not taken.
 104 const Vec2s p = tf.inverseTransform(point).template head<2>();
842
1/2
✓ Branch 1 taken 52 times.
✗ Branch 2 not taken.
 104 polygon.emplace_back(p);
843 }
844 }
845 }
846
847
1/2
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
 36 computeSupportSetConvexHull(polygon, support_set.points());
848 }
849
850 // ============================================================================
851 template <int _SupportOptions, typename IndexType>
852 ✗ void convexSupportSetRecurse(const ConvexBaseTpl<IndexType>* convex,
853 const size_t vertex_idx, const Vec3s& support_dir,
854 const Scalar support_value, const Transform3s& tf,
855 std::vector<int8_t>& visited,
856 SupportSet::Polygon& polygon, Scalar tol) {
857 ✗ if (visited[vertex_idx]) {
858 ✗ return;
859 }
860
861 ✗ visited[vertex_idx] = true;
862 ✗ const std::vector<Vec3s>& points = *(convex->points);
863 ✗ const Vec3s& point = points[vertex_idx];
864 ✗ const Scalar val = point.dot(support_dir);
865 ✗ Scalar swept_sphere_radius = convex->getSweptSphereRadius();
866 ✗ if (support_value - val <= tol) {
867 if (_SupportOptions == SupportOptions::WithSweptSphere) {
868 ✗ const Vec2s p =
869 tf.inverseTransform(point + swept_sphere_radius * support_dir)
870 ✗ .template head<2>();
871 ✗ polygon.emplace_back(p);
872
873 } else {
874 ✗ const Vec2s p = tf.inverseTransform(point).template head<2>();
875 ✗ polygon.emplace_back(p);
876 }
877
878 typedef typename ConvexBaseTpl<IndexType>::Neighbors Neighbors;
879 ✗ const std::vector<Neighbors>& neighbors = *(convex->neighbors);
880 ✗ const Neighbors& point_neighbors = neighbors[vertex_idx];
881 ✗ for (IndexType i = 0; i < point_neighbors.count; ++i) {
882 const IndexType neighbor_index =
883 ✗ convex->neighbor(IndexType(vertex_idx), i);
884 ✗ convexSupportSetRecurse<_SupportOptions, IndexType>(
885 convex, neighbor_index, support_dir, support_value, tf, visited,
886 polygon, tol);
887 }
888 }
889 }
890
891 // ============================================================================
892 template <int _SupportOptions, typename IndexType>
893 ✗ void getShapeSupportSetLog(const ConvexBaseTpl<IndexType>* convex,
894 SupportSet& support_set, int& hint,
895 ShapeSupportData& support_data, size_t /*unused*/,
896 Scalar tol) {
897 ✗ assert(tol > 0);
898 ✗ Vec3s support;
899 ✗ const Vec3s& support_dir = support_set.getNormal();
900 ✗ getShapeSupportLog<SupportOptions::NoSweptSphere>(
901 convex, support_dir, support, hint, support_data);
902 ✗ const Scalar support_value = support.dot(support_dir);
903
904 ✗ std::vector<int8_t>& visited = support_data.visited;
905 // `visited` is guaranteed to be of right size due to previous call to convex
906 // log support function.
907 ✗ std::fill(support_data.visited.begin(), support_data.visited.end(), false);
908
909 ✗ SupportSet::Polygon& polygon = support_data.polygon;
910 ✗ polygon.clear();
911 ✗ const Transform3s& tf = support_set.tf;
912
913 ✗ const size_t vertex_idx = (size_t)(hint);
914 ✗ convexSupportSetRecurse<_SupportOptions, IndexType>(
915 convex, vertex_idx, support_dir, support_value, tf, visited, polygon,
916 tol);
917
918 ✗ computeSupportSetConvexHull(polygon, support_set.points());
919 }
920
921 // ============================================================================
922 template <int _SupportOptions, typename IndexType>
923 ✗ void getShapeSupportSet(const ConvexBaseTpl<IndexType>* convex,
924 SupportSet& support_set, int& hint,
925 ShapeSupportData& support_data,
926 size_t num_sampled_supports /*unused*/, Scalar tol) {
927 ✗ if (convex->num_points >
928 ✗ ConvexBaseTpl<IndexType>::num_vertices_large_convex_threshold &&
929 ✗ convex->neighbors != nullptr) {
930 ✗ getShapeSupportSetLog<_SupportOptions>(
931 convex, support_set, hint, support_data, num_sampled_supports, tol);
932 } else {
933 ✗ getShapeSupportSetLinear<_SupportOptions>(
934 convex, support_set, hint, support_data, num_sampled_supports, tol);
935 }
936 }
937 getShapeSupportSetTplInstantiation(ConvexBaseTpl<Triangle16::IndexType>);
938 getShapeSupportSetTplInstantiation(ConvexBaseTpl<Triangle32::IndexType>);
939
940 // ============================================================================
941 template <int _SupportOptions, typename IndexType>
942 36 void getShapeSupportSet(const SmallConvex<IndexType>* convex,
943 SupportSet& support_set, int& hint /*unused*/,
944 ShapeSupportData& support_data /*unused*/,
945 size_t num_sampled_supports /*unused*/, Scalar tol) {
946 36 getShapeSupportSetLinear<_SupportOptions>(
947 reinterpret_cast<const ConvexBaseTpl<IndexType>*>(convex), support_set,
948 hint, support_data, num_sampled_supports, tol);
949 }
950 getShapeSupportSetTplInstantiation(SmallConvex<Triangle16::IndexType>);
951 getShapeSupportSetTplInstantiation(SmallConvex<Triangle32::IndexType>);
952
953 // ============================================================================
954 template <int _SupportOptions, typename IndexType>
955 ✗ void getShapeSupportSet(const LargeConvex<IndexType>* convex,
956 SupportSet& support_set, int& hint,
957 ShapeSupportData& support_data,
958 size_t num_sampled_supports /*unused*/, Scalar tol) {
959 ✗ getShapeSupportSetLog<_SupportOptions>(
960 reinterpret_cast<const ConvexBaseTpl<IndexType>*>(convex), support_set,
961 hint, support_data, num_sampled_supports, tol);
962 }
963 getShapeSupportSetTplInstantiation(LargeConvex<Triangle16::IndexType>);
964 getShapeSupportSetTplInstantiation(LargeConvex<Triangle32::IndexType>);
965
966 // ============================================================================
967 24 COAL_DLLAPI void computeSupportSetConvexHull(SupportSet::Polygon& cloud,
968 SupportSet::Polygon& cvx_hull) {
969 24 cvx_hull.clear();
970
971
2/2
✓ Branch 1 taken 6 times.
✓ Branch 2 taken 18 times.
 24 if (cloud.size() <= 2) {
972 // Point or segment, nothing to do.
973
2/2
✓ Branch 5 taken 12 times.
✓ Branch 6 taken 6 times.
 18 for (const Vec2s& point : cloud) {
974
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 cvx_hull.emplace_back(point);
975 }
976 14 return;
977 }
978
979
2/2
✓ Branch 1 taken 8 times.
✓ Branch 2 taken 10 times.
 18 if (cloud.size() == 3) {
980 // We have a triangle, we only need to arrange it in a counter clockwise
981 // fashion.
982 //
983 // Put the vector which has the lowest y coordinate first.
984
4/6
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 8 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 3 times.
✓ Branch 9 taken 5 times.
 8 if (cloud[0](1) > cloud[1](1)) {
985 3 std::swap(cloud[0], cloud[1]);
986 }
987
4/6
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 8 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 1 times.
✓ Branch 9 taken 7 times.
 8 if (cloud[0](1) > cloud[2](1)) {
988 1 std::swap(cloud[0], cloud[2]);
989 }
990 8 const Vec2s& a = cloud[0];
991 8 const Vec2s& b = cloud[1];
992 8 const Vec2s& c = cloud[2];
993 const Scalar det =
994
8/16
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 8 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 8 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 8 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 8 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 8 times.
✗ Branch 23 not taken.
 8 (b(0) - a(0)) * (c(1) - a(1)) - (b(1) - a(1)) * (c(0) - a(0));
995
2/2
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 4 times.
 8 if (det < 0) {
996 4 std::swap(cloud[1], cloud[2]);
997 }
998
999
2/2
✓ Branch 5 taken 24 times.
✓ Branch 6 taken 8 times.
 32 for (const Vec2s& point : cloud) {
1000
1/2
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
 24 cvx_hull.emplace_back(point);
1001 }
1002 8 return;
1003 }
1004
1005 // The following is an implementation of the O(nlog(n)) graham scan
1006 // algorithm, used to compute convex-hulls in 2D.
1007 // See https://en.wikipedia.org/wiki/Graham_scan
1008 //
1009 // Step 1 - Compute first element of the convex-hull by computing the support
1010 // in the direction (0, -1) (take the element of the set which has the lowest
1011 // y coordinate).
1012 10 size_t support_idx = 0;
1013
1/2
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
 10 Scalar support_val = cloud[0](1);
1014
2/2
✓ Branch 1 taken 30 times.
✓ Branch 2 taken 10 times.
 40 for (size_t i = 1; i < cloud.size(); ++i) {
1015
1/2
✓ Branch 2 taken 30 times.
✗ Branch 3 not taken.
 30 const Scalar val = cloud[i](1);
1016
2/2
✓ Branch 0 taken 10 times.
✓ Branch 1 taken 20 times.
 30 if (val < support_val) {
1017 10 support_val = val;
1018 10 support_idx = i;
1019 }
1020 }
1021 10 std::swap(cloud[0], cloud[support_idx]);
1022 10 cvx_hull.clear();
1023
1/2
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
 10 cvx_hull.emplace_back(cloud[0]);
1024 10 const Vec2s& v = cvx_hull[0];
1025
1026 // Step 2 - Sort the rest of the point cloud according to the angle made with
1027 // v. Note: we use stable_sort instead of sort because sort can fail if two
1028 // values are identical.
1029
1/2
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
 20 std::stable_sort(
1030 10 cloud.begin() + 1, cloud.end(), [&v](const Vec2s& p1, const Vec2s& p2) {
1031 // p1 is "smaller" than p2 if det(p1 - v, p2 - v) >= 0
1032 const Scalar det =
1033 30 (p1(0) - v(0)) * (p2(1) - v(1)) - (p1(1) - v(1)) * (p2(0) - v(0));
1034
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 30 times.
 30 if (std::abs(det) <= Eigen::NumTraits<Scalar>::dummy_precision()) {
1035 // If two points are identical or (v, p1, p2) are colinear, p1 is
1036 // "smaller" if it is closer to v.
1037 ✗ return ((p1 - v).squaredNorm() <= (p2 - v).squaredNorm());
1038 }
1039 30 return det > 0;
1040 });
1041
1042 // Step 3 - We iterate over the now ordered point of cloud and add the points
1043 // to the cvx-hull if they successively form "left turns" only. A left turn
1044 // is: considering the last three points of the cvx-hull, if they form a
1045 // right-hand basis (determinant > 0) then they make a left turn.
1046 10 auto isRightSided = [](const Vec2s& p1, const Vec2s& p2, const Vec2s& p3) {
1047 // Checks if (p2 - p1, p3 - p1) forms a right-sided base based on
1048 // det(p2 - p1, p3 - p1)
1049 const Scalar det =
1050 10 (p2(0) - p1(0)) * (p3(1) - p1(1)) - (p2(1) - p1(1)) * (p3(0) - p1(0));
1051 // Note: we set a dummy precision threshold so that identical points or
1052 // colinear pionts are not added to the cvx-hull.
1053 10 return det > Eigen::NumTraits<Scalar>::dummy_precision();
1054 };
1055
1056 // We initialize the cvx-hull algo by adding the first three
1057 // (distinct) points of the set.
1058 // These three points are guaranteed, due to the previous sorting,
1059 // to form a right sided basis, hence to form a left turn.
1060 10 size_t cloud_beginning_idx = 1;
1061
2/2
✓ Branch 1 taken 20 times.
✓ Branch 2 taken 10 times.
 30 while (cvx_hull.size() < 3) {
1062 20 const Vec2s& vec = cloud[cloud_beginning_idx];
1063
3/6
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 20 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
 40 if ((cvx_hull.back() - vec).squaredNorm() >
1064 20 Eigen::NumTraits<Scalar>::epsilon()) {
1065
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 cvx_hull.emplace_back(vec);
1066 }
1067 20 ++cloud_beginning_idx;
1068 }
1069 // The convex-hull should wrap counter-clockwise, i.e. three successive
1070 // points should always form a right-sided basis. Every time we do a turn
1071 // in the wrong direction, we remove the last point of the convex-hull.
1072 // When we do a turn in the correct direction, we add a point to the
1073 // convex-hull.
1074
2/2
✓ Branch 1 taken 10 times.
✓ Branch 2 taken 10 times.
 20 for (size_t i = cloud_beginning_idx; i < cloud.size(); ++i) {
1075 10 const Vec2s& vec = cloud[i];
1076
2/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 10 times.
 20 while (cvx_hull.size() > 1 &&
1077
2/4
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 10 times.
 10 !isRightSided(cvx_hull[cvx_hull.size() - 2],
1078 10 cvx_hull[cvx_hull.size() - 1], vec)) {
1079 ✗ cvx_hull.pop_back();
1080 }
1081
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 cvx_hull.emplace_back(vec);
1082 }
1083 }
1084
1085 } // namespace details
1086 } // namespace coal
1087