hpp-fcl  1.4.5
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 
56 const int GST_INDEP HPP_FCL_DEPRECATED = 0;
57 
60 {
63 
66 
71  int b1;
72 
73 
78  int b2;
79 
82 
85 
88 
89 
91  static const int NONE = -1;
92 
93  Contact() : o1(NULL),
94  o2(NULL),
95  b1(NONE),
96  b2(NONE)
97  {}
98 
99  Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_) : o1(o1_),
100  o2(o2_),
101  b1(b1_),
102  b2(b2_)
103  {}
104 
105  Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_,
106  const Vec3f& pos_, const Vec3f& normal_, FCL_REAL depth_) : o1(o1_),
107  o2(o2_),
108  b1(b1_),
109  b2(b2_),
110  normal(normal_),
111  pos(pos_),
112  penetration_depth(depth_)
113  {}
114 
115  bool operator < (const Contact& other) const
116  {
117  if(b1 == other.b1)
118  return b2 < other.b2;
119  return b1 < other.b1;
120  }
121 
122  bool operator == (const Contact& other) const
123  {
124  return o1 == other.o1
125  && o2 == other.o2
126  && b1 == other.b1
127  && b2 == other.b2
128  && normal == other.normal
129  && pos == other.pos
130  && penetration_depth == other.penetration_depth;
131  }
132 
133  bool operator != (const Contact& other) const
134  {
135  return !(*this == other);
136  }
137 };
138 
139 struct QueryResult;
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  {}
158 
159  void updateGuess(const QueryResult& result);
160 };
161 
164 {
167 
170 };
171 
172 inline void QueryRequest::updateGuess(const QueryResult& result)
173 {
174  if (enable_cached_gjk_guess) {
175  cached_gjk_guess = result.cached_gjk_guess;
176  cached_support_func_guess = result.cached_support_func_guess;
177  }
178 }
179 
180 struct CollisionResult;
181 
184 {
185  CONTACT = 0x00001,
187  NO_REQUEST = 0x01000
188 };
189 
192 {
195 
198 
201 
205 
209 
210  explicit CollisionRequest(size_t num_max_contacts_,
211  bool enable_contact_ = false,
212  bool enable_distance_lower_bound_ = false,
213  size_t num_max_cost_sources_ = 1,
214  bool enable_cost_ = false,
215  bool use_approximate_cost_ = true)
216  HPP_FCL_DEPRECATED;
217 
218  explicit CollisionRequest(const CollisionRequestFlag flag, size_t num_max_contacts_) :
219  num_max_contacts(num_max_contacts_),
220  enable_contact(flag & CONTACT),
221  enable_distance_lower_bound (flag & DISTANCE_LOWER_BOUND),
222  security_margin (0),
223  break_distance (1e-3)
224  {
225  }
226 
228  num_max_contacts(1),
229  enable_contact(false),
230  enable_distance_lower_bound (false),
231  security_margin (0),
232  break_distance (1e-3)
233  {
234  }
235 
236  bool isSatisfied(const CollisionResult& result) const;
237 };
238 
241 {
242 private:
244  std::vector<Contact> contacts;
245 
246 public:
252 
253 public:
255  : distance_lower_bound (std::numeric_limits<FCL_REAL>::max())
256  {
257  }
258 
260  inline void updateDistanceLowerBound (const FCL_REAL& distance_lower_bound_)
261  {
262  if (distance_lower_bound_ < distance_lower_bound)
263  distance_lower_bound = distance_lower_bound_;
264  }
265 
267  inline void addContact(const Contact& c)
268  {
269  contacts.push_back(c);
270  }
271 
273  inline bool operator ==(const CollisionResult& other) const
274  {
275  return contacts == other.contacts
276  && distance_lower_bound == other.distance_lower_bound;
277  }
278 
280  bool isCollision() const
281  {
282  return contacts.size() > 0;
283  }
284 
286  size_t numContacts() const
287  {
288  return contacts.size();
289  }
290 
292  const Contact& getContact(size_t i) const
293  {
294  if(contacts.size() == 0)
295  throw std::invalid_argument("The number of contacts is zero. No Contact can be returned.");
296 
297  if(i < contacts.size())
298  return contacts[i];
299  else
300  return contacts.back();
301  }
302 
304  void getContacts(std::vector<Contact>& contacts_) const
305  {
306  contacts_.resize(contacts.size());
307  std::copy(contacts.begin(), contacts.end(), contacts_.begin());
308  }
309 
311  void clear()
312  {
313  contacts.clear();
314  }
315 
318  friend void invertResults(CollisionResult& result);
319 };
320 
321 struct DistanceResult;
322 
325 {
328 
330  FCL_REAL rel_err; // relative error, between 0 and 1
331  FCL_REAL abs_err; // absoluate error
332 
334  DistanceRequest(bool enable_nearest_points_,
335  FCL_REAL rel_err_,
336  FCL_REAL abs_err_,
337  int /*unused*/) HPP_FCL_DEPRECATED :
338  enable_nearest_points(enable_nearest_points_),
339  rel_err(rel_err_),
340  abs_err(abs_err_)
341  {
342  }
343 
347  DistanceRequest(bool enable_nearest_points_ = false,
348  FCL_REAL rel_err_ = 0.0,
349  FCL_REAL abs_err_ = 0.0) :
350  enable_nearest_points(enable_nearest_points_),
351  rel_err(rel_err_),
352  abs_err(abs_err_)
353  {
354  }
355 
356  bool isSatisfied(const DistanceResult& result) const;
357 };
358 
361 {
362 
363 public:
364 
367 
369  Vec3f nearest_points[2];
370 
373 
376 
379 
384  int b1;
385 
390  int b2;
391 
393  static const int NONE = -1;
394 
395  DistanceResult(FCL_REAL min_distance_ =
396  std::numeric_limits<FCL_REAL>::max()):
397  min_distance(min_distance_), o1(NULL), o2(NULL), b1(NONE), b2(NONE)
398  {
399  Vec3f nan (Vec3f::Constant(std::numeric_limits<FCL_REAL>::quiet_NaN()));
400  nearest_points [0] = nearest_points [1] = normal = nan;
401  }
402 
403 
405  void update(FCL_REAL distance, const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_)
406  {
407  if(min_distance > distance)
408  {
409  min_distance = distance;
410  o1 = o1_;
411  o2 = o2_;
412  b1 = b1_;
413  b2 = b2_;
414  }
415  }
416 
419  const CollisionGeometry* o2_, int b1_, int b2_,
420  const Vec3f& p1, const Vec3f& p2, const Vec3f& normal_)
421  {
422  if(min_distance > distance)
423  {
424  min_distance = distance;
425  o1 = o1_;
426  o2 = o2_;
427  b1 = b1_;
428  b2 = b2_;
429  nearest_points[0] = p1;
430  nearest_points[1] = p2;
431  normal = normal_;
432  }
433  }
434 
436  void update(const DistanceResult& other_result)
437  {
438  if(min_distance > other_result.min_distance)
439  {
440  min_distance = other_result.min_distance;
441  o1 = other_result.o1;
442  o2 = other_result.o2;
443  b1 = other_result.b1;
444  b2 = other_result.b2;
445  nearest_points[0] = other_result.nearest_points[0];
446  nearest_points[1] = other_result.nearest_points[1];
447  normal = other_result.normal;
448  }
449  }
450 
452  void clear()
453  {
454  min_distance = std::numeric_limits<FCL_REAL>::max();
455  o1 = NULL;
456  o2 = NULL;
457  b1 = NONE;
458  b2 = NONE;
459  }
460 
462  inline bool operator ==(const DistanceResult& other) const
463  {
464  bool is_same = min_distance == other.min_distance
465  && nearest_points[0] == other.nearest_points[0]
466  && nearest_points[1] == other.nearest_points[1]
467  && o1 == other.o1
468  && o2 == other.o2
469  && b1 == other.b1
470  && b2 == other.b2;
471 
472 // TODO: check also that two GeometryObject are indeed equal.
473  if ((o1 != NULL) xor (other.o1 != NULL)) return false;
474  is_same &= (o1 == other.o1);
475 // else if (o1 != NULL and other.o1 != NULL) is_same &= *o1 == *other.o1;
476 
477  if ((o2 != NULL) xor (other.o2 != NULL)) return false;
478  is_same &= (o2 == other.o2);
479 // else if (o2 != NULL and other.o2 != NULL) is_same &= *o2 == *other.o2;
480 
481  return is_same;
482  }
483 
484 };
485 
487 {return static_cast<CollisionRequestFlag>(~static_cast<const int>(a));}
488 
490 {return static_cast<CollisionRequestFlag>(static_cast<const int>(a) | static_cast<const int>(b));}
491 
493 {return static_cast<CollisionRequestFlag>(static_cast<const int>(a) & static_cast<const int>(b));}
494 
496 {return static_cast<CollisionRequestFlag>(static_cast<const int>(a) ^ static_cast<const int>(b));}
497 
499 {return (CollisionRequestFlag&)((int&)(a) |= static_cast<const int>(b));}
500 
502 {return (CollisionRequestFlag&)((int&)(a) &= static_cast<const int>(b));}
503 
505 {return (CollisionRequestFlag&)((int&)(a) ^= static_cast<const int>(b));}
506 
507 }
508 
509 } // namespace hpp
510 
511 #endif
Contact(const CollisionGeometry *o1_, const CollisionGeometry *o2_, int b1_, int b2_)
Definition: collision_data.h:99
support_func_guess_t cached_support_func_guess
the support function intial guess set by user
Definition: collision_data.h:151
size_t num_max_contacts
The maximum number of contacts will return.
Definition: collision_data.h:194
CollisionRequestFlag operator|(CollisionRequestFlag a, CollisionRequestFlag b)
Definition: collision_data.h:489
DistanceRequest(bool enable_nearest_points_, FCL_REAL rel_err_, FCL_REAL abs_err_, int) HPP_FCL_DEPRECATED
Definition: collision_data.h:334
const int GST_INDEP HPP_FCL_DEPRECATED
Definition: collision_data.h:56
request to the distance computation
Definition: collision_data.h:324
void update(const DistanceResult &other_result)
add distance information into the result
Definition: collision_data.h:436
support_func_guess_t cached_support_func_guess
stores the last support function vertex index, when relevant.
Definition: collision_data.h:169
FCL_REAL distance(const Matrix3f &R0, const Vec3f &T0, const kIOS &b1, const kIOS &b2, Vec3f *P=NULL, Vec3f *Q=NULL) HPP_FCL_DLLAPI
Approximate distance between two kIOS bounding volumes.
const CollisionGeometry * o1
collision object 1
Definition: collision_data.h:375
base class for all query results
Definition: collision_data.h:163
Main namespace.
Definition: AABB.h:43
collision result
Definition: collision_data.h:240
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:390
Vec3f nearest_points[2]
nearest points
Definition: collision_data.h:369
bool enable_cached_gjk_guess
whether enable gjk intial guess
Definition: collision_data.h:145
void updateDistanceLowerBound(const FCL_REAL &distance_lower_bound_)
Update the lower bound only if the distance in inferior.
Definition: collision_data.h:260
CollisionRequestFlag & operator|=(CollisionRequestFlag &a, CollisionRequestFlag b)
Definition: collision_data.h:498
QueryRequest()
Definition: collision_data.h:153
size_t numContacts() const
number of contacts found
Definition: collision_data.h:286
void addContact(const Contact &c)
add one contact into result structure
Definition: collision_data.h:267
DistanceResult(FCL_REAL min_distance_=std::numeric_limits< FCL_REAL >::max())
Definition: collision_data.h:395
FCL_REAL distance_lower_bound
Definition: collision_data.h:251
Definition: collision_data.h:185
void clear()
clear the result
Definition: collision_data.h:452
const CollisionGeometry * o1
collision object 1
Definition: collision_data.h:62
CollisionRequestFlag & operator &=(CollisionRequestFlag &a, CollisionRequestFlag b)
Definition: collision_data.h:501
CollisionRequestFlag
flag declaration for specifying required params in CollisionResult
Definition: collision_data.h:183
CollisionRequestFlag operator^(CollisionRequestFlag a, CollisionRequestFlag b)
Definition: collision_data.h:495
request to the collision algorithm
Definition: collision_data.h:191
void clear()
clear the results obtained
Definition: collision_data.h:311
FCL_REAL penetration_depth
penetration depth
Definition: collision_data.h:87
double FCL_REAL
Definition: data_types.h:69
void updateGuess(const QueryResult &result)
Definition: collision_data.h:172
CollisionRequestFlag & operator^=(CollisionRequestFlag &a, CollisionRequestFlag b)
Definition: collision_data.h:504
FCL_REAL min_distance
minimum distance between two objects. if two objects are in collision, min_distance <= 0...
Definition: collision_data.h:366
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:384
Vec3f normal
In case both objects are in collision, store the normal.
Definition: collision_data.h:372
void getContacts(std::vector< Contact > &contacts_) const
get all the contacts
Definition: collision_data.h:304
Vec3f cached_gjk_guess
the gjk intial guess set by user
Definition: collision_data.h:148
Contact(const CollisionGeometry *o1_, const CollisionGeometry *o2_, int b1_, int b2_, const Vec3f &pos_, const Vec3f &normal_, FCL_REAL depth_)
Definition: collision_data.h:105
Eigen::Vector2i support_func_guess_t
Definition: data_types.h:76
Vec3f pos
contact position, in world space
Definition: collision_data.h:84
FCL_REAL break_distance
Distance below which bounding volumes are broken down. See Collision detection and distance lower bou...
Definition: collision_data.h:208
FCL_REAL rel_err
error threshold for approximate distance
Definition: collision_data.h:330
CollisionRequest()
Definition: collision_data.h:227
bool enable_nearest_points
whether to return the nearest points
Definition: collision_data.h:327
bool enable_distance_lower_bound
Whether a lower bound on distance is returned when objects are disjoint.
Definition: collision_data.h:200
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:71
CollisionResult()
Definition: collision_data.h:254
const Contact & getContact(size_t i) const
get the i-th contact calculated
Definition: collision_data.h:292
CollisionRequestFlag operator &(CollisionRequestFlag a, CollisionRequestFlag b)
Definition: collision_data.h:492
Definition: collision_data.h:186
bool enable_contact
whether the contact information (normal, penetration depth and contact position) will return ...
Definition: collision_data.h:197
Vec3f cached_gjk_guess
stores the last GJK ray when relevant.
Definition: collision_data.h:166
bool isCollision() const
return binary collision result
Definition: collision_data.h:280
base class for all query requests
Definition: collision_data.h:142
CollisionRequestFlag operator~(CollisionRequestFlag a)
Definition: collision_data.h:486
Definition: collision_data.h:187
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:74
const CollisionGeometry * o2
collision object 2
Definition: collision_data.h:65
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:405
Contact information returned by collision.
Definition: collision_data.h:59
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:418
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:78
The geometry for the object for collision or distance computation.
Definition: collision_object.h:63
FCL_REAL abs_err
Definition: collision_data.h:331
Vec3f normal
contact normal, pointing from o1 to o2
Definition: collision_data.h:81
const CollisionGeometry * o2
collision object 2
Definition: collision_data.h:378
DistanceRequest(bool enable_nearest_points_=false, FCL_REAL rel_err_=0.0, FCL_REAL abs_err_=0.0)
Definition: collision_data.h:347
#define HPP_FCL_DLLAPI
Definition: config.hh:64
distance result
Definition: collision_data.h:360
FCL_REAL security_margin
Distance below which objects are considered in collision. See Collision detection and distance lower ...
Definition: collision_data.h:204
Contact()
Definition: collision_data.h:93