GCC Code Coverage Report

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