GCC Code Coverage Report


Directory: ./
File: src/narrowphase/minkowski_difference.cpp
Date: 2025-04-01 09:23:31
Exec Total Coverage
Lines: 124 141 87.9%
Branches: 66 187 35.3%

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 144902796 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 72451398 times.
144902796 assert(dir.norm() > Eigen::NumTraits<Scalar>::epsilon());
53 144902796 getShapeSupport<_SupportOptions>(s0, dir, support0, hint[0], data[0]);
54
55 if (TransformIsIdentity) {
56
2/4
✓ Branch 3 taken 68068881 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 68068881 times.
✗ Branch 7 not taken.
136137762 getShapeSupport<_SupportOptions>(s1, -dir, support1, hint[1], data[1]);
57 } else {
58
4/8
✓ Branch 2 taken 4382517 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4382517 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 4382517 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 4382517 times.
✗ Branch 12 not taken.
8765034 getShapeSupport<_SupportOptions>(s1, -oR1.transpose() * dir, support1,
59 8765034 hint[1], data[1]);
60
2/4
✓ Branch 2 taken 4382517 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4382517 times.
✗ Branch 6 not taken.
8765034 support1 = oR1 * support1 + ot1;
61 }
62 }
63
64 // ============================================================================
65 template <typename Shape0, typename Shape1, bool TransformIsIdentity,
66 int _SupportOptions>
67 144902796 void getSupportFuncTpl(const MinkowskiDiff& md, const Vec3s& dir,
68 Vec3s& support0, Vec3s& support1,
69 support_func_guess_t& hint, ShapeSupportData data[2]) {
70 144902796 getSupportTpl<Shape0, Shape1, TransformIsIdentity, _SupportOptions>(
71 144902796 static_cast<const Shape0*>(md.shapes[0]),
72 144902796 static_cast<const Shape1*>(md.shapes[1]), md.oR1, md.ot1, dir, support0,
73 support1, hint, data);
74 }
75
76 // ============================================================================
77 template <typename Shape0, int _SupportOptions>
78 60066310 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 60063110 swept_sphere_radius[1] = s1->getSweptSphereRadius();
88 }
89
90
8/9
✓ Branch 1 taken 29208521 times.
✓ Branch 2 taken 569319 times.
✓ Branch 3 taken 61311 times.
✓ Branch 4 taken 4385 times.
✓ Branch 5 taken 8483 times.
✓ Branch 6 taken 2336 times.
✓ Branch 7 taken 160403 times.
✓ Branch 8 taken 18397 times.
✗ Branch 9 not taken.
60066310 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 1138638 case GEOM_BOX:
97
2/2
✓ Branch 0 taken 25 times.
✓ Branch 1 taken 569294 times.
1138638 if (identity)
98 50 return getSupportFuncTpl<Shape0, Box, true, _SupportOptions>;
99 else
100 1138588 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 320806 case GEOM_CYLINDER:
130
2/2
✓ Branch 0 taken 8 times.
✓ Branch 1 taken 160395 times.
320806 if (identity)
131 16 return getSupportFuncTpl<Shape0, Cylinder, true, _SupportOptions>;
132 else
133 320790 return getSupportFuncTpl<Shape0, Cylinder, false, _SupportOptions>;
134 36794 case GEOM_CONVEX: {
135 36794 const ConvexBase* convex1 = static_cast<const ConvexBase*>(s1);
136
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 18397 times.
36794 if (static_cast<size_t>(convex1->num_points) >
137 ConvexBase::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, LargeConvex, true, _SupportOptions>;
142 else
143 return getSupportFuncTpl<Shape0, LargeConvex, false, _SupportOptions>;
144 } else {
145
2/2
✓ Branch 0 taken 10 times.
✓ Branch 1 taken 18387 times.
36794 if (identity)
146 20 return getSupportFuncTpl<Shape0, SmallConvex, true, _SupportOptions>;
147 else
148 36774 return getSupportFuncTpl<Shape0, SmallConvex, false, _SupportOptions>;
149 }
150 }
151 default:
152 COAL_THROW_PRETTY("Unsupported geometric shape.", std::logic_error);
153 }
154 }
155
156 // ============================================================================
157 template <int _SupportOptions>
158 60066310 MinkowskiDiff::GetSupportFunction makeGetSupportFunction0(
159 const ShapeBase* s0, const ShapeBase* s1, bool identity,
160 Eigen::Array<Scalar, 1, 2>& swept_sphere_radius, ShapeSupportData data[2]) {
161 if (_SupportOptions == SupportOptions::WithSweptSphere) {
162 // No need to store the information of swept sphere radius
163 3200 swept_sphere_radius[0] = 0;
164 } else {
165 // We store the information of swept sphere radius.
166 // GJK and EPA will use this information to correct the solution they find.
167 60063110 swept_sphere_radius[0] = s0->getSweptSphereRadius();
168 }
169
170
8/9
✓ Branch 1 taken 29205604 times.
✓ Branch 2 taken 419385 times.
✓ Branch 3 taken 230 times.
✓ Branch 4 taken 16066 times.
✓ Branch 5 taken 7836 times.
✓ Branch 6 taken 1316 times.
✓ Branch 7 taken 314816 times.
✓ Branch 8 taken 67902 times.
✗ Branch 9 not taken.
60066310 switch (s0->getNodeType()) {
171 58411208 case GEOM_TRIANGLE:
172 58411208 return makeGetSupportFunction1<TriangleP, _SupportOptions>(
173 58411208 s1, identity, swept_sphere_radius, data);
174 break;
175 838770 case GEOM_BOX:
176 838770 return makeGetSupportFunction1<Box, _SupportOptions>(
177 838770 s1, identity, swept_sphere_radius, data);
178 break;
179 460 case GEOM_SPHERE:
180 if (_SupportOptions == SupportOptions::NoSweptSphere) {
181 // Sphere can always be considered as a swept-sphere point.
182 460 swept_sphere_radius[0] += static_cast<const Sphere*>(s0)->radius;
183 }
184 460 return makeGetSupportFunction1<Sphere, _SupportOptions>(
185 460 s1, identity, swept_sphere_radius, data);
186 break;
187 32132 case GEOM_ELLIPSOID:
188 32132 return makeGetSupportFunction1<Ellipsoid, _SupportOptions>(
189 32132 s1, identity, swept_sphere_radius, data);
190 break;
191 15672 case GEOM_CAPSULE:
192 if (_SupportOptions == SupportOptions::NoSweptSphere) {
193 // Capsule can always be considered as a swept-sphere segment.
194 15032 swept_sphere_radius[0] += static_cast<const Capsule*>(s0)->radius;
195 }
196 15672 return makeGetSupportFunction1<Capsule, _SupportOptions>(
197 15672 s1, identity, swept_sphere_radius, data);
198 break;
199 2632 case GEOM_CONE:
200 2632 return makeGetSupportFunction1<Cone, _SupportOptions>(
201 2632 s1, identity, swept_sphere_radius, data);
202 break;
203 629632 case GEOM_CYLINDER:
204 629632 return makeGetSupportFunction1<Cylinder, _SupportOptions>(
205 629632 s1, identity, swept_sphere_radius, data);
206 break;
207 135804 case GEOM_CONVEX: {
208 135804 const ConvexBase* convex0 = static_cast<const ConvexBase*>(s0);
209
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 67902 times.
135804 if (static_cast<size_t>(convex0->num_points) >
210 ConvexBase::num_vertices_large_convex_threshold) {
211 data[0].visited.assign(convex0->num_points, false);
212 data[0].last_dir.setZero();
213 return makeGetSupportFunction1<LargeConvex, _SupportOptions>(
214 s1, identity, swept_sphere_radius, data);
215 } else
216 135804 return makeGetSupportFunction1<SmallConvex, _SupportOptions>(
217 135804 s1, identity, swept_sphere_radius, data);
218 break;
219 }
220 default:
221 COAL_THROW_PRETTY("Unsupported geometric shape", std::logic_error);
222 }
223 }
224
225 // ============================================================================
226 30101057 bool getNormalizeSupportDirection(const ShapeBase* shape) {
227
8/9
✓ Branch 1 taken 29205604 times.
✓ Branch 2 taken 419521 times.
✓ Branch 3 taken 61383 times.
✓ Branch 4 taken 16818 times.
✓ Branch 5 taken 7868 times.
✓ Branch 6 taken 1348 times.
✓ Branch 7 taken 314848 times.
✓ Branch 8 taken 73667 times.
✗ Branch 9 not taken.
30101057 switch (shape->getNodeType()) {
228 29205604 case GEOM_TRIANGLE:
229 29205604 return (bool)shape_traits<TriangleP>::NeedNesterovNormalizeHeuristic;
230 break;
231 419521 case GEOM_BOX:
232 419521 return (bool)shape_traits<Box>::NeedNesterovNormalizeHeuristic;
233 break;
234 61383 case GEOM_SPHERE:
235 61383 return (bool)shape_traits<Sphere>::NeedNesterovNormalizeHeuristic;
236 break;
237 16818 case GEOM_ELLIPSOID:
238 16818 return (bool)shape_traits<Ellipsoid>::NeedNesterovNormalizeHeuristic;
239 break;
240 7868 case GEOM_CAPSULE:
241 7868 return (bool)shape_traits<Capsule>::NeedNesterovNormalizeHeuristic;
242 break;
243 1348 case GEOM_CONE:
244 1348 return (bool)shape_traits<Cone>::NeedNesterovNormalizeHeuristic;
245 break;
246 314848 case GEOM_CYLINDER:
247 314848 return (bool)shape_traits<Cylinder>::NeedNesterovNormalizeHeuristic;
248 break;
249 73667 case GEOM_CONVEX:
250 73667 return (bool)shape_traits<ConvexBase>::NeedNesterovNormalizeHeuristic;
251 break;
252 default:
253 COAL_THROW_PRETTY("Unsupported geometric shape", std::logic_error);
254 }
255 }
256
257 // ============================================================================
258 30033155 void getNormalizeSupportDirectionFromShapes(const ShapeBase* shape0,
259 const ShapeBase* shape1,
260 bool& normalize_support_direction) {
261
2/2
✓ Branch 1 taken 67902 times.
✓ Branch 2 taken 29965253 times.
30101057 normalize_support_direction = getNormalizeSupportDirection(shape0) &&
262
2/2
✓ Branch 1 taken 5765 times.
✓ Branch 2 taken 62137 times.
67902 getNormalizeSupportDirection(shape1);
263 30033155 }
264
265 // ============================================================================
266 template <int _SupportOptions>
267 1649274 void MinkowskiDiff::set(const ShapeBase* shape0, const ShapeBase* shape1,
268 const Transform3s& tf0, const Transform3s& tf1) {
269 1649274 shapes[0] = shape0;
270 1649274 shapes[1] = shape1;
271 1649274 getNormalizeSupportDirectionFromShapes(shape0, shape1,
272 1649274 normalize_support_direction);
273
274
3/6
✓ Branch 4 taken 824637 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 824637 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 824637 times.
✗ Branch 11 not taken.
1649274 oR1.noalias() = tf0.getRotation().transpose() * tf1.getRotation();
275
4/8
✓ Branch 3 taken 824637 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 824637 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 824637 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 824637 times.
✗ Branch 13 not taken.
1649274 ot1.noalias() = tf0.getRotation().transpose() *
276 1649274 (tf1.getTranslation() - tf0.getTranslation());
277
278
6/8
✓ Branch 2 taken 824637 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 63795 times.
✓ Branch 5 taken 760842 times.
✓ Branch 8 taken 63795 times.
✗ Branch 9 not taken.
✓ Branch 10 taken 57 times.
✓ Branch 11 taken 63738 times.
1649274 bool identity = (oR1.isIdentity() && ot1.isZero());
279
280 3298548 getSupportFunc = makeGetSupportFunction0<_SupportOptions>(
281 1649274 shape0, shape1, identity, swept_sphere_radius, data);
282 1649274 }
283 // clang-format off
284 template void COAL_DLLAPI MinkowskiDiff::set<SupportOptions::NoSweptSphere>(const ShapeBase*, const ShapeBase*);
285
286 template void COAL_DLLAPI MinkowskiDiff::set<SupportOptions::WithSweptSphere>(const ShapeBase*, const ShapeBase*);
287 // clang-format on
288
289 // ============================================================================
290 template <int _SupportOptions>
291 58417036 void MinkowskiDiff::set(const ShapeBase* shape0, const ShapeBase* shape1) {
292 58417036 shapes[0] = shape0;
293 58417036 shapes[1] = shape1;
294 58417036 getNormalizeSupportDirectionFromShapes(shape0, shape1,
295 58417036 normalize_support_direction);
296
297 58417036 oR1.setIdentity();
298 58417036 ot1.setZero();
299
300 116834072 getSupportFunc = makeGetSupportFunction0<_SupportOptions>(
301 58417036 shape0, shape1, true, swept_sphere_radius, data);
302 }
303 // clang-format off
304 template void COAL_DLLAPI MinkowskiDiff::set<SupportOptions::NoSweptSphere>(const ShapeBase*, const ShapeBase*, const Transform3s&, const Transform3s&);
305
306 template void COAL_DLLAPI MinkowskiDiff::set<SupportOptions::WithSweptSphere>(const ShapeBase*, const ShapeBase*, const Transform3s&, const Transform3s&);
307 // clang-format on
308
309 // ============================================================================
310 // clang-format off
311 template Vec3s COAL_DLLAPI MinkowskiDiff::support0<SupportOptions::NoSweptSphere>(const Vec3s&, int&) const;
312
313 template Vec3s COAL_DLLAPI MinkowskiDiff::support0<SupportOptions::WithSweptSphere>(const Vec3s&, int&) const;
314
315 template Vec3s COAL_DLLAPI MinkowskiDiff::support1<SupportOptions::NoSweptSphere>(const Vec3s&, int&) const;
316
317 template Vec3s COAL_DLLAPI MinkowskiDiff::support1<SupportOptions::WithSweptSphere>(const Vec3s&, int&) const;
318 // clang-format on
319
320 } // namespace details
321 } // namespace coal
322