hpp-fcl  1.4.5
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 HPP_FCL_DLLAPI 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 HPP_FCL_DLLAPI 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 HPP_FCL_DLLAPI 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 HPP_FCL_DLLAPI 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 HPP_FCL_DLLAPI 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 HPP_FCL_DLLAPI 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 HPP_FCL_DLLAPI 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 HPP_FCL_DLLAPI 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 HPP_FCL_DLLAPI 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 HPP_FCL_DLLAPI 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 HPP_FCL_DLLAPI 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
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.
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
bool overlap(const Matrix3f &R0, const Vec3f &T0, const AABB &b1, const AABB &b2) HPP_FCL_DLLAPI
Check collision between two aabbs, b1 is in configuration (R0, T0) and b2 is in identity.
void preprocess()
Definition: traversal_node_bvh_shape.h:762
FCL_REAL rel_err
Definition: traversal_node_bvh_shape.h:747
void preprocess()
Definition: traversal_node_bvh_shape.h:669
double FCL_REAL
Definition: data_types.h:69
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
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:74
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
#define HPP_FCL_DLLAPI
Definition: config.hh:64
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:393