hpp-fcl 1.8.1
HPP fork of FCL -- The Flexible Collision Library
Loading...
Searching...
No Matches
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
51
52
53namespace hpp
54{
55namespace fcl
56{
57
60
62template<typename BV, typename S>
63class BVHShapeCollisionTraversalNode : public CollisionTraversalNodeBase
64{
65public:
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(unsigned int b) const
79 {
80 return model1->getBV(b).isLeaf();
81 }
82
84 int getFirstLeftChild(unsigned int b) const
85 {
86 return model1->getBV(b).leftChild();
87 }
88
90 int getFirstRightChild(unsigned 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
105template<typename S, typename BV>
106class ShapeBVHCollisionTraversalNode : public CollisionTraversalNodeBase
107{
108public:
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(unsigned int, unsigned int) const
122 {
123 return false;
124 }
125
127 bool isSecondNodeLeaf(unsigned int b) const
128 {
129 return model2->getBV(b).isLeaf();
130 }
131
133 int getSecondLeftChild(unsigned int b) const
134 {
135 return model2->getBV(b).leftChild();
136 }
137
139 int getSecondRightChild(unsigned 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
155template<typename BV, typename S,
156 int _Options = RelativeTransformationIsIdentity>
157class MeshShapeCollisionTraversalNode : public BVHShapeCollisionTraversalNode<BV, S>
158{
159public:
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(unsigned int b1, unsigned 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(unsigned int b1, unsigned 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(unsigned int b1, unsigned 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
263template<typename S, typename BV,
264 int _Options = RelativeTransformationIsIdentity>
265class ShapeMeshCollisionTraversalNode : public ShapeBVHCollisionTraversalNode<S, BV>
266{
267public:
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(unsigned int /*b1*/, unsigned 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(unsigned int /*b1*/, unsigned 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(unsigned int /*b1*/, unsigned 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
375template<typename BV, typename S>
376class BVHShapeDistanceTraversalNode : public DistanceTraversalNodeBase
377{
378public:
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(unsigned int b) const
391 {
392 return model1->getBV(b).isLeaf();
393 }
394
396 int getFirstLeftChild(unsigned int b) const
397 {
398 return model1->getBV(b).leftChild();
399 }
400
402 int getFirstRightChild(unsigned int b) const
403 {
404 return model1->getBV(b).rightChild();
405 }
406
408 FCL_REAL BVDistanceLowerBound(unsigned int b1, unsigned 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
423template<typename S, typename BV>
424class ShapeBVHDistanceTraversalNode : public DistanceTraversalNodeBase
425{
426public:
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(unsigned int b) const
439 {
440 return model2->getBV(b).isLeaf();
441 }
442
444 int getSecondLeftChild(unsigned int b) const
445 {
446 return model2->getBV(b).leftChild();
447 }
448
450 int getSecondRightChild(unsigned int b) const
451 {
452 return model2->getBV(b).rightChild();
453 }
454
456 FCL_REAL BVDistanceLowerBound(unsigned int /*b1*/, unsigned 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
472template<typename BV, typename S>
473class MeshShapeDistanceTraversalNode : public BVHShapeDistanceTraversalNode<BV, S>
474{
475public:
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(unsigned int b1, unsigned 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
531namespace details
532{
533
534template<typename BV, typename S>
535void meshShapeDistanceOrientedNodeleafComputeDistance(unsigned int b1, unsigned 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
566template<typename BV, typename S>
567static 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
597template<typename S>
598class MeshShapeDistanceTraversalNodeRSS : public MeshShapeDistanceTraversalNode<RSS, S>
599{
600public:
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(unsigned int b1, unsigned 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(unsigned int b1, unsigned 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
629template<typename S>
630class MeshShapeDistanceTraversalNodekIOS : public MeshShapeDistanceTraversalNode<kIOS, S>
631{
632public:
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(unsigned int b1, unsigned 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(unsigned int b1, unsigned 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
661template<typename S>
662class MeshShapeDistanceTraversalNodeOBBRSS : public MeshShapeDistanceTraversalNode<OBBRSS, S>
663{
664public:
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(unsigned int b1, unsigned 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(unsigned int b1, unsigned 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
695template<typename S, typename BV>
696class ShapeMeshDistanceTraversalNode : public ShapeBVHDistanceTraversalNode<S, BV>
697{
698public:
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(unsigned int /*b1*/, unsigned 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
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
754template<typename S>
755class ShapeMeshDistanceTraversalNodeRSS : public ShapeMeshDistanceTraversalNode<S, RSS>
756{
757public:
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(unsigned int /*b1*/, unsigned 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(unsigned int b1, unsigned 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
786template<typename S>
787class ShapeMeshDistanceTraversalNodekIOS : public ShapeMeshDistanceTraversalNode<S, kIOS>
788{
789public:
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(unsigned int /*b1*/, unsigned 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(unsigned int b1, unsigned 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
818template<typename S>
819class ShapeMeshDistanceTraversalNodeOBBRSS : public ShapeMeshDistanceTraversalNode<S, OBBRSS>
820{
821public:
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(unsigned int /*b1*/, unsigned 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(unsigned int b1, unsigned 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
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.
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.
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:67
double FCL_REAL
Definition: data_types.h:66
Main namespace.
Definition: AABB.h:44
static const int NONE
invalid contact primitive information
Definition: collision_data.h:435