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(
int b)
const 80 return model1->getBV(b).isLeaf();
84 int getFirstLeftChild(
int b)
const 86 return model1->getBV(b).leftChild();
90 int getFirstRightChild(
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(
int,
int)
const 127 bool isSecondNodeLeaf(
int b)
const 129 return model2->getBV(b).isLeaf();
133 int getSecondLeftChild(
int b)
const 135 return model2->getBV(b).leftChild();
139 int getSecondRightChild(
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(
int b1,
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(
int b1,
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(
int b1,
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(
int ,
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(
int ,
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(
int ,
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(
int b)
const 392 return model1->getBV(b).isLeaf();
396 int getFirstLeftChild(
int b)
const 398 return model1->getBV(b).leftChild();
402 int getFirstRightChild(
int b)
const 404 return model1->getBV(b).rightChild();
408 FCL_REAL BVDistanceLowerBound(
int b1,
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(
int b)
const 440 return model2->getBV(b).isLeaf();
444 int getSecondLeftChild(
int b)
const 446 return model2->getBV(b).leftChild();
450 int getSecondRightChild(
int b)
const 452 return model2->getBV(b).rightChild();
456 FCL_REAL BVDistanceLowerBound(
int b1,
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(
int b1,
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(
int b1,
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(int b1, 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(int b1, int b2) const
Definition: traversal_node_bvh_shape.h:810
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:68
Definition: traversal_node_setup.h:775
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
FCL_REAL BVDistanceLowerBound(int b1, int b2) const
Definition: traversal_node_bvh_shape.h:804
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.
void leafComputeDistance(int b1, int b2) const
Distance testing between leaves (one shape and one triangle)
Definition: traversal_node_bvh_shape.h:711
void leafComputeDistance(int b1, int b2) const
Definition: traversal_node_bvh_shape.h:653
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:73
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:366