hpp-fcl  1.6.0
HPP fork of FCL -- The Flexible Collision Library
traversal_node_bvhs.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 
38 #ifndef HPP_FCL_TRAVERSAL_NODE_MESHES_H
39 #define HPP_FCL_TRAVERSAL_NODE_MESHES_H
40 
42 
43 #include <hpp/fcl/collision_data.h>
45 #include <hpp/fcl/BV/BV_node.h>
46 #include <hpp/fcl/BV/BV.h>
47 #include <hpp/fcl/BVH/BVH_model.h>
52 
53 #include <limits>
54 #include <vector>
55 #include <cassert>
56 
57 namespace hpp
58 {
59 namespace fcl
60 {
61 
64 
66 template<typename BV>
67 class BVHCollisionTraversalNode : public CollisionTraversalNodeBase
68 {
69 public:
70  BVHCollisionTraversalNode(const CollisionRequest& request) :
71  CollisionTraversalNodeBase (request)
72  {
73  model1 = NULL;
74  model2 = NULL;
75 
76  num_bv_tests = 0;
77  num_leaf_tests = 0;
78  query_time_seconds = 0.0;
79  }
80 
82  bool isFirstNodeLeaf(int b) const
83  {
84  return model1->getBV(b).isLeaf();
85  }
86 
88  bool isSecondNodeLeaf(int b) const
89  {
90  return model2->getBV(b).isLeaf();
91  }
92 
94  bool firstOverSecond(int b1, int b2) const
95  {
96  FCL_REAL sz1 = model1->getBV(b1).bv.size();
97  FCL_REAL sz2 = model2->getBV(b2).bv.size();
98 
99  bool l1 = model1->getBV(b1).isLeaf();
100  bool l2 = model2->getBV(b2).isLeaf();
101 
102  if(l2 || (!l1 && (sz1 > sz2)))
103  return true;
104  return false;
105  }
106 
108  int getFirstLeftChild(int b) const
109  {
110  return model1->getBV(b).leftChild();
111  }
112 
114  int getFirstRightChild(int b) const
115  {
116  return model1->getBV(b).rightChild();
117  }
118 
120  int getSecondLeftChild(int b) const
121  {
122  return model2->getBV(b).leftChild();
123  }
124 
126  int getSecondRightChild(int b) const
127  {
128  return model2->getBV(b).rightChild();
129  }
130 
132  const BVHModel<BV>* model1;
134  const BVHModel<BV>* model2;
135 
137  mutable int num_bv_tests;
138  mutable int num_leaf_tests;
139  mutable FCL_REAL query_time_seconds;
140 };
141 
143 template<typename BV, int _Options = RelativeTransformationIsIdentity>
144 class MeshCollisionTraversalNode : public BVHCollisionTraversalNode<BV>
145 {
146 public:
147  enum {
148  Options = _Options,
149  RTIsIdentity = _Options & RelativeTransformationIsIdentity
150  };
151 
152  MeshCollisionTraversalNode(const CollisionRequest& request) :
153  BVHCollisionTraversalNode<BV> (request)
154  {
155  vertices1 = NULL;
156  vertices2 = NULL;
157  tri_indices1 = NULL;
158  tri_indices2 = NULL;
159  }
160 
162  bool BVDisjoints(int b1, int b2) const
163  {
164  if(this->enable_statistics) this->num_bv_tests++;
165  if (RTIsIdentity)
166  return !this->model1->getBV(b1).overlap(this->model2->getBV(b2));
167  else
168  return !overlap(RT._R(), RT._T(),
169  this->model1->getBV(b1).bv, this->model2->getBV(b2).bv);
170  }
171 
176  bool BVDisjoints(int b1, int b2, FCL_REAL& sqrDistLowerBound) const
177  {
178  if(this->enable_statistics) this->num_bv_tests++;
179  if (RTIsIdentity)
180  return !this->model1->getBV(b1).overlap(this->model2->getBV(b2),
181  this->request, sqrDistLowerBound);
182  else {
183  bool res = !overlap(RT._R(), RT._T(),
184  this->model1->getBV(b1).bv, this->model2->getBV(b2).bv,
185  this->request, sqrDistLowerBound);
186  assert (!res || sqrDistLowerBound > 0);
187  return res;
188  }
189  }
190 
205  void leafCollides(int b1, int b2, FCL_REAL& sqrDistLowerBound) const
206  {
207  if(this->enable_statistics) this->num_leaf_tests++;
208 
209  const BVNode<BV>& node1 = this->model1->getBV(b1);
210  const BVNode<BV>& node2 = this->model2->getBV(b2);
211 
212  int primitive_id1 = node1.primitiveId();
213  int primitive_id2 = node2.primitiveId();
214 
215  const Triangle& tri_id1 = tri_indices1[primitive_id1];
216  const Triangle& tri_id2 = tri_indices2[primitive_id2];
217 
218  const Vec3f& P1 = vertices1[tri_id1[0]];
219  const Vec3f& P2 = vertices1[tri_id1[1]];
220  const Vec3f& P3 = vertices1[tri_id1[2]];
221  const Vec3f& Q1 = vertices2[tri_id2[0]];
222  const Vec3f& Q2 = vertices2[tri_id2[1]];
223  const Vec3f& Q3 = vertices2[tri_id2[2]];
224 
225  TriangleP tri1 (P1, P2, P3);
226  TriangleP tri2 (Q1, Q2, Q3);
227  GJKSolver solver;
228  Vec3f p1, p2; // closest points if no collision contact points if collision.
229  Vec3f normal;
231  solver.shapeDistance (tri1, this->tf1, tri2, this->tf2,
232  distance, p1, p2, normal);
233  FCL_REAL distToCollision = distance - this->request.security_margin;
234  sqrDistLowerBound = distance * distance;
235  if (distToCollision <= 0) { // collision
236  Vec3f p (p1); // contact point
237  FCL_REAL penetrationDepth (0);
238  if(this->result->numContacts() < this->request.num_max_contacts) {
239  // How much (Q1, Q2, Q3) should be moved so that all vertices are
240  // above (P1, P2, P3).
241  penetrationDepth = -distance;
242  if (distance > 0) {
243  normal = (p2-p1).normalized ();
244  p = .5* (p1+p2);
245  }
246  this->result->addContact(Contact(this->model1, this->model2,
247  primitive_id1, primitive_id2,
248  p, normal, penetrationDepth));
249  }
250  }
251  }
252 
253  Vec3f* vertices1;
254  Vec3f* vertices2;
255 
256  Triangle* tri_indices1;
257  Triangle* tri_indices2;
258 
259  details::RelativeTransformation<!bool(RTIsIdentity)> RT;
260 };
261 
263 typedef MeshCollisionTraversalNode<OBB , 0> MeshCollisionTraversalNodeOBB ;
264 typedef MeshCollisionTraversalNode<RSS , 0> MeshCollisionTraversalNodeRSS ;
265 typedef MeshCollisionTraversalNode<kIOS , 0> MeshCollisionTraversalNodekIOS ;
266 typedef MeshCollisionTraversalNode<OBBRSS, 0> MeshCollisionTraversalNodeOBBRSS;
267 
269 
270 namespace details
271 {
272  template<typename BV> struct DistanceTraversalBVDistanceLowerBound_impl
273  {
274  static FCL_REAL run(const BVNode<BV>& b1, const BVNode<BV>& b2)
275  {
276  return b1.distance(b2);
277  }
278  static FCL_REAL run(const Matrix3f& R, const Vec3f& T, const BVNode<BV>& b1, const BVNode<BV>& b2)
279  {
280  return distance(R, T, b1.bv, b2.bv);
281  }
282  };
283 
284  template<> struct DistanceTraversalBVDistanceLowerBound_impl<OBB>
285  {
286  static FCL_REAL run(const BVNode<OBB>& b1, const BVNode<OBB>& b2)
287  {
288  FCL_REAL sqrDistLowerBound;
289  CollisionRequest request (DISTANCE_LOWER_BOUND, 0);
290  // request.break_distance = ?
291  if (b1.overlap(b2, request, sqrDistLowerBound)) {
292  // TODO A penetration upper bound should be computed.
293  return -1;
294  }
295  return sqrt (sqrDistLowerBound);
296  }
297  static FCL_REAL run(const Matrix3f& R, const Vec3f& T, const BVNode<OBB>& b1, const BVNode<OBB>& b2)
298  {
299  FCL_REAL sqrDistLowerBound;
300  CollisionRequest request (DISTANCE_LOWER_BOUND, 0);
301  // request.break_distance = ?
302  if (overlap(R, T, b1.bv, b2.bv, request, sqrDistLowerBound)) {
303  // TODO A penetration upper bound should be computed.
304  return -1;
305  }
306  return sqrt (sqrDistLowerBound);
307  }
308  };
309 
310  template<> struct DistanceTraversalBVDistanceLowerBound_impl<AABB>
311  {
312  static FCL_REAL run(const BVNode<AABB>& b1, const BVNode<AABB>& b2)
313  {
314  FCL_REAL sqrDistLowerBound;
315  CollisionRequest request (DISTANCE_LOWER_BOUND, 0);
316  // request.break_distance = ?
317  if (b1.overlap(b2, request, sqrDistLowerBound)) {
318  // TODO A penetration upper bound should be computed.
319  return -1;
320  }
321  return sqrt (sqrDistLowerBound);
322  }
323  static FCL_REAL run(const Matrix3f& R, const Vec3f& T, const BVNode<AABB>& b1, const BVNode<AABB>& b2)
324  {
325  FCL_REAL sqrDistLowerBound;
326  CollisionRequest request (DISTANCE_LOWER_BOUND, 0);
327  // request.break_distance = ?
328  if (overlap(R, T, b1.bv, b2.bv, request, sqrDistLowerBound)) {
329  // TODO A penetration upper bound should be computed.
330  return -1;
331  }
332  return sqrt (sqrDistLowerBound);
333  }
334  };
335 } // namespace details
336 
339 
341 template<typename BV>
342 class BVHDistanceTraversalNode : public DistanceTraversalNodeBase
343 {
344 public:
345  BVHDistanceTraversalNode() : DistanceTraversalNodeBase()
346  {
347  model1 = NULL;
348  model2 = NULL;
349 
350  num_bv_tests = 0;
351  num_leaf_tests = 0;
352  query_time_seconds = 0.0;
353  }
354 
356  bool isFirstNodeLeaf(int b) const
357  {
358  return model1->getBV(b).isLeaf();
359  }
360 
362  bool isSecondNodeLeaf(int b) const
363  {
364  return model2->getBV(b).isLeaf();
365  }
366 
368  bool firstOverSecond(int b1, int b2) const
369  {
370  FCL_REAL sz1 = model1->getBV(b1).bv.size();
371  FCL_REAL sz2 = model2->getBV(b2).bv.size();
372 
373  bool l1 = model1->getBV(b1).isLeaf();
374  bool l2 = model2->getBV(b2).isLeaf();
375 
376  if(l2 || (!l1 && (sz1 > sz2)))
377  return true;
378  return false;
379  }
380 
382  int getFirstLeftChild(int b) const
383  {
384  return model1->getBV(b).leftChild();
385  }
386 
388  int getFirstRightChild(int b) const
389  {
390  return model1->getBV(b).rightChild();
391  }
392 
394  int getSecondLeftChild(int b) const
395  {
396  return model2->getBV(b).leftChild();
397  }
398 
400  int getSecondRightChild(int b) const
401  {
402  return model2->getBV(b).rightChild();
403  }
404 
406  const BVHModel<BV>* model1;
408  const BVHModel<BV>* model2;
409 
411  mutable int num_bv_tests;
412  mutable int num_leaf_tests;
413  mutable FCL_REAL query_time_seconds;
414 };
415 
416 
418 template<typename BV, int _Options = RelativeTransformationIsIdentity>
419 class MeshDistanceTraversalNode : public BVHDistanceTraversalNode<BV>
420 {
421 public:
422  enum {
423  Options = _Options,
424  RTIsIdentity = _Options & RelativeTransformationIsIdentity
425  };
426 
427  using BVHDistanceTraversalNode<BV>::enable_statistics;
428  using BVHDistanceTraversalNode<BV>::request;
429  using BVHDistanceTraversalNode<BV>::result;
430  using BVHDistanceTraversalNode<BV>::tf1;
431  using BVHDistanceTraversalNode<BV>::model1;
432  using BVHDistanceTraversalNode<BV>::model2;
433  using BVHDistanceTraversalNode<BV>::num_bv_tests;
434  using BVHDistanceTraversalNode<BV>::num_leaf_tests;
435 
436  MeshDistanceTraversalNode() : BVHDistanceTraversalNode<BV>()
437  {
438  vertices1 = NULL;
439  vertices2 = NULL;
440  tri_indices1 = NULL;
441  tri_indices2 = NULL;
442 
443  rel_err = this->request.rel_err;
444  abs_err = this->request.abs_err;
445  }
446 
447  void preprocess()
448  {
449  if(!RTIsIdentity) preprocessOrientedNode();
450  }
451 
452  void postprocess()
453  {
454  if(!RTIsIdentity) postprocessOrientedNode();
455  }
456 
458  FCL_REAL BVDistanceLowerBound(int b1, int b2) const
459  {
460  if(enable_statistics) num_bv_tests++;
461  if (RTIsIdentity)
462  return details::DistanceTraversalBVDistanceLowerBound_impl<BV>
463  ::run (model1->getBV(b1), model2->getBV(b2));
464  else
465  return details::DistanceTraversalBVDistanceLowerBound_impl<BV>
466  ::run (RT._R(), RT._T(), model1->getBV(b1), model2->getBV(b2));
467  }
468 
470  void leafComputeDistance(int b1, int b2) const
471  {
472  if(this->enable_statistics) this->num_leaf_tests++;
473 
474  const BVNode<BV>& node1 = this->model1->getBV(b1);
475  const BVNode<BV>& node2 = this->model2->getBV(b2);
476 
477  int primitive_id1 = node1.primitiveId();
478  int primitive_id2 = node2.primitiveId();
479 
480  const Triangle& tri_id1 = tri_indices1[primitive_id1];
481  const Triangle& tri_id2 = tri_indices2[primitive_id2];
482 
483  const Vec3f& t11 = vertices1[tri_id1[0]];
484  const Vec3f& t12 = vertices1[tri_id1[1]];
485  const Vec3f& t13 = vertices1[tri_id1[2]];
486 
487  const Vec3f& t21 = vertices2[tri_id2[0]];
488  const Vec3f& t22 = vertices2[tri_id2[1]];
489  const Vec3f& t23 = vertices2[tri_id2[2]];
490 
491  // nearest point pair
492  Vec3f P1, P2, normal;
493 
494  FCL_REAL d2;
495  if (RTIsIdentity)
496  d2 = TriangleDistance::sqrTriDistance (t11, t12, t13, t21, t22, t23,
497  P1, P2);
498  else
499  d2 = TriangleDistance::sqrTriDistance (t11, t12, t13, t21, t22, t23,
500  RT._R(), RT._T(),
501  P1, P2);
502  FCL_REAL d = sqrt(d2);
503 
504  this->result->update(d, this->model1, this->model2, primitive_id1,
505  primitive_id2, P1, P2, normal);
506  }
507 
509  bool canStop(FCL_REAL c) const
510  {
511  if((c >= this->result->min_distance - abs_err) && (c * (1 + rel_err) >= this->result->min_distance))
512  return true;
513  return false;
514  }
515 
516  Vec3f* vertices1;
517  Vec3f* vertices2;
518 
519  Triangle* tri_indices1;
520  Triangle* tri_indices2;
521 
523  FCL_REAL rel_err;
524  FCL_REAL abs_err;
525 
526  details::RelativeTransformation<!bool(RTIsIdentity)> RT;
527 
528 private:
529  void preprocessOrientedNode()
530  {
531  const int init_tri_id1 = 0, init_tri_id2 = 0;
532  const Triangle& init_tri1 = tri_indices1[init_tri_id1];
533  const Triangle& init_tri2 = tri_indices2[init_tri_id2];
534 
535  Vec3f init_tri1_points[3];
536  Vec3f init_tri2_points[3];
537 
538  init_tri1_points[0] = vertices1[init_tri1[0]];
539  init_tri1_points[1] = vertices1[init_tri1[1]];
540  init_tri1_points[2] = vertices1[init_tri1[2]];
541 
542  init_tri2_points[0] = vertices2[init_tri2[0]];
543  init_tri2_points[1] = vertices2[init_tri2[1]];
544  init_tri2_points[2] = vertices2[init_tri2[2]];
545 
546  Vec3f p1, p2, normal;
547  FCL_REAL distance = sqrt (TriangleDistance::sqrTriDistance
548  (init_tri1_points[0], init_tri1_points[1],
549  init_tri1_points[2], init_tri2_points[0],
550  init_tri2_points[1], init_tri2_points[2],
551  RT._R(), RT._T(), p1, p2));
552 
553  result->update(distance, model1, model2, init_tri_id1, init_tri_id2, p1, p2,
554  normal);
555  }
556  void postprocessOrientedNode()
557  {
559  if(request.enable_nearest_points && (result->o1 == model1) && (result->o2 == model2))
560  {
561  result->nearest_points[0] = tf1.transform(result->nearest_points[0]);
562  result->nearest_points[1] = tf1.transform(result->nearest_points[1]);
563  }
564  }
565 };
566 
568 typedef MeshDistanceTraversalNode<RSS , 0> MeshDistanceTraversalNodeRSS;
569 typedef MeshDistanceTraversalNode<kIOS , 0> MeshDistanceTraversalNodekIOS;
570 typedef MeshDistanceTraversalNode<OBBRSS, 0> MeshDistanceTraversalNodeOBBRSS;
571 
573 
575 namespace details
576 {
577 
578 template<typename BV>
579 inline const Matrix3f& getBVAxes(const BV& bv)
580 {
581  return bv.axes;
582 }
583 
584 template<>
585 inline const Matrix3f& getBVAxes<OBBRSS>(const OBBRSS& bv)
586 {
587  return bv.obb.axes;
588 }
589 
590 }
591 
592 }
593 
594 } // namespace hpp
595 
597 
598 #endif
Main namespace.
Definition: AABB.h:43
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:68
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.
double FCL_REAL
Definition: data_types.h:66
Definition: traversal_node_setup.h:775
bool overlap(const Matrix3f &R0, const Vec3f &T0, const AABB &b1, const AABB &b2)
Check collision between two aabbs, b1 is in configuration (R0, T0) and b2 is in identity.
Definition: collision_data.h:192
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:67