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 |
* All rights reserved. |
||
7 |
* |
||
8 |
* Redistribution and use in source and binary forms, with or without |
||
9 |
* modification, are permitted provided that the following conditions |
||
10 |
* are met: |
||
11 |
* |
||
12 |
* * Redistributions of source code must retain the above copyright |
||
13 |
* notice, this list of conditions and the following disclaimer. |
||
14 |
* * Redistributions in binary form must reproduce the above |
||
15 |
* copyright notice, this list of conditions and the following |
||
16 |
* disclaimer in the documentation and/or other materials provided |
||
17 |
* with the distribution. |
||
18 |
* * Neither the name of Open Source Robotics Foundation nor the names of its |
||
19 |
* contributors may be used to endorse or promote products derived |
||
20 |
* from this software without specific prior written permission. |
||
21 |
* |
||
22 |
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||
23 |
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||
24 |
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||
25 |
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||
26 |
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||
27 |
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||
28 |
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; |
||
29 |
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER |
||
30 |
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||
31 |
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||
32 |
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||
33 |
* POSSIBILITY OF SUCH DAMAGE. |
||
34 |
*/ |
||
35 |
|||
36 |
/** \author Jia Pan */ |
||
37 |
|||
38 |
#ifndef HPP_FCL_COLLISION_DATA_H |
||
39 |
#define HPP_FCL_COLLISION_DATA_H |
||
40 |
|||
41 |
#include <vector> |
||
42 |
#include <set> |
||
43 |
#include <limits> |
||
44 |
|||
45 |
#include <hpp/fcl/collision_object.h> |
||
46 |
#include <hpp/fcl/config.hh> |
||
47 |
#include <hpp/fcl/data_types.h> |
||
48 |
#include <hpp/fcl/timings.h> |
||
49 |
|||
50 |
namespace hpp { |
||
51 |
namespace fcl { |
||
52 |
|||
53 |
/// @brief Contact information returned by collision |
||
54 |
struct HPP_FCL_DLLAPI Contact { |
||
55 |
/// @brief collision object 1 |
||
56 |
const CollisionGeometry* o1; |
||
57 |
|||
58 |
/// @brief collision object 2 |
||
59 |
const CollisionGeometry* o2; |
||
60 |
|||
61 |
/// @brief contact primitive in object 1 |
||
62 |
/// if object 1 is mesh or point cloud, it is the triangle or point id |
||
63 |
/// if object 1 is geometry shape, it is NONE (-1), |
||
64 |
/// if object 1 is octree, it is the id of the cell |
||
65 |
int b1; |
||
66 |
|||
67 |
/// @brief contact primitive in object 2 |
||
68 |
/// if object 2 is mesh or point cloud, it is the triangle or point id |
||
69 |
/// if object 2 is geometry shape, it is NONE (-1), |
||
70 |
/// if object 2 is octree, it is the id of the cell |
||
71 |
int b2; |
||
72 |
|||
73 |
/// @brief contact normal, pointing from o1 to o2 |
||
74 |
Vec3f normal; |
||
75 |
|||
76 |
/// @brief contact position, in world space |
||
77 |
Vec3f pos; |
||
78 |
|||
79 |
/// @brief penetration depth |
||
80 |
FCL_REAL penetration_depth; |
||
81 |
|||
82 |
/// @brief invalid contact primitive information |
||
83 |
static const int NONE = -1; |
||
84 |
|||
85 |
/// @brief Default constructor |
||
86 |
7 |
Contact() : o1(NULL), o2(NULL), b1(NONE), b2(NONE) {} |
|
87 |
|||
88 |
Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, |
||
89 |
int b2_) |
||
90 |
: o1(o1_), o2(o2_), b1(b1_), b2(b2_) {} |
||
91 |
|||
92 |
19642 |
Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, |
|
93 |
int b2_, const Vec3f& pos_, const Vec3f& normal_, FCL_REAL depth_) |
||
94 |
19642 |
: o1(o1_), |
|
95 |
o2(o2_), |
||
96 |
b1(b1_), |
||
97 |
b2(b2_), |
||
98 |
normal(normal_), |
||
99 |
pos(pos_), |
||
100 |
19642 |
penetration_depth(depth_) {} |
|
101 |
|||
102 |
bool operator<(const Contact& other) const { |
||
103 |
if (b1 == other.b1) return b2 < other.b2; |
||
104 |
return b1 < other.b1; |
||
105 |
} |
||
106 |
|||
107 |
18 |
bool operator==(const Contact& other) const { |
|
108 |
✓✗✓✗ |
18 |
return o1 == other.o1 && o2 == other.o2 && b1 == other.b1 && |
109 |
✓✗✓✗ ✓✗✓✗ |
54 |
b2 == other.b2 && normal == other.normal && pos == other.pos && |
110 |
✓✗ | 36 |
penetration_depth == other.penetration_depth; |
111 |
} |
||
112 |
|||
113 |
bool operator!=(const Contact& other) const { return !(*this == other); } |
||
114 |
}; |
||
115 |
|||
116 |
struct QueryResult; |
||
117 |
|||
118 |
/// @brief base class for all query requests |
||
119 |
722669 |
struct HPP_FCL_DLLAPI QueryRequest { |
|
120 |
// @briefInitial guess to use for the GJK algorithm |
||
121 |
GJKInitialGuess gjk_initial_guess; |
||
122 |
|||
123 |
/// @brief whether enable gjk initial guess |
||
124 |
/// @Deprecated Use gjk_initial_guess instead |
||
125 |
HPP_FCL_DEPRECATED_MESSAGE("Use gjk_initial_guess instead") |
||
126 |
bool enable_cached_gjk_guess; |
||
127 |
|||
128 |
/// @brief whether to enable the Nesterov accleration of GJK |
||
129 |
GJKVariant gjk_variant; |
||
130 |
|||
131 |
/// @brief convergence criterion used to stop GJK |
||
132 |
GJKConvergenceCriterion gjk_convergence_criterion; |
||
133 |
|||
134 |
/// @brief convergence criterion used to stop GJK |
||
135 |
GJKConvergenceCriterionType gjk_convergence_criterion_type; |
||
136 |
|||
137 |
/// @brief tolerance for the GJK algorithm |
||
138 |
FCL_REAL gjk_tolerance; |
||
139 |
|||
140 |
/// @brief maximum iteration for the GJK algorithm |
||
141 |
size_t gjk_max_iterations; |
||
142 |
|||
143 |
/// @brief the gjk initial guess set by user |
||
144 |
Vec3f cached_gjk_guess; |
||
145 |
|||
146 |
/// @brief the support function initial guess set by user |
||
147 |
support_func_guess_t cached_support_func_guess; |
||
148 |
|||
149 |
/// @brief enable timings when performing collision/distance request |
||
150 |
bool enable_timings; |
||
151 |
|||
152 |
/// @brief threshold below which a collision is considered. |
||
153 |
FCL_REAL collision_distance_threshold; |
||
154 |
|||
155 |
HPP_FCL_COMPILER_DIAGNOSTIC_PUSH |
||
156 |
HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS |
||
157 |
/// @brief Default constructor. |
||
158 |
1940429 |
QueryRequest() |
|
159 |
1940429 |
: gjk_initial_guess(GJKInitialGuess::DefaultGuess), |
|
160 |
enable_cached_gjk_guess(false), |
||
161 |
gjk_variant(GJKVariant::DefaultGJK), |
||
162 |
gjk_convergence_criterion(GJKConvergenceCriterion::VDB), |
||
163 |
gjk_convergence_criterion_type(GJKConvergenceCriterionType::Relative), |
||
164 |
gjk_tolerance(1e-6), |
||
165 |
gjk_max_iterations(128), |
||
166 |
cached_gjk_guess(1, 0, 0), |
||
167 |
3880858 |
cached_support_func_guess(support_func_guess_t::Zero()), |
|
168 |
enable_timings(false), |
||
169 |
collision_distance_threshold( |
||
170 |
✓✗✓✗ |
1940429 |
Eigen::NumTraits<FCL_REAL>::dummy_precision()) {} |
171 |
|||
172 |
/// @brief Copy constructor. |
||
173 |
101 |
QueryRequest(const QueryRequest& other) = default; |
|
174 |
|||
175 |
/// @brief Copy assignment operator. |
||
176 |
QueryRequest& operator=(const QueryRequest& other) = default; |
||
177 |
HPP_FCL_COMPILER_DIAGNOSTIC_POP |
||
178 |
|||
179 |
void updateGuess(const QueryResult& result); |
||
180 |
|||
181 |
/// @brief whether two QueryRequest are the same or not |
||
182 |
12 |
inline bool operator==(const QueryRequest& other) const { |
|
183 |
HPP_FCL_COMPILER_DIAGNOSTIC_PUSH |
||
184 |
HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS |
||
185 |
24 |
return gjk_initial_guess == other.gjk_initial_guess && |
|
186 |
✓✗ | 12 |
enable_cached_gjk_guess == other.enable_cached_gjk_guess && |
187 |
✓✗ | 12 |
cached_gjk_guess == other.cached_gjk_guess && |
188 |
✓✗✓✗ |
36 |
cached_support_func_guess == other.cached_support_func_guess && |
189 |
✓✗ | 24 |
enable_timings == other.enable_timings; |
190 |
HPP_FCL_COMPILER_DIAGNOSTIC_POP |
||
191 |
} |
||
192 |
}; |
||
193 |
|||
194 |
/// @brief base class for all query results |
||
195 |
struct HPP_FCL_DLLAPI QueryResult { |
||
196 |
/// @brief stores the last GJK ray when relevant. |
||
197 |
Vec3f cached_gjk_guess; |
||
198 |
|||
199 |
/// @brief stores the last support function vertex index, when relevant. |
||
200 |
support_func_guess_t cached_support_func_guess; |
||
201 |
|||
202 |
/// @brief timings for the given request |
||
203 |
CPUTimes timings; |
||
204 |
|||
205 |
1205543 |
QueryResult() |
|
206 |
: cached_gjk_guess(Vec3f::Zero()), |
||
207 |
✓✗✓✗ ✓✗ |
1205543 |
cached_support_func_guess(support_func_guess_t::Constant(-1)) {} |
208 |
}; |
||
209 |
|||
210 |
15513 |
inline void QueryRequest::updateGuess(const QueryResult& result) { |
|
211 |
✗✓ | 15513 |
if (gjk_initial_guess == GJKInitialGuess::CachedGuess) { |
212 |
cached_gjk_guess = result.cached_gjk_guess; |
||
213 |
cached_support_func_guess = result.cached_support_func_guess; |
||
214 |
} |
||
215 |
// TODO: use gjk_initial_guess instead |
||
216 |
HPP_FCL_COMPILER_DIAGNOSTIC_PUSH |
||
217 |
HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS |
||
218 |
✓✓ | 15513 |
if (enable_cached_gjk_guess) { |
219 |
4 |
cached_gjk_guess = result.cached_gjk_guess; |
|
220 |
4 |
cached_support_func_guess = result.cached_support_func_guess; |
|
221 |
} |
||
222 |
HPP_FCL_COMPILER_DIAGNOSTIC_POP |
||
223 |
15513 |
} |
|
224 |
|||
225 |
struct CollisionResult; |
||
226 |
|||
227 |
/// @brief flag declaration for specifying required params in CollisionResult |
||
228 |
enum CollisionRequestFlag { |
||
229 |
CONTACT = 0x00001, |
||
230 |
DISTANCE_LOWER_BOUND = 0x00002, |
||
231 |
NO_REQUEST = 0x01000 |
||
232 |
}; |
||
233 |
|||
234 |
/// @brief request to the collision algorithm |
||
235 |
struct HPP_FCL_DLLAPI CollisionRequest : QueryRequest { |
||
236 |
/// @brief The maximum number of contacts will return |
||
237 |
size_t num_max_contacts; |
||
238 |
|||
239 |
/// @brief whether the contact information (normal, penetration depth and |
||
240 |
/// contact position) will return |
||
241 |
bool enable_contact; |
||
242 |
|||
243 |
/// Whether a lower bound on distance is returned when objects are disjoint |
||
244 |
bool enable_distance_lower_bound; |
||
245 |
|||
246 |
/// @brief Distance below which objects are considered in collision. |
||
247 |
/// See \ref hpp_fcl_collision_and_distance_lower_bound_computation |
||
248 |
/// @note If set to -inf, the objects tested for collision are considered |
||
249 |
/// as collision free and no test is actually performed by functions |
||
250 |
/// hpp::fcl::collide of class hpp::fcl::ComputeCollision. |
||
251 |
FCL_REAL security_margin; |
||
252 |
|||
253 |
/// @brief Distance below which bounding volumes are broken down. |
||
254 |
/// See \ref hpp_fcl_collision_and_distance_lower_bound_computation |
||
255 |
FCL_REAL break_distance; |
||
256 |
|||
257 |
/// @brief Distance above which GJK solver makes an early stopping. |
||
258 |
/// GJK stops searching for the closest points when it proves that the |
||
259 |
/// distance between two geometries is above this threshold. |
||
260 |
/// |
||
261 |
/// @remarks Consequently, the closest points might be incorrect, but allows |
||
262 |
/// to save computational resources. |
||
263 |
FCL_REAL distance_upper_bound; |
||
264 |
|||
265 |
/// @brief Constructor from a flag and a maximal number of contacts. |
||
266 |
/// |
||
267 |
/// @param[in] flag Collision request flag |
||
268 |
/// @param[in] num_max_contacts Maximal number of allowed contacts |
||
269 |
/// |
||
270 |
18996 |
CollisionRequest(const CollisionRequestFlag flag, size_t num_max_contacts_) |
|
271 |
18996 |
: num_max_contacts(num_max_contacts_), |
|
272 |
18996 |
enable_contact(flag & CONTACT), |
|
273 |
18996 |
enable_distance_lower_bound(flag & DISTANCE_LOWER_BOUND), |
|
274 |
security_margin(0), |
||
275 |
break_distance(1e-3), |
||
276 |
18996 |
distance_upper_bound((std::numeric_limits<FCL_REAL>::max)()) {} |
|
277 |
|||
278 |
/// @brief Default constructor. |
||
279 |
41025 |
CollisionRequest() |
|
280 |
41025 |
: num_max_contacts(1), |
|
281 |
enable_contact(false), |
||
282 |
enable_distance_lower_bound(false), |
||
283 |
security_margin(0), |
||
284 |
break_distance(1e-3), |
||
285 |
41025 |
distance_upper_bound((std::numeric_limits<FCL_REAL>::max)()) {} |
|
286 |
|||
287 |
bool isSatisfied(const CollisionResult& result) const; |
||
288 |
|||
289 |
/// @brief whether two CollisionRequest are the same or not |
||
290 |
6 |
inline bool operator==(const CollisionRequest& other) const { |
|
291 |
6 |
return QueryRequest::operator==(other) && |
|
292 |
✓✗ | 6 |
num_max_contacts == other.num_max_contacts && |
293 |
✓✗ | 6 |
enable_contact == other.enable_contact && |
294 |
✓✗ | 6 |
enable_distance_lower_bound == other.enable_distance_lower_bound && |
295 |
✓✗ | 6 |
security_margin == other.security_margin && |
296 |
✓✗✓✗ |
18 |
break_distance == other.break_distance && |
297 |
✓✗ | 12 |
distance_upper_bound == other.distance_upper_bound; |
298 |
} |
||
299 |
}; |
||
300 |
|||
301 |
/// @brief collision result |
||
302 |
struct HPP_FCL_DLLAPI CollisionResult : QueryResult { |
||
303 |
private: |
||
304 |
/// @brief contact information |
||
305 |
std::vector<Contact> contacts; |
||
306 |
|||
307 |
public: |
||
308 |
/// Lower bound on distance between objects if they are disjoint. |
||
309 |
/// See \ref hpp_fcl_collision_and_distance_lower_bound_computation |
||
310 |
/// @note computed only on request (or if it does not add any computational |
||
311 |
/// overhead). |
||
312 |
FCL_REAL distance_lower_bound; |
||
313 |
|||
314 |
/// @brief nearest points |
||
315 |
/// available only when distance_lower_bound is inferior to |
||
316 |
/// CollisionRequest::break_distance. |
||
317 |
Vec3f nearest_points[2]; |
||
318 |
|||
319 |
public: |
||
320 |
46802 |
CollisionResult() |
|
321 |
✓✓✓✗ |
140406 |
: distance_lower_bound((std::numeric_limits<FCL_REAL>::max)()) {} |
322 |
|||
323 |
/// @brief Update the lower bound only if the distance is inferior. |
||
324 |
8460475 |
inline void updateDistanceLowerBound(const FCL_REAL& distance_lower_bound_) { |
|
325 |
✗✓ | 8460475 |
if (distance_lower_bound_ < distance_lower_bound) |
326 |
distance_lower_bound = distance_lower_bound_; |
||
327 |
8460475 |
} |
|
328 |
|||
329 |
/// @brief add one contact into result structure |
||
330 |
19649 |
inline void addContact(const Contact& c) { contacts.push_back(c); } |
|
331 |
|||
332 |
/// @brief whether two CollisionResult are the same or not |
||
333 |
6 |
inline bool operator==(const CollisionResult& other) const { |
|
334 |
✓✗ | 12 |
return contacts == other.contacts && |
335 |
✓✗ | 12 |
distance_lower_bound == other.distance_lower_bound; |
336 |
} |
||
337 |
|||
338 |
/// @brief return binary collision result |
||
339 |
23233083 |
bool isCollision() const { return contacts.size() > 0; } |
|
340 |
|||
341 |
/// @brief number of contacts found |
||
342 |
8637590 |
size_t numContacts() const { return contacts.size(); } |
|
343 |
|||
344 |
/// @brief get the i-th contact calculated |
||
345 |
261 |
const Contact& getContact(size_t i) const { |
|
346 |
✗✓ | 261 |
if (contacts.size() == 0) |
347 |
throw std::invalid_argument( |
||
348 |
"The number of contacts is zero. No Contact can be returned."); |
||
349 |
|||
350 |
✓✗ | 261 |
if (i < contacts.size()) |
351 |
261 |
return contacts[i]; |
|
352 |
else |
||
353 |
return contacts.back(); |
||
354 |
} |
||
355 |
|||
356 |
/// @brief set the i-th contact calculated |
||
357 |
void setContact(size_t i, const Contact& c) { |
||
358 |
if (contacts.size() == 0) |
||
359 |
throw std::invalid_argument( |
||
360 |
"The number of contacts is zero. No Contact can be returned."); |
||
361 |
|||
362 |
if (i < contacts.size()) |
||
363 |
contacts[i] = c; |
||
364 |
else |
||
365 |
contacts.back() = c; |
||
366 |
} |
||
367 |
|||
368 |
/// @brief get all the contacts |
||
369 |
void getContacts(std::vector<Contact>& contacts_) const { |
||
370 |
contacts_.resize(contacts.size()); |
||
371 |
std::copy(contacts.begin(), contacts.end(), contacts_.begin()); |
||
372 |
} |
||
373 |
|||
374 |
3 |
const std::vector<Contact>& getContacts() const { return contacts; } |
|
375 |
|||
376 |
/// @brief clear the results obtained |
||
377 |
32385 |
void clear() { |
|
378 |
32385 |
distance_lower_bound = (std::numeric_limits<FCL_REAL>::max)(); |
|
379 |
32385 |
contacts.clear(); |
|
380 |
32385 |
distance_lower_bound = (std::numeric_limits<FCL_REAL>::max)(); |
|
381 |
32385 |
timings.clear(); |
|
382 |
32385 |
} |
|
383 |
|||
384 |
/// @brief reposition Contact objects when fcl inverts them |
||
385 |
/// during their construction. |
||
386 |
void swapObjects(); |
||
387 |
}; |
||
388 |
|||
389 |
struct DistanceResult; |
||
390 |
|||
391 |
/// @brief request to the distance computation |
||
392 |
struct HPP_FCL_DLLAPI DistanceRequest : QueryRequest { |
||
393 |
/// @brief whether to return the nearest points |
||
394 |
bool enable_nearest_points; |
||
395 |
|||
396 |
/// @brief error threshold for approximate distance |
||
397 |
FCL_REAL rel_err; // relative error, between 0 and 1 |
||
398 |
FCL_REAL abs_err; // absolute error |
||
399 |
|||
400 |
/// \param enable_nearest_points_ enables the nearest points computation. |
||
401 |
/// \param rel_err_ |
||
402 |
/// \param abs_err_ |
||
403 |
1880408 |
DistanceRequest(bool enable_nearest_points_ = false, FCL_REAL rel_err_ = 0.0, |
|
404 |
FCL_REAL abs_err_ = 0.0) |
||
405 |
1880408 |
: enable_nearest_points(enable_nearest_points_), |
|
406 |
rel_err(rel_err_), |
||
407 |
1880408 |
abs_err(abs_err_) {} |
|
408 |
|||
409 |
bool isSatisfied(const DistanceResult& result) const; |
||
410 |
|||
411 |
/// @brief whether two DistanceRequest are the same or not |
||
412 |
6 |
inline bool operator==(const DistanceRequest& other) const { |
|
413 |
6 |
return QueryRequest::operator==(other) && |
|
414 |
✓✗ | 6 |
enable_nearest_points == other.enable_nearest_points && |
415 |
✓✗✓✗ ✓✗ |
12 |
rel_err == other.rel_err && abs_err == other.abs_err; |
416 |
} |
||
417 |
}; |
||
418 |
|||
419 |
/// @brief distance result |
||
420 |
struct HPP_FCL_DLLAPI DistanceResult : QueryResult { |
||
421 |
public: |
||
422 |
/// @brief minimum distance between two objects. if two objects are in |
||
423 |
/// collision, min_distance <= 0. |
||
424 |
FCL_REAL min_distance; |
||
425 |
|||
426 |
/// @brief nearest points |
||
427 |
Vec3f nearest_points[2]; |
||
428 |
|||
429 |
/// In case both objects are in collision, store the normal |
||
430 |
Vec3f normal; |
||
431 |
|||
432 |
/// @brief collision object 1 |
||
433 |
const CollisionGeometry* o1; |
||
434 |
|||
435 |
/// @brief collision object 2 |
||
436 |
const CollisionGeometry* o2; |
||
437 |
|||
438 |
/// @brief information about the nearest point in object 1 |
||
439 |
/// if object 1 is mesh or point cloud, it is the triangle or point id |
||
440 |
/// if object 1 is geometry shape, it is NONE (-1), |
||
441 |
/// if object 1 is octree, it is the id of the cell |
||
442 |
int b1; |
||
443 |
|||
444 |
/// @brief information about the nearest point in object 2 |
||
445 |
/// if object 2 is mesh or point cloud, it is the triangle or point id |
||
446 |
/// if object 2 is geometry shape, it is NONE (-1), |
||
447 |
/// if object 2 is octree, it is the id of the cell |
||
448 |
int b2; |
||
449 |
|||
450 |
/// @brief invalid contact primitive information |
||
451 |
static const int NONE = -1; |
||
452 |
|||
453 |
1158741 |
DistanceResult( |
|
454 |
FCL_REAL min_distance_ = (std::numeric_limits<FCL_REAL>::max)()) |
||
455 |
✓✓ | 3476223 |
: min_distance(min_distance_), o1(NULL), o2(NULL), b1(NONE), b2(NONE) { |
456 |
const Vec3f nan( |
||
457 |
✓✗✓✗ |
1158741 |
Vec3f::Constant(std::numeric_limits<FCL_REAL>::quiet_NaN())); |
458 |
✓✗✓✗ ✓✗ |
1158741 |
nearest_points[0] = nearest_points[1] = normal = nan; |
459 |
1158741 |
} |
|
460 |
|||
461 |
/// @brief add distance information into the result |
||
462 |
void update(FCL_REAL distance, const CollisionGeometry* o1_, |
||
463 |
const CollisionGeometry* o2_, int b1_, int b2_) { |
||
464 |
if (min_distance > distance) { |
||
465 |
min_distance = distance; |
||
466 |
o1 = o1_; |
||
467 |
o2 = o2_; |
||
468 |
b1 = b1_; |
||
469 |
b2 = b2_; |
||
470 |
} |
||
471 |
} |
||
472 |
|||
473 |
/// @brief add distance information into the result |
||
474 |
1049508 |
void update(FCL_REAL distance, const CollisionGeometry* o1_, |
|
475 |
const CollisionGeometry* o2_, int b1_, int b2_, const Vec3f& p1, |
||
476 |
const Vec3f& p2, const Vec3f& normal_) { |
||
477 |
✓✓ | 1049508 |
if (min_distance > distance) { |
478 |
715697 |
min_distance = distance; |
|
479 |
715697 |
o1 = o1_; |
|
480 |
715697 |
o2 = o2_; |
|
481 |
715697 |
b1 = b1_; |
|
482 |
715697 |
b2 = b2_; |
|
483 |
715697 |
nearest_points[0] = p1; |
|
484 |
715697 |
nearest_points[1] = p2; |
|
485 |
715697 |
normal = normal_; |
|
486 |
} |
||
487 |
1049508 |
} |
|
488 |
|||
489 |
/// @brief add distance information into the result |
||
490 |
void update(const DistanceResult& other_result) { |
||
491 |
if (min_distance > other_result.min_distance) { |
||
492 |
min_distance = other_result.min_distance; |
||
493 |
o1 = other_result.o1; |
||
494 |
o2 = other_result.o2; |
||
495 |
b1 = other_result.b1; |
||
496 |
b2 = other_result.b2; |
||
497 |
nearest_points[0] = other_result.nearest_points[0]; |
||
498 |
nearest_points[1] = other_result.nearest_points[1]; |
||
499 |
normal = other_result.normal; |
||
500 |
} |
||
501 |
} |
||
502 |
|||
503 |
/// @brief clear the result |
||
504 |
695 |
void clear() { |
|
505 |
const Vec3f nan( |
||
506 |
✓✗✓✗ |
695 |
Vec3f::Constant(std::numeric_limits<FCL_REAL>::quiet_NaN())); |
507 |
695 |
min_distance = (std::numeric_limits<FCL_REAL>::max)(); |
|
508 |
695 |
o1 = NULL; |
|
509 |
695 |
o2 = NULL; |
|
510 |
695 |
b1 = NONE; |
|
511 |
695 |
b2 = NONE; |
|
512 |
✓✗✓✗ ✓✗ |
695 |
nearest_points[0] = nearest_points[1] = normal = nan; |
513 |
695 |
timings.clear(); |
|
514 |
695 |
} |
|
515 |
|||
516 |
/// @brief whether two DistanceResult are the same or not |
||
517 |
6 |
inline bool operator==(const DistanceResult& other) const { |
|
518 |
12 |
bool is_same = min_distance == other.min_distance && |
|
519 |
✓✗ | 6 |
nearest_points[0] == other.nearest_points[0] && |
520 |
✓✗ | 6 |
nearest_points[1] == other.nearest_points[1] && |
521 |
✓✗✓✗ ✓✗ |
6 |
normal == other.normal && o1 == other.o1 && o2 == other.o2 && |
522 |
✓✗✓✗ ✓✗ |
12 |
b1 == other.b1 && b2 == other.b2; |
523 |
|||
524 |
// TODO: check also that two GeometryObject are indeed equal. |
||
525 |
✗✓ | 6 |
if ((o1 != NULL) ^ (other.o1 != NULL)) return false; |
526 |
6 |
is_same &= (o1 == other.o1); |
|
527 |
// else if (o1 != NULL and other.o1 != NULL) is_same &= *o1 == *other.o1; |
||
528 |
|||
529 |
✗✓ | 6 |
if ((o2 != NULL) ^ (other.o2 != NULL)) return false; |
530 |
6 |
is_same &= (o2 == other.o2); |
|
531 |
// else if (o2 != NULL and other.o2 != NULL) is_same &= *o2 == *other.o2; |
||
532 |
|||
533 |
6 |
return is_same; |
|
534 |
} |
||
535 |
}; |
||
536 |
|||
537 |
namespace internal { |
||
538 |
92813 |
inline void updateDistanceLowerBoundFromBV(const CollisionRequest& /*req*/, |
|
539 |
CollisionResult& res, |
||
540 |
const FCL_REAL& sqrDistLowerBound) { |
||
541 |
// BV cannot find negative distance. |
||
542 |
✓✓ | 92813 |
if (res.distance_lower_bound <= 0) return; |
543 |
75090 |
FCL_REAL new_dlb = std::sqrt(sqrDistLowerBound); // - req.security_margin; |
|
544 |
✓✓ | 75090 |
if (new_dlb < res.distance_lower_bound) res.distance_lower_bound = new_dlb; |
545 |
} |
||
546 |
|||
547 |
30431922 |
inline void updateDistanceLowerBoundFromLeaf(const CollisionRequest&, |
|
548 |
CollisionResult& res, |
||
549 |
const FCL_REAL& distance, |
||
550 |
const Vec3f& p0, const Vec3f& p1) { |
||
551 |
✓✓ | 30431922 |
if (distance < res.distance_lower_bound) { |
552 |
38759 |
res.distance_lower_bound = distance; |
|
553 |
38759 |
res.nearest_points[0] = p0; |
|
554 |
38759 |
res.nearest_points[1] = p1; |
|
555 |
} |
||
556 |
30431922 |
} |
|
557 |
} // namespace internal |
||
558 |
|||
559 |
inline CollisionRequestFlag operator~(CollisionRequestFlag a) { |
||
560 |
return static_cast<CollisionRequestFlag>(~static_cast<int>(a)); |
||
561 |
} |
||
562 |
|||
563 |
1004 |
inline CollisionRequestFlag operator|(CollisionRequestFlag a, |
|
564 |
CollisionRequestFlag b) { |
||
565 |
1004 |
return static_cast<CollisionRequestFlag>(static_cast<int>(a) | |
|
566 |
1004 |
static_cast<int>(b)); |
|
567 |
} |
||
568 |
|||
569 |
inline CollisionRequestFlag operator&(CollisionRequestFlag a, |
||
570 |
CollisionRequestFlag b) { |
||
571 |
return static_cast<CollisionRequestFlag>(static_cast<int>(a) & |
||
572 |
static_cast<int>(b)); |
||
573 |
} |
||
574 |
|||
575 |
inline CollisionRequestFlag operator^(CollisionRequestFlag a, |
||
576 |
CollisionRequestFlag b) { |
||
577 |
return static_cast<CollisionRequestFlag>(static_cast<int>(a) ^ |
||
578 |
static_cast<int>(b)); |
||
579 |
} |
||
580 |
|||
581 |
inline CollisionRequestFlag& operator|=(CollisionRequestFlag& a, |
||
582 |
CollisionRequestFlag b) { |
||
583 |
return (CollisionRequestFlag&)((int&)(a) |= static_cast<int>(b)); |
||
584 |
} |
||
585 |
|||
586 |
inline CollisionRequestFlag& operator&=(CollisionRequestFlag& a, |
||
587 |
CollisionRequestFlag b) { |
||
588 |
return (CollisionRequestFlag&)((int&)(a) &= static_cast<int>(b)); |
||
589 |
} |
||
590 |
|||
591 |
inline CollisionRequestFlag& operator^=(CollisionRequestFlag& a, |
||
592 |
CollisionRequestFlag b) { |
||
593 |
return (CollisionRequestFlag&)((int&)(a) ^= static_cast<int>(b)); |
||
594 |
} |
||
595 |
|||
596 |
} // namespace fcl |
||
597 |
|||
598 |
} // namespace hpp |
||
599 |
|||
600 |
#endif |
Generated by: GCOVR (Version 4.2) |