GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
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) 2018-2019, Centre National de la Recherche Scientifique |
||
7 |
* All rights reserved. |
||
8 |
* Copyright (c) 2021-2022, INRIA |
||
9 |
* All rights reserved. |
||
10 |
* Redistribution and use in source and binary forms, with or without |
||
11 |
* modification, are permitted provided that the following conditions |
||
12 |
* are met: |
||
13 |
* |
||
14 |
* * Redistributions of source code must retain the above copyright |
||
15 |
* notice, this list of conditions and the following disclaimer. |
||
16 |
* * Redistributions in binary form must reproduce the above |
||
17 |
* copyright notice, this list of conditions and the following |
||
18 |
* disclaimer in the documentation and/or other materials provided |
||
19 |
* with the distribution. |
||
20 |
* * Neither the name of Open Source Robotics Foundation nor the names of its |
||
21 |
* contributors may be used to endorse or promote products derived |
||
22 |
* from this software without specific prior written permission. |
||
23 |
* |
||
24 |
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||
25 |
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||
26 |
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||
27 |
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||
28 |
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||
29 |
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||
30 |
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; |
||
31 |
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER |
||
32 |
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||
33 |
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||
34 |
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||
35 |
* POSSIBILITY OF SUCH DAMAGE. |
||
36 |
*/ |
||
37 |
|||
38 |
/** \author Jia Pan, Florent Lamiraux */ |
||
39 |
|||
40 |
#ifndef HPP_FCL_NARROWPHASE_H |
||
41 |
#define HPP_FCL_NARROWPHASE_H |
||
42 |
|||
43 |
#include <limits> |
||
44 |
#include <iostream> |
||
45 |
|||
46 |
#include <hpp/fcl/narrowphase/gjk.h> |
||
47 |
#include <hpp/fcl/collision_data.h> |
||
48 |
|||
49 |
namespace hpp { |
||
50 |
namespace fcl { |
||
51 |
|||
52 |
/// @brief collision and distance solver based on GJK algorithm implemented in |
||
53 |
/// fcl (rewritten the code from the GJK in bullet) |
||
54 |
struct HPP_FCL_DLLAPI GJKSolver { |
||
55 |
typedef Eigen::Array<FCL_REAL, 1, 2> Array2d; |
||
56 |
|||
57 |
/// @brief initialize GJK |
||
58 |
template <typename S1, typename S2> |
||
59 |
30760994 |
void initialize_gjk(details::GJK& gjk, const details::MinkowskiDiff& shape, |
|
60 |
const S1& s1, const S2& s2, Vec3f& guess, |
||
61 |
support_func_guess_t& support_hint) const { |
||
62 |
✓✗✗✗ |
30760994 |
switch (gjk_initial_guess) { |
63 |
30760994 |
case GJKInitialGuess::DefaultGuess: |
|
64 |
✓✗ | 30760994 |
guess = Vec3f(1, 0, 0); |
65 |
30760994 |
support_hint.setZero(); |
|
66 |
30760994 |
break; |
|
67 |
case GJKInitialGuess::CachedGuess: |
||
68 |
guess = cached_guess; |
||
69 |
support_hint = support_func_cached_guess; |
||
70 |
break; |
||
71 |
case GJKInitialGuess::BoundingVolumeGuess: |
||
72 |
if (s1.aabb_local.volume() < 0 || s2.aabb_local.volume() < 0) { |
||
73 |
HPP_FCL_THROW_PRETTY( |
||
74 |
"computeLocalAABB must have been called on the shapes before " |
||
75 |
"using " |
||
76 |
"GJKInitialGuess::BoundingVolumeGuess.", |
||
77 |
std::logic_error); |
||
78 |
} |
||
79 |
guess.noalias() = s1.aabb_local.center() - |
||
80 |
(shape.oR1 * s2.aabb_local.center() + shape.ot1); |
||
81 |
support_hint.setZero(); |
||
82 |
break; |
||
83 |
default: |
||
84 |
HPP_FCL_THROW_PRETTY("Wrong initial guess for GJK.", std::logic_error); |
||
85 |
} |
||
86 |
// TODO: use gjk_initial_guess instead |
||
87 |
HPP_FCL_COMPILER_DIAGNOSTIC_PUSH |
||
88 |
HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS |
||
89 |
✓✓ | 30760994 |
if (enable_cached_guess) { |
90 |
8 |
guess = cached_guess; |
|
91 |
8 |
support_hint = support_func_cached_guess; |
|
92 |
} |
||
93 |
HPP_FCL_COMPILER_DIAGNOSTIC_POP |
||
94 |
|||
95 |
30760994 |
gjk.setDistanceEarlyBreak(distance_upper_bound); |
|
96 |
|||
97 |
30760994 |
gjk.gjk_variant = gjk_variant; |
|
98 |
30760994 |
gjk.convergence_criterion = gjk_convergence_criterion; |
|
99 |
30760994 |
gjk.convergence_criterion_type = gjk_convergence_criterion_type; |
|
100 |
30760994 |
} |
|
101 |
|||
102 |
/// @brief intersection checking between two shapes |
||
103 |
template <typename S1, typename S2> |
||
104 |
8100 |
bool shapeIntersect(const S1& s1, const Transform3f& tf1, const S2& s2, |
|
105 |
const Transform3f& tf2, FCL_REAL& distance_lower_bound, |
||
106 |
bool enable_penetration, Vec3f* contact_points, |
||
107 |
Vec3f* normal) const { |
||
108 |
✓✗ | 16200 |
details::MinkowskiDiff shape; |
109 |
✓✗ | 8100 |
shape.set(&s1, &s2, tf1, tf2); |
110 |
|||
111 |
✓✗ | 8100 |
Vec3f guess; |
112 |
✓✗ | 8100 |
support_func_guess_t support_hint; |
113 |
✓✗ | 8100 |
details::GJK gjk((unsigned int)gjk_max_iterations, gjk_tolerance); |
114 |
✓✗ | 8100 |
initialize_gjk(gjk, shape, s1, s2, guess, support_hint); |
115 |
|||
116 |
✓✗ | 8100 |
details::GJK::Status gjk_status = gjk.evaluate(shape, guess, support_hint); |
117 |
HPP_FCL_COMPILER_DIAGNOSTIC_PUSH |
||
118 |
HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS |
||
119 |
✓✗ | 8100 |
if (gjk_initial_guess == GJKInitialGuess::CachedGuess || |
120 |
✗✓ | 8100 |
enable_cached_guess) { |
121 |
cached_guess = gjk.getGuessFromSimplex(); |
||
122 |
support_func_cached_guess = gjk.support_hint; |
||
123 |
} |
||
124 |
HPP_FCL_COMPILER_DIAGNOSTIC_POP |
||
125 |
|||
126 |
✓✗✓✗ |
8100 |
Vec3f w0, w1; |
127 |
✓✓✗ | 8100 |
switch (gjk_status) { |
128 |
3564 |
case details::GJK::Inside: |
|
129 |
✓✓✓✗ ✓✗ |
3564 |
if (!enable_penetration && contact_points == NULL && normal == NULL) |
130 |
3464 |
return true; |
|
131 |
✓✗✗✓ |
100 |
if (gjk.hasPenetrationInformation(shape)) { |
132 |
gjk.getClosestPoints(shape, w0, w1); |
||
133 |
distance_lower_bound = gjk.distance; |
||
134 |
if (normal) |
||
135 |
(*normal).noalias() = tf1.getRotation() * (w0 - w1).normalized(); |
||
136 |
if (contact_points) *contact_points = tf1.transform((w0 + w1) / 2); |
||
137 |
return true; |
||
138 |
} else { |
||
139 |
100 |
details::EPA epa(epa_max_face_num, epa_max_vertex_num, |
|
140 |
✓✗ | 100 |
epa_max_iterations, epa_tolerance); |
141 |
✓✗✓✗ ✓✗ |
100 |
details::EPA::Status epa_status = epa.evaluate(gjk, -guess); |
142 |
✗✓✗✗ |
100 |
if (epa_status & details::EPA::Valid || |
143 |
epa_status == details::EPA::OutOfFaces // Warnings |
||
144 |
|| epa_status == details::EPA::OutOfVertices // Warnings |
||
145 |
) { |
||
146 |
✓✗ | 100 |
epa.getClosestPoints(shape, w0, w1); |
147 |
100 |
distance_lower_bound = -epa.depth; |
|
148 |
✓✗✓✗ ✓✗✓✗ |
100 |
if (normal) (*normal).noalias() = tf1.getRotation() * epa.normal; |
149 |
✓✗ | 100 |
if (contact_points) |
150 |
✓✗✓✗ |
100 |
*contact_points = |
151 |
✓✗ | 200 |
tf1.transform(w0 - epa.normal * (epa.depth * 0.5)); |
152 |
100 |
return true; |
|
153 |
} else if (epa_status == details::EPA::FallBack) { |
||
154 |
epa.getClosestPoints(shape, w0, w1); |
||
155 |
distance_lower_bound = -epa.depth; // Should be zero |
||
156 |
if (normal) (*normal).noalias() = tf1.getRotation() * epa.normal; |
||
157 |
if (contact_points) *contact_points = tf1.transform(w0); |
||
158 |
return true; |
||
159 |
} |
||
160 |
distance_lower_bound = -(std::numeric_limits<FCL_REAL>::max)(); |
||
161 |
// EPA failed but we know there is a collision so we should |
||
162 |
return true; |
||
163 |
} |
||
164 |
break; |
||
165 |
4536 |
case details::GJK::Valid: |
|
166 |
4536 |
distance_lower_bound = gjk.distance; |
|
167 |
4536 |
break; |
|
168 |
4536 |
default:; |
|
169 |
} |
||
170 |
|||
171 |
4536 |
return false; |
|
172 |
} |
||
173 |
|||
174 |
//// @brief intersection checking between one shape and a triangle with |
||
175 |
/// transformation |
||
176 |
/// @return true if the shape are colliding. |
||
177 |
template <typename S> |
||
178 |
5342 |
bool shapeTriangleInteraction(const S& s, const Transform3f& tf1, |
|
179 |
const Vec3f& P1, const Vec3f& P2, |
||
180 |
const Vec3f& P3, const Transform3f& tf2, |
||
181 |
FCL_REAL& distance, Vec3f& p1, Vec3f& p2, |
||
182 |
Vec3f& normal) const { |
||
183 |
bool col; |
||
184 |
// Express everything in frame 1 |
||
185 |
✓✗ | 5342 |
const Transform3f tf_1M2(tf1.inverseTimes(tf2)); |
186 |
✓✗✓✗ ✓✗✓✗ |
10684 |
TriangleP tri(tf_1M2.transform(P1), tf_1M2.transform(P2), |
187 |
tf_1M2.transform(P3)); |
||
188 |
|||
189 |
✓✗ | 5342 |
details::MinkowskiDiff shape; |
190 |
✓✗ | 5342 |
shape.set(&s, &tri); |
191 |
|||
192 |
✓✗ | 5342 |
Vec3f guess; |
193 |
✓✗ | 5342 |
support_func_guess_t support_hint; |
194 |
✓✗ | 5342 |
details::GJK gjk((unsigned int)gjk_max_iterations, gjk_tolerance); |
195 |
✓✗ | 5342 |
initialize_gjk(gjk, shape, s, tri, guess, support_hint); |
196 |
|||
197 |
✓✗ | 5342 |
details::GJK::Status gjk_status = gjk.evaluate(shape, guess, support_hint); |
198 |
|||
199 |
HPP_FCL_COMPILER_DIAGNOSTIC_PUSH |
||
200 |
HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS |
||
201 |
✓✗ | 5342 |
if (gjk_initial_guess == GJKInitialGuess::CachedGuess || |
202 |
✗✓ | 5342 |
enable_cached_guess) { |
203 |
cached_guess = gjk.getGuessFromSimplex(); |
||
204 |
support_func_cached_guess = gjk.support_hint; |
||
205 |
} |
||
206 |
HPP_FCL_COMPILER_DIAGNOSTIC_PUSH |
||
207 |
|||
208 |
✓✗✓✗ |
5342 |
Vec3f w0, w1; |
209 |
✓✓✗ | 5342 |
switch (gjk_status) { |
210 |
226 |
case details::GJK::Inside: |
|
211 |
226 |
col = true; |
|
212 |
✓✗✗✓ |
226 |
if (gjk.hasPenetrationInformation(shape)) { |
213 |
gjk.getClosestPoints(shape, w0, w1); |
||
214 |
distance = gjk.distance; |
||
215 |
normal.noalias() = tf1.getRotation() * (w0 - w1).normalized(); |
||
216 |
p1 = p2 = tf1.transform((w0 + w1) / 2); |
||
217 |
} else { |
||
218 |
226 |
details::EPA epa(epa_max_face_num, epa_max_vertex_num, |
|
219 |
✓✗ | 226 |
epa_max_iterations, epa_tolerance); |
220 |
✓✗✓✗ ✓✗ |
226 |
details::EPA::Status epa_status = epa.evaluate(gjk, -guess); |
221 |
✓✓✓✗ |
226 |
if (epa_status & details::EPA::Valid || |
222 |
epa_status == details::EPA::OutOfFaces // Warnings |
||
223 |
✗✓ | 16 |
|| epa_status == details::EPA::OutOfVertices // Warnings |
224 |
) { |
||
225 |
✓✗ | 210 |
epa.getClosestPoints(shape, w0, w1); |
226 |
210 |
distance = -epa.depth; |
|
227 |
✓✗✓✗ ✓✗ |
210 |
normal.noalias() = tf1.getRotation() * epa.normal; |
228 |
✓✗✓✗ ✓✗✓✗ |
210 |
p1 = p2 = tf1.transform(w0 - epa.normal * (epa.depth * 0.5)); |
229 |
✗✓ | 210 |
assert(distance <= 1e-6); |
230 |
} else { |
||
231 |
16 |
distance = -(std::numeric_limits<FCL_REAL>::max)(); |
|
232 |
✓✗ | 16 |
gjk.getClosestPoints(shape, w0, w1); |
233 |
✓✗✓✗ |
16 |
p1 = p2 = tf1.transform(w0); |
234 |
} |
||
235 |
} |
||
236 |
226 |
break; |
|
237 |
5116 |
case details::GJK::Valid: |
|
238 |
case details::GJK::EarlyStopped: |
||
239 |
case details::GJK::Failed: |
||
240 |
5116 |
col = false; |
|
241 |
|||
242 |
✓✗ | 5116 |
gjk.getClosestPoints(shape, p1, p2); |
243 |
// TODO On degenerated case, the closest point may be wrong |
||
244 |
// (i.e. an object face normal is colinear to gjk.ray |
||
245 |
// assert (distance == (w0 - w1).norm()); |
||
246 |
5116 |
distance = gjk.distance; |
|
247 |
|||
248 |
✓✗ | 5116 |
p1 = tf1.transform(p1); |
249 |
✓✗ | 5116 |
p2 = tf1.transform(p2); |
250 |
✗✓ | 5116 |
assert(distance > 0); |
251 |
5116 |
break; |
|
252 |
default: |
||
253 |
assert(false && "should not reach type part."); |
||
254 |
throw std::logic_error("GJKSolver: should not reach this part."); |
||
255 |
} |
||
256 |
10684 |
return col; |
|
257 |
} |
||
258 |
|||
259 |
/// @brief distance computation between two shapes |
||
260 |
template <typename S1, typename S2> |
||
261 |
827671 |
bool shapeDistance(const S1& s1, const Transform3f& tf1, const S2& s2, |
|
262 |
const Transform3f& tf2, FCL_REAL& distance, Vec3f& p1, |
||
263 |
Vec3f& p2, Vec3f& normal) const { |
||
264 |
#ifndef NDEBUG |
||
265 |
827671 |
FCL_REAL eps(sqrt(std::numeric_limits<FCL_REAL>::epsilon())); |
|
266 |
#endif |
||
267 |
✓✗ | 1655342 |
details::MinkowskiDiff shape; |
268 |
✓✗ | 827671 |
shape.set(&s1, &s2, tf1, tf2); |
269 |
|||
270 |
✓✗ | 827671 |
Vec3f guess; |
271 |
✓✗ | 827671 |
support_func_guess_t support_hint; |
272 |
✓✗ | 827671 |
details::GJK gjk((unsigned int)gjk_max_iterations, gjk_tolerance); |
273 |
✓✗ | 827671 |
initialize_gjk(gjk, shape, s1, s2, guess, support_hint); |
274 |
|||
275 |
✓✗ | 827671 |
details::GJK::Status gjk_status = gjk.evaluate(shape, guess, support_hint); |
276 |
✓✗ | 827671 |
if (gjk_initial_guess == GJKInitialGuess::CachedGuess || |
277 |
✗✓ | 827671 |
enable_cached_guess) { |
278 |
✗✗ | 4 |
cached_guess = gjk.getGuessFromSimplex(); |
279 |
✗✗ | 4 |
support_func_cached_guess = gjk.support_hint; |
280 |
} |
||
281 |
|||
282 |
✗✓ | 827671 |
if (gjk_status == details::GJK::Failed) { |
283 |
// TODO: understand why GJK fails between cylinder and box |
||
284 |
assert(distance * distance < sqrt(eps)); |
||
285 |
Vec3f w0, w1; |
||
286 |
gjk.getClosestPoints(shape, w0, w1); |
||
287 |
distance = 0; |
||
288 |
p1 = tf1.transform(w0); |
||
289 |
p2 = tf1.transform(w1); |
||
290 |
normal.setZero(); |
||
291 |
return false; |
||
292 |
✓✓ | 827671 |
} else if (gjk_status == details::GJK::Valid) { |
293 |
✓✗ | 814928 |
gjk.getClosestPoints(shape, p1, p2); |
294 |
// TODO On degenerated case, the closest point may be wrong |
||
295 |
// (i.e. an object face normal is colinear to gjk.ray |
||
296 |
// assert (distance == (w0 - w1).norm()); |
||
297 |
814928 |
distance = gjk.distance; |
|
298 |
|||
299 |
✓✗✓✗ ✓✗ |
814928 |
normal.noalias() = tf1.getRotation() * gjk.ray; |
300 |
✓✗ | 814928 |
normal.normalize(); |
301 |
✓✗ | 814928 |
p1 = tf1.transform(p1); |
302 |
✓✗ | 814928 |
p2 = tf1.transform(p2); |
303 |
814928 |
return true; |
|
304 |
✗✓ | 12743 |
} else if (gjk_status == details::GJK::EarlyStopped) { |
305 |
2 |
distance = gjk.distance; |
|
306 |
✗✗✗✗ ✗✗ |
2 |
p1 = p2 = normal = |
307 |
✗✗ | 2 |
Vec3f::Constant(std::numeric_limits<FCL_REAL>::quiet_NaN()); |
308 |
2 |
return true; |
|
309 |
} else { |
||
310 |
✗✓ | 12741 |
assert(gjk_status == details::GJK::Inside); |
311 |
✓✗✓✓ |
12741 |
if (gjk.hasPenetrationInformation(shape)) { |
312 |
✓✗ | 12296 |
gjk.getClosestPoints(shape, p1, p2); |
313 |
12296 |
distance = gjk.distance; |
|
314 |
// Return contact points in case of collision |
||
315 |
// p1 = tf1.transform (p1); |
||
316 |
// p2 = tf1.transform (p2); |
||
317 |
✓✗✓✗ ✓✗✓✗ |
12296 |
normal.noalias() = tf1.getRotation() * (p1 - p2); |
318 |
✓✗ | 12296 |
normal.normalize(); |
319 |
✓✗ | 12296 |
p1 = tf1.transform(p1); |
320 |
✓✗ | 12296 |
p2 = tf1.transform(p2); |
321 |
} else { |
||
322 |
details::EPA epa(epa_max_face_num, epa_max_vertex_num, |
||
323 |
✓✗ | 445 |
epa_max_iterations, epa_tolerance); |
324 |
✓✗✓✗ ✓✗ |
445 |
details::EPA::Status epa_status = epa.evaluate(gjk, -guess); |
325 |
✓✓✓✓ |
445 |
if (epa_status & details::EPA::Valid || |
326 |
epa_status == details::EPA::OutOfFaces // Warnings |
||
327 |
✓✗ | 6 |
|| epa_status == details::EPA::OutOfVertices // Warnings |
328 |
✓✗ | 6 |
|| epa_status == details::EPA::FallBack) { |
329 |
✓✗✓✗ |
443 |
Vec3f w0, w1; |
330 |
✓✗ | 443 |
epa.getClosestPoints(shape, w0, w1); |
331 |
✗✓ | 443 |
assert(epa.depth >= -eps); |
332 |
443 |
distance = (std::min)(0., -epa.depth); |
|
333 |
✓✗✓✗ ✓✗ |
443 |
normal.noalias() = tf1.getRotation() * epa.normal; |
334 |
✓✗ | 443 |
p1 = tf1.transform(w0); |
335 |
✓✗ | 443 |
p2 = tf1.transform(w1); |
336 |
443 |
return false; |
|
337 |
} |
||
338 |
2 |
distance = -(std::numeric_limits<FCL_REAL>::max)(); |
|
339 |
✗✗ | 2 |
gjk.getClosestPoints(shape, p1, p2); |
340 |
✗✗ | 2 |
p1 = tf1.transform(p1); |
341 |
✗✗ | 2 |
p2 = tf1.transform(p2); |
342 |
} |
||
343 |
12298 |
return false; |
|
344 |
} |
||
345 |
} |
||
346 |
|||
347 |
HPP_FCL_COMPILER_DIAGNOSTIC_PUSH |
||
348 |
HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS |
||
349 |
/// @brief Default constructor for GJK algorithm |
||
350 |
29180286 |
GJKSolver() { |
|
351 |
29180286 |
gjk_max_iterations = 128; |
|
352 |
29180286 |
gjk_tolerance = 1e-6; |
|
353 |
29180286 |
epa_max_face_num = 128; |
|
354 |
29180286 |
epa_max_vertex_num = 64; |
|
355 |
29180286 |
epa_max_iterations = 255; |
|
356 |
29180286 |
epa_tolerance = 1e-6; |
|
357 |
29180286 |
enable_cached_guess = false; // TODO: use gjk_initial_guess instead |
|
358 |
✓✗ | 29180286 |
cached_guess = Vec3f(1, 0, 0); |
359 |
✓✗ | 29180286 |
support_func_cached_guess = support_func_guess_t::Zero(); |
360 |
29180286 |
distance_upper_bound = (std::numeric_limits<FCL_REAL>::max)(); |
|
361 |
29180286 |
gjk_initial_guess = GJKInitialGuess::DefaultGuess; |
|
362 |
29180286 |
gjk_variant = GJKVariant::DefaultGJK; |
|
363 |
29180286 |
gjk_convergence_criterion = GJKConvergenceCriterion::VDB; |
|
364 |
29180286 |
gjk_convergence_criterion_type = GJKConvergenceCriterionType::Relative; |
|
365 |
29180286 |
} |
|
366 |
|||
367 |
/// @brief Constructor from a DistanceRequest |
||
368 |
/// |
||
369 |
/// \param[in] request DistanceRequest input |
||
370 |
/// |
||
371 |
21454 |
GJKSolver(const DistanceRequest& request) { |
|
372 |
✓✗ | 21454 |
cached_guess = Vec3f(1, 0, 0); |
373 |
✓✗ | 21454 |
support_func_cached_guess = support_func_guess_t::Zero(); |
374 |
21454 |
distance_upper_bound = (std::numeric_limits<FCL_REAL>::max)(); |
|
375 |
|||
376 |
// EPS settings |
||
377 |
21454 |
epa_max_face_num = 128; |
|
378 |
21454 |
epa_max_vertex_num = 64; |
|
379 |
21454 |
epa_max_iterations = 255; |
|
380 |
21454 |
epa_tolerance = 1e-6; |
|
381 |
|||
382 |
21454 |
set(request); |
|
383 |
21454 |
} |
|
384 |
|||
385 |
/// @brief setter from a DistanceRequest |
||
386 |
/// |
||
387 |
/// \param[in] request DistanceRequest input |
||
388 |
/// |
||
389 |
21454 |
void set(const DistanceRequest& request) { |
|
390 |
21454 |
gjk_initial_guess = request.gjk_initial_guess; |
|
391 |
// TODO: use gjk_initial_guess instead |
||
392 |
21454 |
enable_cached_guess = request.enable_cached_gjk_guess; |
|
393 |
21454 |
gjk_variant = request.gjk_variant; |
|
394 |
21454 |
gjk_convergence_criterion = request.gjk_convergence_criterion; |
|
395 |
21454 |
gjk_convergence_criterion_type = request.gjk_convergence_criterion_type; |
|
396 |
21454 |
gjk_tolerance = request.gjk_tolerance; |
|
397 |
21454 |
gjk_max_iterations = request.gjk_max_iterations; |
|
398 |
✓✗ | 21454 |
if (gjk_initial_guess == GJKInitialGuess::CachedGuess || |
399 |
✗✓ | 21454 |
enable_cached_guess) { |
400 |
cached_guess = request.cached_gjk_guess; |
||
401 |
support_func_cached_guess = request.cached_support_func_guess; |
||
402 |
} |
||
403 |
21454 |
} |
|
404 |
|||
405 |
/// @brief Constructor from a CollisionRequest |
||
406 |
/// |
||
407 |
/// \param[in] request CollisionRequest input |
||
408 |
/// |
||
409 |
1235472 |
GJKSolver(const CollisionRequest& request) { |
|
410 |
✓✗ | 1235472 |
cached_guess = Vec3f(1, 0, 0); |
411 |
✓✗ | 1235472 |
support_func_cached_guess = support_func_guess_t::Zero(); |
412 |
1235472 |
distance_upper_bound = (std::numeric_limits<FCL_REAL>::max)(); |
|
413 |
|||
414 |
// EPS settings |
||
415 |
1235472 |
epa_max_face_num = 128; |
|
416 |
1235472 |
epa_max_vertex_num = 64; |
|
417 |
1235472 |
epa_max_iterations = 255; |
|
418 |
1235472 |
epa_tolerance = 1e-6; |
|
419 |
|||
420 |
1235472 |
set(request); |
|
421 |
1235472 |
} |
|
422 |
|||
423 |
/// @brief setter from a CollisionRequest |
||
424 |
/// |
||
425 |
/// \param[in] request CollisionRequest input |
||
426 |
/// |
||
427 |
1235474 |
void set(const CollisionRequest& request) { |
|
428 |
1235474 |
gjk_initial_guess = request.gjk_initial_guess; |
|
429 |
// TODO: use gjk_initial_guess instead |
||
430 |
1235474 |
enable_cached_guess = request.enable_cached_gjk_guess; |
|
431 |
1235474 |
gjk_variant = request.gjk_variant; |
|
432 |
1235474 |
gjk_convergence_criterion = request.gjk_convergence_criterion; |
|
433 |
1235474 |
gjk_convergence_criterion_type = request.gjk_convergence_criterion_type; |
|
434 |
1235474 |
gjk_tolerance = request.gjk_tolerance; |
|
435 |
1235474 |
gjk_max_iterations = request.gjk_max_iterations; |
|
436 |
✓✗ | 1235474 |
if (gjk_initial_guess == GJKInitialGuess::CachedGuess || |
437 |
✓✓ | 1235474 |
enable_cached_guess) { |
438 |
4 |
cached_guess = request.cached_gjk_guess; |
|
439 |
4 |
support_func_cached_guess = request.cached_support_func_guess; |
|
440 |
} |
||
441 |
|||
442 |
// The distance upper bound should be at least greater to the requested |
||
443 |
// security margin. Otherwise, we will likely miss some collisions. |
||
444 |
1235474 |
distance_upper_bound = (std::max)( |
|
445 |
1235474 |
0., (std::max)(request.distance_upper_bound, request.security_margin)); |
|
446 |
1235474 |
} |
|
447 |
|||
448 |
/// @brief Copy constructor |
||
449 |
GJKSolver(const GJKSolver& other) = default; |
||
450 |
|||
451 |
HPP_FCL_COMPILER_DIAGNOSTIC_PUSH |
||
452 |
HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS |
||
453 |
bool operator==(const GJKSolver& other) const { |
||
454 |
return epa_max_face_num == other.epa_max_face_num && |
||
455 |
epa_max_vertex_num == other.epa_max_vertex_num && |
||
456 |
epa_max_iterations == other.epa_max_iterations && |
||
457 |
epa_tolerance == other.epa_tolerance && |
||
458 |
gjk_max_iterations == other.gjk_max_iterations && |
||
459 |
enable_cached_guess == |
||
460 |
other.enable_cached_guess && // TODO: use gjk_initial_guess |
||
461 |
// instead |
||
462 |
cached_guess == other.cached_guess && |
||
463 |
support_func_cached_guess == other.support_func_cached_guess && |
||
464 |
distance_upper_bound == other.distance_upper_bound && |
||
465 |
gjk_initial_guess == other.gjk_initial_guess && |
||
466 |
gjk_variant == other.gjk_variant && |
||
467 |
gjk_convergence_criterion == other.gjk_convergence_criterion && |
||
468 |
gjk_convergence_criterion_type == |
||
469 |
other.gjk_convergence_criterion_type; |
||
470 |
} |
||
471 |
HPP_FCL_COMPILER_DIAGNOSTIC_POP |
||
472 |
|||
473 |
bool operator!=(const GJKSolver& other) const { return !(*this == other); } |
||
474 |
|||
475 |
/// @brief maximum number of simplex face used in EPA algorithm |
||
476 |
unsigned int epa_max_face_num; |
||
477 |
|||
478 |
/// @brief maximum number of simplex vertex used in EPA algorithm |
||
479 |
unsigned int epa_max_vertex_num; |
||
480 |
|||
481 |
/// @brief maximum number of iterations used for EPA iterations |
||
482 |
unsigned int epa_max_iterations; |
||
483 |
|||
484 |
/// @brief the threshold used in EPA to stop iteration |
||
485 |
FCL_REAL epa_tolerance; |
||
486 |
|||
487 |
/// @brief the threshold used in GJK to stop iteration |
||
488 |
mutable FCL_REAL gjk_tolerance; |
||
489 |
|||
490 |
/// @brief maximum number of iterations used for GJK iterations |
||
491 |
mutable size_t gjk_max_iterations; |
||
492 |
|||
493 |
/// @brief Whether smart guess can be provided |
||
494 |
/// @Deprecated Use gjk_initial_guess instead |
||
495 |
HPP_FCL_DEPRECATED_MESSAGE("Use gjk_initial_guess instead") |
||
496 |
mutable bool enable_cached_guess; |
||
497 |
|||
498 |
/// @brief smart guess |
||
499 |
mutable Vec3f cached_guess; |
||
500 |
|||
501 |
/// @brief which warm start to use for GJK |
||
502 |
mutable GJKInitialGuess gjk_initial_guess; |
||
503 |
|||
504 |
/// @brief Variant to use for the GJK algorithm |
||
505 |
mutable GJKVariant gjk_variant; |
||
506 |
|||
507 |
/// @brief Criterion used to stop GJK |
||
508 |
mutable GJKConvergenceCriterion gjk_convergence_criterion; |
||
509 |
|||
510 |
/// @brief Relative or absolute |
||
511 |
mutable GJKConvergenceCriterionType gjk_convergence_criterion_type; |
||
512 |
|||
513 |
/// @brief smart guess for the support function |
||
514 |
mutable support_func_guess_t support_func_cached_guess; |
||
515 |
|||
516 |
/// @brief Distance above which the GJK solver stoppes its computations and |
||
517 |
/// processes to an early stopping. |
||
518 |
/// The two witness points are incorrect, but with the guaranty that |
||
519 |
/// the two shapes have a distance greather than distance_upper_bound. |
||
520 |
mutable FCL_REAL distance_upper_bound; |
||
521 |
|||
522 |
public: |
||
523 |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
||
524 |
}; |
||
525 |
|||
526 |
template <> |
||
527 |
HPP_FCL_DLLAPI bool GJKSolver::shapeTriangleInteraction( |
||
528 |
const Sphere& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2, |
||
529 |
const Vec3f& P3, const Transform3f& tf2, FCL_REAL& distance, Vec3f& p1, |
||
530 |
Vec3f& p2, Vec3f& normal) const; |
||
531 |
|||
532 |
template <> |
||
533 |
HPP_FCL_DLLAPI bool GJKSolver::shapeTriangleInteraction( |
||
534 |
const Halfspace& s, const Transform3f& tf1, const Vec3f& P1, |
||
535 |
const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, |
||
536 |
FCL_REAL& distance, Vec3f& p1, Vec3f& p2, Vec3f& normal) const; |
||
537 |
|||
538 |
template <> |
||
539 |
HPP_FCL_DLLAPI bool GJKSolver::shapeTriangleInteraction( |
||
540 |
const Plane& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2, |
||
541 |
const Vec3f& P3, const Transform3f& tf2, FCL_REAL& distance, Vec3f& p1, |
||
542 |
Vec3f& p2, Vec3f& normal) const; |
||
543 |
|||
544 |
#define SHAPE_INTERSECT_SPECIALIZATION_BASE(S1, S2) \ |
||
545 |
template <> \ |
||
546 |
HPP_FCL_DLLAPI bool GJKSolver::shapeIntersect<S1, S2>( \ |
||
547 |
const S1& s1, const Transform3f& tf1, const S2& s2, \ |
||
548 |
const Transform3f& tf2, FCL_REAL& distance_lower_bound, bool, \ |
||
549 |
Vec3f* contact_points, Vec3f* normal) const |
||
550 |
|||
551 |
#define SHAPE_INTERSECT_SPECIALIZATION(S1, S2) \ |
||
552 |
SHAPE_INTERSECT_SPECIALIZATION_BASE(S1, S2); \ |
||
553 |
SHAPE_INTERSECT_SPECIALIZATION_BASE(S2, S1) |
||
554 |
|||
555 |
SHAPE_INTERSECT_SPECIALIZATION(Sphere, Capsule); |
||
556 |
SHAPE_INTERSECT_SPECIALIZATION_BASE(Sphere, Sphere); |
||
557 |
SHAPE_INTERSECT_SPECIALIZATION(Sphere, Box); |
||
558 |
SHAPE_INTERSECT_SPECIALIZATION(Sphere, Halfspace); |
||
559 |
SHAPE_INTERSECT_SPECIALIZATION(Sphere, Plane); |
||
560 |
|||
561 |
SHAPE_INTERSECT_SPECIALIZATION(Halfspace, Box); |
||
562 |
SHAPE_INTERSECT_SPECIALIZATION(Halfspace, Capsule); |
||
563 |
SHAPE_INTERSECT_SPECIALIZATION(Halfspace, Cylinder); |
||
564 |
SHAPE_INTERSECT_SPECIALIZATION(Halfspace, Cone); |
||
565 |
SHAPE_INTERSECT_SPECIALIZATION(Halfspace, Plane); |
||
566 |
|||
567 |
SHAPE_INTERSECT_SPECIALIZATION(Plane, Box); |
||
568 |
SHAPE_INTERSECT_SPECIALIZATION(Plane, Capsule); |
||
569 |
SHAPE_INTERSECT_SPECIALIZATION(Plane, Cylinder); |
||
570 |
SHAPE_INTERSECT_SPECIALIZATION(Plane, Cone); |
||
571 |
|||
572 |
#undef SHAPE_INTERSECT_SPECIALIZATION |
||
573 |
#undef SHAPE_INTERSECT_SPECIALIZATION_BASE |
||
574 |
|||
575 |
#define SHAPE_DISTANCE_SPECIALIZATION_BASE(S1, S2) \ |
||
576 |
template <> \ |
||
577 |
HPP_FCL_DLLAPI bool GJKSolver::shapeDistance<S1, S2>( \ |
||
578 |
const S1& s1, const Transform3f& tf1, const S2& s2, \ |
||
579 |
const Transform3f& tf2, FCL_REAL& dist, Vec3f& p1, Vec3f& p2, \ |
||
580 |
Vec3f& normal) const |
||
581 |
|||
582 |
#define SHAPE_DISTANCE_SPECIALIZATION(S1, S2) \ |
||
583 |
SHAPE_DISTANCE_SPECIALIZATION_BASE(S1, S2); \ |
||
584 |
SHAPE_DISTANCE_SPECIALIZATION_BASE(S2, S1) |
||
585 |
|||
586 |
SHAPE_DISTANCE_SPECIALIZATION(Sphere, Capsule); |
||
587 |
SHAPE_DISTANCE_SPECIALIZATION(Sphere, Box); |
||
588 |
SHAPE_DISTANCE_SPECIALIZATION(Sphere, Cylinder); |
||
589 |
SHAPE_DISTANCE_SPECIALIZATION_BASE(Sphere, Sphere); |
||
590 |
SHAPE_DISTANCE_SPECIALIZATION_BASE(Capsule, Capsule); |
||
591 |
SHAPE_DISTANCE_SPECIALIZATION_BASE(TriangleP, TriangleP); |
||
592 |
|||
593 |
#undef SHAPE_DISTANCE_SPECIALIZATION |
||
594 |
#undef SHAPE_DISTANCE_SPECIALIZATION_BASE |
||
595 |
|||
596 |
#if !(__cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1600)) |
||
597 |
#pragma GCC diagnostic push |
||
598 |
#pragma GCC diagnostic ignored "-Wc99-extensions" |
||
599 |
#endif |
||
600 |
/// \name Shape intersection specializations |
||
601 |
/// \{ |
||
602 |
|||
603 |
// param doc is the doxygen detailled description (should be enclosed in /** */ |
||
604 |
// and contain no dot for some obscure reasons). |
||
605 |
#define HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape1, Shape2, doc) \ |
||
606 |
/** @brief Fast implementation for Shape1-Shape2 collision. */ \ |
||
607 |
doc template <> \ |
||
608 |
HPP_FCL_DLLAPI bool GJKSolver::shapeIntersect<Shape1, Shape2>( \ |
||
609 |
const Shape1& s1, const Transform3f& tf1, const Shape2& s2, \ |
||
610 |
const Transform3f& tf2, FCL_REAL& distance_lower_bound, \ |
||
611 |
bool enable_penetration, Vec3f* contact_points, Vec3f* normal) const |
||
612 |
#define HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Shape, doc) \ |
||
613 |
HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape, Shape, doc) |
||
614 |
#define HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Shape1, Shape2, doc) \ |
||
615 |
HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape1, Shape2, doc); \ |
||
616 |
HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape2, Shape1, doc) |
||
617 |
|||
618 |
HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Sphere, ); |
||
619 |
HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Sphere, Capsule, ); |
||
620 |
HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Sphere, Halfspace, ); |
||
621 |
HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Sphere, Plane, ); |
||
622 |
|||
623 |
template <> |
||
624 |
HPP_FCL_DLLAPI bool GJKSolver::shapeIntersect<Box, Sphere>( |
||
625 |
const Box& s1, const Transform3f& tf1, const Sphere& s2, |
||
626 |
const Transform3f& tf2, FCL_REAL& distance_lower_bound, |
||
627 |
bool enable_penetration, Vec3f* contact_points, Vec3f* normal) const; |
||
628 |
|||
629 |
#ifdef IS_DOXYGEN // for doxygen only |
||
630 |
/** \todo currently disabled and to re-enable it, API of function |
||
631 |
* \ref obbDisjointAndLowerBoundDistance should be modified. |
||
632 |
* */ |
||
633 |
template <> |
||
634 |
HPP_FCL_DLLAPI bool GJKSolver::shapeIntersect<Box, Box>( |
||
635 |
const Box& s1, const Transform3f& tf1, const Box& s2, |
||
636 |
const Transform3f& tf2, FCL_REAL& distance_lower_bound, |
||
637 |
bool enable_penetration, Vec3f* contact_points, Vec3f* normal) const; |
||
638 |
#endif |
||
639 |
// HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Box,); |
||
640 |
HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Box, Halfspace, ); |
||
641 |
HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Box, Plane, ); |
||
642 |
|||
643 |
HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Capsule, Halfspace, ); |
||
644 |
HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Capsule, Plane, ); |
||
645 |
|||
646 |
HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Cylinder, Halfspace, ); |
||
647 |
HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Cylinder, Plane, ); |
||
648 |
|||
649 |
HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Cone, Halfspace, ); |
||
650 |
HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Cone, Plane, ); |
||
651 |
|||
652 |
HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Halfspace, ); |
||
653 |
|||
654 |
HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Plane, ); |
||
655 |
HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Plane, Halfspace, ); |
||
656 |
|||
657 |
#undef HPP_FCL_DECLARE_SHAPE_INTERSECT |
||
658 |
#undef HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF |
||
659 |
#undef HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR |
||
660 |
|||
661 |
/// \} |
||
662 |
|||
663 |
/// \name Shape triangle interaction specializations |
||
664 |
/// \{ |
||
665 |
|||
666 |
// param doc is the doxygen detailled description (should be enclosed in /** */ |
||
667 |
// and contain no dot for some obscure reasons). |
||
668 |
#define HPP_FCL_DECLARE_SHAPE_TRIANGLE(Shape, doc) \ |
||
669 |
/** @brief Fast implementation for Shape-Triangle interaction. */ \ |
||
670 |
doc template <> \ |
||
671 |
HPP_FCL_DLLAPI bool GJKSolver::shapeTriangleInteraction<Shape>( \ |
||
672 |
const Shape& s, const Transform3f& tf1, const Vec3f& P1, \ |
||
673 |
const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, \ |
||
674 |
FCL_REAL& distance, Vec3f& p1, Vec3f& p2, Vec3f& normal) const |
||
675 |
|||
676 |
HPP_FCL_DECLARE_SHAPE_TRIANGLE(Sphere, ); |
||
677 |
HPP_FCL_DECLARE_SHAPE_TRIANGLE(Halfspace, ); |
||
678 |
HPP_FCL_DECLARE_SHAPE_TRIANGLE(Plane, ); |
||
679 |
|||
680 |
#undef HPP_FCL_DECLARE_SHAPE_TRIANGLE |
||
681 |
|||
682 |
/// \} |
||
683 |
|||
684 |
/// \name Shape distance specializations |
||
685 |
/// \{ |
||
686 |
|||
687 |
// param doc is the doxygen detailled description (should be enclosed in /** */ |
||
688 |
// and contain no dot for some obscure reasons). |
||
689 |
#define HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape1, Shape2, doc) \ |
||
690 |
/** @brief Fast implementation for Shape1-Shape2 distance. */ \ |
||
691 |
doc template <> \ |
||
692 |
bool HPP_FCL_DLLAPI GJKSolver::shapeDistance<Shape1, Shape2>( \ |
||
693 |
const Shape1& s1, const Transform3f& tf1, const Shape2& s2, \ |
||
694 |
const Transform3f& tf2, FCL_REAL& dist, Vec3f& p1, Vec3f& p2, \ |
||
695 |
Vec3f& normal) const |
||
696 |
#define HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF(Shape, doc) \ |
||
697 |
HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape, Shape, doc) |
||
698 |
#define HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR(Shape1, Shape2, doc) \ |
||
699 |
HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape1, Shape2, doc); \ |
||
700 |
HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape2, Shape1, doc) |
||
701 |
|||
702 |
HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR(Sphere, Box, ); |
||
703 |
HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR(Sphere, Capsule, ); |
||
704 |
HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR(Sphere, Cylinder, ); |
||
705 |
HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF(Sphere, ); |
||
706 |
|||
707 |
HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF( |
||
708 |
Capsule, |
||
709 |
/** Closest points are based on two line-segments. */ |
||
710 |
); |
||
711 |
|||
712 |
HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF( |
||
713 |
TriangleP, |
||
714 |
/** Do not run EPA algorithm to compute penetration depth. Use a dedicated |
||
715 |
method. */ |
||
716 |
); |
||
717 |
|||
718 |
#undef HPP_FCL_DECLARE_SHAPE_DISTANCE |
||
719 |
#undef HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF |
||
720 |
#undef HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR |
||
721 |
|||
722 |
/// \} |
||
723 |
#if !(__cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1600)) |
||
724 |
#pragma GCC diagnostic pop |
||
725 |
#endif |
||
726 |
} // namespace fcl |
||
727 |
|||
728 |
} // namespace hpp |
||
729 |
|||
730 |
#endif |
Generated by: GCOVR (Version 4.2) |