hpp-fcl  1.8.0
HPP fork of FCL -- The Flexible Collision Library
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 
51 namespace hpp
52 {
53 namespace 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 
136 struct 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 
190 inline void QueryRequest::updateGuess(const QueryResult& result)
191 {
195  }
196 }
197 
198 struct CollisionResult;
199 
202 {
203  CONTACT = 0x00001,
205  NO_REQUEST = 0x01000
206 };
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 {
278 private:
280  std::vector<Contact> contacts;
281 
282 public:
288 
289 public:
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 
362  void swapObjects();
363 };
364 
365 struct DistanceResult;
366 
369 {
372 
374  FCL_REAL rel_err; // relative error, between 0 and 1
375  FCL_REAL abs_err; // absoluate 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 
405 public:
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 
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
Contact(const CollisionGeometry *o1_, const CollisionGeometry *o2_, int b1_, int b2_)
Definition: collision_data.h:96
support_func_guess_t cached_support_func_guess
the support function intial guess set by user
Definition: collision_data.h:148
size_t num_max_contacts
The maximum number of contacts will return.
Definition: collision_data.h:212
CollisionRequestFlag operator|(CollisionRequestFlag a, CollisionRequestFlag b)
Definition: collision_data.h:535
request to the distance computation
Definition: collision_data.h:368
bool operator==(const QueryRequest &other) const
whether two QueryRequest are the same or not
Definition: collision_data.h:163
void update(const DistanceResult &other_result)
add distance information into the result
Definition: collision_data.h:478
support_func_guess_t cached_support_func_guess
stores the last support function vertex index, when relevant.
Definition: collision_data.h:179
const CollisionGeometry * o1
collision object 1
Definition: collision_data.h:417
base class for all query results
Definition: collision_data.h:173
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
Main namespace.
Definition: AABB.h:43
collision result
Definition: collision_data.h:276
int b2
information about the nearest point in object 2 if object 2 is mesh or point cloud, it is the triangle or point id if object 2 is geometry shape, it is NONE (-1), if object 2 is octree, it is the id of the cell
Definition: collision_data.h:432
Vec3f nearest_points[2]
nearest points
Definition: collision_data.h:411
bool enable_cached_gjk_guess
whether enable gjk intial guess
Definition: collision_data.h:142
void updateDistanceLowerBound(const FCL_REAL &distance_lower_bound_)
Update the lower bound only if the distance is inferior.
Definition: collision_data.h:296
CollisionRequestFlag & operator|=(CollisionRequestFlag &a, CollisionRequestFlag b)
Definition: collision_data.h:544
QueryRequest()
Definition: collision_data.h:153
size_t numContacts() const
number of contacts found
Definition: collision_data.h:322
void addContact(const Contact &c)
add one contact into result structure
Definition: collision_data.h:303
FCL_REAL distance_lower_bound
Definition: collision_data.h:287
Definition: collision_data.h:203
void clear()
clear the result
Definition: collision_data.h:494
const CollisionGeometry * o1
collision object 1
Definition: collision_data.h:60
CollisionRequestFlag & operator &=(CollisionRequestFlag &a, CollisionRequestFlag b)
Definition: collision_data.h:547
CollisionRequestFlag
flag declaration for specifying required params in CollisionResult
Definition: collision_data.h:201
CollisionRequestFlag operator^(CollisionRequestFlag a, CollisionRequestFlag b)
Definition: collision_data.h:541
request to the collision algorithm
Definition: collision_data.h:209
void clear()
clear the results obtained
Definition: collision_data.h:352
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.
FCL_REAL penetration_depth
penetration depth
Definition: collision_data.h:84
double FCL_REAL
Definition: data_types.h:66
void updateGuess(const QueryResult &result)
Definition: collision_data.h:190
CollisionRequestFlag & operator^=(CollisionRequestFlag &a, CollisionRequestFlag b)
Definition: collision_data.h:550
Definition: timings.h:16
FCL_REAL min_distance
minimum distance between two objects. if two objects are in collision, min_distance <= 0...
Definition: collision_data.h:408
int b1
information about the nearest point in object 1 if object 1 is mesh or point cloud, it is the triangle or point id if object 1 is geometry shape, it is NONE (-1), if object 1 is octree, it is the id of the cell
Definition: collision_data.h:426
Vec3f normal
In case both objects are in collision, store the normal.
Definition: collision_data.h:414
void getContacts(std::vector< Contact > &contacts_) const
get all the contacts
Definition: collision_data.h:340
Vec3f cached_gjk_guess
the gjk intial guess set by user
Definition: collision_data.h:145
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
Eigen::Vector2i support_func_guess_t
Definition: data_types.h:73
Vec3f pos
contact position, in world space
Definition: collision_data.h:81
CPUTimes timings
timings for the given request
Definition: collision_data.h:182
bool enable_timings
enable timings when performing collision/distance request
Definition: collision_data.h:151
DistanceResult(FCL_REAL min_distance_=(std::numeric_limits< FCL_REAL >::max)())
Definition: collision_data.h:437
FCL_REAL break_distance
Distance below which bounding volumes are broken down. See Collision detection and distance lower bou...
Definition: collision_data.h:226
FCL_REAL rel_err
error threshold for approximate distance
Definition: collision_data.h:374
CollisionRequest()
Default constructor.
Definition: collision_data.h:250
bool enable_nearest_points
whether to return the nearest points
Definition: collision_data.h:371
bool enable_distance_lower_bound
Whether a lower bound on distance is returned when objects are disjoint.
Definition: collision_data.h:218
QueryResult()
Definition: collision_data.h:184
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
CollisionResult()
Definition: collision_data.h:290
const Contact & getContact(size_t i) const
get the i-th contact calculated
Definition: collision_data.h:328
CollisionRequestFlag operator &(CollisionRequestFlag a, CollisionRequestFlag b)
Definition: collision_data.h:538
Definition: collision_data.h:204
bool enable_contact
whether the contact information (normal, penetration depth and contact position) will return ...
Definition: collision_data.h:215
Vec3f cached_gjk_guess
stores the last GJK ray when relevant.
Definition: collision_data.h:176
bool isCollision() const
return binary collision result
Definition: collision_data.h:316
base class for all query requests
Definition: collision_data.h:139
CollisionRequestFlag operator~(CollisionRequestFlag a)
Definition: collision_data.h:532
Definition: collision_data.h:205
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:67
const CollisionGeometry * o2
collision object 2
Definition: collision_data.h:63
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
Contact information returned by collision.
Definition: collision_data.h:57
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
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
The geometry for the object for collision or distance computation.
Definition: collision_object.h:66
FCL_REAL abs_err
Definition: collision_data.h:375
const std::vector< Contact > & getContacts() const
Definition: collision_data.h:346
Vec3f normal
contact normal, pointing from o1 to o2
Definition: collision_data.h:78
const CollisionGeometry * o2
collision object 2
Definition: collision_data.h:420
DistanceRequest(bool enable_nearest_points_=false, FCL_REAL rel_err_=0.0, FCL_REAL abs_err_=0.0)
Definition: collision_data.h:380
#define HPP_FCL_DLLAPI
Definition: config.hh:64
CollisionRequest(const CollisionRequestFlag flag, size_t num_max_contacts_)
Constructor from a flag and a maximal number of contacts.
Definition: collision_data.h:239
distance result
Definition: collision_data.h:402
FCL_REAL security_margin
Distance below which objects are considered in collision. See Collision detection and distance lower ...
Definition: collision_data.h:222
Contact()
Default constructor.
Definition: collision_data.h:90