39 #ifndef HPP_FCL_TRAVERSAL_NODE_MESH_SHAPE_H 40 #define HPP_FCL_TRAVERSAL_NODE_MESH_SHAPE_H 62 template<
typename BV,
typename S>
63 class BVHShapeCollisionTraversalNode :
public CollisionTraversalNodeBase
66 BVHShapeCollisionTraversalNode(
const CollisionRequest& request) :
67 CollisionTraversalNodeBase (request)
74 query_time_seconds = 0.0;
78 bool isFirstNodeLeaf(
unsigned int b)
const 80 return model1->getBV(b).isLeaf();
84 int getFirstLeftChild(
unsigned int b)
const 86 return model1->getBV(b).leftChild();
90 int getFirstRightChild(
unsigned int b)
const 92 return model1->getBV(b).rightChild();
95 const BVHModel<BV>* model1;
99 mutable int num_bv_tests;
100 mutable int num_leaf_tests;
101 mutable FCL_REAL query_time_seconds;
105 template<
typename S,
typename BV>
106 class ShapeBVHCollisionTraversalNode :
public CollisionTraversalNodeBase
109 ShapeBVHCollisionTraversalNode(
const CollisionRequest& request) :
110 CollisionTraversalNodeBase(request)
117 query_time_seconds = 0.0;
121 bool firstOverSecond(
unsigned int,
unsigned int)
const 127 bool isSecondNodeLeaf(
unsigned int b)
const 129 return model2->getBV(b).isLeaf();
133 int getSecondLeftChild(
unsigned int b)
const 135 return model2->getBV(b).leftChild();
139 int getSecondRightChild(
unsigned int b)
const 141 return model2->getBV(b).rightChild();
145 const BVHModel<BV>* model2;
148 mutable int num_bv_tests;
149 mutable int num_leaf_tests;
150 mutable FCL_REAL query_time_seconds;
155 template<
typename BV,
typename S,
156 int _Options = RelativeTransformationIsIdentity>
157 class MeshShapeCollisionTraversalNode :
public BVHShapeCollisionTraversalNode<BV, S>
162 RTIsIdentity = _Options & RelativeTransformationIsIdentity
165 MeshShapeCollisionTraversalNode(
const CollisionRequest& request) :
166 BVHShapeCollisionTraversalNode<BV, S> (request)
175 bool BVDisjoints(
unsigned int b1,
unsigned int )
const 177 if(this->enable_statistics) this->num_bv_tests++;
179 return !this->model1->getBV(b1).bv.overlap(this->model2_bv);
181 return !
overlap(this->tf1.getRotation(), this->tf1.getTranslation(), this->model2_bv, this->model1->getBV(b1).bv);
189 bool BVDisjoints(
unsigned int b1,
unsigned int ,
FCL_REAL& sqrDistLowerBound)
const 191 if(this->enable_statistics) this->num_bv_tests++;
194 res = !this->model1->getBV(b1).bv.overlap(this->model2_bv, this->request, sqrDistLowerBound);
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);
204 void leafCollides(
unsigned int b1,
unsigned int ,
FCL_REAL& sqrDistLowerBound)
const 206 if(this->enable_statistics) this->num_leaf_tests++;
207 const BVNode<BV>& node = this->model1->getBV(b1);
209 int primitive_id = node.primitiveId();
211 const Triangle& tri_id = tri_indices[primitive_id];
213 const Vec3f& p1 = vertices[tri_id[0]];
214 const Vec3f& p2 = vertices[tri_id[1]];
215 const Vec3f& p3 = vertices[tri_id[2]];
223 static const Transform3f Id;
225 nsolver->shapeTriangleInteraction(*(this->model2), this->tf2, p1, p2, p3,
226 Id , distance, c2, c1, normal);
229 nsolver->shapeTriangleInteraction(*(this->model2), this->tf2, p1, p2, p3,
230 this->tf1, distance, c2, c1, normal);
234 if(this->request.num_max_contacts > this->result->numContacts())
236 this->result->addContact(Contact(this->model1, this->model2,
238 c1, -normal, -distance));
239 assert (this->result->isCollision ());
243 sqrDistLowerBound = distance *
distance;
244 assert (distance > 0);
245 if ( this->request.security_margin > 0
246 && distance <= this->request.security_margin)
248 this->result->addContact(Contact(this->model1, this->model2,
250 .5 * (c1+c2), (c2-c1).normalized (),
253 assert (!this->result->isCollision () || sqrDistLowerBound > 0);
257 Triangle* tri_indices;
259 const GJKSolver* nsolver;
263 template<
typename S,
typename BV,
264 int _Options = RelativeTransformationIsIdentity>
265 class ShapeMeshCollisionTraversalNode :
public ShapeBVHCollisionTraversalNode<S, BV>
270 RTIsIdentity = _Options & RelativeTransformationIsIdentity
273 ShapeMeshCollisionTraversalNode() : ShapeBVHCollisionTraversalNode<S, BV>()
283 bool BVDisjoints(
unsigned int ,
unsigned int b2)
const 285 if(this->enable_statistics) this->num_bv_tests++;
287 return !this->model2->getBV(b2).bv.overlap(this->model1_bv);
289 return !
overlap(this->tf2.getRotation(), this->tf2.getTranslation(), this->model1_bv, this->model2->getBV(b2).bv);
296 bool BVDisjoints(
unsigned int ,
unsigned int b2,
FCL_REAL& sqrDistLowerBound)
const 298 if(this->enable_statistics) this->num_bv_tests++;
301 res = !this->model2->getBV(b2).bv.overlap(this->model1_bv, sqrDistLowerBound);
303 res = !
overlap(this->tf2.getRotation(), this->tf2.getTranslation(),
304 this->model1_bv, this->model2->getBV(b2).bv,
306 assert (!res || sqrDistLowerBound > 0);
311 void leafCollides(
unsigned int ,
unsigned int b2,
FCL_REAL& sqrDistLowerBound)
const 313 if(this->enable_statistics) this->num_leaf_tests++;
314 const BVNode<BV>& node = this->model2->getBV(b2);
316 int primitive_id = node.primitiveId();
318 const Triangle& tri_id = tri_indices[primitive_id];
320 const Vec3f& p1 = vertices[tri_id[0]];
321 const Vec3f& p2 = vertices[tri_id[1]];
322 const Vec3f& p3 = vertices[tri_id[2]];
330 static const Transform3f Id;
332 nsolver->shapeTriangleInteraction(*(this->model1), this->tf1, p1, p2, p3,
333 Id , c1, c2, distance, normal);
336 nsolver->shapeTriangleInteraction(*(this->model1), this->tf1, p1, p2, p3,
337 this->tf2, c1, c2, distance, normal);
341 if(this->request.num_max_contacts > this->result->numContacts())
343 this->result->addContact (Contact(this->model1 , this->model2,
345 c1, normal, -distance));
346 assert (this->result->isCollision ());
350 sqrDistLowerBound = distance *
distance;
351 assert (distance > 0);
352 if ( this->request.security_margin == 0
353 && distance <= this->request.security_margin)
355 this->result.addContact (Contact(this->model1 , this->model2,
357 .5 * (c1+c2), (c2-c1).normalized (),
360 assert (!this->result->isCollision () || sqrDistLowerBound > 0);
364 Triangle* tri_indices;
366 const GJKSolver* nsolver;
375 template<
typename BV,
typename S>
376 class BVHShapeDistanceTraversalNode :
public DistanceTraversalNodeBase
379 BVHShapeDistanceTraversalNode() : DistanceTraversalNodeBase()
386 query_time_seconds = 0.0;
390 bool isFirstNodeLeaf(
unsigned int b)
const 392 return model1->getBV(b).isLeaf();
396 int getFirstLeftChild(
unsigned int b)
const 398 return model1->getBV(b).leftChild();
402 int getFirstRightChild(
unsigned int b)
const 404 return model1->getBV(b).rightChild();
408 FCL_REAL BVDistanceLowerBound(
unsigned int b1,
unsigned int )
const 410 return model1->getBV(b1).bv.distance(model2_bv);
413 const BVHModel<BV>* model1;
417 mutable int num_bv_tests;
418 mutable int num_leaf_tests;
419 mutable FCL_REAL query_time_seconds;
423 template<
typename S,
typename BV>
424 class ShapeBVHDistanceTraversalNode :
public DistanceTraversalNodeBase
427 ShapeBVHDistanceTraversalNode() : DistanceTraversalNodeBase()
434 query_time_seconds = 0.0;
438 bool isSecondNodeLeaf(
unsigned int b)
const 440 return model2->getBV(b).isLeaf();
444 int getSecondLeftChild(
unsigned int b)
const 446 return model2->getBV(b).leftChild();
450 int getSecondRightChild(
unsigned int b)
const 452 return model2->getBV(b).rightChild();
456 FCL_REAL BVDistanceLowerBound(
unsigned int ,
unsigned int b2)
const 458 return model1_bv.distance(model2->getBV(b2).bv);
462 const BVHModel<BV>* model2;
465 mutable int num_bv_tests;
466 mutable int num_leaf_tests;
467 mutable FCL_REAL query_time_seconds;
472 template<
typename BV,
typename S>
473 class MeshShapeDistanceTraversalNode :
public BVHShapeDistanceTraversalNode<BV, S>
476 MeshShapeDistanceTraversalNode() : BVHShapeDistanceTraversalNode<BV, S>()
488 void leafComputeDistance(
unsigned int b1,
unsigned int )
const 490 if(this->enable_statistics) this->num_leaf_tests++;
492 const BVNode<BV>& node = this->model1->getBV(b1);
494 int primitive_id = node.primitiveId();
496 const Triangle& tri_id = tri_indices[primitive_id];
498 const Vec3f& p1 = vertices[tri_id[0]];
499 const Vec3f& p2 = vertices[tri_id[1]];
500 const Vec3f& p3 = vertices[tri_id[2]];
503 Vec3f closest_p1, closest_p2, normal;
504 nsolver->shapeTriangleInteraction(*(this->model2), this->tf2, p1, p2, p3,
505 Transform3f (), d, closest_p2, closest_p1,
508 this->result->update(d, this->model1, this->model2, primitive_id,
516 if((c >= this->result->min_distance - abs_err) && (c * (1 + rel_err) >= this->result->min_distance))
522 Triangle* tri_indices;
527 const GJKSolver* nsolver;
534 template<
typename BV,
typename S>
535 void meshShapeDistanceOrientedNodeleafComputeDistance(
unsigned int b1,
unsigned int ,
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& ,
544 DistanceResult& result)
546 if(enable_statistics) num_leaf_tests++;
548 const BVNode<BV>& node = model1->getBV(b1);
549 int primitive_id = node.primitiveId();
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]];
557 Vec3f closest_p1, closest_p2, normal;
558 nsolver->shapeTriangleInteraction(model2, tf2, p1, p2, p3, tf1, distance,
559 closest_p2, closest_p1, normal);
562 closest_p1, closest_p2, normal);
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& ,
572 DistanceResult& result)
574 const Triangle& init_tri = tri_indices[init_tri_id];
576 const Vec3f& p1 = vertices[init_tri[0]];
577 const Vec3f& p2 = vertices[init_tri[1]];
578 const Vec3f& p3 = vertices[init_tri[2]];
581 Vec3f closest_p1, closest_p2, normal;
582 nsolver->shapeTriangleInteraction(model2, tf2, p1, p2, p3, tf1, distance,
583 closest_p2, closest_p1, normal);
586 closest_p1, closest_p2, normal);
607 details::distancePreprocessOrientedNode(this->model1, this->vertices, this->tri_indices, 0,
608 *(this->model2), this->tf1, this->tf2, this->nsolver, this->request, *(this->result));
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);
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));
639 details::distancePreprocessOrientedNode(this->model1, this->vertices, this->tri_indices, 0,
640 *(this->model2), this->tf1, this->tf2, this->nsolver, this->request, *(this->result));
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);
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));
671 details::distancePreprocessOrientedNode(this->model1, this->vertices, this->tri_indices, 0,
672 *(this->model2), this->tf1, this->tf2, this->nsolver, this->request, *(this->result));
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);
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));
695 template<
typename S,
typename BV>
713 if(this->enable_statistics) this->num_leaf_tests++;
715 const BVNode<BV>& node = this->model2->getBV(b2);
717 int primitive_id = node.primitiveId();
719 const Triangle& tri_id = tri_indices[primitive_id];
721 const Vec3f& p1 = vertices[tri_id[0]];
722 const Vec3f& p2 = vertices[tri_id[1]];
723 const Vec3f& p3 = vertices[tri_id[2]];
726 Vec3f closest_p1, closest_p2, normal;
727 nsolver->shapeTriangleInteraction(*(this->model1), this->tf1, p1, p2, p3,
728 Transform3f (), distance, closest_p1,
731 this->result->update(distance, this->model1, this->model2,
739 if((c >= this->result->min_distance - abs_err) && (c * (1 + rel_err) >= this->result->min_distance))
764 details::distancePreprocessOrientedNode(this->model2, this->vertices, this->tri_indices, 0,
765 *(this->model1), this->tf2, this->tf1, this->nsolver, this->request, *(this->result));
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);
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));
796 details::distancePreprocessOrientedNode(this->model2, this->vertices, this->tri_indices, 0,
797 *(this->model1), this->tf2, this->tf1, this->nsolver, *(this->result));
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);
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));
828 details::distancePreprocessOrientedNode(this->model2, this->vertices, this->tri_indices, 0,
829 *(this->model1), this->tf2, this->tf1, this->nsolver, *(this->result));
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);
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));
ShapeMeshDistanceTraversalNode()
Definition: traversal_node_bvh_shape.h:699
Main namespace.
Definition: AABB.h:43
FCL_REAL BVDistanceLowerBound(unsigned int b1, unsigned int) const
Definition: traversal_node_bvh_shape.h:647
void postprocess()
Definition: traversal_node_bvh_shape.h:800
void preprocess()
Definition: traversal_node_bvh_shape.h:637
ShapeMeshDistanceTraversalNodekIOS()
Definition: traversal_node_bvh_shape.h:790
void leafComputeDistance(unsigned int b1, unsigned int b2) const
Definition: traversal_node_bvh_shape.h:810
void leafComputeDistance(unsigned int, unsigned int b2) const
Distance testing between leaves (one shape and one triangle)
Definition: traversal_node_bvh_shape.h:711
FCL_REAL rel_err
Definition: traversal_node_bvh_shape.h:747
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.
double FCL_REAL
Definition: data_types.h:66
Definition: traversal_node_setup.h:851
void preprocess()
Definition: traversal_node_bvh_shape.h:794
Triangle * tri_indices
Definition: traversal_node_bvh_shape.h:745
MeshShapeDistanceTraversalNodekIOS()
Definition: traversal_node_bvh_shape.h:633
Definition: traversal_node_bvh_shape.h:787
Traversal node for distance between shape and mesh.
Definition: traversal_node_bvh_shape.h:696
void postprocess()
Definition: traversal_node_bvh_shape.h:643
void leafComputeDistance(unsigned int b1, unsigned int b2) const
Definition: traversal_node_bvh_shape.h:653
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.
FCL_REAL BVDistanceLowerBound(unsigned int, unsigned int b2) const
Definition: traversal_node_bvh_shape.h:804
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:67
bool canStop(FCL_REAL c) const
Whether the traversal process can stop early.
Definition: traversal_node_bvh_shape.h:737
Definition: traversal_node_bvh_shape.h:630
Vec3f * vertices
Definition: traversal_node_bvh_shape.h:744
FCL_REAL abs_err
Definition: traversal_node_bvh_shape.h:748
const GJKSolver * nsolver
Definition: traversal_node_bvh_shape.h:750
static const int NONE
invalid contact primitive information
Definition: collision_data.h:435