hpp-fcl 1.8.1
HPP fork of FCL -- The Flexible Collision Library
Loading...
Searching...
No Matches
collision_data.h
Go to the documentation of this file.
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
39#ifndef HPP_FCL_COLLISION_DATA_H
40#define HPP_FCL_COLLISION_DATA_H
41
42#include <vector>
43#include <set>
44#include <limits>
45
47#include <hpp/fcl/config.hh>
48#include <hpp/fcl/data_types.h>
49#include <hpp/fcl/timings.h>
50
51namespace hpp
52{
53namespace fcl
54{
55
58{
61
64
69 int b1;
70
75 int b2;
76
79
82
85
87 static const int NONE = -1;
88
90 Contact() : o1(NULL),
91 o2(NULL),
92 b1(NONE),
93 b2(NONE)
94 {}
95
96 Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_) : o1(o1_),
97 o2(o2_),
98 b1(b1_),
99 b2(b2_)
100 {}
101
102 Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_,
103 const Vec3f& pos_, const Vec3f& normal_, FCL_REAL depth_) : o1(o1_),
104 o2(o2_),
105 b1(b1_),
106 b2(b2_),
107 normal(normal_),
108 pos(pos_),
109 penetration_depth(depth_)
110 {}
111
112 bool operator < (const Contact& other) const
113 {
114 if(b1 == other.b1)
115 return b2 < other.b2;
116 return b1 < other.b1;
117 }
118
119 bool operator == (const Contact& other) const
120 {
121 return o1 == other.o1
122 && o2 == other.o2
123 && b1 == other.b1
124 && b2 == other.b2
125 && normal == other.normal
126 && pos == other.pos
127 && penetration_depth == other.penetration_depth;
128 }
129
130 bool operator != (const Contact& other) const
131 {
132 return !(*this == other);
133 }
134};
135
136struct QueryResult;
137
140{
143
146
149
152
154 enable_cached_gjk_guess (false),
155 cached_gjk_guess (1,0,0),
156 cached_support_func_guess(support_func_guess_t::Zero()),
157 enable_timings(false)
158 {}
159
160 void updateGuess(const QueryResult& result);
161
163 inline bool operator ==(const QueryRequest& other) const
164 {
165 return enable_cached_gjk_guess == other.enable_cached_gjk_guess
166 && cached_gjk_guess == other.cached_gjk_guess
167 && cached_support_func_guess == other.cached_support_func_guess
168 && enable_timings == other.enable_timings;
169 }
170};
171
174{
177
180
183
185 : cached_gjk_guess(Vec3f::Zero())
186 , cached_support_func_guess(support_func_guess_t::Constant(-1))
187 {}
188};
189
190inline void QueryRequest::updateGuess(const QueryResult& result)
191{
195 }
196}
197
198struct CollisionResult;
199
202{
203 CONTACT = 0x00001,
205 NO_REQUEST = 0x01000
207
210{
213
216
219
223
227
233
239 CollisionRequest(const CollisionRequestFlag flag, size_t num_max_contacts_) :
240 num_max_contacts(num_max_contacts_),
241 enable_contact(flag & CONTACT),
242 enable_distance_lower_bound (flag & DISTANCE_LOWER_BOUND),
243 security_margin (0),
244 break_distance (1e-3),
245 distance_upper_bound ((std::numeric_limits<FCL_REAL>::max)())
246 {
247 }
248
251 num_max_contacts(1),
252 enable_contact(false),
253 enable_distance_lower_bound (false),
254 security_margin (0),
255 break_distance (1e-3),
256 distance_upper_bound ((std::numeric_limits<FCL_REAL>::max)())
257 {
258 }
259
260 bool isSatisfied(const CollisionResult& result) const;
261
263 inline bool operator ==(const CollisionRequest& other) const
264 {
265 return QueryRequest::operator==(other)
266 && num_max_contacts == other.num_max_contacts
267 && enable_contact == other.enable_contact
268 && enable_distance_lower_bound == other.enable_distance_lower_bound
269 && security_margin == other.security_margin
270 && break_distance == other.break_distance
271 && distance_upper_bound == other.distance_upper_bound;
272 }
273};
274
277{
278private:
280 std::vector<Contact> contacts;
281
282public:
288
289public:
291 : distance_lower_bound ((std::numeric_limits<FCL_REAL>::max)())
292 {
293 }
294
296 inline void updateDistanceLowerBound (const FCL_REAL& distance_lower_bound_)
297 {
298 if (distance_lower_bound_ < distance_lower_bound)
299 distance_lower_bound = distance_lower_bound_;
300 }
301
303 inline void addContact(const Contact& c)
304 {
305 contacts.push_back(c);
306 }
307
309 inline bool operator ==(const CollisionResult& other) const
310 {
311 return contacts == other.contacts
312 && distance_lower_bound == other.distance_lower_bound;
313 }
314
316 bool isCollision() const
317 {
318 return contacts.size() > 0;
319 }
320
322 size_t numContacts() const
323 {
324 return contacts.size();
325 }
326
328 const Contact& getContact(size_t i) const
329 {
330 if(contacts.size() == 0)
331 throw std::invalid_argument("The number of contacts is zero. No Contact can be returned.");
332
333 if(i < contacts.size())
334 return contacts[i];
335 else
336 return contacts.back();
337 }
338
340 void getContacts(std::vector<Contact>& contacts_) const
341 {
342 contacts_.resize(contacts.size());
343 std::copy(contacts.begin(), contacts.end(), contacts_.begin());
344 }
345
346 const std::vector<Contact> & getContacts() const
347 {
348 return contacts;
349 }
350
352 void clear()
353 {
354 distance_lower_bound = (std::numeric_limits<FCL_REAL>::max)();
355 contacts.clear();
356 distance_lower_bound = (std::numeric_limits<FCL_REAL>::max)();
357 timings.clear();
358 }
359
363};
364
365struct DistanceResult;
366
369{
372
374 FCL_REAL rel_err; // relative error, between 0 and 1
375 FCL_REAL abs_err; // absolute error
376
380 DistanceRequest(bool enable_nearest_points_ = false,
381 FCL_REAL rel_err_ = 0.0,
382 FCL_REAL abs_err_ = 0.0) :
383 enable_nearest_points(enable_nearest_points_),
384 rel_err(rel_err_),
385 abs_err(abs_err_)
386 {
387 }
388
389 bool isSatisfied(const DistanceResult& result) const;
390
392 inline bool operator ==(const DistanceRequest& other) const
393 {
394 return QueryRequest::operator==(other)
395 && enable_nearest_points == other.enable_nearest_points
396 && rel_err == other.rel_err
397 && abs_err == other.abs_err;
398 }
399};
400
403{
404
405public:
406
409
411 Vec3f nearest_points[2];
412
415
418
421
426 int b1;
427
432 int b2;
433
435 static const int NONE = -1;
436
437 DistanceResult(FCL_REAL min_distance_ =
438 (std::numeric_limits<FCL_REAL>::max)()):
439 min_distance(min_distance_), o1(NULL), o2(NULL), b1(NONE), b2(NONE)
440 {
441 const Vec3f nan (Vec3f::Constant(std::numeric_limits<FCL_REAL>::quiet_NaN()));
442 nearest_points [0] = nearest_points [1] = normal = nan;
443 }
444
445
447 void update(FCL_REAL distance, const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_)
448 {
449 if(min_distance > distance)
450 {
451 min_distance = distance;
452 o1 = o1_;
453 o2 = o2_;
454 b1 = b1_;
455 b2 = b2_;
456 }
457 }
458
460 void update(FCL_REAL distance, const CollisionGeometry* o1_,
461 const CollisionGeometry* o2_, int b1_, int b2_,
462 const Vec3f& p1, const Vec3f& p2, const Vec3f& normal_)
463 {
464 if(min_distance > distance)
465 {
466 min_distance = distance;
467 o1 = o1_;
468 o2 = o2_;
469 b1 = b1_;
470 b2 = b2_;
471 nearest_points[0] = p1;
472 nearest_points[1] = p2;
473 normal = normal_;
474 }
475 }
476
478 void update(const DistanceResult& other_result)
479 {
480 if(min_distance > other_result.min_distance)
481 {
482 min_distance = other_result.min_distance;
483 o1 = other_result.o1;
484 o2 = other_result.o2;
485 b1 = other_result.b1;
486 b2 = other_result.b2;
487 nearest_points[0] = other_result.nearest_points[0];
488 nearest_points[1] = other_result.nearest_points[1];
489 normal = other_result.normal;
490 }
491 }
492
494 void clear()
495 {
496 const Vec3f nan (Vec3f::Constant(std::numeric_limits<FCL_REAL>::quiet_NaN()));
497 min_distance = (std::numeric_limits<FCL_REAL>::max)();
498 o1 = NULL;
499 o2 = NULL;
500 b1 = NONE;
501 b2 = NONE;
502 nearest_points [0] = nearest_points [1] = normal = nan;
503 timings.clear();
504 }
505
507 inline bool operator ==(const DistanceResult& other) const
508 {
509 bool is_same = min_distance == other.min_distance
510 && nearest_points[0] == other.nearest_points[0]
511 && nearest_points[1] == other.nearest_points[1]
512 && normal == other.normal
513 && o1 == other.o1
514 && o2 == other.o2
515 && b1 == other.b1
516 && b2 == other.b2;
517
518// TODO: check also that two GeometryObject are indeed equal.
519 if ((o1 != NULL) ^ (other.o1 != NULL)) return false;
520 is_same &= (o1 == other.o1);
521// else if (o1 != NULL and other.o1 != NULL) is_same &= *o1 == *other.o1;
522
523 if ((o2 != NULL) ^ (other.o2 != NULL)) return false;
524 is_same &= (o2 == other.o2);
525// else if (o2 != NULL and other.o2 != NULL) is_same &= *o2 == *other.o2;
526
527 return is_same;
528 }
529
530};
531
533{return static_cast<CollisionRequestFlag>(~static_cast<int>(a));}
534
536{return static_cast<CollisionRequestFlag>(static_cast<int>(a) | static_cast<int>(b));}
537
539{return static_cast<CollisionRequestFlag>(static_cast<int>(a) & static_cast<int>(b));}
540
542{return static_cast<CollisionRequestFlag>(static_cast<int>(a) ^ static_cast<int>(b));}
543
545{return (CollisionRequestFlag&)((int&)(a) |= static_cast<int>(b));}
546
548{return (CollisionRequestFlag&)((int&)(a) &= static_cast<int>(b));}
549
551{return (CollisionRequestFlag&)((int&)(a) ^= static_cast<int>(b));}
552
553}
554
555} // namespace hpp
556
557#endif
The geometry for the object for collision or distance computation.
Definition: collision_object.h:67
#define HPP_FCL_DLLAPI
Definition: config.hh:64
FCL_REAL distance(const Matrix3f &R0, const Vec3f &T0, const kIOS &b1, const kIOS &b2, Vec3f *P=NULL, Vec3f *Q=NULL)
Approximate distance between two kIOS bounding volumes.
CollisionRequestFlag operator&(CollisionRequestFlag a, CollisionRequestFlag b)
Definition: collision_data.h:538
CollisionRequestFlag & operator|=(CollisionRequestFlag &a, CollisionRequestFlag b)
Definition: collision_data.h:544
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:67
CollisionRequestFlag operator~(CollisionRequestFlag a)
Definition: collision_data.h:532
CollisionRequestFlag
flag declaration for specifying required params in CollisionResult
Definition: collision_data.h:202
@ NO_REQUEST
Definition: collision_data.h:205
@ DISTANCE_LOWER_BOUND
Definition: collision_data.h:204
@ CONTACT
Definition: collision_data.h:203
CollisionRequestFlag operator^(CollisionRequestFlag a, CollisionRequestFlag b)
Definition: collision_data.h:541
CollisionRequestFlag operator|(CollisionRequestFlag a, CollisionRequestFlag b)
Definition: collision_data.h:535
CollisionRequestFlag & operator^=(CollisionRequestFlag &a, CollisionRequestFlag b)
Definition: collision_data.h:550
double FCL_REAL
Definition: data_types.h:66
CollisionRequestFlag & operator&=(CollisionRequestFlag &a, CollisionRequestFlag b)
Definition: collision_data.h:547
Eigen::Vector2i support_func_guess_t
Definition: data_types.h:73
Main namespace.
Definition: AABB.h:44
Definition: timings.h:17
request to the collision algorithm
Definition: collision_data.h:210
size_t num_max_contacts
The maximum number of contacts will return.
Definition: collision_data.h:212
bool enable_distance_lower_bound
Whether a lower bound on distance is returned when objects are disjoint.
Definition: collision_data.h:218
FCL_REAL distance_upper_bound
Distance above which GJK solver makes an early stopping. GJK stops searching for the closest points w...
Definition: collision_data.h:232
CollisionRequest(const CollisionRequestFlag flag, size_t num_max_contacts_)
Constructor from a flag and a maximal number of contacts.
Definition: collision_data.h:239
FCL_REAL security_margin
Distance below which objects are considered in collision. See Collision detection and distance lower ...
Definition: collision_data.h:222
bool enable_contact
whether the contact information (normal, penetration depth and contact position) will return
Definition: collision_data.h:215
bool isSatisfied(const CollisionResult &result) const
CollisionRequest()
Default constructor.
Definition: collision_data.h:250
FCL_REAL break_distance
Distance below which bounding volumes are broken down. See Collision detection and distance lower bou...
Definition: collision_data.h:226
collision result
Definition: collision_data.h:277
void clear()
clear the results obtained
Definition: collision_data.h:352
void updateDistanceLowerBound(const FCL_REAL &distance_lower_bound_)
Update the lower bound only if the distance is inferior.
Definition: collision_data.h:296
void addContact(const Contact &c)
add one contact into result structure
Definition: collision_data.h:303
bool isCollision() const
return binary collision result
Definition: collision_data.h:316
const Contact & getContact(size_t i) const
get the i-th contact calculated
Definition: collision_data.h:328
void getContacts(std::vector< Contact > &contacts_) const
get all the contacts
Definition: collision_data.h:340
FCL_REAL distance_lower_bound
Definition: collision_data.h:287
CollisionResult()
Definition: collision_data.h:290
size_t numContacts() const
number of contacts found
Definition: collision_data.h:322
const std::vector< Contact > & getContacts() const
Definition: collision_data.h:346
void swapObjects()
reposition Contact objects when fcl inverts them during their construction.
Contact information returned by collision.
Definition: collision_data.h:58
FCL_REAL penetration_depth
penetration depth
Definition: collision_data.h:84
Contact()
Default constructor.
Definition: collision_data.h:90
Vec3f pos
contact position, in world space
Definition: collision_data.h:81
int b1
contact primitive in object 1 if object 1 is mesh or point cloud, it is the triangle or point id if o...
Definition: collision_data.h:69
Contact(const CollisionGeometry *o1_, const CollisionGeometry *o2_, int b1_, int b2_, const Vec3f &pos_, const Vec3f &normal_, FCL_REAL depth_)
Definition: collision_data.h:102
int b2
contact primitive in object 2 if object 2 is mesh or point cloud, it is the triangle or point id if o...
Definition: collision_data.h:75
const CollisionGeometry * o2
collision object 2
Definition: collision_data.h:63
const CollisionGeometry * o1
collision object 1
Definition: collision_data.h:60
Vec3f normal
contact normal, pointing from o1 to o2
Definition: collision_data.h:78
Contact(const CollisionGeometry *o1_, const CollisionGeometry *o2_, int b1_, int b2_)
Definition: collision_data.h:96
request to the distance computation
Definition: collision_data.h:369
bool enable_nearest_points
whether to return the nearest points
Definition: collision_data.h:371
FCL_REAL rel_err
error threshold for approximate distance
Definition: collision_data.h:374
DistanceRequest(bool enable_nearest_points_=false, FCL_REAL rel_err_=0.0, FCL_REAL abs_err_=0.0)
Definition: collision_data.h:380
bool isSatisfied(const DistanceResult &result) const
FCL_REAL abs_err
Definition: collision_data.h:375
distance result
Definition: collision_data.h:403
const CollisionGeometry * o2
collision object 2
Definition: collision_data.h:420
DistanceResult(FCL_REAL min_distance_=(std::numeric_limits< FCL_REAL >::max)())
Definition: collision_data.h:437
void update(FCL_REAL distance, const CollisionGeometry *o1_, const CollisionGeometry *o2_, int b1_, int b2_)
add distance information into the result
Definition: collision_data.h:447
void update(const DistanceResult &other_result)
add distance information into the result
Definition: collision_data.h:478
Vec3f normal
In case both objects are in collision, store the normal.
Definition: collision_data.h:414
const CollisionGeometry * o1
collision object 1
Definition: collision_data.h:417
void update(FCL_REAL distance, const CollisionGeometry *o1_, const CollisionGeometry *o2_, int b1_, int b2_, const Vec3f &p1, const Vec3f &p2, const Vec3f &normal_)
add distance information into the result
Definition: collision_data.h:460
int b2
information about the nearest point in object 2 if object 2 is mesh or point cloud,...
Definition: collision_data.h:432
Vec3f nearest_points[2]
nearest points
Definition: collision_data.h:411
int b1
information about the nearest point in object 1 if object 1 is mesh or point cloud,...
Definition: collision_data.h:426
void clear()
clear the result
Definition: collision_data.h:494
FCL_REAL min_distance
minimum distance between two objects. if two objects are in collision, min_distance <= 0.
Definition: collision_data.h:408
base class for all query requests
Definition: collision_data.h:140
Vec3f cached_gjk_guess
the gjk initial guess set by user
Definition: collision_data.h:145
support_func_guess_t cached_support_func_guess
the support function initial guess set by user
Definition: collision_data.h:148
QueryRequest()
Definition: collision_data.h:153
void updateGuess(const QueryResult &result)
Definition: collision_data.h:190
bool enable_timings
enable timings when performing collision/distance request
Definition: collision_data.h:151
bool enable_cached_gjk_guess
whether enable gjk initial guess
Definition: collision_data.h:142
base class for all query results
Definition: collision_data.h:174
support_func_guess_t cached_support_func_guess
stores the last support function vertex index, when relevant.
Definition: collision_data.h:179
Vec3f cached_gjk_guess
stores the last GJK ray when relevant.
Definition: collision_data.h:176
QueryResult()
Definition: collision_data.h:184
CPUTimes timings
timings for the given request
Definition: collision_data.h:182