hpp-fcl: include/hpp/fcl/internal/traversal_node_bvh_shape.h Source File

hpp-fcl 1.6.0 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>
45 #include <hpp/fcl/shape/geometric_shapes.h>
46 #include <hpp/fcl/narrowphase/narrowphase.h>
47 #include <hpp/fcl/shape/geometric_shapes_utility.h>
48 #include <hpp/fcl/internal/traversal_node_base.h>
49 #include <hpp/fcl/internal/traversal.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
217 FCL_REAL distance;
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
324 FCL_REAL distance;
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
556 FCL_REAL distance;
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
580 FCL_REAL distance;
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
725 FCL_REAL distance;
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
744 Vec3f* vertices;
745 Triangle* tri_indices;
746
747 FCL_REAL rel_err;
748 FCL_REAL abs_err;
749
750 const GJKSolver* nsolver;
751 };
752
754 template<typename S>
755 class ShapeMeshDistanceTraversalNodeRSS : public ShapeMeshDistanceTraversalNode<S, RSS>
756 {
757 public:
758 ShapeMeshDistanceTraversalNodeRSS() : ShapeMeshDistanceTraversalNode<S, RSS>()
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>
787 class ShapeMeshDistanceTraversalNodekIOS : public ShapeMeshDistanceTraversalNode<S, kIOS>
788 {
789 public:
790 ShapeMeshDistanceTraversalNodekIOS() : ShapeMeshDistanceTraversalNode<S, kIOS>()
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>
819 class ShapeMeshDistanceTraversalNodeOBBRSS : public ShapeMeshDistanceTraversalNode<S, OBBRSS>
820 {
821 public:
822 ShapeMeshDistanceTraversalNodeOBBRSS() : ShapeMeshDistanceTraversalNode<S, OBBRSS>()
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
ShapeMeshDistanceTraversalNodekIOS::ShapeMeshDistanceTraversalNodekIOS
ShapeMeshDistanceTraversalNodekIOS()
Definition: traversal_node_bvh_shape.h:790
BVH_model.h
ShapeMeshDistanceTraversalNodeRSS::ShapeMeshDistanceTraversalNodeRSS
ShapeMeshDistanceTraversalNodeRSS()
Definition: traversal_node_bvh_shape.h:758
ShapeMeshDistanceTraversalNodeRSS::leafComputeDistance
void leafComputeDistance(int b1, int b2) const
Definition: traversal_node_bvh_shape.h:778
hpp::fcl::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:67
ShapeMeshDistanceTraversalNodeRSS::postprocess
void postprocess()
Definition: traversal_node_bvh_shape.h:768
MeshShapeDistanceTraversalNodekIOS::postprocess
void postprocess()
Definition: traversal_node_bvh_shape.h:643
ShapeMeshDistanceTraversalNodeOBBRSS::ShapeMeshDistanceTraversalNodeOBBRSS
ShapeMeshDistanceTraversalNodeOBBRSS()
Definition: traversal_node_bvh_shape.h:822
ShapeMeshDistanceTraversalNodekIOS::BVDistanceLowerBound
FCL_REAL BVDistanceLowerBound(int b1, int b2) const
Definition: traversal_node_bvh_shape.h:804
ShapeMeshDistanceTraversalNode
Traversal node for distance between shape and mesh.
Definition: traversal_node_bvh_shape.h:697
ShapeMeshDistanceTraversalNode::ShapeMeshDistanceTraversalNode
ShapeMeshDistanceTraversalNode()
Definition: traversal_node_bvh_shape.h:699
MeshShapeDistanceTraversalNodeOBBRSS::BVDistanceLowerBound
FCL_REAL BVDistanceLowerBound(int b1, int) const
Definition: traversal_node_bvh_shape.h:680
MeshShapeDistanceTraversalNodeOBBRSS
Definition: traversal_node_bvh_shape.h:663
ShapeMeshDistanceTraversalNodekIOS::leafComputeDistance
void leafComputeDistance(int b1, int b2) const
Definition: traversal_node_bvh_shape.h:810
hpp::fcl::DistanceResult::NONE
static const int NONE
invalid contact primitive information
Definition: collision_data.h:400
ShapeMeshDistanceTraversalNodeOBBRSS::postprocess
void postprocess()
Definition: traversal_node_bvh_shape.h:832
traversal_node_base.h
MeshShapeDistanceTraversalNodeOBBRSS::leafComputeDistance
void leafComputeDistance(int b1, int b2) const
Definition: traversal_node_bvh_shape.h:686
ShapeMeshDistanceTraversalNode::abs_err
FCL_REAL abs_err
Definition: traversal_node_bvh_shape.h:748
ShapeMeshDistanceTraversalNode::vertices
Vec3f * vertices
Definition: traversal_node_bvh_shape.h:744
geometric_shapes_utility.h
ShapeMeshDistanceTraversalNode::rel_err
FCL_REAL rel_err
Definition: traversal_node_bvh_shape.h:747
narrowphase.h
MeshShapeDistanceTraversalNodeOBBRSS::postprocess
void postprocess()
Definition: traversal_node_bvh_shape.h:675
ShapeMeshDistanceTraversalNode::nsolver
const GJKSolver * nsolver
Definition: traversal_node_bvh_shape.h:750
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:66
ShapeMeshDistanceTraversalNodeRSS
Traversal node for distance between shape and mesh, when mesh BVH is one of the oriented node (RSS,...
Definition: traversal_node_bvh_shape.h:756
details
Definition: traversal_node_setup.h:776
ShapeMeshDistanceTraversalNodekIOS::preprocess
void preprocess()
Definition: traversal_node_bvh_shape.h:794
ShapeMeshDistanceTraversalNodeOBBRSS
Definition: traversal_node_bvh_shape.h:820
MeshShapeDistanceTraversalNodekIOS::preprocess
void preprocess()
Definition: traversal_node_bvh_shape.h:637
MeshShapeDistanceTraversalNodekIOS::BVDistanceLowerBound
FCL_REAL BVDistanceLowerBound(int b1, int) const
Definition: traversal_node_bvh_shape.h:647
ShapeMeshDistanceTraversalNodeRSS::preprocess
void preprocess()
Definition: traversal_node_bvh_shape.h:762
collision_data.h
hpp
Main namespace.
Definition: AABB.h:44
hpp::fcl::overlap
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.
ShapeMeshDistanceTraversalNodeRSS::BVDistanceLowerBound
FCL_REAL BVDistanceLowerBound(int b1, int b2) const
Definition: traversal_node_bvh_shape.h:772
ShapeMeshDistanceTraversalNodekIOS::postprocess
void postprocess()
Definition: traversal_node_bvh_shape.h:800
ShapeMeshDistanceTraversalNodeOBBRSS::BVDistanceLowerBound
FCL_REAL BVDistanceLowerBound(int b1, int b2) const
Definition: traversal_node_bvh_shape.h:836
hpp::fcl::Contact::NONE
static const int NONE
invalid contact primitive information
Definition: collision_data.h:89
MeshShapeDistanceTraversalNodeOBBRSS::preprocess
void preprocess()
Definition: traversal_node_bvh_shape.h:669
MeshShapeDistanceTraversalNodeRSS::BVDistanceLowerBound
FCL_REAL BVDistanceLowerBound(int b1, int) const
Definition: traversal_node_bvh_shape.h:615
MeshShapeDistanceTraversalNodeOBBRSS::MeshShapeDistanceTraversalNodeOBBRSS
MeshShapeDistanceTraversalNodeOBBRSS()
Definition: traversal_node_bvh_shape.h:665
MeshShapeDistanceTraversalNodekIOS::leafComputeDistance
void leafComputeDistance(int b1, int b2) const
Definition: traversal_node_bvh_shape.h:653
MeshShapeDistanceTraversalNodeRSS::preprocess
void preprocess()
Definition: traversal_node_bvh_shape.h:605
hpp::fcl::distance
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.
geometric_shapes.h
MeshShapeDistanceTraversalNodeRSS::leafComputeDistance
void leafComputeDistance(int b1, int b2) const
Definition: traversal_node_bvh_shape.h:621
MeshShapeDistanceTraversalNodekIOS
Definition: traversal_node_bvh_shape.h:631
MeshShapeDistanceTraversalNodeRSS::MeshShapeDistanceTraversalNodeRSS
MeshShapeDistanceTraversalNodeRSS()
Definition: traversal_node_bvh_shape.h:601
ShapeMeshDistanceTraversalNodeOBBRSS::preprocess
void preprocess()
Definition: traversal_node_bvh_shape.h:826
MeshShapeDistanceTraversalNodekIOS::MeshShapeDistanceTraversalNodekIOS
MeshShapeDistanceTraversalNodekIOS()
Definition: traversal_node_bvh_shape.h:633
MeshShapeDistanceTraversalNodeRSS
Traversal node for distance between mesh and shape, when mesh BVH is one of the oriented node (RSS,...
Definition: traversal_node_bvh_shape.h:599
traversal.h
ShapeMeshDistanceTraversalNodekIOS
Definition: traversal_node_bvh_shape.h:788
ShapeMeshDistanceTraversalNode::canStop
bool canStop(FCL_REAL c) const
Whether the traversal process can stop early.
Definition: traversal_node_bvh_shape.h:737
ShapeMeshDistanceTraversalNodeOBBRSS::leafComputeDistance
void leafComputeDistance(int b1, int b2) const
Definition: traversal_node_bvh_shape.h:842
MeshShapeDistanceTraversalNodeRSS::postprocess
void postprocess()
Definition: traversal_node_bvh_shape.h:611
ShapeMeshDistanceTraversalNode::leafComputeDistance
void leafComputeDistance(int b1, int b2) const
Distance testing between leaves (one shape and one triangle)
Definition: traversal_node_bvh_shape.h:711
ShapeMeshDistanceTraversalNode::tri_indices
Triangle * tri_indices
Definition: traversal_node_bvh_shape.h:745