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 |