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

hpp-fcl 1.6.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 return model1->getBV(b).isLeaf();
85 }
86
88 bool isSecondNodeLeaf(int b) const
89 {
90 return model2->getBV(b).isLeaf();
91 }
92
94 bool firstOverSecond(int b1, int b2) const
95 {
96 FCL_REAL sz1 = model1->getBV(b1).bv.size();
97 FCL_REAL sz2 = model2->getBV(b2).bv.size();
98
99 bool l1 = model1->getBV(b1).isLeaf();
100 bool l2 = model2->getBV(b2).isLeaf();
101
102 if(l2 || (!l1 && (sz1 > sz2)))
103 return true;
104 return false;
105 }
106
108 int getFirstLeftChild(int b) const
109 {
110 return model1->getBV(b).leftChild();
111 }
112
114 int getFirstRightChild(int b) const
115 {
116 return model1->getBV(b).rightChild();
117 }
118
120 int getSecondLeftChild(int b) const
121 {
122 return model2->getBV(b).leftChild();
123 }
124
126 int getSecondRightChild(int b) const
127 {
128 return model2->getBV(b).rightChild();
129 }
130
132 const BVHModel<BV>* model1;
134 const BVHModel<BV>* model2;
135
137 mutable int num_bv_tests;
138 mutable int num_leaf_tests;
139 mutable FCL_REAL query_time_seconds;
140 };
141
143 template<typename BV, int _Options = RelativeTransformationIsIdentity>
144 class MeshCollisionTraversalNode : public BVHCollisionTraversalNode<BV>
145 {
146 public:
147 enum {
148 Options = _Options,
149 RTIsIdentity = _Options & RelativeTransformationIsIdentity
150 };
151
152 MeshCollisionTraversalNode(const CollisionRequest& request) :
153 BVHCollisionTraversalNode<BV> (request)
154 {
155 vertices1 = NULL;
156 vertices2 = NULL;
157 tri_indices1 = NULL;
158 tri_indices2 = NULL;
159 }
160
162 bool BVDisjoints(int b1, int b2) const
163 {
164 if(this->enable_statistics) this->num_bv_tests++;
165 if (RTIsIdentity)
166 return !this->model1->getBV(b1).overlap(this->model2->getBV(b2));
167 else
168 return !overlap(RT._R(), RT._T(),
169 this->model1->getBV(b1).bv, this->model2->getBV(b2).bv);
170 }
171
176 bool BVDisjoints(int b1, int b2, FCL_REAL& sqrDistLowerBound) const
177 {
178 if(this->enable_statistics) this->num_bv_tests++;
179 if (RTIsIdentity)
180 return !this->model1->getBV(b1).overlap(this->model2->getBV(b2),
181 this->request, sqrDistLowerBound);
182 else {
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);
187 return res;
188 }
189 }
190
205 void leafCollides(int b1, int b2, FCL_REAL& sqrDistLowerBound) const
206 {
207 if(this->enable_statistics) this->num_leaf_tests++;
208
209 const BVNode<BV>& node1 = this->model1->getBV(b1);
210 const BVNode<BV>& node2 = this->model2->getBV(b2);
211
212 int primitive_id1 = node1.primitiveId();
213 int primitive_id2 = node2.primitiveId();
214
215 const Triangle& tri_id1 = tri_indices1[primitive_id1];
216 const Triangle& tri_id2 = tri_indices2[primitive_id2];
217
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]];
224
225 TriangleP tri1 (P1, P2, P3);
226 TriangleP tri2 (Q1, Q2, Q3);
227 GJKSolver solver;
228 Vec3f p1, p2; // closest points if no collision contact points if collision.
229 Vec3f normal;
230 FCL_REAL distance;
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) { // collision
236 Vec3f p (p1); // contact point
237 FCL_REAL penetrationDepth (0);
238 if(this->result->numContacts() < this->request.num_max_contacts) {
239 // How much (Q1, Q2, Q3) should be moved so that all vertices are
240 // above (P1, P2, P3).
241 penetrationDepth = -distance;
242 if (distance > 0) {
243 normal = (p2-p1).normalized ();
244 p = .5* (p1+p2);
245 }
246 this->result->addContact(Contact(this->model1, this->model2,
247 primitive_id1, primitive_id2,
248 p, normal, penetrationDepth));
249 }
250 }
251 }
252
253 Vec3f* vertices1;
254 Vec3f* vertices2;
255
256 Triangle* tri_indices1;
257 Triangle* tri_indices2;
258
259 details::RelativeTransformation<!bool(RTIsIdentity)> RT;
260 };
261
263 typedef MeshCollisionTraversalNode<OBB , 0> MeshCollisionTraversalNodeOBB ;
264 typedef MeshCollisionTraversalNode<RSS , 0> MeshCollisionTraversalNodeRSS ;
265 typedef MeshCollisionTraversalNode<kIOS , 0> MeshCollisionTraversalNodekIOS ;
266 typedef MeshCollisionTraversalNode<OBBRSS, 0> MeshCollisionTraversalNodeOBBRSS;
267
269
270 namespace details
271 {
272 template<typename BV> struct DistanceTraversalBVDistanceLowerBound_impl
273 {
274 static FCL_REAL run(const BVNode<BV>& b1, const BVNode<BV>& b2)
275 {
276 return b1.distance(b2);
277 }
278 static FCL_REAL run(const Matrix3f& R, const Vec3f& T, const BVNode<BV>& b1, const BVNode<BV>& b2)
279 {
280 return distance(R, T, b1.bv, b2.bv);
281 }
282 };
283
284 template<> struct DistanceTraversalBVDistanceLowerBound_impl<OBB>
285 {
286 static FCL_REAL run(const BVNode<OBB>& b1, const BVNode<OBB>& b2)
287 {
288 FCL_REAL sqrDistLowerBound;
289 CollisionRequest request (DISTANCE_LOWER_BOUND, 0);
290 // request.break_distance = ?
291 if (b1.overlap(b2, request, sqrDistLowerBound)) {
292 // TODO A penetration upper bound should be computed.
293 return -1;
294 }
295 return sqrt (sqrDistLowerBound);
296 }
297 static FCL_REAL run(const Matrix3f& R, const Vec3f& T, const BVNode<OBB>& b1, const BVNode<OBB>& b2)
298 {
299 FCL_REAL sqrDistLowerBound;
300 CollisionRequest request (DISTANCE_LOWER_BOUND, 0);
301 // request.break_distance = ?
302 if (overlap(R, T, b1.bv, b2.bv, request, sqrDistLowerBound)) {
303 // TODO A penetration upper bound should be computed.
304 return -1;
305 }
306 return sqrt (sqrDistLowerBound);
307 }
308 };
309
310 template<> struct DistanceTraversalBVDistanceLowerBound_impl<AABB>
311 {
312 static FCL_REAL run(const BVNode<AABB>& b1, const BVNode<AABB>& b2)
313 {
314 FCL_REAL sqrDistLowerBound;
315 CollisionRequest request (DISTANCE_LOWER_BOUND, 0);
316 // request.break_distance = ?
317 if (b1.overlap(b2, request, sqrDistLowerBound)) {
318 // TODO A penetration upper bound should be computed.
319 return -1;
320 }
321 return sqrt (sqrDistLowerBound);
322 }
323 static FCL_REAL run(const Matrix3f& R, const Vec3f& T, const BVNode<AABB>& b1, const BVNode<AABB>& b2)
324 {
325 FCL_REAL sqrDistLowerBound;
326 CollisionRequest request (DISTANCE_LOWER_BOUND, 0);
327 // request.break_distance = ?
328 if (overlap(R, T, b1.bv, b2.bv, request, sqrDistLowerBound)) {
329 // TODO A penetration upper bound should be computed.
330 return -1;
331 }
332 return sqrt (sqrDistLowerBound);
333 }
334 };
335 } // namespace details
336
339
341 template<typename BV>
342 class BVHDistanceTraversalNode : public DistanceTraversalNodeBase
343 {
344 public:
345 BVHDistanceTraversalNode() : DistanceTraversalNodeBase()
346 {
347 model1 = NULL;
348 model2 = NULL;
349
350 num_bv_tests = 0;
351 num_leaf_tests = 0;
352 query_time_seconds = 0.0;
353 }
354
356 bool isFirstNodeLeaf(int b) const
357 {
358 return model1->getBV(b).isLeaf();
359 }
360
362 bool isSecondNodeLeaf(int b) const
363 {
364 return model2->getBV(b).isLeaf();
365 }
366
368 bool firstOverSecond(int b1, int b2) const
369 {
370 FCL_REAL sz1 = model1->getBV(b1).bv.size();
371 FCL_REAL sz2 = model2->getBV(b2).bv.size();
372
373 bool l1 = model1->getBV(b1).isLeaf();
374 bool l2 = model2->getBV(b2).isLeaf();
375
376 if(l2 || (!l1 && (sz1 > sz2)))
377 return true;
378 return false;
379 }
380
382 int getFirstLeftChild(int b) const
383 {
384 return model1->getBV(b).leftChild();
385 }
386
388 int getFirstRightChild(int b) const
389 {
390 return model1->getBV(b).rightChild();
391 }
392
394 int getSecondLeftChild(int b) const
395 {
396 return model2->getBV(b).leftChild();
397 }
398
400 int getSecondRightChild(int b) const
401 {
402 return model2->getBV(b).rightChild();
403 }
404
406 const BVHModel<BV>* model1;
408 const BVHModel<BV>* model2;
409
411 mutable int num_bv_tests;
412 mutable int num_leaf_tests;
413 mutable FCL_REAL query_time_seconds;
414 };
415
416
418 template<typename BV, int _Options = RelativeTransformationIsIdentity>
419 class MeshDistanceTraversalNode : public BVHDistanceTraversalNode<BV>
420 {
421 public:
422 enum {
423 Options = _Options,
424 RTIsIdentity = _Options & RelativeTransformationIsIdentity
425 };
426
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;
435
436 MeshDistanceTraversalNode() : BVHDistanceTraversalNode<BV>()
437 {
438 vertices1 = NULL;
439 vertices2 = NULL;
440 tri_indices1 = NULL;
441 tri_indices2 = NULL;
442
443 rel_err = this->request.rel_err;
444 abs_err = this->request.abs_err;
445 }
446
447 void preprocess()
448 {
449 if(!RTIsIdentity) preprocessOrientedNode();
450 }
451
452 void postprocess()
453 {
454 if(!RTIsIdentity) postprocessOrientedNode();
455 }
456
458 FCL_REAL BVDistanceLowerBound(int b1, int b2) const
459 {
460 if(enable_statistics) num_bv_tests++;
461 if (RTIsIdentity)
462 return details::DistanceTraversalBVDistanceLowerBound_impl<BV>
463 ::run (model1->getBV(b1), model2->getBV(b2));
464 else
465 return details::DistanceTraversalBVDistanceLowerBound_impl<BV>
466 ::run (RT._R(), RT._T(), model1->getBV(b1), model2->getBV(b2));
467 }
468
470 void leafComputeDistance(int b1, int b2) const
471 {
472 if(this->enable_statistics) this->num_leaf_tests++;
473
474 const BVNode<BV>& node1 = this->model1->getBV(b1);
475 const BVNode<BV>& node2 = this->model2->getBV(b2);
476
477 int primitive_id1 = node1.primitiveId();
478 int primitive_id2 = node2.primitiveId();
479
480 const Triangle& tri_id1 = tri_indices1[primitive_id1];
481 const Triangle& tri_id2 = tri_indices2[primitive_id2];
482
483 const Vec3f& t11 = vertices1[tri_id1[0]];
484 const Vec3f& t12 = vertices1[tri_id1[1]];
485 const Vec3f& t13 = vertices1[tri_id1[2]];
486
487 const Vec3f& t21 = vertices2[tri_id2[0]];
488 const Vec3f& t22 = vertices2[tri_id2[1]];
489 const Vec3f& t23 = vertices2[tri_id2[2]];
490
491 // nearest point pair
492 Vec3f P1, P2, normal;
493
494 FCL_REAL d2;
495 if (RTIsIdentity)
496 d2 = TriangleDistance::sqrTriDistance (t11, t12, t13, t21, t22, t23,
497 P1, P2);
498 else
499 d2 = TriangleDistance::sqrTriDistance (t11, t12, t13, t21, t22, t23,
500 RT._R(), RT._T(),
501 P1, P2);
502 FCL_REAL d = sqrt(d2);
503
504 this->result->update(d, this->model1, this->model2, primitive_id1,
505 primitive_id2, P1, P2, normal);
506 }
507
509 bool canStop(FCL_REAL c) const
510 {
511 if((c >= this->result->min_distance - abs_err) && (c * (1 + rel_err) >= this->result->min_distance))
512 return true;
513 return false;
514 }
515
516 Vec3f* vertices1;
517 Vec3f* vertices2;
518
519 Triangle* tri_indices1;
520 Triangle* tri_indices2;
521
523 FCL_REAL rel_err;
524 FCL_REAL abs_err;
525
526 details::RelativeTransformation<!bool(RTIsIdentity)> RT;
527
528 private:
529 void preprocessOrientedNode()
530 {
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];
534
535 Vec3f init_tri1_points[3];
536 Vec3f init_tri2_points[3];
537
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]];
541
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]];
545
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));
552
553 result->update(distance, model1, model2, init_tri_id1, init_tri_id2, p1, p2,
554 normal);
555 }
556 void postprocessOrientedNode()
557 {
559 if(request.enable_nearest_points && (result->o1 == model1) && (result->o2 == model2))
560 {
561 result->nearest_points[0] = tf1.transform(result->nearest_points[0]);
562 result->nearest_points[1] = tf1.transform(result->nearest_points[1]);
563 }
564 }
565 };
566
568 typedef MeshDistanceTraversalNode<RSS , 0> MeshDistanceTraversalNodeRSS;
569 typedef MeshDistanceTraversalNode<kIOS , 0> MeshDistanceTraversalNodekIOS;
570 typedef MeshDistanceTraversalNode<OBBRSS, 0> MeshDistanceTraversalNodeOBBRSS;
571
573
575 namespace details
576 {
577
578 template<typename BV>
579 inline const Matrix3f& getBVAxes(const BV& bv)
580 {
581 return bv.axes;
582 }
583
584 template<>
585 inline const Matrix3f& getBVAxes<OBBRSS>(const OBBRSS& bv)
586 {
587 return bv.obb.axes;
588 }
589
590 }
591
592 }
593
594 } // namespace hpp
595
597
598 #endif
BV.h
BVH_model.h
hpp::fcl::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:67
traversal_node_base.h
narrowphase.h
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:66
details
Definition: traversal_node_setup.h:776
collision_data.h
hpp
Main namespace.
Definition: AABB.h:44
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.
BV_node.h
hpp::fcl::Matrix3f
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:68
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.
geometric_shapes.h
intersect.h
traversal.h
hpp::fcl::DISTANCE_LOWER_BOUND
@ DISTANCE_LOWER_BOUND
Definition: collision_data.h:192