38 #ifndef HPP_FCL_TRAVERSAL_NODE_MESHES_H 39 #define HPP_FCL_TRAVERSAL_NODE_MESHES_H 64 template <
typename BV>
65 class BVHCollisionTraversalNode :
public CollisionTraversalNodeBase {
67 BVHCollisionTraversalNode(
const CollisionRequest& request)
68 : CollisionTraversalNodeBase(request) {
74 query_time_seconds = 0.0;
78 bool isFirstNodeLeaf(
unsigned int b)
const {
79 assert(model1 != NULL &&
"model1 is NULL");
80 return model1->getBV(b).isLeaf();
84 bool isSecondNodeLeaf(
unsigned int b)
const {
85 assert(model2 != NULL &&
"model2 is NULL");
86 return model2->getBV(b).isLeaf();
90 bool firstOverSecond(
unsigned int b1,
unsigned int b2)
const {
91 FCL_REAL sz1 = model1->getBV(b1).bv.size();
92 FCL_REAL sz2 = model2->getBV(b2).bv.size();
94 bool l1 = model1->getBV(b1).isLeaf();
95 bool l2 = model2->getBV(b2).isLeaf();
97 if (l2 || (!l1 && (sz1 > sz2)))
return true;
102 int getFirstLeftChild(
unsigned int b)
const {
103 return model1->getBV(b).leftChild();
107 int getFirstRightChild(
unsigned int b)
const {
108 return model1->getBV(b).rightChild();
112 int getSecondLeftChild(
unsigned int b)
const {
113 return model2->getBV(b).leftChild();
117 int getSecondRightChild(
unsigned int b)
const {
118 return model2->getBV(b).rightChild();
122 const BVHModel<BV>* model1;
124 const BVHModel<BV>* model2;
127 mutable int num_bv_tests;
128 mutable int num_leaf_tests;
129 mutable FCL_REAL query_time_seconds;
133 template <
typename BV,
int _Options = RelativeTransformationIsIdentity>
134 class MeshCollisionTraversalNode :
public BVHCollisionTraversalNode<BV> {
138 RTIsIdentity = _Options & RelativeTransformationIsIdentity
141 MeshCollisionTraversalNode(
const CollisionRequest& request)
142 : BVHCollisionTraversalNode<BV>(request) {
153 bool BVDisjoints(
unsigned int b1,
unsigned int b2,
154 FCL_REAL& sqrDistLowerBound)
const {
155 if (this->enable_statistics) this->num_bv_tests++;
158 disjoint = !this->model1->getBV(b1).overlap(
159 this->model2->getBV(b2), this->request, sqrDistLowerBound);
161 disjoint = !
overlap(RT._R(), RT._T(), this->model2->getBV(b2).bv,
162 this->model1->getBV(b1).bv, this->request,
185 void leafCollides(
unsigned int b1,
unsigned int b2,
186 FCL_REAL& sqrDistLowerBound)
const {
187 if (this->enable_statistics) this->num_leaf_tests++;
189 const BVNode<BV>& node1 = this->model1->getBV(b1);
190 const BVNode<BV>& node2 = this->model2->getBV(b2);
192 int primitive_id1 = node1.primitiveId();
193 int primitive_id2 = node2.primitiveId();
195 const Triangle& tri_id1 = tri_indices1[primitive_id1];
196 const Triangle& tri_id2 = tri_indices2[primitive_id2];
198 const Vec3f&
P1 = vertices1[tri_id1[0]];
199 const Vec3f&
P2 = vertices1[tri_id1[1]];
200 const Vec3f&
P3 = vertices1[tri_id1[2]];
201 const Vec3f&
Q1 = vertices2[tri_id2[0]];
202 const Vec3f&
Q2 = vertices2[tri_id2[1]];
203 const Vec3f&
Q3 = vertices2[tri_id2[2]];
205 TriangleP tri1(P1, P2, P3);
206 TriangleP tri2(Q1, Q2, Q3);
212 solver.shapeDistance(tri1, this->
tf1, tri2, this->
tf2, distance, p1, p2,
215 const FCL_REAL distToCollision = distance - this->request.security_margin;
216 if (distToCollision <=
217 this->request.collision_distance_threshold) {
218 sqrDistLowerBound = 0;
220 if (this->result->numContacts() < this->request.num_max_contacts) {
224 normal = (p2 -
p1).normalized();
227 this->result->addContact(Contact(this->model1, this->model2,
228 primitive_id1, primitive_id2, p,
232 sqrDistLowerBound = distToCollision * distToCollision;
235 distToCollision, p1, p2);
241 Triangle* tri_indices1;
242 Triangle* tri_indices2;
244 details::RelativeTransformation<!bool(RTIsIdentity)> RT;
249 typedef MeshCollisionTraversalNode<OBB, 0> MeshCollisionTraversalNodeOBB;
250 typedef MeshCollisionTraversalNode<RSS, 0> MeshCollisionTraversalNodeRSS;
251 typedef MeshCollisionTraversalNode<kIOS, 0> MeshCollisionTraversalNodekIOS;
252 typedef MeshCollisionTraversalNode<OBBRSS, 0> MeshCollisionTraversalNodeOBBRSS;
257 template <
typename BV>
258 struct DistanceTraversalBVDistanceLowerBound_impl {
259 static FCL_REAL run(
const BVNode<BV>& b1,
const BVNode<BV>& b2) {
260 return b1.distance(b2);
263 const BVNode<BV>& b2) {
264 return distance(R, T, b1.bv, b2.bv);
269 struct DistanceTraversalBVDistanceLowerBound_impl<OBB> {
270 static FCL_REAL run(
const BVNode<OBB>& b1,
const BVNode<OBB>& b2) {
274 if (b1.overlap(b2, request, sqrDistLowerBound)) {
278 return sqrt(sqrDistLowerBound);
281 const BVNode<OBB>& b2) {
285 if (
overlap(R, T, b1.bv, b2.bv, request, sqrDistLowerBound)) {
289 return sqrt(sqrDistLowerBound);
294 struct DistanceTraversalBVDistanceLowerBound_impl<AABB> {
295 static FCL_REAL run(
const BVNode<AABB>& b1,
const BVNode<AABB>& b2) {
299 if (b1.overlap(b2, request, sqrDistLowerBound)) {
303 return sqrt(sqrDistLowerBound);
306 const BVNode<AABB>& b2) {
310 if (
overlap(R, T, b1.bv, b2.bv, request, sqrDistLowerBound)) {
314 return sqrt(sqrDistLowerBound);
323 template <
typename BV>
324 class BVHDistanceTraversalNode :
public DistanceTraversalNodeBase {
326 BVHDistanceTraversalNode() : DistanceTraversalNodeBase() {
332 query_time_seconds = 0.0;
336 bool isFirstNodeLeaf(
unsigned int b)
const {
337 return model1->getBV(b).isLeaf();
341 bool isSecondNodeLeaf(
unsigned int b)
const {
342 return model2->getBV(b).isLeaf();
346 bool firstOverSecond(
unsigned int b1,
unsigned int b2)
const {
347 FCL_REAL sz1 = model1->getBV(b1).bv.size();
348 FCL_REAL sz2 = model2->getBV(b2).bv.size();
350 bool l1 = model1->getBV(b1).isLeaf();
351 bool l2 = model2->getBV(b2).isLeaf();
353 if (l2 || (!l1 && (sz1 > sz2)))
return true;
358 int getFirstLeftChild(
unsigned int b)
const {
359 return model1->getBV(b).leftChild();
363 int getFirstRightChild(
unsigned int b)
const {
364 return model1->getBV(b).rightChild();
368 int getSecondLeftChild(
unsigned int b)
const {
369 return model2->getBV(b).leftChild();
373 int getSecondRightChild(
unsigned int b)
const {
374 return model2->getBV(b).rightChild();
378 const BVHModel<BV>* model1;
380 const BVHModel<BV>* model2;
383 mutable int num_bv_tests;
384 mutable int num_leaf_tests;
385 mutable FCL_REAL query_time_seconds;
389 template <
typename BV,
int _Options = RelativeTransformationIsIdentity>
390 class MeshDistanceTraversalNode :
public BVHDistanceTraversalNode<BV> {
394 RTIsIdentity = _Options & RelativeTransformationIsIdentity
397 using BVHDistanceTraversalNode<BV>::enable_statistics;
398 using BVHDistanceTraversalNode<BV>::request;
399 using BVHDistanceTraversalNode<BV>::result;
401 using BVHDistanceTraversalNode<BV>::model1;
402 using BVHDistanceTraversalNode<BV>::model2;
403 using BVHDistanceTraversalNode<BV>::num_bv_tests;
404 using BVHDistanceTraversalNode<BV>::num_leaf_tests;
406 MeshDistanceTraversalNode() : BVHDistanceTraversalNode<BV>() {
412 rel_err = this->request.rel_err;
413 abs_err = this->request.abs_err;
417 if (!RTIsIdentity) preprocessOrientedNode();
421 if (!RTIsIdentity) postprocessOrientedNode();
425 FCL_REAL BVDistanceLowerBound(
unsigned int b1,
unsigned int b2)
const {
426 if (enable_statistics) num_bv_tests++;
429 model1->getBV(b1), model2->getBV(b2));
432 RT._R(), RT._T(), model1->getBV(b1), model2->getBV(b2));
436 void leafComputeDistance(
unsigned int b1,
unsigned int b2)
const {
437 if (this->enable_statistics) this->num_leaf_tests++;
439 const BVNode<BV>& node1 = this->model1->getBV(b1);
440 const BVNode<BV>& node2 = this->model2->getBV(b2);
442 int primitive_id1 = node1.primitiveId();
443 int primitive_id2 = node2.primitiveId();
445 const Triangle& tri_id1 = tri_indices1[primitive_id1];
446 const Triangle& tri_id2 = tri_indices2[primitive_id2];
448 const Vec3f& t11 = vertices1[tri_id1[0]];
449 const Vec3f& t12 = vertices1[tri_id1[1]];
450 const Vec3f& t13 = vertices1[tri_id1[2]];
452 const Vec3f& t21 = vertices2[tri_id2[0]];
453 const Vec3f& t22 = vertices2[tri_id2[1]];
454 const Vec3f& t23 = vertices2[tri_id2[2]];
461 d2 = TriangleDistance::sqrTriDistance(t11, t12, t13, t21, t22, t23, P1,
464 d2 = TriangleDistance::sqrTriDistance(t11, t12, t13, t21, t22, t23,
465 RT._R(), RT._T(),
P1,
P2);
468 this->result->update(d, this->model1, this->model2, primitive_id1,
469 primitive_id2, P1, P2, normal);
474 if ((c >= this->result->min_distance - abs_err) &&
475 (c * (1 + rel_err) >= this->result->min_distance))
483 Triangle* tri_indices1;
484 Triangle* tri_indices2;
490 details::RelativeTransformation<!bool(RTIsIdentity)> RT;
493 void preprocessOrientedNode() {
494 const int init_tri_id1 = 0, init_tri_id2 = 0;
495 const Triangle& init_tri1 = tri_indices1[init_tri_id1];
496 const Triangle& init_tri2 = tri_indices2[init_tri_id2];
498 Vec3f init_tri1_points[3];
499 Vec3f init_tri2_points[3];
501 init_tri1_points[0] = vertices1[init_tri1[0]];
502 init_tri1_points[1] = vertices1[init_tri1[1]];
503 init_tri1_points[2] = vertices1[init_tri1[2]];
505 init_tri2_points[0] = vertices2[init_tri2[0]];
506 init_tri2_points[1] = vertices2[init_tri2[1]];
507 init_tri2_points[2] = vertices2[init_tri2[2]];
510 FCL_REAL distance = sqrt(TriangleDistance::sqrTriDistance(
511 init_tri1_points[0], init_tri1_points[1], init_tri1_points[2],
512 init_tri2_points[0], init_tri2_points[1], init_tri2_points[2], RT._R(),
515 result->update(distance, model1, model2, init_tri_id1, init_tri_id2, p1, p2,
518 void postprocessOrientedNode() {
522 if (request.enable_nearest_points && (result->o1 == model1) &&
523 (result->o2 == model2)) {
524 result->nearest_points[0] =
tf1.transform(result->nearest_points[0]);
525 result->nearest_points[1] =
tf1.transform(result->nearest_points[1]);
532 typedef MeshDistanceTraversalNode<RSS, 0> MeshDistanceTraversalNodeRSS;
533 typedef MeshDistanceTraversalNode<kIOS, 0> MeshDistanceTraversalNodekIOS;
534 typedef MeshDistanceTraversalNode<OBBRSS, 0> MeshDistanceTraversalNodeOBBRSS;
542 template <
typename BV>
543 inline const Matrix3f& getBVAxes(
const BV& bv) {
548 inline const Matrix3f& getBVAxes<OBBRSS>(
const OBBRSS& bv) {
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
HPP_FCL_DLLAPI 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.
void updateDistanceLowerBoundFromLeaf(const CollisionRequest &, CollisionResult &res, const FCL_REAL &distance, const Vec3f &p0, const Vec3f &p1)
void updateDistanceLowerBoundFromBV(const CollisionRequest &, CollisionResult &res, const FCL_REAL &sqrDistLowerBound)
HPP_FCL_DLLAPI 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