GCC Code Coverage Report

Directory: ./
File: src/narrowphase/minkowski_difference.cpp
Date: 2025-06-03 17:15:40
Exec Total Coverage
Lines: 127 166 76.5%
Functions: 157 998 15.7%
Branches: 71 212 33.5%
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/minkowski_difference.h"
40 #include "coal/shape/geometric_shapes_traits.h"
41
42 namespace coal {
43 namespace details {
44
45 // ============================================================================
46 template <typename Shape0, typename Shape1, bool TransformIsIdentity,
47 int _SupportOptions>
48 144902834 void getSupportTpl(const Shape0* s0, const Shape1* s1, const Matrix3s& oR1,
49 const Vec3s& ot1, const Vec3s& dir, Vec3s& support0,
50 Vec3s& support1, support_func_guess_t& hint,
51 ShapeSupportData data[2]) {
52
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 72451417 times.
 144902834 assert(dir.norm() > Eigen::NumTraits<Scalar>::epsilon());
53 144902834 getShapeSupport<_SupportOptions>(s0, dir, support0, hint[0], data[0]);
54
55 if (TransformIsIdentity) {
56
3/6
✓ Branch 2 taken 68068880 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 68068880 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 68068880 times.
✗ Branch 9 not taken.
 136137760 getShapeSupport<_SupportOptions>(s1, -dir, support1, hint[1], data[1]);
57 } else {
58
5/10
✓ Branch 1 taken 4382537 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4382537 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4382537 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4382537 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 4382537 times.
✗ Branch 14 not taken.
 8765074 getShapeSupport<_SupportOptions>(s1, -oR1.transpose() * dir, support1,
59 8765074 hint[1], data[1]);
60
3/6
✓ Branch 1 taken 4382537 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4382537 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4382537 times.
✗ Branch 8 not taken.
 8765074 support1 = oR1 * support1 + ot1;
61 }
62 144902834 }
63
64 // ============================================================================
65 template <typename Shape0, typename Shape1, bool TransformIsIdentity,
66 int _SupportOptions>
67 144902834 void getSupportFuncTpl(const MinkowskiDiff& md, const Vec3s& dir,
68 Vec3s& support0, Vec3s& support1,
69 support_func_guess_t& hint, ShapeSupportData data[2]) {
70 144902834 getSupportTpl<Shape0, Shape1, TransformIsIdentity, _SupportOptions>(
71 144902834 static_cast<const Shape0*>(md.shapes[0]),
72 144902834 static_cast<const Shape1*>(md.shapes[1]), md.oR1, md.ot1, dir, support0,
73 support1, hint, data);
74 144902834 }
75
76 // ============================================================================
77 template <typename Shape0, int _SupportOptions>
78 60066314 MinkowskiDiff::GetSupportFunction makeGetSupportFunction1(
79 const ShapeBase* s1, bool identity,
80 Eigen::Array<Scalar, 1, 2>& swept_sphere_radius, ShapeSupportData data[2]) {
81 if (_SupportOptions == SupportOptions::WithSweptSphere) {
82 // No need to store the information of swept sphere radius
83 3200 swept_sphere_radius[1] = 0;
84 } else {
85 // We store the information of swept sphere radius.
86 // GJK and EPA will use this information to correct the solution they find.
87 60063114 swept_sphere_radius[1] = s1->getSweptSphereRadius();
88 }
89
90
8/10
✓ Branch 1 taken 29208521 times.
✓ Branch 2 taken 569324 times.
✓ Branch 3 taken 61311 times.
✓ Branch 4 taken 4385 times.
✓ Branch 5 taken 8483 times.
✓ Branch 6 taken 2336 times.
✓ Branch 7 taken 160400 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 18397 times.
✗ Branch 10 not taken.
 60066314 switch (s1->getNodeType()) {
91 58417042 case GEOM_TRIANGLE:
92
2/2
✓ Branch 0 taken 29208515 times.
✓ Branch 1 taken 6 times.
 58417042 if (identity)
93 58417030 return getSupportFuncTpl<Shape0, TriangleP, true, _SupportOptions>;
94 else
95 12 return getSupportFuncTpl<Shape0, TriangleP, false, _SupportOptions>;
96 1138648 case GEOM_BOX:
97
2/2
✓ Branch 0 taken 25 times.
✓ Branch 1 taken 569299 times.
 1138648 if (identity)
98 50 return getSupportFuncTpl<Shape0, Box, true, _SupportOptions>;
99 else
100 1138598 return getSupportFuncTpl<Shape0, Box, false, _SupportOptions>;
101 122622 case GEOM_SPHERE:
102 if (_SupportOptions == SupportOptions::NoSweptSphere) {
103 // Sphere can be considered a swept-sphere point.
104 122622 swept_sphere_radius[1] += static_cast<const Sphere*>(s1)->radius;
105 }
106
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 61311 times.
 122622 if (identity)
107 ✗ return getSupportFuncTpl<Shape0, Sphere, true, _SupportOptions>;
108 else
109 122622 return getSupportFuncTpl<Shape0, Sphere, false, _SupportOptions>;
110 8770 case GEOM_ELLIPSOID:
111
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 4384 times.
 8770 if (identity)
112 2 return getSupportFuncTpl<Shape0, Ellipsoid, true, _SupportOptions>;
113 else
114 8768 return getSupportFuncTpl<Shape0, Ellipsoid, false, _SupportOptions>;
115 16966 case GEOM_CAPSULE:
116 if (_SupportOptions == SupportOptions::NoSweptSphere) {
117 // Sphere can be considered as a swept-sphere segment.
118 16646 swept_sphere_radius[1] += static_cast<const Capsule*>(s1)->radius;
119 }
120
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 8483 times.
 16966 if (identity)
121 ✗ return getSupportFuncTpl<Shape0, Capsule, true, _SupportOptions>;
122 else
123 16966 return getSupportFuncTpl<Shape0, Capsule, false, _SupportOptions>;
124 4672 case GEOM_CONE:
125
2/2
✓ Branch 0 taken 16 times.
✓ Branch 1 taken 2320 times.
 4672 if (identity)
126 32 return getSupportFuncTpl<Shape0, Cone, true, _SupportOptions>;
127 else
128 4640 return getSupportFuncTpl<Shape0, Cone, false, _SupportOptions>;
129 320800 case GEOM_CYLINDER:
130
2/2
✓ Branch 0 taken 8 times.
✓ Branch 1 taken 160392 times.
 320800 if (identity)
131 16 return getSupportFuncTpl<Shape0, Cylinder, true, _SupportOptions>;
132 else
133 320784 return getSupportFuncTpl<Shape0, Cylinder, false, _SupportOptions>;
134 ✗ case GEOM_CONVEX16: {
135 ✗ const ConvexBase16* convex1 = static_cast<const ConvexBase16*>(s1);
136 ✗ if (static_cast<size_t>(convex1->num_points) >
137 ConvexBase16::num_vertices_large_convex_threshold) {
138 ✗ data[1].visited.assign(convex1->num_points, false);
139 ✗ data[1].last_dir.setZero();
140 ✗ if (identity)
141 ✗ return getSupportFuncTpl<Shape0, LargeConvex16, true,
142 _SupportOptions>;
143 else
144 ✗ return getSupportFuncTpl<Shape0, LargeConvex16, false,
145 _SupportOptions>;
146 } else {
147 ✗ if (identity)
148 ✗ return getSupportFuncTpl<Shape0, SmallConvex16, true,
149 _SupportOptions>;
150 else
151 ✗ return getSupportFuncTpl<Shape0, SmallConvex16, false,
152 _SupportOptions>;
153 }
154 }
155 36794 case GEOM_CONVEX32: {
156 36794 const ConvexBase32* convex1 = static_cast<const ConvexBase32*>(s1);
157
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 18397 times.
 36794 if (static_cast<size_t>(convex1->num_points) >
158 ConvexBase32::num_vertices_large_convex_threshold) {
159 ✗ data[1].visited.assign(convex1->num_points, false);
160 ✗ data[1].last_dir.setZero();
161 ✗ if (identity)
162 ✗ return getSupportFuncTpl<Shape0, LargeConvex32, true,
163 _SupportOptions>;
164 else
165 ✗ return getSupportFuncTpl<Shape0, LargeConvex32, false,
166 _SupportOptions>;
167 } else {
168
2/2
✓ Branch 0 taken 10 times.
✓ Branch 1 taken 18387 times.
 36794 if (identity)
169 20 return getSupportFuncTpl<Shape0, SmallConvex32, true,
170 _SupportOptions>;
171 else
172 36774 return getSupportFuncTpl<Shape0, SmallConvex32, false,
173 _SupportOptions>;
174 }
175 }
176 ✗ default:
177 ✗ COAL_THROW_PRETTY("Unsupported geometric shape.", std::logic_error);
178 }
179 }
180
181 // ============================================================================
182 template <int _SupportOptions>
183 60066314 MinkowskiDiff::GetSupportFunction makeGetSupportFunction0(
184 const ShapeBase* s0, const ShapeBase* s1, bool identity,
185 Eigen::Array<Scalar, 1, 2>& swept_sphere_radius, ShapeSupportData data[2]) {
186 if (_SupportOptions == SupportOptions::WithSweptSphere) {
187 // No need to store the information of swept sphere radius
188 3200 swept_sphere_radius[0] = 0;
189 } else {
190 // We store the information of swept sphere radius.
191 // GJK and EPA will use this information to correct the solution they find.
192 60063114 swept_sphere_radius[0] = s0->getSweptSphereRadius();
193 }
194
195
8/10
✓ Branch 1 taken 29205604 times.
✓ Branch 2 taken 419380 times.
✓ Branch 3 taken 230 times.
✓ Branch 4 taken 16066 times.
✓ Branch 5 taken 7836 times.
✓ Branch 6 taken 1316 times.
✓ Branch 7 taken 314823 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 67902 times.
✗ Branch 10 not taken.
 60066314 switch (s0->getNodeType()) {
196 58411208 case GEOM_TRIANGLE:
197 58411208 return makeGetSupportFunction1<TriangleP, _SupportOptions>(
198 58411208 s1, identity, swept_sphere_radius, data);
199 break;
200 838760 case GEOM_BOX:
201 838760 return makeGetSupportFunction1<Box, _SupportOptions>(
202 838760 s1, identity, swept_sphere_radius, data);
203 break;
204 460 case GEOM_SPHERE:
205 if (_SupportOptions == SupportOptions::NoSweptSphere) {
206 // Sphere can always be considered as a swept-sphere point.
207 460 swept_sphere_radius[0] += static_cast<const Sphere*>(s0)->radius;
208 }
209 460 return makeGetSupportFunction1<Sphere, _SupportOptions>(
210 460 s1, identity, swept_sphere_radius, data);
211 break;
212 32132 case GEOM_ELLIPSOID:
213 32132 return makeGetSupportFunction1<Ellipsoid, _SupportOptions>(
214 32132 s1, identity, swept_sphere_radius, data);
215 break;
216 15672 case GEOM_CAPSULE:
217 if (_SupportOptions == SupportOptions::NoSweptSphere) {
218 // Capsule can always be considered as a swept-sphere segment.
219 15032 swept_sphere_radius[0] += static_cast<const Capsule*>(s0)->radius;
220 }
221 15672 return makeGetSupportFunction1<Capsule, _SupportOptions>(
222 15672 s1, identity, swept_sphere_radius, data);
223 break;
224 2632 case GEOM_CONE:
225 2632 return makeGetSupportFunction1<Cone, _SupportOptions>(
226 2632 s1, identity, swept_sphere_radius, data);
227 break;
228 629646 case GEOM_CYLINDER:
229 629646 return makeGetSupportFunction1<Cylinder, _SupportOptions>(
230 629646 s1, identity, swept_sphere_radius, data);
231 break;
232 ✗ case GEOM_CONVEX16: {
233 ✗ const ConvexBase16* convex0 = static_cast<const ConvexBase16*>(s0);
234 ✗ if (static_cast<size_t>(convex0->num_points) >
235 ConvexBase16::num_vertices_large_convex_threshold) {
236 ✗ data[0].visited.assign(convex0->num_points, false);
237 ✗ data[0].last_dir.setZero();
238 ✗ return makeGetSupportFunction1<LargeConvex16, _SupportOptions>(
239 ✗ s1, identity, swept_sphere_radius, data);
240 } else
241 ✗ return makeGetSupportFunction1<SmallConvex16, _SupportOptions>(
242 ✗ s1, identity, swept_sphere_radius, data);
243 break;
244 }
245 135804 case GEOM_CONVEX32: {
246 135804 const ConvexBase32* convex0 = static_cast<const ConvexBase32*>(s0);
247
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 67902 times.
 135804 if (static_cast<size_t>(convex0->num_points) >
248 ConvexBase32::num_vertices_large_convex_threshold) {
249 ✗ data[0].visited.assign(convex0->num_points, false);
250 ✗ data[0].last_dir.setZero();
251 ✗ return makeGetSupportFunction1<LargeConvex32, _SupportOptions>(
252 ✗ s1, identity, swept_sphere_radius, data);
253 } else
254 135804 return makeGetSupportFunction1<SmallConvex32, _SupportOptions>(
255 135804 s1, identity, swept_sphere_radius, data);
256 break;
257 }
258 ✗ default:
259 ✗ COAL_THROW_PRETTY("Unsupported geometric shape", std::logic_error);
260 }
261 }
262
263 // ============================================================================
264 30101059 bool getNormalizeSupportDirection(const ShapeBase* shape) {
265
8/10
✓ Branch 1 taken 29205604 times.
✓ Branch 2 taken 419516 times.
✓ Branch 3 taken 61383 times.
✓ Branch 4 taken 16818 times.
✓ Branch 5 taken 7868 times.
✓ Branch 6 taken 1348 times.
✓ Branch 7 taken 314855 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 73667 times.
✗ Branch 10 not taken.
 30101059 switch (shape->getNodeType()) {
266 29205604 case GEOM_TRIANGLE:
267 29205604 return (bool)shape_traits<TriangleP>::NeedNesterovNormalizeHeuristic;
268 break;
269 419516 case GEOM_BOX:
270 419516 return (bool)shape_traits<Box>::NeedNesterovNormalizeHeuristic;
271 break;
272 61383 case GEOM_SPHERE:
273 61383 return (bool)shape_traits<Sphere>::NeedNesterovNormalizeHeuristic;
274 break;
275 16818 case GEOM_ELLIPSOID:
276 16818 return (bool)shape_traits<Ellipsoid>::NeedNesterovNormalizeHeuristic;
277 break;
278 7868 case GEOM_CAPSULE:
279 7868 return (bool)shape_traits<Capsule>::NeedNesterovNormalizeHeuristic;
280 break;
281 1348 case GEOM_CONE:
282 1348 return (bool)shape_traits<Cone>::NeedNesterovNormalizeHeuristic;
283 break;
284 314855 case GEOM_CYLINDER:
285 314855 return (bool)shape_traits<Cylinder>::NeedNesterovNormalizeHeuristic;
286 break;
287 ✗ case GEOM_CONVEX16:
288 ✗ return (bool)shape_traits<ConvexBase16>::NeedNesterovNormalizeHeuristic;
289 break;
290 73667 case GEOM_CONVEX32:
291 73667 return (bool)shape_traits<ConvexBase32>::NeedNesterovNormalizeHeuristic;
292 break;
293 ✗ default:
294 ✗ COAL_THROW_PRETTY("Unsupported geometric shape", std::logic_error);
295 }
296 }
297
298 // ============================================================================
299 30033157 void getNormalizeSupportDirectionFromShapes(const ShapeBase* shape0,
300 const ShapeBase* shape1,
301 bool& normalize_support_direction) {
302
2/2
✓ Branch 1 taken 67902 times.
✓ Branch 2 taken 29965255 times.
 30101059 normalize_support_direction = getNormalizeSupportDirection(shape0) &&
303
2/2
✓ Branch 1 taken 5765 times.
✓ Branch 2 taken 62137 times.
 67902 getNormalizeSupportDirection(shape1);
304 30033157 }
305
306 // ============================================================================
307 template <int _SupportOptions>
308 1649278 void MinkowskiDiff::set(const ShapeBase* shape0, const ShapeBase* shape1,
309 const Transform3s& tf0, const Transform3s& tf1) {
310 1649278 shapes[0] = shape0;
311 1649278 shapes[1] = shape1;
312 1649278 getNormalizeSupportDirectionFromShapes(shape0, shape1,
313 1649278 normalize_support_direction);
314
315
4/8
✓ Branch 3 taken 824639 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 824639 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 824639 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 824639 times.
✗ Branch 13 not taken.
 1649278 oR1.noalias() = tf0.getRotation().transpose() * tf1.getRotation();
316
5/10
✓ Branch 1 taken 824639 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 824639 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 824639 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 824639 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 824639 times.
✗ Branch 15 not taken.
 1649278 ot1.noalias() = tf0.getRotation().transpose() *
317 1649278 (tf1.getTranslation() - tf0.getTranslation());
318
319
6/8
✓ Branch 2 taken 824639 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 63794 times.
✓ Branch 5 taken 760845 times.
✓ Branch 8 taken 63794 times.
✗ Branch 9 not taken.
✓ Branch 10 taken 57 times.
✓ Branch 11 taken 63737 times.
 1649278 bool identity = (oR1.isIdentity() && ot1.isZero());
320
321 3298556 getSupportFunc = makeGetSupportFunction0<_SupportOptions>(
322 1649278 shape0, shape1, identity, swept_sphere_radius, data);
323 1649278 }
324 // clang-format off
325 template void COAL_DLLAPI MinkowskiDiff::set<SupportOptions::NoSweptSphere>(const ShapeBase*, const ShapeBase*);
326
327 template void COAL_DLLAPI MinkowskiDiff::set<SupportOptions::WithSweptSphere>(const ShapeBase*, const ShapeBase*);
328 // clang-format on
329
330 // ============================================================================
331 template <int _SupportOptions>
332 58417036 void MinkowskiDiff::set(const ShapeBase* shape0, const ShapeBase* shape1) {
333 58417036 shapes[0] = shape0;
334 58417036 shapes[1] = shape1;
335 58417036 getNormalizeSupportDirectionFromShapes(shape0, shape1,
336 58417036 normalize_support_direction);
337
338 58417036 oR1.setIdentity();
339 58417036 ot1.setZero();
340
341 116834072 getSupportFunc = makeGetSupportFunction0<_SupportOptions>(
342 58417036 shape0, shape1, true, swept_sphere_radius, data);
343 58417036 }
344 // clang-format off
345 template void COAL_DLLAPI MinkowskiDiff::set<SupportOptions::NoSweptSphere>(const ShapeBase*, const ShapeBase*, const Transform3s&, const Transform3s&);
346
347 template void COAL_DLLAPI MinkowskiDiff::set<SupportOptions::WithSweptSphere>(const ShapeBase*, const ShapeBase*, const Transform3s&, const Transform3s&);
348 // clang-format on
349
350 // ============================================================================
351 // clang-format off
352 template Vec3s COAL_DLLAPI MinkowskiDiff::support0<SupportOptions::NoSweptSphere>(const Vec3s&, int&) const;
353
354 template Vec3s COAL_DLLAPI MinkowskiDiff::support0<SupportOptions::WithSweptSphere>(const Vec3s&, int&) const;
355
356 template Vec3s COAL_DLLAPI MinkowskiDiff::support1<SupportOptions::NoSweptSphere>(const Vec3s&, int&) const;
357
358 template Vec3s COAL_DLLAPI MinkowskiDiff::support1<SupportOptions::WithSweptSphere>(const Vec3s&, int&) const;
359 // clang-format on
360
361 } // namespace details
362 } // namespace coal
363