38#ifndef HPP_FCL_TRAVERSAL_NODE_MESH_SHAPE_H
39#define HPP_FCL_TRAVERSAL_NODE_MESH_SHAPE_H
58template <
typename BV,
typename S>
59class BVHShapeCollisionTraversalNode :
public CollisionTraversalNodeBase {
61 BVHShapeCollisionTraversalNode(
const CollisionRequest& request)
62 : CollisionTraversalNodeBase(request) {
68 query_time_seconds = 0.0;
72 bool isFirstNodeLeaf(
unsigned int b)
const {
73 return model1->getBV(b).isLeaf();
77 int getFirstLeftChild(
unsigned int b)
const {
78 return model1->getBV(b).leftChild();
82 int getFirstRightChild(
unsigned int b)
const {
83 return model1->getBV(b).rightChild();
86 const BVHModel<BV>* model1;
90 mutable int num_bv_tests;
91 mutable int num_leaf_tests;
96template <
typename S,
typename BV>
97class ShapeBVHCollisionTraversalNode :
public CollisionTraversalNodeBase {
99 ShapeBVHCollisionTraversalNode(
const CollisionRequest& request)
100 : CollisionTraversalNodeBase(request) {
106 query_time_seconds = 0.0;
110 bool firstOverSecond(
unsigned int,
unsigned int)
const {
return false; }
113 bool isSecondNodeLeaf(
unsigned int b)
const {
114 return model2->getBV(b).isLeaf();
118 int getSecondLeftChild(
unsigned int b)
const {
119 return model2->getBV(b).leftChild();
123 int getSecondRightChild(
unsigned int b)
const {
124 return model2->getBV(b).rightChild();
128 const BVHModel<BV>* model2;
131 mutable int num_bv_tests;
132 mutable int num_leaf_tests;
133 mutable FCL_REAL query_time_seconds;
137template <
typename BV,
typename S,
138 int _Options = RelativeTransformationIsIdentity>
139class MeshShapeCollisionTraversalNode
140 :
public BVHShapeCollisionTraversalNode<BV, S> {
144 RTIsIdentity = _Options & RelativeTransformationIsIdentity
147 MeshShapeCollisionTraversalNode(
const CollisionRequest& request)
148 : BVHShapeCollisionTraversalNode<BV, S>(request) {
160 bool BVDisjoints(
unsigned int b1,
unsigned int ,
161 FCL_REAL& sqrDistLowerBound)
const {
162 if (this->enable_statistics) this->num_bv_tests++;
165 disjoint = !this->model1->getBV(b1).bv.overlap(
166 this->model2_bv, this->request, sqrDistLowerBound);
168 disjoint = !
overlap(this->tf1.getRotation(), this->tf1.getTranslation(),
169 this->model1->getBV(b1).bv, this->model2_bv,
170 this->request, sqrDistLowerBound);
172 internal::updateDistanceLowerBoundFromBV(this->request, *this->result,
174 assert(!disjoint || sqrDistLowerBound > 0);
179 void leafCollides(
unsigned int b1,
unsigned int ,
180 FCL_REAL& sqrDistLowerBound)
const {
181 if (this->enable_statistics) this->num_leaf_tests++;
182 const BVNode<BV>& node = this->model1->getBV(b1);
184 int primitive_id = node.primitiveId();
186 const Triangle& tri_id = tri_indices[primitive_id];
188 const Vec3f& p1 = vertices[tri_id[0]];
189 const Vec3f& p2 = vertices[tri_id[1]];
190 const Vec3f& p3 = vertices[tri_id[2]];
198 static const Transform3f Id;
199 collision = nsolver->shapeTriangleInteraction(
200 *(this->model2), this->tf2, p1, p2, p3, Id, distance, c2, c1, normal);
202 collision = nsolver->shapeTriangleInteraction(*(this->model2), this->tf2,
203 p1, p2, p3, this->tf1,
204 distance, c2, c1, normal);
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());
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));
224 sqrDistLowerBound = distToCollision * distToCollision;
226 internal::updateDistanceLowerBoundFromLeaf(this->request, *this->result,
227 distToCollision, c1, c2);
229 assert(this->result->isCollision() || sqrDistLowerBound > 0);
233 Triangle* tri_indices;
235 const GJKSolver* nsolver;
239template <
typename S,
typename BV,
240 int _Options = RelativeTransformationIsIdentity>
241class ShapeMeshCollisionTraversalNode
242 :
public ShapeBVHCollisionTraversalNode<S, BV> {
246 RTIsIdentity = _Options & RelativeTransformationIsIdentity
249 ShapeMeshCollisionTraversalNode() : ShapeBVHCollisionTraversalNode<S, BV>() {
260 bool BVDisjoints(
unsigned int ,
unsigned int b2,
261 FCL_REAL& sqrDistLowerBound)
const {
262 if (this->enable_statistics) this->num_bv_tests++;
265 disjoint = !this->model2->getBV(b2).bv.overlap(this->model1_bv,
268 disjoint = !
overlap(this->tf2.getRotation(), this->tf2.getTranslation(),
269 this->model2->getBV(b2).bv, this->model1_bv,
272 internal::updateDistanceLowerBoundFromBV(this->request, *this->result,
274 assert(!disjoint || sqrDistLowerBound > 0);
279 void leafCollides(
unsigned int ,
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);
284 int primitive_id = node.primitiveId();
286 const Triangle& tri_id = tri_indices[primitive_id];
288 const Vec3f& p1 = vertices[tri_id[0]];
289 const Vec3f& p2 = vertices[tri_id[1]];
290 const Vec3f& p3 = vertices[tri_id[2]];
298 static const Transform3f Id;
299 collision = nsolver->shapeTriangleInteraction(
300 *(this->model1), this->tf1, p1, p2, p3, Id, c1, c2, distance, normal);
302 collision = nsolver->shapeTriangleInteraction(*(this->model1), this->tf1,
303 p1, p2, p3, this->tf2, c1,
304 c2, distance, normal);
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,
314 assert(this->result->isCollision());
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));
324 sqrDistLowerBound = distToCollision * distToCollision;
326 internal::updateDistanceLowerBoundFromLeaf(this->request, *this->result,
327 distToCollision, c1, c2);
329 assert(this->result->isCollision() || sqrDistLowerBound > 0);
333 Triangle* tri_indices;
335 const GJKSolver* nsolver;
344template <
typename BV,
typename S>
345class BVHShapeDistanceTraversalNode :
public DistanceTraversalNodeBase {
347 BVHShapeDistanceTraversalNode() : DistanceTraversalNodeBase() {
353 query_time_seconds = 0.0;
357 bool isFirstNodeLeaf(
unsigned int b)
const {
358 return model1->getBV(b).isLeaf();
362 int getFirstLeftChild(
unsigned int b)
const {
363 return model1->getBV(b).leftChild();
367 int getFirstRightChild(
unsigned int b)
const {
368 return model1->getBV(b).rightChild();
372 FCL_REAL BVDistanceLowerBound(
unsigned int b1,
unsigned int )
const {
373 return model1->getBV(b1).bv.distance(model2_bv);
376 const BVHModel<BV>* model1;
380 mutable int num_bv_tests;
381 mutable int num_leaf_tests;
382 mutable FCL_REAL query_time_seconds;
386template <
typename S,
typename BV>
387class ShapeBVHDistanceTraversalNode :
public DistanceTraversalNodeBase {
389 ShapeBVHDistanceTraversalNode() : DistanceTraversalNodeBase() {
395 query_time_seconds = 0.0;
399 bool isSecondNodeLeaf(
unsigned int b)
const {
400 return model2->getBV(b).isLeaf();
404 int getSecondLeftChild(
unsigned int b)
const {
405 return model2->getBV(b).leftChild();
409 int getSecondRightChild(
unsigned int b)
const {
410 return model2->getBV(b).rightChild();
414 FCL_REAL BVDistanceLowerBound(
unsigned int ,
unsigned int b2)
const {
415 return model1_bv.distance(model2->getBV(b2).bv);
419 const BVHModel<BV>* model2;
422 mutable int num_bv_tests;
423 mutable int num_leaf_tests;
424 mutable FCL_REAL query_time_seconds;
428template <
typename BV,
typename S>
429class MeshShapeDistanceTraversalNode
430 :
public BVHShapeDistanceTraversalNode<BV, S> {
432 MeshShapeDistanceTraversalNode() : BVHShapeDistanceTraversalNode<BV, S>() {
443 void leafComputeDistance(
unsigned int b1,
unsigned int )
const {
444 if (this->enable_statistics) this->num_leaf_tests++;
446 const BVNode<BV>& node = this->model1->getBV(b1);
448 int primitive_id = node.primitiveId();
450 const Triangle& tri_id = tri_indices[primitive_id];
452 const Vec3f& p1 = vertices[tri_id[0]];
453 const Vec3f& p2 = vertices[tri_id[1]];
454 const Vec3f& p3 = vertices[tri_id[2]];
457 Vec3f closest_p1, closest_p2, normal;
458 nsolver->shapeTriangleInteraction(*(this->model2), this->tf2, p1, p2, p3,
459 Transform3f(), d, closest_p2, closest_p1,
462 this->result->update(d, this->model1, this->model2, primitive_id,
463 DistanceResult::NONE, closest_p1, closest_p2, normal);
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))
475 Triangle* tri_indices;
480 const GJKSolver* nsolver;
486template <
typename BV,
typename S>
487void meshShapeDistanceOrientedNodeleafComputeDistance(
488 unsigned int b1,
unsigned int ,
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& , DistanceResult& result) {
493 if (enable_statistics) num_leaf_tests++;
495 const BVNode<BV>& node = model1->getBV(b1);
496 int primitive_id = node.primitiveId();
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]];
504 Vec3f closest_p1, closest_p2, normal;
505 nsolver->shapeTriangleInteraction(model2, tf2, p1, p2, p3, tf1,
distance,
506 closest_p2, closest_p1, normal);
509 closest_p1, closest_p2, normal);
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& , DistanceResult& result) {
518 const Triangle& init_tri = tri_indices[init_tri_id];
520 const Vec3f& p1 = vertices[init_tri[0]];
521 const Vec3f& p2 = vertices[init_tri[1]];
522 const Vec3f& p3 = vertices[init_tri[2]];
525 Vec3f closest_p1, closest_p2, normal;
526 nsolver->shapeTriangleInteraction(model2, tf2, p1, p2, p3, tf1,
distance,
527 closest_p2, closest_p1, normal);
530 closest_p1, closest_p2, normal);
540class MeshShapeDistanceTraversalNodeRSS
541 :
public MeshShapeDistanceTraversalNode<RSS, S> {
543 MeshShapeDistanceTraversalNodeRSS()
544 : MeshShapeDistanceTraversalNode<RSS, S>() {}
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));
552 void postprocess() {}
554 FCL_REAL BVDistanceLowerBound(
unsigned int b1,
unsigned int )
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);
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,
570class MeshShapeDistanceTraversalNodekIOS
571 :
public MeshShapeDistanceTraversalNode<kIOS, S> {
573 MeshShapeDistanceTraversalNodekIOS()
574 : MeshShapeDistanceTraversalNode<kIOS, S>() {}
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));
582 void postprocess() {}
584 FCL_REAL BVDistanceLowerBound(
unsigned int b1,
unsigned int )
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);
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,
600class MeshShapeDistanceTraversalNodeOBBRSS
601 :
public MeshShapeDistanceTraversalNode<OBBRSS, S> {
603 MeshShapeDistanceTraversalNodeOBBRSS()
604 : MeshShapeDistanceTraversalNode<OBBRSS, S>() {}
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));
612 void postprocess() {}
614 FCL_REAL BVDistanceLowerBound(
unsigned int b1,
unsigned int )
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);
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,
630template <
typename S,
typename BV>
631class ShapeMeshDistanceTraversalNode
632 :
public ShapeBVHDistanceTraversalNode<S, BV> {
634 ShapeMeshDistanceTraversalNode() : ShapeBVHDistanceTraversalNode<S, BV>() {
645 void leafComputeDistance(
unsigned int ,
unsigned int b2)
const {
646 if (this->enable_statistics) this->num_leaf_tests++;
648 const BVNode<BV>& node = this->model2->getBV(b2);
650 int primitive_id = node.primitiveId();
652 const Triangle& tri_id = tri_indices[primitive_id];
654 const Vec3f& p1 = vertices[tri_id[0]];
655 const Vec3f& p2 = vertices[tri_id[1]];
656 const Vec3f& p3 = vertices[tri_id[2]];
659 Vec3f closest_p1, closest_p2, normal;
660 nsolver->shapeTriangleInteraction(*(this->model1), this->tf1, p1, p2, p3,
661 Transform3f(), distance, closest_p1,
664 this->result->update(distance, this->model1, this->model2,
665 DistanceResult::NONE, primitive_id, closest_p1,
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))
678 Triangle* tri_indices;
683 const GJKSolver* nsolver;
689class ShapeMeshDistanceTraversalNodeRSS
690 :
public ShapeMeshDistanceTraversalNode<S, RSS> {
692 ShapeMeshDistanceTraversalNodeRSS()
693 : ShapeMeshDistanceTraversalNode<S, RSS>() {}
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));
701 void postprocess() {}
703 FCL_REAL BVDistanceLowerBound(
unsigned int ,
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);
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,
719class ShapeMeshDistanceTraversalNodekIOS
720 :
public ShapeMeshDistanceTraversalNode<S, kIOS> {
722 ShapeMeshDistanceTraversalNodekIOS()
723 : ShapeMeshDistanceTraversalNode<S, kIOS>() {}
726 details::distancePreprocessOrientedNode(
727 this->model2, this->vertices, this->tri_indices, 0, *(this->model1),
728 this->tf2, this->tf1, this->nsolver, *(this->result));
731 void postprocess() {}
733 FCL_REAL BVDistanceLowerBound(
unsigned int ,
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);
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,
749class ShapeMeshDistanceTraversalNodeOBBRSS
750 :
public ShapeMeshDistanceTraversalNode<S, OBBRSS> {
752 ShapeMeshDistanceTraversalNodeOBBRSS()
753 : ShapeMeshDistanceTraversalNode<S, OBBRSS>() {}
756 details::distancePreprocessOrientedNode(
757 this->model2, this->vertices, this->tri_indices, 0, *(this->model1),
758 this->tf2, this->tf1, this->nsolver, *(this->result));
761 void postprocess() {}
763 FCL_REAL BVDistanceLowerBound(
unsigned int ,
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);
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,
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