hpp-fcl  1.6.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 
50 
51 namespace hpp
52 {
53 namespace fcl
54 {
55 
58 {
61 
64 
69  int b1;
70 
71 
76  int b2;
77 
80 
83 
86 
87 
89  static const int NONE = -1;
90 
91  Contact() : o1(NULL),
92  o2(NULL),
93  b1(NONE),
94  b2(NONE)
95  {}
96 
97  Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_) : o1(o1_),
98  o2(o2_),
99  b1(b1_),
100  b2(b2_)
101  {}
102 
103  Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_,
104  const Vec3f& pos_, const Vec3f& normal_, FCL_REAL depth_) : o1(o1_),
105  o2(o2_),
106  b1(b1_),
107  b2(b2_),
108  normal(normal_),
109  pos(pos_),
110  penetration_depth(depth_)
111  {}
112 
113  bool operator < (const Contact& other) const
114  {
115  if(b1 == other.b1)
116  return b2 < other.b2;
117  return b1 < other.b1;
118  }
119 
120  bool operator == (const Contact& other) const
121  {
122  return o1 == other.o1
123  && o2 == other.o2
124  && b1 == other.b1
125  && b2 == other.b2
126  && normal == other.normal
127  && pos == other.pos
128  && penetration_depth == other.penetration_depth;
129  }
130 
131  bool operator != (const Contact& other) const
132  {
133  return !(*this == other);
134  }
135 };
136 
137 struct QueryResult;
138 
141 {
144 
147 
150 
152  enable_cached_gjk_guess (false),
153  cached_gjk_guess (1,0,0),
154  cached_support_func_guess(support_func_guess_t::Zero())
155  {}
156 
157  void updateGuess(const QueryResult& result);
158 
160  inline bool operator ==(const QueryRequest& other) const
161  {
162  return enable_cached_gjk_guess == other.enable_cached_gjk_guess
163  && cached_gjk_guess == other.cached_gjk_guess
164  && cached_support_func_guess == other.cached_support_func_guess;
165  }
166 };
167 
170 {
173 
176 };
177 
178 inline void QueryRequest::updateGuess(const QueryResult& result)
179 {
180  if (enable_cached_gjk_guess) {
181  cached_gjk_guess = result.cached_gjk_guess;
182  cached_support_func_guess = result.cached_support_func_guess;
183  }
184 }
185 
186 struct CollisionResult;
187 
190 {
191  CONTACT = 0x00001,
193  NO_REQUEST = 0x01000
194 };
195 
198 {
201 
204 
207 
211 
215 
216  explicit CollisionRequest(const CollisionRequestFlag flag, size_t num_max_contacts_) :
217  num_max_contacts(num_max_contacts_),
218  enable_contact(flag & CONTACT),
219  enable_distance_lower_bound (flag & DISTANCE_LOWER_BOUND),
220  security_margin (0),
221  break_distance (1e-3)
222  {
223  }
224 
226  num_max_contacts(1),
227  enable_contact(false),
228  enable_distance_lower_bound (false),
229  security_margin (0),
230  break_distance (1e-3)
231  {
232  }
233 
234  bool isSatisfied(const CollisionResult& result) const;
235 
237  inline bool operator ==(const CollisionRequest& other) const
238  {
239  return QueryRequest::operator==(other)
240  && num_max_contacts == other.num_max_contacts
241  && enable_contact == other.enable_contact
242  && enable_distance_lower_bound == other.enable_distance_lower_bound
243  && security_margin == other.security_margin
244  && break_distance == other.break_distance;
245  }
246 };
247 
250 {
251 private:
253  std::vector<Contact> contacts;
254 
255 public:
261 
262 public:
264  : distance_lower_bound ((std::numeric_limits<FCL_REAL>::max)())
265  {
266  }
267 
269  inline void updateDistanceLowerBound (const FCL_REAL& distance_lower_bound_)
270  {
271  if (distance_lower_bound_ < distance_lower_bound)
272  distance_lower_bound = distance_lower_bound_;
273  }
274 
276  inline void addContact(const Contact& c)
277  {
278  contacts.push_back(c);
279  }
280 
282  inline bool operator ==(const CollisionResult& other) const
283  {
284  return contacts == other.contacts
285  && distance_lower_bound == other.distance_lower_bound;
286  }
287 
289  bool isCollision() const
290  {
291  return contacts.size() > 0;
292  }
293 
295  size_t numContacts() const
296  {
297  return contacts.size();
298  }
299 
301  const Contact& getContact(size_t i) const
302  {
303  if(contacts.size() == 0)
304  throw std::invalid_argument("The number of contacts is zero. No Contact can be returned.");
305 
306  if(i < contacts.size())
307  return contacts[i];
308  else
309  return contacts.back();
310  }
311 
313  void getContacts(std::vector<Contact>& contacts_) const
314  {
315  contacts_.resize(contacts.size());
316  std::copy(contacts.begin(), contacts.end(), contacts_.begin());
317  }
318 
320  void clear()
321  {
322  contacts.clear();
323  }
324 
327  void swapObjects();
328 };
329 
330 struct DistanceResult;
331 
334 {
337 
339  FCL_REAL rel_err; // relative error, between 0 and 1
340  FCL_REAL abs_err; // absoluate error
341 
345  DistanceRequest(bool enable_nearest_points_ = false,
346  FCL_REAL rel_err_ = 0.0,
347  FCL_REAL abs_err_ = 0.0) :
348  enable_nearest_points(enable_nearest_points_),
349  rel_err(rel_err_),
350  abs_err(abs_err_)
351  {
352  }
353 
354  bool isSatisfied(const DistanceResult& result) const;
355 
357  inline bool operator ==(const DistanceRequest& other) const
358  {
359  return QueryRequest::operator==(other)
360  && enable_nearest_points == other.enable_nearest_points
361  && rel_err == other.rel_err
362  && abs_err == other.abs_err;
363  }
364 };
365 
368 {
369 
370 public:
371 
374 
376  Vec3f nearest_points[2];
377 
380 
383 
386 
391  int b1;
392 
397  int b2;
398 
400  static const int NONE = -1;
401 
402  DistanceResult(FCL_REAL min_distance_ =
403  (std::numeric_limits<FCL_REAL>::max)()):
404  min_distance(min_distance_), o1(NULL), o2(NULL), b1(NONE), b2(NONE)
405  {
406  Vec3f nan (Vec3f::Constant(std::numeric_limits<FCL_REAL>::quiet_NaN()));
407  nearest_points [0] = nearest_points [1] = normal = nan;
408  }
409 
410 
412  void update(FCL_REAL distance, const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_)
413  {
414  if(min_distance > distance)
415  {
416  min_distance = distance;
417  o1 = o1_;
418  o2 = o2_;
419  b1 = b1_;
420  b2 = b2_;
421  }
422  }
423 
426  const CollisionGeometry* o2_, int b1_, int b2_,
427  const Vec3f& p1, const Vec3f& p2, const Vec3f& normal_)
428  {
429  if(min_distance > distance)
430  {
431  min_distance = distance;
432  o1 = o1_;
433  o2 = o2_;
434  b1 = b1_;
435  b2 = b2_;
436  nearest_points[0] = p1;
437  nearest_points[1] = p2;
438  normal = normal_;
439  }
440  }
441 
443  void update(const DistanceResult& other_result)
444  {
445  if(min_distance > other_result.min_distance)
446  {
447  min_distance = other_result.min_distance;
448  o1 = other_result.o1;
449  o2 = other_result.o2;
450  b1 = other_result.b1;
451  b2 = other_result.b2;
452  nearest_points[0] = other_result.nearest_points[0];
453  nearest_points[1] = other_result.nearest_points[1];
454  normal = other_result.normal;
455  }
456  }
457 
459  void clear()
460  {
461  min_distance = (std::numeric_limits<FCL_REAL>::max)();
462  o1 = NULL;
463  o2 = NULL;
464  b1 = NONE;
465  b2 = NONE;
466  }
467 
469  inline bool operator ==(const DistanceResult& other) const
470  {
471  bool is_same = min_distance == other.min_distance
472  && nearest_points[0] == other.nearest_points[0]
473  && nearest_points[1] == other.nearest_points[1]
474  && o1 == other.o1
475  && o2 == other.o2
476  && b1 == other.b1
477  && b2 == other.b2;
478 
479 // TODO: check also that two GeometryObject are indeed equal.
480  if ((o1 != NULL) ^ (other.o1 != NULL)) return false;
481  is_same &= (o1 == other.o1);
482 // else if (o1 != NULL and other.o1 != NULL) is_same &= *o1 == *other.o1;
483 
484  if ((o2 != NULL) ^ (other.o2 != NULL)) return false;
485  is_same &= (o2 == other.o2);
486 // else if (o2 != NULL and other.o2 != NULL) is_same &= *o2 == *other.o2;
487 
488  return is_same;
489  }
490 
491 };
492 
494 {return static_cast<CollisionRequestFlag>(~static_cast<const int>(a));}
495 
497 {return static_cast<CollisionRequestFlag>(static_cast<const int>(a) | static_cast<const int>(b));}
498 
500 {return static_cast<CollisionRequestFlag>(static_cast<const int>(a) & static_cast<const int>(b));}
501 
503 {return static_cast<CollisionRequestFlag>(static_cast<const int>(a) ^ static_cast<const int>(b));}
504 
506 {return (CollisionRequestFlag&)((int&)(a) |= static_cast<const int>(b));}
507 
509 {return (CollisionRequestFlag&)((int&)(a) &= static_cast<const int>(b));}
510 
512 {return (CollisionRequestFlag&)((int&)(a) ^= static_cast<const int>(b));}
513 
514 }
515 
516 } // namespace hpp
517 
518 #endif
Contact(const CollisionGeometry *o1_, const CollisionGeometry *o2_, int b1_, int b2_)
Definition: collision_data.h:97
support_func_guess_t cached_support_func_guess
the support function intial guess set by user
Definition: collision_data.h:149
size_t num_max_contacts
The maximum number of contacts will return.
Definition: collision_data.h:200
CollisionRequestFlag operator|(CollisionRequestFlag a, CollisionRequestFlag b)
Definition: collision_data.h:496
request to the distance computation
Definition: collision_data.h:333
bool operator==(const QueryRequest &other) const
whether two QueryRequest are the same or not
Definition: collision_data.h:160
void update(const DistanceResult &other_result)
add distance information into the result
Definition: collision_data.h:443
support_func_guess_t cached_support_func_guess
stores the last support function vertex index, when relevant.
Definition: collision_data.h:175
const CollisionGeometry * o1
collision object 1
Definition: collision_data.h:382
base class for all query results
Definition: collision_data.h:169
Main namespace.
Definition: AABB.h:43
collision result
Definition: collision_data.h:249
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:397
Vec3f nearest_points[2]
nearest points
Definition: collision_data.h:376
bool enable_cached_gjk_guess
whether enable gjk intial guess
Definition: collision_data.h:143
void updateDistanceLowerBound(const FCL_REAL &distance_lower_bound_)
Update the lower bound only if the distance in inferior.
Definition: collision_data.h:269
CollisionRequestFlag & operator|=(CollisionRequestFlag &a, CollisionRequestFlag b)
Definition: collision_data.h:505
QueryRequest()
Definition: collision_data.h:151
size_t numContacts() const
number of contacts found
Definition: collision_data.h:295
void addContact(const Contact &c)
add one contact into result structure
Definition: collision_data.h:276
FCL_REAL distance_lower_bound
Definition: collision_data.h:260
Definition: collision_data.h:191
void clear()
clear the result
Definition: collision_data.h:459
const CollisionGeometry * o1
collision object 1
Definition: collision_data.h:60
CollisionRequestFlag & operator &=(CollisionRequestFlag &a, CollisionRequestFlag b)
Definition: collision_data.h:508
CollisionRequestFlag
flag declaration for specifying required params in CollisionResult
Definition: collision_data.h:189
CollisionRequestFlag operator^(CollisionRequestFlag a, CollisionRequestFlag b)
Definition: collision_data.h:502
request to the collision algorithm
Definition: collision_data.h:197
void clear()
clear the results obtained
Definition: collision_data.h:320
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:85
double FCL_REAL
Definition: data_types.h:66
void updateGuess(const QueryResult &result)
Definition: collision_data.h:178
CollisionRequestFlag & operator^=(CollisionRequestFlag &a, CollisionRequestFlag b)
Definition: collision_data.h:511
FCL_REAL min_distance
minimum distance between two objects. if two objects are in collision, min_distance <= 0...
Definition: collision_data.h:373
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:391
Vec3f normal
In case both objects are in collision, store the normal.
Definition: collision_data.h:379
void getContacts(std::vector< Contact > &contacts_) const
get all the contacts
Definition: collision_data.h:313
Vec3f cached_gjk_guess
the gjk intial guess set by user
Definition: collision_data.h:146
Contact(const CollisionGeometry *o1_, const CollisionGeometry *o2_, int b1_, int b2_, const Vec3f &pos_, const Vec3f &normal_, FCL_REAL depth_)
Definition: collision_data.h:103
Eigen::Vector2i support_func_guess_t
Definition: data_types.h:69
Vec3f pos
contact position, in world space
Definition: collision_data.h:82
DistanceResult(FCL_REAL min_distance_=(std::numeric_limits< FCL_REAL >::max)())
Definition: collision_data.h:402
FCL_REAL break_distance
Distance below which bounding volumes are broken down. See Collision detection and distance lower bou...
Definition: collision_data.h:214
FCL_REAL rel_err
error threshold for approximate distance
Definition: collision_data.h:339
CollisionRequest()
Definition: collision_data.h:225
bool enable_nearest_points
whether to return the nearest points
Definition: collision_data.h:336
bool enable_distance_lower_bound
Whether a lower bound on distance is returned when objects are disjoint.
Definition: collision_data.h:206
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:263
const Contact & getContact(size_t i) const
get the i-th contact calculated
Definition: collision_data.h:301
CollisionRequestFlag operator &(CollisionRequestFlag a, CollisionRequestFlag b)
Definition: collision_data.h:499
Definition: collision_data.h:192
bool enable_contact
whether the contact information (normal, penetration depth and contact position) will return ...
Definition: collision_data.h:203
Vec3f cached_gjk_guess
stores the last GJK ray when relevant.
Definition: collision_data.h:172
bool isCollision() const
return binary collision result
Definition: collision_data.h:289
base class for all query requests
Definition: collision_data.h:140
CollisionRequestFlag operator~(CollisionRequestFlag a)
Definition: collision_data.h:493
Definition: collision_data.h:193
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:412
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:425
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:76
The geometry for the object for collision or distance computation.
Definition: collision_object.h:63
FCL_REAL abs_err
Definition: collision_data.h:340
Vec3f normal
contact normal, pointing from o1 to o2
Definition: collision_data.h:79
const CollisionGeometry * o2
collision object 2
Definition: collision_data.h:385
DistanceRequest(bool enable_nearest_points_=false, FCL_REAL rel_err_=0.0, FCL_REAL abs_err_=0.0)
Definition: collision_data.h:345
#define HPP_FCL_DLLAPI
Definition: config.hh:64
CollisionRequest(const CollisionRequestFlag flag, size_t num_max_contacts_)
Definition: collision_data.h:216
distance result
Definition: collision_data.h:367
FCL_REAL security_margin
Distance below which objects are considered in collision. See Collision detection and distance lower ...
Definition: collision_data.h:210
Contact()
Definition: collision_data.h:91