hpp-fcl  1.4.4
HPP fork of FCL -- The Flexible Collision Library
traversal_node_bvh_shape.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_TRAVERSAL_NODE_MESH_SHAPE_H
40 #define HPP_FCL_TRAVERSAL_NODE_MESH_SHAPE_H
41 
43 
44 #include <hpp/fcl/collision_data.h>
50 #include <hpp/fcl/BVH/BVH_model.h>
51 
52 
53 namespace hpp
54 {
55 namespace fcl
56 {
57 
60 
62 template<typename BV, typename S>
63 class BVHShapeCollisionTraversalNode : public CollisionTraversalNodeBase
64 {
65 public:
66  BVHShapeCollisionTraversalNode(const CollisionRequest& request) :
67  CollisionTraversalNodeBase (request)
68  {
69  model1 = NULL;
70  model2 = NULL;
71 
72  num_bv_tests = 0;
73  num_leaf_tests = 0;
74  query_time_seconds = 0.0;
75  }
76 
78  bool isFirstNodeLeaf(int b) const
79  {
80  return model1->getBV(b).isLeaf();
81  }
82 
84  int getFirstLeftChild(int b) const
85  {
86  return model1->getBV(b).leftChild();
87  }
88 
90  int getFirstRightChild(int b) const
91  {
92  return model1->getBV(b).rightChild();
93  }
94 
95  const BVHModel<BV>* model1;
96  const S* model2;
97  BV model2_bv;
98 
99  mutable int num_bv_tests;
100  mutable int num_leaf_tests;
101  mutable FCL_REAL query_time_seconds;
102 };
103 
105 template<typename S, typename BV>
106 class ShapeBVHCollisionTraversalNode : public CollisionTraversalNodeBase
107 {
108 public:
109  ShapeBVHCollisionTraversalNode(const CollisionRequest& request) :
110  CollisionTraversalNodeBase(request)
111  {
112  model1 = NULL;
113  model2 = NULL;
114 
115  num_bv_tests = 0;
116  num_leaf_tests = 0;
117  query_time_seconds = 0.0;
118  }
119 
121  bool firstOverSecond(int, int) const
122  {
123  return false;
124  }
125 
127  bool isSecondNodeLeaf(int b) const
128  {
129  return model2->getBV(b).isLeaf();
130  }
131 
133  int getSecondLeftChild(int b) const
134  {
135  return model2->getBV(b).leftChild();
136  }
137 
139  int getSecondRightChild(int b) const
140  {
141  return model2->getBV(b).rightChild();
142  }
143 
144  const S* model1;
145  const BVHModel<BV>* model2;
146  BV model1_bv;
147 
148  mutable int num_bv_tests;
149  mutable int num_leaf_tests;
150  mutable FCL_REAL query_time_seconds;
151 };
152 
153 
155 template<typename BV, typename S,
156  int _Options = RelativeTransformationIsIdentity>
157 class MeshShapeCollisionTraversalNode : public BVHShapeCollisionTraversalNode<BV, S>
158 {
159 public:
160  enum {
161  Options = _Options,
162  RTIsIdentity = _Options & RelativeTransformationIsIdentity
163  };
164 
165  MeshShapeCollisionTraversalNode(const CollisionRequest& request) :
166  BVHShapeCollisionTraversalNode<BV, S> (request)
167  {
168  vertices = NULL;
169  tri_indices = NULL;
170 
171  nsolver = NULL;
172  }
173 
175  bool BVDisjoints(int b1, int /*b2*/) const
176  {
177  if(this->enable_statistics) this->num_bv_tests++;
178  if (RTIsIdentity)
179  return !this->model1->getBV(b1).bv.overlap(this->model2_bv);
180  else
181  return !overlap(this->tf1.getRotation(), this->tf1.getTranslation(), this->model2_bv, this->model1->getBV(b1).bv);
182  }
183 
189  bool BVDisjoints(int b1, int /*b2*/, FCL_REAL& sqrDistLowerBound) const
190  {
191  if(this->enable_statistics) this->num_bv_tests++;
192  bool res;
193  if (RTIsIdentity)
194  res = !this->model1->getBV(b1).bv.overlap(this->model2_bv, this->request, sqrDistLowerBound);
195  else
196  res = !overlap(this->tf1.getRotation(), this->tf1.getTranslation(),
197  this->model2_bv, this->model1->getBV(b1).bv,
198  this->request, sqrDistLowerBound);
199  assert (!res || sqrDistLowerBound > 0);
200  return res;
201  }
202 
204  void leafCollides(int b1, int /*b2*/, FCL_REAL& sqrDistLowerBound) const
205  {
206  if(this->enable_statistics) this->num_leaf_tests++;
207  const BVNode<BV>& node = this->model1->getBV(b1);
208 
209  int primitive_id = node.primitiveId();
210 
211  const Triangle& tri_id = tri_indices[primitive_id];
212 
213  const Vec3f& p1 = vertices[tri_id[0]];
214  const Vec3f& p2 = vertices[tri_id[1]];
215  const Vec3f& p3 = vertices[tri_id[2]];
216 
218  Vec3f normal;
219  Vec3f c1, c2; // closest point
220 
221  bool collision;
222  if (RTIsIdentity) {
223  static const Transform3f Id;
224  collision =
225  nsolver->shapeTriangleInteraction(*(this->model2), this->tf2, p1, p2, p3,
226  Id , distance, c2, c1, normal);
227  } else {
228  collision =
229  nsolver->shapeTriangleInteraction(*(this->model2), this->tf2, p1, p2, p3,
230  this->tf1, distance, c2, c1, normal);
231  }
232 
233  if(collision) {
234  if(this->request.num_max_contacts > this->result->numContacts())
235  {
236  this->result->addContact(Contact(this->model1, this->model2,
237  primitive_id, Contact::NONE,
238  c1, -normal, -distance));
239  assert (this->result->isCollision ());
240  return;
241  }
242  }
243  sqrDistLowerBound = distance * distance;
244  assert (distance > 0);
245  if ( this->request.security_margin > 0
246  && distance <= this->request.security_margin)
247  {
248  this->result->addContact(Contact(this->model1, this->model2,
249  primitive_id, Contact::NONE,
250  .5 * (c1+c2), (c2-c1).normalized (),
251  -distance));
252  }
253  assert (!this->result->isCollision () || sqrDistLowerBound > 0);
254  }
255 
256  Vec3f* vertices;
257  Triangle* tri_indices;
258 
259  const GJKSolver* nsolver;
260 };
261 
263 template<typename S, typename BV,
264  int _Options = RelativeTransformationIsIdentity>
265 class ShapeMeshCollisionTraversalNode : public ShapeBVHCollisionTraversalNode<S, BV>
266 {
267 public:
268  enum {
269  Options = _Options,
270  RTIsIdentity = _Options & RelativeTransformationIsIdentity
271  };
272 
273  ShapeMeshCollisionTraversalNode() : ShapeBVHCollisionTraversalNode<S, BV>()
274  {
275  vertices = NULL;
276  tri_indices = NULL;
277 
278  nsolver = NULL;
279  }
280 
283  bool BVDisjoints(int /*b1*/, int b2) const
284  {
285  if(this->enable_statistics) this->num_bv_tests++;
286  if (RTIsIdentity)
287  return !this->model2->getBV(b2).bv.overlap(this->model1_bv);
288  else
289  return !overlap(this->tf2.getRotation(), this->tf2.getTranslation(), this->model1_bv, this->model2->getBV(b2).bv);
290  }
291 
296  bool BVDisjoints(int /*b1*/, int b2, FCL_REAL& sqrDistLowerBound) const
297  {
298  if(this->enable_statistics) this->num_bv_tests++;
299  bool res;
300  if (RTIsIdentity)
301  res = !this->model2->getBV(b2).bv.overlap(this->model1_bv, sqrDistLowerBound);
302  else
303  res = !overlap(this->tf2.getRotation(), this->tf2.getTranslation(),
304  this->model1_bv, this->model2->getBV(b2).bv,
305  sqrDistLowerBound);
306  assert (!res || sqrDistLowerBound > 0);
307  return res;
308  }
309 
311  void leafCollides(int /*b1*/, int b2, FCL_REAL& sqrDistLowerBound) const
312  {
313  if(this->enable_statistics) this->num_leaf_tests++;
314  const BVNode<BV>& node = this->model2->getBV(b2);
315 
316  int primitive_id = node.primitiveId();
317 
318  const Triangle& tri_id = tri_indices[primitive_id];
319 
320  const Vec3f& p1 = vertices[tri_id[0]];
321  const Vec3f& p2 = vertices[tri_id[1]];
322  const Vec3f& p3 = vertices[tri_id[2]];
323 
325  Vec3f normal;
326  Vec3f c1, c2; // closest points
327 
328  bool collision;
329  if (RTIsIdentity) {
330  static const Transform3f Id;
331  collision =
332  nsolver->shapeTriangleInteraction(*(this->model1), this->tf1, p1, p2, p3,
333  Id , c1, c2, distance, normal);
334  } else {
335  collision =
336  nsolver->shapeTriangleInteraction(*(this->model1), this->tf1, p1, p2, p3,
337  this->tf2, c1, c2, distance, normal);
338  }
339 
340  if (collision) {
341  if(this->request.num_max_contacts > this->result->numContacts())
342  {
343  this->result->addContact (Contact(this->model1 , this->model2,
344  Contact::NONE, primitive_id,
345  c1, normal, -distance));
346  assert (this->result->isCollision ());
347  return;
348  }
349  }
350  sqrDistLowerBound = distance * distance;
351  assert (distance > 0);
352  if ( this->request.security_margin == 0
353  && distance <= this->request.security_margin)
354  {
355  this->result.addContact (Contact(this->model1 , this->model2,
356  Contact::NONE, primitive_id,
357  .5 * (c1+c2), (c2-c1).normalized (),
358  -distance));
359  }
360  assert (!this->result->isCollision () || sqrDistLowerBound > 0);
361  }
362 
363  Vec3f* vertices;
364  Triangle* tri_indices;
365 
366  const GJKSolver* nsolver;
367 };
368 
370 
373 
375 template<typename BV, typename S>
376 class BVHShapeDistanceTraversalNode : public DistanceTraversalNodeBase
377 {
378 public:
379  BVHShapeDistanceTraversalNode() : DistanceTraversalNodeBase()
380  {
381  model1 = NULL;
382  model2 = NULL;
383 
384  num_bv_tests = 0;
385  num_leaf_tests = 0;
386  query_time_seconds = 0.0;
387  }
388 
390  bool isFirstNodeLeaf(int b) const
391  {
392  return model1->getBV(b).isLeaf();
393  }
394 
396  int getFirstLeftChild(int b) const
397  {
398  return model1->getBV(b).leftChild();
399  }
400 
402  int getFirstRightChild(int b) const
403  {
404  return model1->getBV(b).rightChild();
405  }
406 
408  FCL_REAL BVDistanceLowerBound(int b1, int /*b2*/) const
409  {
410  return model1->getBV(b1).bv.distance(model2_bv);
411  }
412 
413  const BVHModel<BV>* model1;
414  const S* model2;
415  BV model2_bv;
416 
417  mutable int num_bv_tests;
418  mutable int num_leaf_tests;
419  mutable FCL_REAL query_time_seconds;
420 };
421 
423 template<typename S, typename BV>
424 class ShapeBVHDistanceTraversalNode : public DistanceTraversalNodeBase
425 {
426 public:
427  ShapeBVHDistanceTraversalNode() : DistanceTraversalNodeBase()
428  {
429  model1 = NULL;
430  model2 = NULL;
431 
432  num_bv_tests = 0;
433  num_leaf_tests = 0;
434  query_time_seconds = 0.0;
435  }
436 
438  bool isSecondNodeLeaf(int b) const
439  {
440  return model2->getBV(b).isLeaf();
441  }
442 
444  int getSecondLeftChild(int b) const
445  {
446  return model2->getBV(b).leftChild();
447  }
448 
450  int getSecondRightChild(int b) const
451  {
452  return model2->getBV(b).rightChild();
453  }
454 
456  FCL_REAL BVDistanceLowerBound(int b1, int b2) const
457  {
458  return model1_bv.distance(model2->getBV(b2).bv);
459  }
460 
461  const S* model1;
462  const BVHModel<BV>* model2;
463  BV model1_bv;
464 
465  mutable int num_bv_tests;
466  mutable int num_leaf_tests;
467  mutable FCL_REAL query_time_seconds;
468 };
469 
470 
472 template<typename BV, typename S>
473 class MeshShapeDistanceTraversalNode : public BVHShapeDistanceTraversalNode<BV, S>
474 {
475 public:
476  MeshShapeDistanceTraversalNode() : BVHShapeDistanceTraversalNode<BV, S>()
477  {
478  vertices = NULL;
479  tri_indices = NULL;
480 
481  rel_err = 0;
482  abs_err = 0;
483 
484  nsolver = NULL;
485  }
486 
488  void leafComputeDistance(int b1, int /*b2*/) const
489  {
490  if(this->enable_statistics) this->num_leaf_tests++;
491 
492  const BVNode<BV>& node = this->model1->getBV(b1);
493 
494  int primitive_id = node.primitiveId();
495 
496  const Triangle& tri_id = tri_indices[primitive_id];
497 
498  const Vec3f& p1 = vertices[tri_id[0]];
499  const Vec3f& p2 = vertices[tri_id[1]];
500  const Vec3f& p3 = vertices[tri_id[2]];
501 
502  FCL_REAL d;
503  Vec3f closest_p1, closest_p2, normal;
504  nsolver->shapeTriangleInteraction(*(this->model2), this->tf2, p1, p2, p3,
505  Transform3f (), d, closest_p2, closest_p1,
506  normal);
507 
508  this->result->update(d, this->model1, this->model2, primitive_id,
509  DistanceResult::NONE, closest_p1, closest_p2,
510  normal);
511  }
512 
514  bool canStop(FCL_REAL c) const
515  {
516  if((c >= this->result->min_distance - abs_err) && (c * (1 + rel_err) >= this->result->min_distance))
517  return true;
518  return false;
519  }
520 
521  Vec3f* vertices;
522  Triangle* tri_indices;
523 
524  FCL_REAL rel_err;
525  FCL_REAL abs_err;
526 
527  const GJKSolver* nsolver;
528 };
529 
531 namespace details
532 {
533 
534 template<typename BV, typename S>
535 void meshShapeDistanceOrientedNodeleafComputeDistance(int b1, int /* b2 */,
536  const BVHModel<BV>* model1, const S& model2,
537  Vec3f* vertices, Triangle* tri_indices,
538  const Transform3f& tf1,
539  const Transform3f& tf2,
540  const GJKSolver* nsolver,
541  bool enable_statistics,
542  int & num_leaf_tests,
543  const DistanceRequest& /* request */,
544  DistanceResult& result)
545 {
546  if(enable_statistics) num_leaf_tests++;
547 
548  const BVNode<BV>& node = model1->getBV(b1);
549  int primitive_id = node.primitiveId();
550 
551  const Triangle& tri_id = tri_indices[primitive_id];
552  const Vec3f& p1 = vertices[tri_id[0]];
553  const Vec3f& p2 = vertices[tri_id[1]];
554  const Vec3f& p3 = vertices[tri_id[2]];
555 
557  Vec3f closest_p1, closest_p2, normal;
558  nsolver->shapeTriangleInteraction(model2, tf2, p1, p2, p3, tf1, distance,
559  closest_p2, closest_p1, normal);
560 
561  result.update(distance, model1, &model2, primitive_id, DistanceResult::NONE,
562  closest_p1, closest_p2, normal);
563 }
564 
565 
566 template<typename BV, typename S>
567 static inline void distancePreprocessOrientedNode(const BVHModel<BV>* model1,
568  Vec3f* vertices, Triangle* tri_indices, int init_tri_id,
569  const S& model2, const Transform3f& tf1, const Transform3f& tf2,
570  const GJKSolver* nsolver,
571  const DistanceRequest& /* request */,
572  DistanceResult& result)
573 {
574  const Triangle& init_tri = tri_indices[init_tri_id];
575 
576  const Vec3f& p1 = vertices[init_tri[0]];
577  const Vec3f& p2 = vertices[init_tri[1]];
578  const Vec3f& p3 = vertices[init_tri[2]];
579 
581  Vec3f closest_p1, closest_p2, normal;
582  nsolver->shapeTriangleInteraction(model2, tf2, p1, p2, p3, tf1, distance,
583  closest_p2, closest_p1, normal);
584 
585  result.update(distance, model1, &model2, init_tri_id, DistanceResult::NONE,
586  closest_p1, closest_p2, normal);
587 }
588 
589 
590 }
591 
593 
594 
595 
597 template<typename S>
598 class MeshShapeDistanceTraversalNodeRSS : public MeshShapeDistanceTraversalNode<RSS, S>
599 {
600 public:
601  MeshShapeDistanceTraversalNodeRSS() : MeshShapeDistanceTraversalNode<RSS, S>()
602  {
603  }
604 
605  void preprocess()
606  {
607  details::distancePreprocessOrientedNode(this->model1, this->vertices, this->tri_indices, 0,
608  *(this->model2), this->tf1, this->tf2, this->nsolver, this->request, *(this->result));
609  }
610 
611  void postprocess()
612  {
613  }
614 
615  FCL_REAL BVDistanceLowerBound(int b1, int /*b2*/) const
616  {
617  if(this->enable_statistics) this->num_bv_tests++;
618  return distance(this->tf1.getRotation(), this->tf1.getTranslation(), this->model2_bv, this->model1->getBV(b1).bv);
619  }
620 
621  void leafComputeDistance(int b1, int b2) const
622  {
623  details::meshShapeDistanceOrientedNodeleafComputeDistance(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
624  this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->request, *(this->result));
625  }
626 };
627 
628 
629 template<typename S>
630 class MeshShapeDistanceTraversalNodekIOS : public MeshShapeDistanceTraversalNode<kIOS, S>
631 {
632 public:
633  MeshShapeDistanceTraversalNodekIOS() : MeshShapeDistanceTraversalNode<kIOS, S>()
634  {
635  }
636 
637  void preprocess()
638  {
639  details::distancePreprocessOrientedNode(this->model1, this->vertices, this->tri_indices, 0,
640  *(this->model2), this->tf1, this->tf2, this->nsolver, this->request, *(this->result));
641  }
642 
643  void postprocess()
644  {
645  }
646 
647  FCL_REAL BVDistanceLowerBound(int b1, int /*b2*/) const
648  {
649  if(this->enable_statistics) this->num_bv_tests++;
650  return distance(this->tf1.getRotation(), this->tf1.getTranslation(), this->model2_bv, this->model1->getBV(b1).bv);
651  }
652 
653  void leafComputeDistance(int b1, int b2) const
654  {
655  details::meshShapeDistanceOrientedNodeleafComputeDistance(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
656  this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->request, *(this->result));
657  }
658 
659 };
660 
661 template<typename S>
662 class MeshShapeDistanceTraversalNodeOBBRSS : public MeshShapeDistanceTraversalNode<OBBRSS, S>
663 {
664 public:
665  MeshShapeDistanceTraversalNodeOBBRSS() : MeshShapeDistanceTraversalNode<OBBRSS, S>()
666  {
667  }
668 
669  void preprocess()
670  {
671  details::distancePreprocessOrientedNode(this->model1, this->vertices, this->tri_indices, 0,
672  *(this->model2), this->tf1, this->tf2, this->nsolver, this->request, *(this->result));
673  }
674 
675  void postprocess()
676  {
677 
678  }
679 
680  FCL_REAL BVDistanceLowerBound(int b1, int /*b2*/) const
681  {
682  if(this->enable_statistics) this->num_bv_tests++;
683  return distance(this->tf1.getRotation(), this->tf1.getTranslation(), this->model2_bv, this->model1->getBV(b1).bv);
684  }
685 
686  void leafComputeDistance(int b1, int b2) const
687  {
688  details::meshShapeDistanceOrientedNodeleafComputeDistance(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
689  this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->request, *(this->result));
690  }
691 
692 };
693 
695 template<typename S, typename BV>
696 class ShapeMeshDistanceTraversalNode : public ShapeBVHDistanceTraversalNode<S, BV>
697 {
698 public:
699  ShapeMeshDistanceTraversalNode() : ShapeBVHDistanceTraversalNode<S, BV>()
700  {
701  vertices = NULL;
702  tri_indices = NULL;
703 
704  rel_err = 0;
705  abs_err = 0;
706 
707  nsolver = NULL;
708  }
709 
711  void leafComputeDistance(int b1, int b2) const
712  {
713  if(this->enable_statistics) this->num_leaf_tests++;
714 
715  const BVNode<BV>& node = this->model2->getBV(b2);
716 
717  int primitive_id = node.primitiveId();
718 
719  const Triangle& tri_id = tri_indices[primitive_id];
720 
721  const Vec3f& p1 = vertices[tri_id[0]];
722  const Vec3f& p2 = vertices[tri_id[1]];
723  const Vec3f& p3 = vertices[tri_id[2]];
724 
726  Vec3f closest_p1, closest_p2, normal;
727  nsolver->shapeTriangleInteraction(*(this->model1), this->tf1, p1, p2, p3,
728  Transform3f (), distance, closest_p1,
729  closest_p2, normal);
730 
731  this->result->update(distance, this->model1, this->model2,
732  DistanceResult::NONE, primitive_id, closest_p1,
733  closest_p2, normal);
734  }
735 
737  bool canStop(FCL_REAL c) const
738  {
739  if((c >= this->result->min_distance - abs_err) && (c * (1 + rel_err) >= this->result->min_distance))
740  return true;
741  return false;
742  }
743 
745  Triangle* tri_indices;
746 
749 
750  const GJKSolver* nsolver;
751 };
752 
754 template<typename S>
756 {
757 public:
759  {
760  }
761 
762  void preprocess()
763  {
764  details::distancePreprocessOrientedNode(this->model2, this->vertices, this->tri_indices, 0,
765  *(this->model1), this->tf2, this->tf1, this->nsolver, this->request, *(this->result));
766  }
767 
768  void postprocess()
769  {
770  }
771 
772  FCL_REAL BVDistanceLowerBound(int b1, int b2) const
773  {
774  if(this->enable_statistics) this->num_bv_tests++;
775  return distance(this->tf2.getRotation(), this->tf2.getTranslation(), this->model1_bv, this->model2->getBV(b2).bv);
776  }
777 
778  void leafComputeDistance(int b1, int b2) const
779  {
780  details::meshShapeDistanceOrientedNodeleafComputeDistance(b2, b1, this->model2, *(this->model1), this->vertices, this->tri_indices,
781  this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->request, *(this->result));
782  }
783 
784 };
785 
786 template<typename S>
788 {
789 public:
791  {
792  }
793 
794  void preprocess()
795  {
796  details::distancePreprocessOrientedNode(this->model2, this->vertices, this->tri_indices, 0,
797  *(this->model1), this->tf2, this->tf1, this->nsolver, *(this->result));
798  }
799 
800  void postprocess()
801  {
802  }
803 
804  FCL_REAL BVDistanceLowerBound(int b1, int b2) const
805  {
806  if(this->enable_statistics) this->num_bv_tests++;
807  return distance(this->tf2.getRotation(), this->tf2.getTranslation(), this->model1_bv, this->model2->getBV(b2).bv);
808  }
809 
810  void leafComputeDistance(int b1, int b2) const
811  {
812  details::meshShapeDistanceOrientedNodeleafComputeDistance(b2, b1, this->model2, *(this->model1), this->vertices, this->tri_indices,
813  this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->request, *(this->result));
814  }
815 
816 };
817 
818 template<typename S>
820 {
821 public:
823  {
824  }
825 
826  void preprocess()
827  {
828  details::distancePreprocessOrientedNode(this->model2, this->vertices, this->tri_indices, 0,
829  *(this->model1), this->tf2, this->tf1, this->nsolver, *(this->result));
830  }
831 
832  void postprocess()
833  {
834  }
835 
836  FCL_REAL BVDistanceLowerBound(int b1, int b2) const
837  {
838  if(this->enable_statistics) this->num_bv_tests++;
839  return distance(this->tf2.getRotation(), this->tf2.getTranslation(), this->model1_bv, this->model2->getBV(b2).bv);
840  }
841 
842  void leafComputeDistance(int b1, int b2) const
843  {
844  details::meshShapeDistanceOrientedNodeleafComputeDistance(b2, b1, this->model2, *(this->model1), this->vertices, this->tri_indices,
845  this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->request, *(this->result));
846  }
847 
848 };
849 
851 
852 }
853 
854 } // namespace hpp
855 
857 
858 #endif
MeshShapeDistanceTraversalNodeRSS()
Definition: traversal_node_bvh_shape.h:601
ShapeMeshDistanceTraversalNode()
Definition: traversal_node_bvh_shape.h:699
void postprocess()
Definition: traversal_node_bvh_shape.h:832
void leafComputeDistance(int b1, int b2) const
Definition: traversal_node_bvh_shape.h:842
Main namespace.
Definition: AABB.h:43
FCL_REAL BVDistanceLowerBound(int b1, int b2) const
Definition: traversal_node_bvh_shape.h:772
FCL_REAL BVDistanceLowerBound(int b1, int) const
Definition: traversal_node_bvh_shape.h:615
FCL_REAL BVDistanceLowerBound(int b1, int) const
Definition: traversal_node_bvh_shape.h:647
ShapeMeshDistanceTraversalNodeRSS()
Definition: traversal_node_bvh_shape.h:758
void postprocess()
Definition: traversal_node_bvh_shape.h:800
void preprocess()
Definition: traversal_node_bvh_shape.h:637
ShapeMeshDistanceTraversalNodekIOS()
Definition: traversal_node_bvh_shape.h:790
void leafComputeDistance(int b1, int b2) const
Definition: traversal_node_bvh_shape.h:810
void preprocess()
Definition: traversal_node_bvh_shape.h:762
FCL_REAL rel_err
Definition: traversal_node_bvh_shape.h:747
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.
void preprocess()
Definition: traversal_node_bvh_shape.h:669
double FCL_REAL
Definition: data_types.h:68
ShapeMeshDistanceTraversalNodeOBBRSS()
Definition: traversal_node_bvh_shape.h:822
Definition: traversal_node_setup.h:775
static const int NONE
invalid contact primitive information
Definition: collision_data.h:91
void preprocess()
Definition: traversal_node_bvh_shape.h:794
Triangle * tri_indices
Definition: traversal_node_bvh_shape.h:745
MeshShapeDistanceTraversalNodekIOS()
Definition: traversal_node_bvh_shape.h:633
Definition: traversal_node_bvh_shape.h:787
void leafComputeDistance(int b1, int b2) const
Definition: traversal_node_bvh_shape.h:778
void leafComputeDistance(int b1, int b2) const
Definition: traversal_node_bvh_shape.h:686
void postprocess()
Definition: traversal_node_bvh_shape.h:768
Traversal node for distance between shape and mesh.
Definition: traversal_node_bvh_shape.h:696
Traversal node for distance between shape and mesh, when mesh BVH is one of the oriented node (RSS...
Definition: traversal_node_bvh_shape.h:755
void postprocess()
Definition: traversal_node_bvh_shape.h:643
void leafComputeDistance(int b1, int b2) const
Definition: traversal_node_bvh_shape.h:621
FCL_REAL BVDistanceLowerBound(int b1, int b2) const
Definition: traversal_node_bvh_shape.h:804
void postprocess()
Definition: traversal_node_bvh_shape.h:611
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.
Traversal node for distance between mesh and shape, when mesh BVH is one of the oriented node (RSS...
Definition: traversal_node_bvh_shape.h:598
void leafComputeDistance(int b1, int b2) const
Distance testing between leaves (one shape and one triangle)
Definition: traversal_node_bvh_shape.h:711
void leafComputeDistance(int b1, int b2) const
Definition: traversal_node_bvh_shape.h:653
void preprocess()
Definition: traversal_node_bvh_shape.h:826
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:73
void postprocess()
Definition: traversal_node_bvh_shape.h:675
FCL_REAL BVDistanceLowerBound(int b1, int) const
Definition: traversal_node_bvh_shape.h:680
void preprocess()
Definition: traversal_node_bvh_shape.h:605
Definition: traversal_node_bvh_shape.h:819
bool canStop(FCL_REAL c) const
Whether the traversal process can stop early.
Definition: traversal_node_bvh_shape.h:737
MeshShapeDistanceTraversalNodeOBBRSS()
Definition: traversal_node_bvh_shape.h:665
Definition: traversal_node_bvh_shape.h:630
FCL_REAL BVDistanceLowerBound(int b1, int b2) const
Definition: traversal_node_bvh_shape.h:836
Vec3f * vertices
Definition: traversal_node_bvh_shape.h:744
FCL_REAL abs_err
Definition: traversal_node_bvh_shape.h:748
const GJKSolver * nsolver
Definition: traversal_node_bvh_shape.h:750
Definition: traversal_node_bvh_shape.h:662
static const int NONE
invalid contact primitive information
Definition: collision_data.h:366