hpp-fcl 2.4.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
38#ifndef HPP_FCL_TRAVERSAL_NODE_MESH_SHAPE_H
39#define HPP_FCL_TRAVERSAL_NODE_MESH_SHAPE_H
40
42
50
51namespace hpp {
52namespace fcl {
53
56
58template <typename BV, typename S>
59class BVHShapeCollisionTraversalNode : public CollisionTraversalNodeBase {
60 public:
61 BVHShapeCollisionTraversalNode(const CollisionRequest& request)
62 : CollisionTraversalNodeBase(request) {
63 model1 = NULL;
64 model2 = NULL;
65
66 num_bv_tests = 0;
67 num_leaf_tests = 0;
68 query_time_seconds = 0.0;
69 }
70
72 bool isFirstNodeLeaf(unsigned int b) const {
73 return model1->getBV(b).isLeaf();
74 }
75
77 int getFirstLeftChild(unsigned int b) const {
78 return model1->getBV(b).leftChild();
79 }
80
82 int getFirstRightChild(unsigned int b) const {
83 return model1->getBV(b).rightChild();
84 }
85
86 const BVHModel<BV>* model1;
87 const S* model2;
88 BV model2_bv;
89
90 mutable int num_bv_tests;
91 mutable int num_leaf_tests;
92 mutable FCL_REAL query_time_seconds;
93};
94
96template <typename S, typename BV>
97class ShapeBVHCollisionTraversalNode : public CollisionTraversalNodeBase {
98 public:
99 ShapeBVHCollisionTraversalNode(const CollisionRequest& request)
100 : CollisionTraversalNodeBase(request) {
101 model1 = NULL;
102 model2 = NULL;
103
104 num_bv_tests = 0;
105 num_leaf_tests = 0;
106 query_time_seconds = 0.0;
107 }
108
110 bool firstOverSecond(unsigned int, unsigned int) const { return false; }
111
113 bool isSecondNodeLeaf(unsigned int b) const {
114 return model2->getBV(b).isLeaf();
115 }
116
118 int getSecondLeftChild(unsigned int b) const {
119 return model2->getBV(b).leftChild();
120 }
121
123 int getSecondRightChild(unsigned int b) const {
124 return model2->getBV(b).rightChild();
125 }
126
127 const S* model1;
128 const BVHModel<BV>* model2;
129 BV model1_bv;
130
131 mutable int num_bv_tests;
132 mutable int num_leaf_tests;
133 mutable FCL_REAL query_time_seconds;
134};
135
137template <typename BV, typename S,
138 int _Options = RelativeTransformationIsIdentity>
139class MeshShapeCollisionTraversalNode
140 : public BVHShapeCollisionTraversalNode<BV, S> {
141 public:
142 enum {
143 Options = _Options,
144 RTIsIdentity = _Options & RelativeTransformationIsIdentity
145 };
146
147 MeshShapeCollisionTraversalNode(const CollisionRequest& request)
148 : BVHShapeCollisionTraversalNode<BV, S>(request) {
149 vertices = NULL;
150 tri_indices = NULL;
151
152 nsolver = NULL;
153 }
154
160 bool BVDisjoints(unsigned int b1, unsigned int /*b2*/,
161 FCL_REAL& sqrDistLowerBound) const {
162 if (this->enable_statistics) this->num_bv_tests++;
163 bool disjoint;
164 if (RTIsIdentity)
165 disjoint = !this->model1->getBV(b1).bv.overlap(
166 this->model2_bv, this->request, sqrDistLowerBound);
167 else
168 disjoint = !overlap(this->tf1.getRotation(), this->tf1.getTranslation(),
169 this->model1->getBV(b1).bv, this->model2_bv,
170 this->request, sqrDistLowerBound);
171 if (disjoint)
172 internal::updateDistanceLowerBoundFromBV(this->request, *this->result,
173 sqrDistLowerBound);
174 assert(!disjoint || sqrDistLowerBound > 0);
175 return disjoint;
176 }
177
179 void leafCollides(unsigned int b1, unsigned int /*b2*/,
180 FCL_REAL& sqrDistLowerBound) const {
181 if (this->enable_statistics) this->num_leaf_tests++;
182 const BVNode<BV>& node = this->model1->getBV(b1);
183
184 int primitive_id = node.primitiveId();
185
186 const Triangle& tri_id = tri_indices[primitive_id];
187
188 const Vec3f& p1 = vertices[tri_id[0]];
189 const Vec3f& p2 = vertices[tri_id[1]];
190 const Vec3f& p3 = vertices[tri_id[2]];
191
193 Vec3f normal;
194 Vec3f c1, c2; // closest point
195
196 bool collision;
197 if (RTIsIdentity) {
198 static const Transform3f Id;
199 collision = nsolver->shapeTriangleInteraction(
200 *(this->model2), this->tf2, p1, p2, p3, Id, distance, c2, c1, normal);
201 } else {
202 collision = nsolver->shapeTriangleInteraction(*(this->model2), this->tf2,
203 p1, p2, p3, this->tf1,
204 distance, c2, c1, normal);
205 }
206
207 FCL_REAL distToCollision = distance - this->request.security_margin;
208 if (collision) {
209 sqrDistLowerBound = 0;
210 if (this->request.num_max_contacts > this->result->numContacts()) {
211 this->result->addContact(Contact(this->model1, this->model2,
212 primitive_id, Contact::NONE, c1,
213 -normal, -distance));
214 assert(this->result->isCollision());
215 }
216 } else if (distToCollision <= this->request.collision_distance_threshold) {
217 sqrDistLowerBound = 0;
218 if (this->request.num_max_contacts > this->result->numContacts()) {
219 this->result->addContact(
220 Contact(this->model1, this->model2, primitive_id, Contact::NONE,
221 .5 * (c1 + c2), (c2 - c1).normalized(), -distance));
222 }
223 } else
224 sqrDistLowerBound = distToCollision * distToCollision;
225
226 internal::updateDistanceLowerBoundFromLeaf(this->request, *this->result,
227 distToCollision, c1, c2);
228
229 assert(this->result->isCollision() || sqrDistLowerBound > 0);
230 }
231
232 Vec3f* vertices;
233 Triangle* tri_indices;
234
235 const GJKSolver* nsolver;
236};
237
239template <typename S, typename BV,
240 int _Options = RelativeTransformationIsIdentity>
241class ShapeMeshCollisionTraversalNode
242 : public ShapeBVHCollisionTraversalNode<S, BV> {
243 public:
244 enum {
245 Options = _Options,
246 RTIsIdentity = _Options & RelativeTransformationIsIdentity
247 };
248
249 ShapeMeshCollisionTraversalNode() : ShapeBVHCollisionTraversalNode<S, BV>() {
250 vertices = NULL;
251 tri_indices = NULL;
252
253 nsolver = NULL;
254 }
255
260 bool BVDisjoints(unsigned int /*b1*/, unsigned int b2,
261 FCL_REAL& sqrDistLowerBound) const {
262 if (this->enable_statistics) this->num_bv_tests++;
263 bool disjoint;
264 if (RTIsIdentity)
265 disjoint = !this->model2->getBV(b2).bv.overlap(this->model1_bv,
266 sqrDistLowerBound);
267 else
268 disjoint = !overlap(this->tf2.getRotation(), this->tf2.getTranslation(),
269 this->model2->getBV(b2).bv, this->model1_bv,
270 sqrDistLowerBound);
271 if (disjoint)
272 internal::updateDistanceLowerBoundFromBV(this->request, *this->result,
273 sqrDistLowerBound);
274 assert(!disjoint || sqrDistLowerBound > 0);
275 return disjoint;
276 }
277
279 void leafCollides(unsigned int /*b1*/, unsigned int b2,
280 FCL_REAL& sqrDistLowerBound) const {
281 if (this->enable_statistics) this->num_leaf_tests++;
282 const BVNode<BV>& node = this->model2->getBV(b2);
283
284 int primitive_id = node.primitiveId();
285
286 const Triangle& tri_id = tri_indices[primitive_id];
287
288 const Vec3f& p1 = vertices[tri_id[0]];
289 const Vec3f& p2 = vertices[tri_id[1]];
290 const Vec3f& p3 = vertices[tri_id[2]];
291
293 Vec3f normal;
294 Vec3f c1, c2; // closest points
295
296 bool collision;
297 if (RTIsIdentity) {
298 static const Transform3f Id;
299 collision = nsolver->shapeTriangleInteraction(
300 *(this->model1), this->tf1, p1, p2, p3, Id, c1, c2, distance, normal);
301 } else {
302 collision = nsolver->shapeTriangleInteraction(*(this->model1), this->tf1,
303 p1, p2, p3, this->tf2, c1,
304 c2, distance, normal);
305 }
306
307 FCL_REAL distToCollision = distance - this->request.security_margin;
308 if (collision) {
309 sqrDistLowerBound = 0;
310 if (this->request.num_max_contacts > this->result->numContacts()) {
311 this->result->addContact(Contact(this->model1, this->model2,
312 Contact::NONE, primitive_id, c1,
313 normal, -distance));
314 assert(this->result->isCollision());
315 }
316 } else if (distToCollision <= this->request.collision_distance_threshold) {
317 sqrDistLowerBound = 0;
318 if (this->request.num_max_contacts > this->result->numContacts()) {
319 this->result->addContact(
320 Contact(this->model1, this->model2, Contact::NONE, primitive_id,
321 .5 * (c1 + c2), (c2 - c1).normalized(), -distance));
322 }
323 } else
324 sqrDistLowerBound = distToCollision * distToCollision;
325
326 internal::updateDistanceLowerBoundFromLeaf(this->request, *this->result,
327 distToCollision, c1, c2);
328
329 assert(this->result->isCollision() || sqrDistLowerBound > 0);
330 }
331
332 Vec3f* vertices;
333 Triangle* tri_indices;
334
335 const GJKSolver* nsolver;
336};
337
339
342
344template <typename BV, typename S>
345class BVHShapeDistanceTraversalNode : public DistanceTraversalNodeBase {
346 public:
347 BVHShapeDistanceTraversalNode() : DistanceTraversalNodeBase() {
348 model1 = NULL;
349 model2 = NULL;
350
351 num_bv_tests = 0;
352 num_leaf_tests = 0;
353 query_time_seconds = 0.0;
354 }
355
357 bool isFirstNodeLeaf(unsigned int b) const {
358 return model1->getBV(b).isLeaf();
359 }
360
362 int getFirstLeftChild(unsigned int b) const {
363 return model1->getBV(b).leftChild();
364 }
365
367 int getFirstRightChild(unsigned int b) const {
368 return model1->getBV(b).rightChild();
369 }
370
372 FCL_REAL BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const {
373 return model1->getBV(b1).bv.distance(model2_bv);
374 }
375
376 const BVHModel<BV>* model1;
377 const S* model2;
378 BV model2_bv;
379
380 mutable int num_bv_tests;
381 mutable int num_leaf_tests;
382 mutable FCL_REAL query_time_seconds;
383};
384
386template <typename S, typename BV>
387class ShapeBVHDistanceTraversalNode : public DistanceTraversalNodeBase {
388 public:
389 ShapeBVHDistanceTraversalNode() : DistanceTraversalNodeBase() {
390 model1 = NULL;
391 model2 = NULL;
392
393 num_bv_tests = 0;
394 num_leaf_tests = 0;
395 query_time_seconds = 0.0;
396 }
397
399 bool isSecondNodeLeaf(unsigned int b) const {
400 return model2->getBV(b).isLeaf();
401 }
402
404 int getSecondLeftChild(unsigned int b) const {
405 return model2->getBV(b).leftChild();
406 }
407
409 int getSecondRightChild(unsigned int b) const {
410 return model2->getBV(b).rightChild();
411 }
412
414 FCL_REAL BVDistanceLowerBound(unsigned int /*b1*/, unsigned int b2) const {
415 return model1_bv.distance(model2->getBV(b2).bv);
416 }
417
418 const S* model1;
419 const BVHModel<BV>* model2;
420 BV model1_bv;
421
422 mutable int num_bv_tests;
423 mutable int num_leaf_tests;
424 mutable FCL_REAL query_time_seconds;
425};
426
428template <typename BV, typename S>
429class MeshShapeDistanceTraversalNode
430 : public BVHShapeDistanceTraversalNode<BV, S> {
431 public:
432 MeshShapeDistanceTraversalNode() : BVHShapeDistanceTraversalNode<BV, S>() {
433 vertices = NULL;
434 tri_indices = NULL;
435
436 rel_err = 0;
437 abs_err = 0;
438
439 nsolver = NULL;
440 }
441
443 void leafComputeDistance(unsigned int b1, unsigned int /*b2*/) const {
444 if (this->enable_statistics) this->num_leaf_tests++;
445
446 const BVNode<BV>& node = this->model1->getBV(b1);
447
448 int primitive_id = node.primitiveId();
449
450 const Triangle& tri_id = tri_indices[primitive_id];
451
452 const Vec3f& p1 = vertices[tri_id[0]];
453 const Vec3f& p2 = vertices[tri_id[1]];
454 const Vec3f& p3 = vertices[tri_id[2]];
455
456 FCL_REAL d;
457 Vec3f closest_p1, closest_p2, normal;
458 nsolver->shapeTriangleInteraction(*(this->model2), this->tf2, p1, p2, p3,
459 Transform3f(), d, closest_p2, closest_p1,
460 normal);
461
462 this->result->update(d, this->model1, this->model2, primitive_id,
463 DistanceResult::NONE, closest_p1, closest_p2, normal);
464 }
465
467 bool canStop(FCL_REAL c) const {
468 if ((c >= this->result->min_distance - abs_err) &&
469 (c * (1 + rel_err) >= this->result->min_distance))
470 return true;
471 return false;
472 }
473
474 Vec3f* vertices;
475 Triangle* tri_indices;
476
477 FCL_REAL rel_err;
478 FCL_REAL abs_err;
479
480 const GJKSolver* nsolver;
481};
482
484namespace details {
485
486template <typename BV, typename S>
487void meshShapeDistanceOrientedNodeleafComputeDistance(
488 unsigned int b1, unsigned int /* b2 */, const BVHModel<BV>* model1,
489 const S& model2, Vec3f* vertices, Triangle* tri_indices,
490 const Transform3f& tf1, const Transform3f& tf2, const GJKSolver* nsolver,
491 bool enable_statistics, int& num_leaf_tests,
492 const DistanceRequest& /* request */, DistanceResult& result) {
493 if (enable_statistics) num_leaf_tests++;
494
495 const BVNode<BV>& node = model1->getBV(b1);
496 int primitive_id = node.primitiveId();
497
498 const Triangle& tri_id = tri_indices[primitive_id];
499 const Vec3f& p1 = vertices[tri_id[0]];
500 const Vec3f& p2 = vertices[tri_id[1]];
501 const Vec3f& p3 = vertices[tri_id[2]];
502
504 Vec3f closest_p1, closest_p2, normal;
505 nsolver->shapeTriangleInteraction(model2, tf2, p1, p2, p3, tf1, distance,
506 closest_p2, closest_p1, normal);
507
508 result.update(distance, model1, &model2, primitive_id, DistanceResult::NONE,
509 closest_p1, closest_p2, normal);
510}
511
512template <typename BV, typename S>
513static inline void distancePreprocessOrientedNode(
514 const BVHModel<BV>* model1, Vec3f* vertices, Triangle* tri_indices,
515 int init_tri_id, const S& model2, const Transform3f& tf1,
516 const Transform3f& tf2, const GJKSolver* nsolver,
517 const DistanceRequest& /* request */, DistanceResult& result) {
518 const Triangle& init_tri = tri_indices[init_tri_id];
519
520 const Vec3f& p1 = vertices[init_tri[0]];
521 const Vec3f& p2 = vertices[init_tri[1]];
522 const Vec3f& p3 = vertices[init_tri[2]];
523
525 Vec3f closest_p1, closest_p2, normal;
526 nsolver->shapeTriangleInteraction(model2, tf2, p1, p2, p3, tf1, distance,
527 closest_p2, closest_p1, normal);
528
529 result.update(distance, model1, &model2, init_tri_id, DistanceResult::NONE,
530 closest_p1, closest_p2, normal);
531}
532
533} // namespace details
534
536
539template <typename S>
540class MeshShapeDistanceTraversalNodeRSS
541 : public MeshShapeDistanceTraversalNode<RSS, S> {
542 public:
543 MeshShapeDistanceTraversalNodeRSS()
544 : MeshShapeDistanceTraversalNode<RSS, S>() {}
545
546 void preprocess() {
547 details::distancePreprocessOrientedNode(
548 this->model1, this->vertices, this->tri_indices, 0, *(this->model2),
549 this->tf1, this->tf2, this->nsolver, this->request, *(this->result));
550 }
551
552 void postprocess() {}
553
554 FCL_REAL BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const {
555 if (this->enable_statistics) this->num_bv_tests++;
556 return distance(this->tf1.getRotation(), this->tf1.getTranslation(),
557 this->model2_bv, this->model1->getBV(b1).bv);
558 }
559
560 void leafComputeDistance(unsigned int b1, unsigned int b2) const {
561 details::meshShapeDistanceOrientedNodeleafComputeDistance(
562 b1, b2, this->model1, *(this->model2), this->vertices,
563 this->tri_indices, this->tf1, this->tf2, this->nsolver,
564 this->enable_statistics, this->num_leaf_tests, this->request,
565 *(this->result));
566 }
567};
568
569template <typename S>
570class MeshShapeDistanceTraversalNodekIOS
571 : public MeshShapeDistanceTraversalNode<kIOS, S> {
572 public:
573 MeshShapeDistanceTraversalNodekIOS()
574 : MeshShapeDistanceTraversalNode<kIOS, S>() {}
575
576 void preprocess() {
577 details::distancePreprocessOrientedNode(
578 this->model1, this->vertices, this->tri_indices, 0, *(this->model2),
579 this->tf1, this->tf2, this->nsolver, this->request, *(this->result));
580 }
581
582 void postprocess() {}
583
584 FCL_REAL BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const {
585 if (this->enable_statistics) this->num_bv_tests++;
586 return distance(this->tf1.getRotation(), this->tf1.getTranslation(),
587 this->model2_bv, this->model1->getBV(b1).bv);
588 }
589
590 void leafComputeDistance(unsigned int b1, unsigned int b2) const {
591 details::meshShapeDistanceOrientedNodeleafComputeDistance(
592 b1, b2, this->model1, *(this->model2), this->vertices,
593 this->tri_indices, this->tf1, this->tf2, this->nsolver,
594 this->enable_statistics, this->num_leaf_tests, this->request,
595 *(this->result));
596 }
597};
598
599template <typename S>
600class MeshShapeDistanceTraversalNodeOBBRSS
601 : public MeshShapeDistanceTraversalNode<OBBRSS, S> {
602 public:
603 MeshShapeDistanceTraversalNodeOBBRSS()
604 : MeshShapeDistanceTraversalNode<OBBRSS, S>() {}
605
606 void preprocess() {
607 details::distancePreprocessOrientedNode(
608 this->model1, this->vertices, this->tri_indices, 0, *(this->model2),
609 this->tf1, this->tf2, this->nsolver, this->request, *(this->result));
610 }
611
612 void postprocess() {}
613
614 FCL_REAL BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const {
615 if (this->enable_statistics) this->num_bv_tests++;
616 return distance(this->tf1.getRotation(), this->tf1.getTranslation(),
617 this->model2_bv, this->model1->getBV(b1).bv);
618 }
619
620 void leafComputeDistance(unsigned int b1, unsigned int b2) const {
621 details::meshShapeDistanceOrientedNodeleafComputeDistance(
622 b1, b2, this->model1, *(this->model2), this->vertices,
623 this->tri_indices, this->tf1, this->tf2, this->nsolver,
624 this->enable_statistics, this->num_leaf_tests, this->request,
625 *(this->result));
626 }
627};
628
630template <typename S, typename BV>
631class ShapeMeshDistanceTraversalNode
632 : public ShapeBVHDistanceTraversalNode<S, BV> {
633 public:
634 ShapeMeshDistanceTraversalNode() : ShapeBVHDistanceTraversalNode<S, BV>() {
635 vertices = NULL;
636 tri_indices = NULL;
637
638 rel_err = 0;
639 abs_err = 0;
640
641 nsolver = NULL;
642 }
643
645 void leafComputeDistance(unsigned int /*b1*/, unsigned int b2) const {
646 if (this->enable_statistics) this->num_leaf_tests++;
647
648 const BVNode<BV>& node = this->model2->getBV(b2);
649
650 int primitive_id = node.primitiveId();
651
652 const Triangle& tri_id = tri_indices[primitive_id];
653
654 const Vec3f& p1 = vertices[tri_id[0]];
655 const Vec3f& p2 = vertices[tri_id[1]];
656 const Vec3f& p3 = vertices[tri_id[2]];
657
659 Vec3f closest_p1, closest_p2, normal;
660 nsolver->shapeTriangleInteraction(*(this->model1), this->tf1, p1, p2, p3,
661 Transform3f(), distance, closest_p1,
662 closest_p2, normal);
663
664 this->result->update(distance, this->model1, this->model2,
665 DistanceResult::NONE, primitive_id, closest_p1,
666 closest_p2, normal);
667 }
668
670 bool canStop(FCL_REAL c) const {
671 if ((c >= this->result->min_distance - abs_err) &&
672 (c * (1 + rel_err) >= this->result->min_distance))
673 return true;
674 return false;
675 }
676
677 Vec3f* vertices;
678 Triangle* tri_indices;
679
680 FCL_REAL rel_err;
681 FCL_REAL abs_err;
682
683 const GJKSolver* nsolver;
684};
685
688template <typename S>
689class ShapeMeshDistanceTraversalNodeRSS
690 : public ShapeMeshDistanceTraversalNode<S, RSS> {
691 public:
692 ShapeMeshDistanceTraversalNodeRSS()
693 : ShapeMeshDistanceTraversalNode<S, RSS>() {}
694
695 void preprocess() {
696 details::distancePreprocessOrientedNode(
697 this->model2, this->vertices, this->tri_indices, 0, *(this->model1),
698 this->tf2, this->tf1, this->nsolver, this->request, *(this->result));
699 }
700
701 void postprocess() {}
702
703 FCL_REAL BVDistanceLowerBound(unsigned int /*b1*/, unsigned int b2) const {
704 if (this->enable_statistics) this->num_bv_tests++;
705 return distance(this->tf2.getRotation(), this->tf2.getTranslation(),
706 this->model1_bv, this->model2->getBV(b2).bv);
707 }
708
709 void leafComputeDistance(unsigned int b1, unsigned int b2) const {
710 details::meshShapeDistanceOrientedNodeleafComputeDistance(
711 b2, b1, this->model2, *(this->model1), this->vertices,
712 this->tri_indices, this->tf2, this->tf1, this->nsolver,
713 this->enable_statistics, this->num_leaf_tests, this->request,
714 *(this->result));
715 }
716};
717
718template <typename S>
719class ShapeMeshDistanceTraversalNodekIOS
720 : public ShapeMeshDistanceTraversalNode<S, kIOS> {
721 public:
722 ShapeMeshDistanceTraversalNodekIOS()
723 : ShapeMeshDistanceTraversalNode<S, kIOS>() {}
724
725 void preprocess() {
726 details::distancePreprocessOrientedNode(
727 this->model2, this->vertices, this->tri_indices, 0, *(this->model1),
728 this->tf2, this->tf1, this->nsolver, *(this->result));
729 }
730
731 void postprocess() {}
732
733 FCL_REAL BVDistanceLowerBound(unsigned int /*b1*/, unsigned int b2) const {
734 if (this->enable_statistics) this->num_bv_tests++;
735 return distance(this->tf2.getRotation(), this->tf2.getTranslation(),
736 this->model1_bv, this->model2->getBV(b2).bv);
737 }
738
739 void leafComputeDistance(unsigned int b1, unsigned int b2) const {
740 details::meshShapeDistanceOrientedNodeleafComputeDistance(
741 b2, b1, this->model2, *(this->model1), this->vertices,
742 this->tri_indices, this->tf2, this->tf1, this->nsolver,
743 this->enable_statistics, this->num_leaf_tests, this->request,
744 *(this->result));
745 }
746};
747
748template <typename S>
749class ShapeMeshDistanceTraversalNodeOBBRSS
750 : public ShapeMeshDistanceTraversalNode<S, OBBRSS> {
751 public:
752 ShapeMeshDistanceTraversalNodeOBBRSS()
753 : ShapeMeshDistanceTraversalNode<S, OBBRSS>() {}
754
755 void preprocess() {
756 details::distancePreprocessOrientedNode(
757 this->model2, this->vertices, this->tri_indices, 0, *(this->model1),
758 this->tf2, this->tf1, this->nsolver, *(this->result));
759 }
760
761 void postprocess() {}
762
763 FCL_REAL BVDistanceLowerBound(unsigned int /*b1*/, unsigned int b2) const {
764 if (this->enable_statistics) this->num_bv_tests++;
765 return distance(this->tf2.getRotation(), this->tf2.getTranslation(),
766 this->model1_bv, this->model2->getBV(b2).bv);
767 }
768
769 void leafComputeDistance(unsigned int b1, unsigned int b2) const {
770 details::meshShapeDistanceOrientedNodeleafComputeDistance(
771 b2, b1, this->model2, *(this->model1), this->vertices,
772 this->tri_indices, this->tf2, this->tf1, this->nsolver,
773 this->enable_statistics, this->num_leaf_tests, this->request,
774 *(this->result));
775 }
776};
777
779
780} // namespace fcl
781
782} // namespace hpp
783
785
786#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:66
double FCL_REAL
Definition: data_types.h:65
Main namespace.
Definition: broadphase_bruteforce.h:44
static const int NONE
invalid contact primitive information
Definition: collision_data.h:451