38 #ifndef HPP_FCL_TRAVERSAL_NODE_MESHES_H 39 #define HPP_FCL_TRAVERSAL_NODE_MESHES_H 67 class BVHCollisionTraversalNode :
public CollisionTraversalNodeBase
70 BVHCollisionTraversalNode(
const CollisionRequest& request) :
71 CollisionTraversalNodeBase (request)
78 query_time_seconds = 0.0;
82 bool isFirstNodeLeaf(
int b)
const 84 return model1->getBV(b).isLeaf();
88 bool isSecondNodeLeaf(
int b)
const 90 return model2->getBV(b).isLeaf();
94 bool firstOverSecond(
int b1,
int b2)
const 96 FCL_REAL sz1 = model1->getBV(b1).bv.size();
97 FCL_REAL sz2 = model2->getBV(b2).bv.size();
99 bool l1 = model1->getBV(b1).isLeaf();
100 bool l2 = model2->getBV(b2).isLeaf();
102 if(l2 || (!l1 && (sz1 > sz2)))
108 int getFirstLeftChild(
int b)
const 110 return model1->getBV(b).leftChild();
114 int getFirstRightChild(
int b)
const 116 return model1->getBV(b).rightChild();
120 int getSecondLeftChild(
int b)
const 122 return model2->getBV(b).leftChild();
126 int getSecondRightChild(
int b)
const 128 return model2->getBV(b).rightChild();
132 const BVHModel<BV>* model1;
134 const BVHModel<BV>* model2;
137 mutable int num_bv_tests;
138 mutable int num_leaf_tests;
139 mutable FCL_REAL query_time_seconds;
143 template<
typename BV,
int _Options = RelativeTransformationIsIdentity>
144 class MeshCollisionTraversalNode :
public BVHCollisionTraversalNode<BV>
149 RTIsIdentity = _Options & RelativeTransformationIsIdentity
152 MeshCollisionTraversalNode(
const CollisionRequest& request) :
153 BVHCollisionTraversalNode<BV> (request)
162 bool BVDisjoints(
int b1,
int b2)
const 164 if(this->enable_statistics) this->num_bv_tests++;
166 return !this->model1->getBV(b1).overlap(this->model2->getBV(b2));
168 return !
overlap(RT._R(), RT._T(),
169 this->model1->getBV(b1).bv, this->model2->getBV(b2).bv);
176 bool BVDisjoints(
int b1,
int b2,
FCL_REAL& sqrDistLowerBound)
const 178 if(this->enable_statistics) this->num_bv_tests++;
180 return !this->model1->getBV(b1).overlap(this->model2->getBV(b2),
181 this->request, sqrDistLowerBound);
183 bool res = !
overlap(RT._R(), RT._T(),
184 this->model1->getBV(b1).bv, this->model2->getBV(b2).bv,
185 this->request, sqrDistLowerBound);
186 assert (!res || sqrDistLowerBound > 0);
205 void leafCollides(
int b1,
int b2,
FCL_REAL& sqrDistLowerBound)
const 207 if(this->enable_statistics) this->num_leaf_tests++;
209 const BVNode<BV>& node1 = this->model1->getBV(b1);
210 const BVNode<BV>& node2 = this->model2->getBV(b2);
212 int primitive_id1 = node1.primitiveId();
213 int primitive_id2 = node2.primitiveId();
215 const Triangle& tri_id1 = tri_indices1[primitive_id1];
216 const Triangle& tri_id2 = tri_indices2[primitive_id2];
218 const Vec3f& P1 = vertices1[tri_id1[0]];
219 const Vec3f& P2 = vertices1[tri_id1[1]];
220 const Vec3f& P3 = vertices1[tri_id1[2]];
221 const Vec3f& Q1 = vertices2[tri_id2[0]];
222 const Vec3f& Q2 = vertices2[tri_id2[1]];
223 const Vec3f& Q3 = vertices2[tri_id2[2]];
225 TriangleP tri1 (P1, P2, P3);
226 TriangleP tri2 (Q1, Q2, Q3);
231 solver.shapeDistance (tri1, this->tf1, tri2, this->tf2,
232 distance, p1, p2, normal);
233 FCL_REAL distToCollision = distance - this->request.security_margin;
234 sqrDistLowerBound = distance *
distance;
235 if (distToCollision <= 0) {
238 if(this->result->numContacts() < this->request.num_max_contacts) {
243 normal = (p2-p1).normalized ();
246 this->result->addContact(Contact(this->model1, this->model2,
247 primitive_id1, primitive_id2,
248 p, normal, penetrationDepth));
256 Triangle* tri_indices1;
257 Triangle* tri_indices2;
259 details::RelativeTransformation<!bool(RTIsIdentity)> RT;
263 typedef MeshCollisionTraversalNode<OBB , 0> MeshCollisionTraversalNodeOBB ;
264 typedef MeshCollisionTraversalNode<RSS , 0> MeshCollisionTraversalNodeRSS ;
265 typedef MeshCollisionTraversalNode<kIOS , 0> MeshCollisionTraversalNodekIOS ;
266 typedef MeshCollisionTraversalNode<OBBRSS, 0> MeshCollisionTraversalNodeOBBRSS;
272 template<
typename BV>
struct DistanceTraversalBVDistanceLowerBound_impl
274 static FCL_REAL run(
const BVNode<BV>& b1,
const BVNode<BV>& b2)
276 return b1.distance(b2);
280 return distance(R, T, b1.bv, b2.bv);
284 template<>
struct DistanceTraversalBVDistanceLowerBound_impl<OBB>
286 static FCL_REAL run(
const BVNode<OBB>& b1,
const BVNode<OBB>& b2)
291 if (b1.overlap(b2, request, sqrDistLowerBound)) {
295 return sqrt (sqrDistLowerBound);
302 if (
overlap(R, T, b1.bv, b2.bv, request, sqrDistLowerBound)) {
306 return sqrt (sqrDistLowerBound);
310 template<>
struct DistanceTraversalBVDistanceLowerBound_impl<AABB>
312 static FCL_REAL run(
const BVNode<AABB>& b1,
const BVNode<AABB>& b2)
317 if (b1.overlap(b2, request, sqrDistLowerBound)) {
321 return sqrt (sqrDistLowerBound);
323 static FCL_REAL run(
const Matrix3f& R,
const Vec3f& T,
const BVNode<AABB>& b1,
const BVNode<AABB>& b2)
328 if (
overlap(R, T, b1.bv, b2.bv, request, sqrDistLowerBound)) {
332 return sqrt (sqrDistLowerBound);
341 template<
typename BV>
342 class BVHDistanceTraversalNode :
public DistanceTraversalNodeBase
345 BVHDistanceTraversalNode() : DistanceTraversalNodeBase()
352 query_time_seconds = 0.0;
356 bool isFirstNodeLeaf(
int b)
const 358 return model1->getBV(b).isLeaf();
362 bool isSecondNodeLeaf(
int b)
const 364 return model2->getBV(b).isLeaf();
368 bool firstOverSecond(
int b1,
int b2)
const 370 FCL_REAL sz1 = model1->getBV(b1).bv.size();
371 FCL_REAL sz2 = model2->getBV(b2).bv.size();
373 bool l1 = model1->getBV(b1).isLeaf();
374 bool l2 = model2->getBV(b2).isLeaf();
376 if(l2 || (!l1 && (sz1 > sz2)))
382 int getFirstLeftChild(
int b)
const 384 return model1->getBV(b).leftChild();
388 int getFirstRightChild(
int b)
const 390 return model1->getBV(b).rightChild();
394 int getSecondLeftChild(
int b)
const 396 return model2->getBV(b).leftChild();
400 int getSecondRightChild(
int b)
const 402 return model2->getBV(b).rightChild();
406 const BVHModel<BV>* model1;
408 const BVHModel<BV>* model2;
411 mutable int num_bv_tests;
412 mutable int num_leaf_tests;
413 mutable FCL_REAL query_time_seconds;
418 template<
typename BV,
int _Options = RelativeTransformationIsIdentity>
419 class MeshDistanceTraversalNode :
public BVHDistanceTraversalNode<BV>
424 RTIsIdentity = _Options & RelativeTransformationIsIdentity
427 using BVHDistanceTraversalNode<BV>::enable_statistics;
428 using BVHDistanceTraversalNode<BV>::request;
429 using BVHDistanceTraversalNode<BV>::result;
430 using BVHDistanceTraversalNode<BV>::tf1;
431 using BVHDistanceTraversalNode<BV>::model1;
432 using BVHDistanceTraversalNode<BV>::model2;
433 using BVHDistanceTraversalNode<BV>::num_bv_tests;
434 using BVHDistanceTraversalNode<BV>::num_leaf_tests;
436 MeshDistanceTraversalNode() : BVHDistanceTraversalNode<BV>()
443 rel_err = this->request.rel_err;
444 abs_err = this->request.abs_err;
449 if(!RTIsIdentity) preprocessOrientedNode();
454 if(!RTIsIdentity) postprocessOrientedNode();
458 FCL_REAL BVDistanceLowerBound(
int b1,
int b2)
const 460 if(enable_statistics) num_bv_tests++;
462 return details::DistanceTraversalBVDistanceLowerBound_impl<BV>
463 ::run (model1->getBV(b1), model2->getBV(b2));
465 return details::DistanceTraversalBVDistanceLowerBound_impl<BV>
466 ::run (RT._R(), RT._T(), model1->getBV(b1), model2->getBV(b2));
470 void leafComputeDistance(
int b1,
int b2)
const 472 if(this->enable_statistics) this->num_leaf_tests++;
474 const BVNode<BV>& node1 = this->model1->getBV(b1);
475 const BVNode<BV>& node2 = this->model2->getBV(b2);
477 int primitive_id1 = node1.primitiveId();
478 int primitive_id2 = node2.primitiveId();
480 const Triangle& tri_id1 = tri_indices1[primitive_id1];
481 const Triangle& tri_id2 = tri_indices2[primitive_id2];
483 const Vec3f& t11 = vertices1[tri_id1[0]];
484 const Vec3f& t12 = vertices1[tri_id1[1]];
485 const Vec3f& t13 = vertices1[tri_id1[2]];
487 const Vec3f& t21 = vertices2[tri_id2[0]];
488 const Vec3f& t22 = vertices2[tri_id2[1]];
489 const Vec3f& t23 = vertices2[tri_id2[2]];
492 Vec3f P1, P2, normal;
496 d2 = TriangleDistance::sqrTriDistance (t11, t12, t13, t21, t22, t23,
499 d2 = TriangleDistance::sqrTriDistance (t11, t12, t13, t21, t22, t23,
504 this->result->update(d, this->model1, this->model2, primitive_id1,
505 primitive_id2, P1, P2, normal);
511 if((c >= this->result->min_distance - abs_err) && (c * (1 + rel_err) >= this->result->min_distance))
519 Triangle* tri_indices1;
520 Triangle* tri_indices2;
526 details::RelativeTransformation<!bool(RTIsIdentity)> RT;
529 void preprocessOrientedNode()
531 const int init_tri_id1 = 0, init_tri_id2 = 0;
532 const Triangle& init_tri1 = tri_indices1[init_tri_id1];
533 const Triangle& init_tri2 = tri_indices2[init_tri_id2];
535 Vec3f init_tri1_points[3];
536 Vec3f init_tri2_points[3];
538 init_tri1_points[0] = vertices1[init_tri1[0]];
539 init_tri1_points[1] = vertices1[init_tri1[1]];
540 init_tri1_points[2] = vertices1[init_tri1[2]];
542 init_tri2_points[0] = vertices2[init_tri2[0]];
543 init_tri2_points[1] = vertices2[init_tri2[1]];
544 init_tri2_points[2] = vertices2[init_tri2[2]];
546 Vec3f p1, p2, normal;
547 FCL_REAL distance = sqrt (TriangleDistance::sqrTriDistance
548 (init_tri1_points[0], init_tri1_points[1],
549 init_tri1_points[2], init_tri2_points[0],
550 init_tri2_points[1], init_tri2_points[2],
551 RT._R(), RT._T(), p1, p2));
553 result->update(distance, model1, model2, init_tri_id1, init_tri_id2, p1, p2,
556 void postprocessOrientedNode()
559 if(request.enable_nearest_points && (result->o1 == model1) && (result->o2 == model2))
561 result->nearest_points[0] = tf1.transform(result->nearest_points[0]);
562 result->nearest_points[1] = tf1.transform(result->nearest_points[1]);
568 typedef MeshDistanceTraversalNode<RSS , 0> MeshDistanceTraversalNodeRSS;
569 typedef MeshDistanceTraversalNode<kIOS , 0> MeshDistanceTraversalNodekIOS;
570 typedef MeshDistanceTraversalNode<OBBRSS, 0> MeshDistanceTraversalNodeOBBRSS;
578 template<
typename BV>
579 inline const Matrix3f& getBVAxes(
const BV& bv)
585 inline const Matrix3f& getBVAxes<OBBRSS>(
const OBBRSS& bv)
Main namespace.
Definition: AABB.h:43
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:68
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:775
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.
Definition: collision_data.h:192
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:67