hpp-fcl: include/hpp/fcl/internal/traversal_node_bvhs.h Source File

hpp-fcl 1.7.0 HPP fork of FCL -- The Flexible Collision Library

traversal_node_bvhs.h
Go to the documentation of this file.
1 /*
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011-2014, Willow Garage, Inc.
5 * Copyright (c) 2014-2015, Open Source Robotics Foundation
6 * All rights reserved.
7 *
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
10 * are met:
11 *
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * * Neither the name of Open Source Robotics Foundation nor the names of its
19 * contributors may be used to endorse or promote products derived
20 * from this software without specific prior written permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 * POSSIBILITY OF SUCH DAMAGE.
34 */
35
38 #ifndef HPP_FCL_TRAVERSAL_NODE_MESHES_H
39 #define HPP_FCL_TRAVERSAL_NODE_MESHES_H
40
42
43 #include <hpp/fcl/collision_data.h>
44 #include <hpp/fcl/internal/traversal_node_base.h>
45 #include <hpp/fcl/BV/BV_node.h>
46 #include <hpp/fcl/BV/BV.h>
47 #include <hpp/fcl/BVH/BVH_model.h>
48 #include <hpp/fcl/internal/intersect.h>
49 #include <hpp/fcl/shape/geometric_shapes.h>
50 #include <hpp/fcl/narrowphase/narrowphase.h>
51 #include <hpp/fcl/internal/traversal.h>
52
53 #include <limits>
54 #include <vector>
55 #include <cassert>
56
57 namespace hpp
58 {
59 namespace fcl
60 {
61
64
66 template<typename BV>
67 class BVHCollisionTraversalNode : public CollisionTraversalNodeBase
68 {
69 public:
70 BVHCollisionTraversalNode(const CollisionRequest& request) :
71 CollisionTraversalNodeBase (request)
72 {
73 model1 = NULL;
74 model2 = NULL;
75
76 num_bv_tests = 0;
77 num_leaf_tests = 0;
78 query_time_seconds = 0.0;
79 }
80
82 bool isFirstNodeLeaf(int b) const
83 {
84 assert(model1 != NULL && "model1 is NULL");
85 return model1->getBV(b).isLeaf();
86 }
87
89 bool isSecondNodeLeaf(int b) const
90 {
91 assert(model2 != NULL && "model2 is NULL");
92 return model2->getBV(b).isLeaf();
93 }
94
96 bool firstOverSecond(int b1, int b2) const
97 {
98 FCL_REAL sz1 = model1->getBV(b1).bv.size();
99 FCL_REAL sz2 = model2->getBV(b2).bv.size();
100
101 bool l1 = model1->getBV(b1).isLeaf();
102 bool l2 = model2->getBV(b2).isLeaf();
103
104 if(l2 || (!l1 && (sz1 > sz2)))
105 return true;
106 return false;
107 }
108
110 int getFirstLeftChild(int b) const
111 {
112 return model1->getBV(b).leftChild();
113 }
114
116 int getFirstRightChild(int b) const
117 {
118 return model1->getBV(b).rightChild();
119 }
120
122 int getSecondLeftChild(int b) const
123 {
124 return model2->getBV(b).leftChild();
125 }
126
128 int getSecondRightChild(int b) const
129 {
130 return model2->getBV(b).rightChild();
131 }
132
134 const BVHModel<BV>* model1;
136 const BVHModel<BV>* model2;
137
139 mutable int num_bv_tests;
140 mutable int num_leaf_tests;
141 mutable FCL_REAL query_time_seconds;
142 };
143
145 template<typename BV, int _Options = RelativeTransformationIsIdentity>
146 class MeshCollisionTraversalNode : public BVHCollisionTraversalNode<BV>
147 {
148 public:
149 enum {
150 Options = _Options,
151 RTIsIdentity = _Options & RelativeTransformationIsIdentity
152 };
153
154 MeshCollisionTraversalNode(const CollisionRequest& request) :
155 BVHCollisionTraversalNode<BV> (request)
156 {
157 vertices1 = NULL;
158 vertices2 = NULL;
159 tri_indices1 = NULL;
160 tri_indices2 = NULL;
161 }
162
164 bool BVDisjoints(int b1, int b2) const
165 {
166 if(this->enable_statistics) this->num_bv_tests++;
167 if (RTIsIdentity)
168 return !this->model1->getBV(b1).overlap(this->model2->getBV(b2));
169 else
170 return !overlap(RT._R(), RT._T(),
171 this->model1->getBV(b1).bv, this->model2->getBV(b2).bv);
172 }
173
178 bool BVDisjoints(int b1, int b2, FCL_REAL& sqrDistLowerBound) const
179 {
180 if(this->enable_statistics) this->num_bv_tests++;
181 if (RTIsIdentity)
182 return !this->model1->getBV(b1).overlap(this->model2->getBV(b2),
183 this->request, sqrDistLowerBound);
184 else {
185 bool res = !overlap(RT._R(), RT._T(),
186 this->model1->getBV(b1).bv, this->model2->getBV(b2).bv,
187 this->request, sqrDistLowerBound);
188 assert (!res || sqrDistLowerBound > 0);
189 return res;
190 }
191 }
192
207 void leafCollides(int b1, int b2, FCL_REAL& sqrDistLowerBound) const
208 {
209 if(this->enable_statistics) this->num_leaf_tests++;
210
211 const BVNode<BV>& node1 = this->model1->getBV(b1);
212 const BVNode<BV>& node2 = this->model2->getBV(b2);
213
214 int primitive_id1 = node1.primitiveId();
215 int primitive_id2 = node2.primitiveId();
216
217 const Triangle& tri_id1 = tri_indices1[primitive_id1];
218 const Triangle& tri_id2 = tri_indices2[primitive_id2];
219
220 const Vec3f& P1 = vertices1[tri_id1[0]];
221 const Vec3f& P2 = vertices1[tri_id1[1]];
222 const Vec3f& P3 = vertices1[tri_id1[2]];
223 const Vec3f& Q1 = vertices2[tri_id2[0]];
224 const Vec3f& Q2 = vertices2[tri_id2[1]];
225 const Vec3f& Q3 = vertices2[tri_id2[2]];
226
227 TriangleP tri1 (P1, P2, P3);
228 TriangleP tri2 (Q1, Q2, Q3);
229 GJKSolver solver;
230 Vec3f p1, p2; // closest points if no collision contact points if collision.
231 Vec3f normal;
232 FCL_REAL distance;
233 solver.shapeDistance (tri1, this->tf1, tri2, this->tf2,
234 distance, p1, p2, normal);
235 FCL_REAL distToCollision = distance - this->request.security_margin;
236 sqrDistLowerBound = distance * distance;
237 if (distToCollision <= 0) { // collision
238 Vec3f p (p1); // contact point
239 FCL_REAL penetrationDepth (0);
240 if(this->result->numContacts() < this->request.num_max_contacts) {
241 // How much (Q1, Q2, Q3) should be moved so that all vertices are
242 // above (P1, P2, P3).
243 penetrationDepth = -distance;
244 if (distance > 0) {
245 normal = (p2-p1).normalized ();
246 p = .5* (p1+p2);
247 }
248 this->result->addContact(Contact(this->model1, this->model2,
249 primitive_id1, primitive_id2,
250 p, normal, penetrationDepth));
251 }
252 }
253 }
254
255 Vec3f* vertices1;
256 Vec3f* vertices2;
257
258 Triangle* tri_indices1;
259 Triangle* tri_indices2;
260
261 details::RelativeTransformation<!bool(RTIsIdentity)> RT;
262 };
263
265 typedef MeshCollisionTraversalNode<OBB , 0> MeshCollisionTraversalNodeOBB ;
266 typedef MeshCollisionTraversalNode<RSS , 0> MeshCollisionTraversalNodeRSS ;
267 typedef MeshCollisionTraversalNode<kIOS , 0> MeshCollisionTraversalNodekIOS ;
268 typedef MeshCollisionTraversalNode<OBBRSS, 0> MeshCollisionTraversalNodeOBBRSS;
269
271
272 namespace details
273 {
274 template<typename BV> struct DistanceTraversalBVDistanceLowerBound_impl
275 {
276 static FCL_REAL run(const BVNode<BV>& b1, const BVNode<BV>& b2)
277 {
278 return b1.distance(b2);
279 }
280 static FCL_REAL run(const Matrix3f& R, const Vec3f& T, const BVNode<BV>& b1, const BVNode<BV>& b2)
281 {
282 return distance(R, T, b1.bv, b2.bv);
283 }
284 };
285
286 template<> struct DistanceTraversalBVDistanceLowerBound_impl<OBB>
287 {
288 static FCL_REAL run(const BVNode<OBB>& b1, const BVNode<OBB>& b2)
289 {
290 FCL_REAL sqrDistLowerBound;
291 CollisionRequest request (DISTANCE_LOWER_BOUND, 0);
292 // request.break_distance = ?
293 if (b1.overlap(b2, request, sqrDistLowerBound)) {
294 // TODO A penetration upper bound should be computed.
295 return -1;
296 }
297 return sqrt (sqrDistLowerBound);
298 }
299 static FCL_REAL run(const Matrix3f& R, const Vec3f& T, const BVNode<OBB>& b1, const BVNode<OBB>& b2)
300 {
301 FCL_REAL sqrDistLowerBound;
302 CollisionRequest request (DISTANCE_LOWER_BOUND, 0);
303 // request.break_distance = ?
304 if (overlap(R, T, b1.bv, b2.bv, request, sqrDistLowerBound)) {
305 // TODO A penetration upper bound should be computed.
306 return -1;
307 }
308 return sqrt (sqrDistLowerBound);
309 }
310 };
311
312 template<> struct DistanceTraversalBVDistanceLowerBound_impl<AABB>
313 {
314 static FCL_REAL run(const BVNode<AABB>& b1, const BVNode<AABB>& b2)
315 {
316 FCL_REAL sqrDistLowerBound;
317 CollisionRequest request (DISTANCE_LOWER_BOUND, 0);
318 // request.break_distance = ?
319 if (b1.overlap(b2, request, sqrDistLowerBound)) {
320 // TODO A penetration upper bound should be computed.
321 return -1;
322 }
323 return sqrt (sqrDistLowerBound);
324 }
325 static FCL_REAL run(const Matrix3f& R, const Vec3f& T, const BVNode<AABB>& b1, const BVNode<AABB>& b2)
326 {
327 FCL_REAL sqrDistLowerBound;
328 CollisionRequest request (DISTANCE_LOWER_BOUND, 0);
329 // request.break_distance = ?
330 if (overlap(R, T, b1.bv, b2.bv, request, sqrDistLowerBound)) {
331 // TODO A penetration upper bound should be computed.
332 return -1;
333 }
334 return sqrt (sqrDistLowerBound);
335 }
336 };
337 } // namespace details
338
341
343 template<typename BV>
344 class BVHDistanceTraversalNode : public DistanceTraversalNodeBase
345 {
346 public:
347 BVHDistanceTraversalNode() : DistanceTraversalNodeBase()
348 {
349 model1 = NULL;
350 model2 = NULL;
351
352 num_bv_tests = 0;
353 num_leaf_tests = 0;
354 query_time_seconds = 0.0;
355 }
356
358 bool isFirstNodeLeaf(int b) const
359 {
360 return model1->getBV(b).isLeaf();
361 }
362
364 bool isSecondNodeLeaf(int b) const
365 {
366 return model2->getBV(b).isLeaf();
367 }
368
370 bool firstOverSecond(int b1, int b2) const
371 {
372 FCL_REAL sz1 = model1->getBV(b1).bv.size();
373 FCL_REAL sz2 = model2->getBV(b2).bv.size();
374
375 bool l1 = model1->getBV(b1).isLeaf();
376 bool l2 = model2->getBV(b2).isLeaf();
377
378 if(l2 || (!l1 && (sz1 > sz2)))
379 return true;
380 return false;
381 }
382
384 int getFirstLeftChild(int b) const
385 {
386 return model1->getBV(b).leftChild();
387 }
388
390 int getFirstRightChild(int b) const
391 {
392 return model1->getBV(b).rightChild();
393 }
394
396 int getSecondLeftChild(int b) const
397 {
398 return model2->getBV(b).leftChild();
399 }
400
402 int getSecondRightChild(int b) const
403 {
404 return model2->getBV(b).rightChild();
405 }
406
408 const BVHModel<BV>* model1;
410 const BVHModel<BV>* model2;
411
413 mutable int num_bv_tests;
414 mutable int num_leaf_tests;
415 mutable FCL_REAL query_time_seconds;
416 };
417
418
420 template<typename BV, int _Options = RelativeTransformationIsIdentity>
421 class MeshDistanceTraversalNode : public BVHDistanceTraversalNode<BV>
422 {
423 public:
424 enum {
425 Options = _Options,
426 RTIsIdentity = _Options & RelativeTransformationIsIdentity
427 };
428
429 using BVHDistanceTraversalNode<BV>::enable_statistics;
430 using BVHDistanceTraversalNode<BV>::request;
431 using BVHDistanceTraversalNode<BV>::result;
432 using BVHDistanceTraversalNode<BV>::tf1;
433 using BVHDistanceTraversalNode<BV>::model1;
434 using BVHDistanceTraversalNode<BV>::model2;
435 using BVHDistanceTraversalNode<BV>::num_bv_tests;
436 using BVHDistanceTraversalNode<BV>::num_leaf_tests;
437
438 MeshDistanceTraversalNode() : BVHDistanceTraversalNode<BV>()
439 {
440 vertices1 = NULL;
441 vertices2 = NULL;
442 tri_indices1 = NULL;
443 tri_indices2 = NULL;
444
445 rel_err = this->request.rel_err;
446 abs_err = this->request.abs_err;
447 }
448
449 void preprocess()
450 {
451 if(!RTIsIdentity) preprocessOrientedNode();
452 }
453
454 void postprocess()
455 {
456 if(!RTIsIdentity) postprocessOrientedNode();
457 }
458
460 FCL_REAL BVDistanceLowerBound(int b1, int b2) const
461 {
462 if(enable_statistics) num_bv_tests++;
463 if (RTIsIdentity)
464 return details::DistanceTraversalBVDistanceLowerBound_impl<BV>
465 ::run (model1->getBV(b1), model2->getBV(b2));
466 else
467 return details::DistanceTraversalBVDistanceLowerBound_impl<BV>
468 ::run (RT._R(), RT._T(), model1->getBV(b1), model2->getBV(b2));
469 }
470
472 void leafComputeDistance(int b1, int b2) const
473 {
474 if(this->enable_statistics) this->num_leaf_tests++;
475
476 const BVNode<BV>& node1 = this->model1->getBV(b1);
477 const BVNode<BV>& node2 = this->model2->getBV(b2);
478
479 int primitive_id1 = node1.primitiveId();
480 int primitive_id2 = node2.primitiveId();
481
482 const Triangle& tri_id1 = tri_indices1[primitive_id1];
483 const Triangle& tri_id2 = tri_indices2[primitive_id2];
484
485 const Vec3f& t11 = vertices1[tri_id1[0]];
486 const Vec3f& t12 = vertices1[tri_id1[1]];
487 const Vec3f& t13 = vertices1[tri_id1[2]];
488
489 const Vec3f& t21 = vertices2[tri_id2[0]];
490 const Vec3f& t22 = vertices2[tri_id2[1]];
491 const Vec3f& t23 = vertices2[tri_id2[2]];
492
493 // nearest point pair
494 Vec3f P1, P2, normal;
495
496 FCL_REAL d2;
497 if (RTIsIdentity)
498 d2 = TriangleDistance::sqrTriDistance (t11, t12, t13, t21, t22, t23,
499 P1, P2);
500 else
501 d2 = TriangleDistance::sqrTriDistance (t11, t12, t13, t21, t22, t23,
502 RT._R(), RT._T(),
503 P1, P2);
504 FCL_REAL d = sqrt(d2);
505
506 this->result->update(d, this->model1, this->model2, primitive_id1,
507 primitive_id2, P1, P2, normal);
508 }
509
511 bool canStop(FCL_REAL c) const
512 {
513 if((c >= this->result->min_distance - abs_err) && (c * (1 + rel_err) >= this->result->min_distance))
514 return true;
515 return false;
516 }
517
518 Vec3f* vertices1;
519 Vec3f* vertices2;
520
521 Triangle* tri_indices1;
522 Triangle* tri_indices2;
523
525 FCL_REAL rel_err;
526 FCL_REAL abs_err;
527
528 details::RelativeTransformation<!bool(RTIsIdentity)> RT;
529
530 private:
531 void preprocessOrientedNode()
532 {
533 const int init_tri_id1 = 0, init_tri_id2 = 0;
534 const Triangle& init_tri1 = tri_indices1[init_tri_id1];
535 const Triangle& init_tri2 = tri_indices2[init_tri_id2];
536
537 Vec3f init_tri1_points[3];
538 Vec3f init_tri2_points[3];
539
540 init_tri1_points[0] = vertices1[init_tri1[0]];
541 init_tri1_points[1] = vertices1[init_tri1[1]];
542 init_tri1_points[2] = vertices1[init_tri1[2]];
543
544 init_tri2_points[0] = vertices2[init_tri2[0]];
545 init_tri2_points[1] = vertices2[init_tri2[1]];
546 init_tri2_points[2] = vertices2[init_tri2[2]];
547
548 Vec3f p1, p2, normal;
549 FCL_REAL distance = sqrt (TriangleDistance::sqrTriDistance
550 (init_tri1_points[0], init_tri1_points[1],
551 init_tri1_points[2], init_tri2_points[0],
552 init_tri2_points[1], init_tri2_points[2],
553 RT._R(), RT._T(), p1, p2));
554
555 result->update(distance, model1, model2, init_tri_id1, init_tri_id2, p1, p2,
556 normal);
557 }
558 void postprocessOrientedNode()
559 {
561 if(request.enable_nearest_points && (result->o1 == model1) && (result->o2 == model2))
562 {
563 result->nearest_points[0] = tf1.transform(result->nearest_points[0]);
564 result->nearest_points[1] = tf1.transform(result->nearest_points[1]);
565 }
566 }
567 };
568
570 typedef MeshDistanceTraversalNode<RSS , 0> MeshDistanceTraversalNodeRSS;
571 typedef MeshDistanceTraversalNode<kIOS , 0> MeshDistanceTraversalNodekIOS;
572 typedef MeshDistanceTraversalNode<OBBRSS, 0> MeshDistanceTraversalNodeOBBRSS;
573
575
577 namespace details
578 {
579
580 template<typename BV>
581 inline const Matrix3f& getBVAxes(const BV& bv)
582 {
583 return bv.axes;
584 }
585
586 template<>
587 inline const Matrix3f& getBVAxes<OBBRSS>(const OBBRSS& bv)
588 {
589 return bv.obb.axes;
590 }
591
592 }
593
594 }
595
596 } // namespace hpp
597
599
600 #endif
BV_node.h
BV.h
BVH_model.h
collision_data.h
geometric_shapes.h
hpp::fcl::distance
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.
hpp::fcl::overlap
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.
intersect.h
details
Definition: traversal_node_setup.h:776
hpp::fcl::Matrix3f
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:68
hpp::fcl::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:67
hpp::fcl::DISTANCE_LOWER_BOUND
@ DISTANCE_LOWER_BOUND
Definition: collision_data.h:198
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:66
hpp
Main namespace.
Definition: AABB.h:44
narrowphase.h
traversal.h
traversal_node_base.h