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

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

traversal_node_octree.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_OCTREE_H
39 #define HPP_FCL_TRAVERSAL_NODE_OCTREE_H
40
42
43 #include <hpp/fcl/collision_data.h>
44 #include <hpp/fcl/internal/traversal_node_base.h>
45 #include <hpp/fcl/narrowphase/narrowphase.h>
46 #include <hpp/fcl/octree.h>
47 #include <hpp/fcl/BVH/BVH_model.h>
48 #include <hpp/fcl/shape/geometric_shapes_utility.h>
49
50 namespace hpp
51 {
52 namespace fcl
53 {
54
56 class HPP_FCL_DLLAPI OcTreeSolver
57 {
58 private:
59 const GJKSolver* solver;
60
61 mutable const CollisionRequest* crequest;
62 mutable const DistanceRequest* drequest;
63
64 mutable CollisionResult* cresult;
65 mutable DistanceResult* dresult;
66
67 public:
68 OcTreeSolver(const GJKSolver* solver_) : solver(solver_),
69 crequest(NULL),
70 drequest(NULL),
71 cresult(NULL),
72 dresult(NULL)
73 {
74 }
75
77 void OcTreeIntersect(const OcTree* tree1, const OcTree* tree2,
78 const Transform3f& tf1, const Transform3f& tf2,
79 const CollisionRequest& request_,
80 CollisionResult& result_) const
81 {
82 crequest = &request_;
83 cresult = &result_;
84
85 OcTreeIntersectRecurse(tree1, tree1->getRoot(), tree1->getRootBV(),
86 tree2, tree2->getRoot(), tree2->getRootBV(),
87 tf1, tf2);
88 }
89
91 void OcTreeDistance(const OcTree* tree1, const OcTree* tree2,
92 const Transform3f& tf1, const Transform3f& tf2,
93 const DistanceRequest& request_,
94 DistanceResult& result_) const
95 {
96 drequest = &request_;
97 dresult = &result_;
98
99 OcTreeDistanceRecurse(tree1, tree1->getRoot(), tree1->getRootBV(),
100 tree2, tree2->getRoot(), tree2->getRootBV(),
101 tf1, tf2);
102 }
103
105 template<typename BV>
106 void OcTreeMeshIntersect(const OcTree* tree1, const BVHModel<BV>* tree2,
107 const Transform3f& tf1, const Transform3f& tf2,
108 const CollisionRequest& request_,
109 CollisionResult& result_) const
110 {
111 crequest = &request_;
112 cresult = &result_;
113
114 OcTreeMeshIntersectRecurse(tree1, tree1->getRoot(), tree1->getRootBV(),
115 tree2, 0,
116 tf1, tf2);
117 }
118
120 template<typename BV>
121 void OcTreeMeshDistance(const OcTree* tree1, const BVHModel<BV>* tree2,
122 const Transform3f& tf1, const Transform3f& tf2,
123 const DistanceRequest& request_,
124 DistanceResult& result_) const
125 {
126 drequest = &request_;
127 dresult = &result_;
128
129 OcTreeMeshDistanceRecurse(tree1, tree1->getRoot(), tree1->getRootBV(),
130 tree2, 0,
131 tf1, tf2);
132 }
133
135 template<typename BV>
136 void MeshOcTreeIntersect(const BVHModel<BV>* tree1, const OcTree* tree2,
137 const Transform3f& tf1, const Transform3f& tf2,
138 const CollisionRequest& request_,
139 CollisionResult& result_) const
140
141 {
142 crequest = &request_;
143 cresult = &result_;
144
145 OcTreeMeshIntersectRecurse(tree2, tree2->getRoot(), tree2->getRootBV(),
146 tree1, 0,
147 tf2, tf1);
148 }
149
151 template<typename BV>
152 void MeshOcTreeDistance(const BVHModel<BV>* tree1, const OcTree* tree2,
153 const Transform3f& tf1, const Transform3f& tf2,
154 const DistanceRequest& request_,
155 DistanceResult& result_) const
156 {
157 drequest = &request_;
158 dresult = &result_;
159
160 OcTreeMeshDistanceRecurse(tree1, 0,
161 tree2, tree2->getRoot(), tree2->getRootBV(),
162 tf1, tf2);
163 }
164
166 template<typename S>
167 void OcTreeShapeIntersect(const OcTree* tree, const S& s,
168 const Transform3f& tf1, const Transform3f& tf2,
169 const CollisionRequest& request_,
170 CollisionResult& result_) const
171 {
172 crequest = &request_;
173 cresult = &result_;
174
175 AABB bv2;
176 computeBV<AABB>(s, Transform3f(), bv2);
177 OBB obb2;
178 convertBV(bv2, tf2, obb2);
179 OcTreeShapeIntersectRecurse(tree, tree->getRoot(), tree->getRootBV(),
180 s, obb2,
181 tf1, tf2);
182
183 }
184
186 template<typename S>
187 void ShapeOcTreeIntersect(const S& s, const OcTree* tree,
188 const Transform3f& tf1, const Transform3f& tf2,
189 const CollisionRequest& request_,
190 CollisionResult& result_) const
191 {
192 crequest = &request_;
193 cresult = &result_;
194
195 AABB bv1;
196 computeBV<AABB>(s, Transform3f(), bv1);
197 OBB obb1;
198 convertBV(bv1, tf1, obb1);
199 OcTreeShapeIntersectRecurse(tree, tree->getRoot(), tree->getRootBV(),
200 s, obb1,
201 tf2, tf1);
202 }
203
205 template<typename S>
206 void OcTreeShapeDistance(const OcTree* tree, const S& s,
207 const Transform3f& tf1, const Transform3f& tf2,
208 const DistanceRequest& request_,
209 DistanceResult& result_) const
210 {
211 drequest = &request_;
212 dresult = &result_;
213
214 AABB aabb2;
215 computeBV<AABB>(s, tf2, aabb2);
216 OcTreeShapeDistanceRecurse(tree, tree->getRoot(), tree->getRootBV(),
217 s, aabb2,
218 tf1, tf2);
219 }
220
222 template<typename S>
223 void ShapeOcTreeDistance(const S& s, const OcTree* tree,
224 const Transform3f& tf1, const Transform3f& tf2,
225 const DistanceRequest& request_,
226 DistanceResult& result_) const
227 {
228 drequest = &request_;
229 dresult = &result_;
230
231 AABB aabb1;
232 computeBV<AABB>(s, tf1, aabb1);
233 OcTreeShapeDistanceRecurse(tree, tree->getRoot(), tree->getRootBV(),
234 s, aabb1,
235 tf2, tf1);
236 }
237
238
239 private:
240 template<typename S>
241 bool OcTreeShapeDistanceRecurse(const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1,
242 const S& s, const AABB& aabb2,
243 const Transform3f& tf1, const Transform3f& tf2) const
244 {
245 if(!tree1->nodeHasChildren(root1))
246 {
247 if(tree1->isNodeOccupied(root1))
248 {
249 Box box;
250 Transform3f box_tf;
251 constructBox(bv1, tf1, box, box_tf);
252
253 FCL_REAL dist;
254 Vec3f closest_p1, closest_p2, normal;
255 solver->shapeDistance(box, box_tf, s, tf2, dist, closest_p1,
256 closest_p2, normal);
257
258 dresult->update(dist, tree1, &s, (int) (root1 - tree1->getRoot()),
259 DistanceResult::NONE, closest_p1, closest_p2,
260 normal);
261
262 return drequest->isSatisfied(*dresult);
263 }
264 else
265 return false;
266 }
267
268 if(!tree1->isNodeOccupied(root1)) return false;
269
270 for(unsigned int i = 0; i < 8; ++i)
271 {
272 if(tree1->nodeChildExists(root1, i))
273 {
274 const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
275 AABB child_bv;
276 computeChildBV(bv1, i, child_bv);
277
278 AABB aabb1;
279 convertBV(child_bv, tf1, aabb1);
280 FCL_REAL d = aabb1.distance(aabb2);
281 if(d < dresult->min_distance)
282 {
283 if(OcTreeShapeDistanceRecurse(tree1, child, child_bv, s, aabb2, tf1, tf2))
284 return true;
285 }
286 }
287 }
288
289 return false;
290 }
291
292 template<typename S>
293 bool OcTreeShapeIntersectRecurse(const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1,
294 const S& s, const OBB& obb2,
295 const Transform3f& tf1, const Transform3f& tf2) const
296 {
297 if(!root1)
298 {
299 OBB obb1;
300 convertBV(bv1, tf1, obb1);
301 if(obb1.overlap(obb2))
302 {
303 Box box;
304 Transform3f box_tf;
305 constructBox(bv1, tf1, box, box_tf);
306
307 FCL_REAL distance;
308 if(solver->shapeIntersect(box, box_tf, s, tf2, distance, false, NULL, NULL))
309 {
310 AABB overlap_part;
311 AABB aabb1, aabb2;
312 computeBV<AABB, Box>(box, box_tf, aabb1);
313 computeBV<AABB, S>(s, tf2, aabb2);
314 aabb1.overlap(aabb2, overlap_part);
315 }
316 }
317
318 return false;
319 }
320 else if(!tree1->nodeHasChildren(root1))
321 {
322 if(tree1->isNodeOccupied(root1)) // occupied area
323 {
324 OBB obb1;
325 convertBV(bv1, tf1, obb1);
326 if(obb1.overlap(obb2))
327 {
328 Box box;
329 Transform3f box_tf;
330 constructBox(bv1, tf1, box, box_tf);
331
332 FCL_REAL distance;
333 if(!crequest->enable_contact)
334 {
335 if(solver->shapeIntersect(box, box_tf, s, tf2, distance, false, NULL, NULL))
336 {
337 if(cresult->numContacts() < crequest->num_max_contacts)
338 cresult->addContact(Contact(tree1, &s, static_cast<int>(root1 - tree1->getRoot()), Contact::NONE));
339 }
340 }
341 else
342 {
343 Vec3f contact;
344 Vec3f normal;
345
346 if(solver->shapeIntersect(box, box_tf, s, tf2, distance, true, &contact, &normal))
347 {
348 if(cresult->numContacts() < crequest->num_max_contacts)
349 cresult->addContact(Contact(tree1, &s, static_cast<int>(root1 - tree1->getRoot()), Contact::NONE, contact, normal, distance));
350 }
351 }
352
353 return crequest->isSatisfied(*cresult);
354 }
355 else return false;
356 }
357 else // free area
358 return false;
359 }
360
364 if(tree1->isNodeFree(root1)) return false;
365 else if((tree1->isNodeUncertain(root1) || s.isUncertain())) return false;
366 else
367 {
368 OBB obb1;
369 convertBV(bv1, tf1, obb1);
370 if(!obb1.overlap(obb2)) return false;
371 }
372
373 for(unsigned int i = 0; i < 8; ++i)
374 {
375 if(tree1->nodeChildExists(root1, i))
376 {
377 const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
378 AABB child_bv;
379 computeChildBV(bv1, i, child_bv);
380
381 if(OcTreeShapeIntersectRecurse(tree1, child, child_bv, s, obb2, tf1, tf2))
382 return true;
383 }
384 }
385
386 return false;
387 }
388
389 template<typename BV>
390 bool OcTreeMeshDistanceRecurse(const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1,
391 const BVHModel<BV>* tree2, int root2,
392 const Transform3f& tf1, const Transform3f& tf2) const
393 {
394 if(!tree1->nodeHasChildren(root1) && tree2->getBV(root2).isLeaf())
395 {
396 if(tree1->isNodeOccupied(root1))
397 {
398 Box box;
399 Transform3f box_tf;
400 constructBox(bv1, tf1, box, box_tf);
401
402 int primitive_id = tree2->getBV(root2).primitiveId();
403 const Triangle& tri_id = tree2->tri_indices[primitive_id];
404 const Vec3f& p1 = tree2->vertices[tri_id[0]];
405 const Vec3f& p2 = tree2->vertices[tri_id[1]];
406 const Vec3f& p3 = tree2->vertices[tri_id[2]];
407
408 FCL_REAL dist;
409 Vec3f closest_p1, closest_p2, normal;
410 solver->shapeTriangleInteraction(box, box_tf, p1, p2, p3, tf2, dist,
411 closest_p1, closest_p2, normal);
412
413 dresult->update(dist, tree1, tree2, (int) (root1 - tree1->getRoot()),
414 primitive_id, closest_p1, closest_p2, normal);
415
416 return drequest->isSatisfied(*dresult);
417 }
418 else
419 return false;
420 }
421
422 if(!tree1->isNodeOccupied(root1)) return false;
423
424 if(tree2->getBV(root2).isLeaf() || (tree1->nodeHasChildren(root1) && (bv1.size() > tree2->getBV(root2).bv.size())))
425 {
426 for(unsigned int i = 0; i < 8; ++i)
427 {
428 if(tree1->nodeChildExists(root1, i))
429 {
430 const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
431 AABB child_bv;
432 computeChildBV(bv1, i, child_bv);
433
434 FCL_REAL d;
435 AABB aabb1, aabb2;
436 convertBV(child_bv, tf1, aabb1);
437 convertBV(tree2->getBV(root2).bv, tf2, aabb2);
438 d = aabb1.distance(aabb2);
439
440 if(d < dresult->min_distance)
441 {
442 if(OcTreeMeshDistanceRecurse(tree1, child, child_bv, tree2, root2, tf1, tf2))
443 return true;
444 }
445 }
446 }
447 }
448 else
449 {
450 FCL_REAL d;
451 AABB aabb1, aabb2;
452 convertBV(bv1, tf1, aabb1);
453 int child = tree2->getBV(root2).leftChild();
454 convertBV(tree2->getBV(child).bv, tf2, aabb2);
455 d = aabb1.distance(aabb2);
456
457 if(d < dresult->min_distance)
458 {
459 if(OcTreeMeshDistanceRecurse(tree1, root1, bv1, tree2, child, tf1, tf2))
460 return true;
461 }
462
463 child = tree2->getBV(root2).rightChild();
464 convertBV(tree2->getBV(child).bv, tf2, aabb2);
465 d = aabb1.distance(aabb2);
466
467 if(d < dresult->min_distance)
468 {
469 if(OcTreeMeshDistanceRecurse(tree1, root1, bv1, tree2, child, tf1, tf2))
470 return true;
471 }
472 }
473
474 return false;
475 }
476
477
478 template<typename BV>
479 bool OcTreeMeshIntersectRecurse(const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1,
480 const BVHModel<BV>* tree2, int root2,
481 const Transform3f& tf1, const Transform3f& tf2) const
482 {
483 if(!root1)
484 {
485 if(tree2->getBV(root2).isLeaf())
486 {
487 OBB obb1, obb2;
488 convertBV(bv1, tf1, obb1);
489 convertBV(tree2->getBV(root2).bv, tf2, obb2);
490 if(obb1.overlap(obb2))
491 {
492 Box box;
493 Transform3f box_tf;
494 constructBox(bv1, tf1, box, box_tf);
495
496 int primitive_id = tree2->getBV(root2).primitiveId();
497 const Triangle& tri_id = tree2->tri_indices[primitive_id];
498 const Vec3f& p1 = tree2->vertices[tri_id[0]];
499 const Vec3f& p2 = tree2->vertices[tri_id[1]];
500 const Vec3f& p3 = tree2->vertices[tri_id[2]];
501 Vec3f c1, c2, normal;
502 FCL_REAL distance;
503 if(solver->shapeTriangleInteraction
504 (box, box_tf, p1, p2, p3, tf2, distance, c1, c2, normal))
505 {
506 AABB overlap_part;
507 AABB aabb1;
508 computeBV<AABB, Box>(box, box_tf, aabb1);
509 AABB aabb2(tf2.transform(p1), tf2.transform(p2), tf2.transform(p3));
510 aabb1.overlap(aabb2, overlap_part);
511 }
512 }
513
514 return false;
515 }
516 else
517 {
518 if(OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2, tree2->getBV(root2).leftChild(), tf1, tf2))
519 return true;
520
521 if(OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2, tree2->getBV(root2).rightChild(), tf1, tf2))
522 return true;
523
524 return false;
525 }
526 }
527 else if(!tree1->nodeHasChildren(root1) && tree2->getBV(root2).isLeaf())
528 {
529 if(tree1->isNodeOccupied(root1))
530 {
531 OBB obb1, obb2;
532 convertBV(bv1, tf1, obb1);
533 convertBV(tree2->getBV(root2).bv, tf2, obb2);
534 if(obb1.overlap(obb2))
535 {
536 Box box;
537 Transform3f box_tf;
538 constructBox(bv1, tf1, box, box_tf);
539
540 int primitive_id = tree2->getBV(root2).primitiveId();
541 const Triangle& tri_id = tree2->tri_indices[primitive_id];
542 const Vec3f& p1 = tree2->vertices[tri_id[0]];
543 const Vec3f& p2 = tree2->vertices[tri_id[1]];
544 const Vec3f& p3 = tree2->vertices[tri_id[2]];
545
546 if(!crequest->enable_contact)
547 {
548 Vec3f c1, c2, normal;
549 FCL_REAL distance;
550 if(solver->shapeTriangleInteraction
551 (box, box_tf, p1, p2, p3, tf2, distance, c1, c2, normal))
552 {
553 if(cresult->numContacts() < crequest->num_max_contacts)
554 cresult->addContact(Contact(tree1, tree2,
555 (int)(root1 - tree1->getRoot()),
556 primitive_id));
557 }
558 }
559 else
560 {
561 Vec3f c1, c2;
562 FCL_REAL distance;
563 Vec3f normal;
564
565 if(solver->shapeTriangleInteraction(box, box_tf, p1, p2, p3, tf2,
566 distance, c1, c2, normal))
567 {
568 assert (crequest->security_margin == 0);
569 if(cresult->numContacts() < crequest->num_max_contacts)
570 cresult->addContact
571 (Contact(tree1, tree2, (int) (root1 - tree1->getRoot()),
572 primitive_id, c1, normal, -distance));
573 }
574 }
575
576 return crequest->isSatisfied(*cresult);
577 }
578 else
579 return false;
580 }
581 else // free area
582 return false;
583 }
584
588 if(tree1->isNodeFree(root1)) return false;
589 else if((tree1->isNodeUncertain(root1) || tree2->isUncertain()))
590 return false;
591 else
592 {
593 OBB obb1, obb2;
594 convertBV(bv1, tf1, obb1);
595 convertBV(tree2->getBV(root2).bv, tf2, obb2);
596 if(!obb1.overlap(obb2)) return false;
597 }
598
599 if(tree2->getBV(root2).isLeaf() || (tree1->nodeHasChildren(root1) && (bv1.size() > tree2->getBV(root2).bv.size())))
600 {
601 for(unsigned int i = 0; i < 8; ++i)
602 {
603 if(tree1->nodeChildExists(root1, i))
604 {
605 const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
606 AABB child_bv;
607 computeChildBV(bv1, i, child_bv);
608
609 if(OcTreeMeshIntersectRecurse(tree1, child, child_bv, tree2, root2, tf1, tf2))
610 return true;
611 }
612 }
613 }
614 else
615 {
616 if(OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2, tree2->getBV(root2).leftChild(), tf1, tf2))
617 return true;
618
619 if(OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2, tree2->getBV(root2).rightChild(), tf1, tf2))
620 return true;
621
622 }
623
624 return false;
625 }
626
627 bool OcTreeDistanceRecurse(const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1,
628 const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& bv2,
629 const Transform3f& tf1, const Transform3f& tf2) const
630 {
631 if(!tree1->nodeHasChildren(root1) && !tree2->nodeHasChildren(root2))
632 {
633 if(tree1->isNodeOccupied(root1) && tree2->isNodeOccupied(root2))
634 {
635 Box box1, box2;
636 Transform3f box1_tf, box2_tf;
637 constructBox(bv1, tf1, box1, box1_tf);
638 constructBox(bv2, tf2, box2, box2_tf);
639
640 FCL_REAL dist;
641 Vec3f closest_p1, closest_p2, normal;
642 solver->shapeDistance(box1, box1_tf, box2, box2_tf, dist, closest_p1,
643 closest_p2, normal);
644
645 dresult->update(dist, tree1, tree2, (int) (root1 - tree1->getRoot()),
646 (int) (root2 - tree2->getRoot()),
647 closest_p1, closest_p2, normal);
648
649 return drequest->isSatisfied(*dresult);
650 }
651 else
652 return false;
653 }
654
655 if(!tree1->isNodeOccupied(root1) || !tree2->isNodeOccupied(root2)) return false;
656
657 if(!tree2->nodeHasChildren(root2) || (tree1->nodeHasChildren(root1) && (bv1.size() > bv2.size())))
658 {
659 for(unsigned int i = 0; i < 8; ++i)
660 {
661 if(tree1->nodeChildExists(root1, i))
662 {
663 const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
664 AABB child_bv;
665 computeChildBV(bv1, i, child_bv);
666
667 FCL_REAL d;
668 AABB aabb1, aabb2;
669 convertBV(bv1, tf1, aabb1);
670 convertBV(bv2, tf2, aabb2);
671 d = aabb1.distance(aabb2);
672
673 if(d < dresult->min_distance)
674 {
675
676 if(OcTreeDistanceRecurse(tree1, child, child_bv, tree2, root2, bv2, tf1, tf2))
677 return true;
678 }
679 }
680 }
681 }
682 else
683 {
684 for(unsigned int i = 0; i < 8; ++i)
685 {
686 if(tree2->nodeChildExists(root2, i))
687 {
688 const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i);
689 AABB child_bv;
690 computeChildBV(bv2, i, child_bv);
691
692 FCL_REAL d;
693 AABB aabb1, aabb2;
694 convertBV(bv1, tf1, aabb1);
695 convertBV(bv2, tf2, aabb2);
696 d = aabb1.distance(aabb2);
697
698 if(d < dresult->min_distance)
699 {
700 if(OcTreeDistanceRecurse(tree1, root1, bv1, tree2, child, child_bv, tf1, tf2))
701 return true;
702 }
703 }
704 }
705 }
706
707 return false;
708 }
709
710
711 bool OcTreeIntersectRecurse(const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1,
712 const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& bv2,
713 const Transform3f& tf1, const Transform3f& tf2) const
714 {
715 if(!root1 && !root2)
716 {
717 OBB obb1, obb2;
718 convertBV(bv1, tf1, obb1);
719 convertBV(bv2, tf2, obb2);
720
721 if(obb1.overlap(obb2))
722 {
723 Box box1, box2;
724 Transform3f box1_tf, box2_tf;
725 constructBox(bv1, tf1, box1, box1_tf);
726 constructBox(bv2, tf2, box2, box2_tf);
727
728 AABB overlap_part;
729 AABB aabb1, aabb2;
730 computeBV<AABB, Box>(box1, box1_tf, aabb1);
731 computeBV<AABB, Box>(box2, box2_tf, aabb2);
732 aabb1.overlap(aabb2, overlap_part);
733 }
734
735 return false;
736 }
737 else if(!root1 && root2)
738 {
739 if(tree2->nodeHasChildren(root2))
740 {
741 for(unsigned int i = 0; i < 8; ++i)
742 {
743 if(tree2->nodeChildExists(root2, i))
744 {
745 const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i);
746 AABB child_bv;
747 computeChildBV(bv2, i, child_bv);
748 if(OcTreeIntersectRecurse(tree1, NULL, bv1, tree2, child, child_bv, tf1, tf2))
749 return true;
750 }
751 else
752 {
753 AABB child_bv;
754 computeChildBV(bv2, i, child_bv);
755 if(OcTreeIntersectRecurse(tree1, NULL, bv1, tree2, NULL, child_bv, tf1, tf2))
756 return true;
757 }
758 }
759 }
760 else
761 {
762 if(OcTreeIntersectRecurse(tree1, NULL, bv1, tree2, NULL, bv2, tf1, tf2))
763 return true;
764 }
765
766 return false;
767 }
768 else if(root1 && !root2)
769 {
770 if(tree1->nodeHasChildren(root1))
771 {
772 for(unsigned int i = 0; i < 8; ++i)
773 {
774 if(tree1->nodeChildExists(root1, i))
775 {
776 const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
777 AABB child_bv;
778 computeChildBV(bv1, i, child_bv);
779 if(OcTreeIntersectRecurse(tree1, child, child_bv, tree2, NULL, bv2, tf1, tf2))
780 return true;
781 }
782 else
783 {
784 AABB child_bv;
785 computeChildBV(bv1, i, child_bv);
786 if(OcTreeIntersectRecurse(tree1, NULL, child_bv, tree2, NULL, bv2, tf1, tf2))
787 return true;
788 }
789 }
790 }
791 else
792 {
793 if(OcTreeIntersectRecurse(tree1, NULL, bv1, tree2, NULL, bv2, tf1, tf2))
794 return true;
795 }
796
797 return false;
798 }
799 else if(!tree1->nodeHasChildren(root1) && !tree2->nodeHasChildren(root2))
800 {
801 if(tree1->isNodeOccupied(root1) && tree2->isNodeOccupied(root2)) // occupied area
802 {
803 if(!crequest->enable_contact)
804 {
805 OBB obb1, obb2;
806 convertBV(bv1, tf1, obb1);
807 convertBV(bv2, tf2, obb2);
808
809 if(obb1.overlap(obb2))
810 {
811 if(cresult->numContacts() < crequest->num_max_contacts)
812 cresult->addContact(Contact(tree1, tree2, static_cast<int>(root1 - tree1->getRoot()), static_cast<int>(root2 - tree2->getRoot())));
813 }
814 }
815 else
816 {
817 Box box1, box2;
818 Transform3f box1_tf, box2_tf;
819 constructBox(bv1, tf1, box1, box1_tf);
820 constructBox(bv2, tf2, box2, box2_tf);
821
822 Vec3f contact;
823 FCL_REAL distance;
824 Vec3f normal;
825 if(solver->shapeIntersect(box1, box1_tf, box2, box2_tf, distance, true, &contact, &normal))
826 {
827 if(cresult->numContacts() < crequest->num_max_contacts)
828 cresult->addContact(Contact(tree1, tree2, static_cast<int>(root1 - tree1->getRoot()), static_cast<int>(root2 - tree2->getRoot()), contact, normal, distance));
829 }
830 }
831
832 return crequest->isSatisfied(*cresult);
833 }
834 else // free area (at least one node is free)
835 return false;
836 }
837
841 if(tree1->isNodeFree(root1) || tree2->isNodeFree(root2)) return false;
842 else if((tree1->isNodeUncertain(root1) || tree2->isNodeUncertain(root2)))
843 return false;
844 else
845 {
846 OBB obb1, obb2;
847 convertBV(bv1, tf1, obb1);
848 convertBV(bv2, tf2, obb2);
849 if(!obb1.overlap(obb2)) return false;
850 }
851
852 if(!tree2->nodeHasChildren(root2) || (tree1->nodeHasChildren(root1) && (bv1.size() > bv2.size())))
853 {
854 for(unsigned int i = 0; i < 8; ++i)
855 {
856 if(tree1->nodeChildExists(root1, i))
857 {
858 const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
859 AABB child_bv;
860 computeChildBV(bv1, i, child_bv);
861
862 if(OcTreeIntersectRecurse(tree1, child, child_bv,
863 tree2, root2, bv2,
864 tf1, tf2))
865 return true;
866 }
867 }
868 }
869 else
870 {
871 for(unsigned int i = 0; i < 8; ++i)
872 {
873 if(tree2->nodeChildExists(root2, i))
874 {
875 const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i);
876 AABB child_bv;
877 computeChildBV(bv2, i, child_bv);
878
879 if(OcTreeIntersectRecurse(tree1, root1, bv1,
880 tree2, child, child_bv,
881 tf1, tf2))
882 return true;
883 }
884 }
885 }
886
887 return false;
888 }
889 };
890
893
895 class HPP_FCL_DLLAPI OcTreeCollisionTraversalNode : public CollisionTraversalNodeBase
896 {
897 public:
898 OcTreeCollisionTraversalNode(const CollisionRequest& request) :
899 CollisionTraversalNodeBase (request)
900 {
901 model1 = NULL;
902 model2 = NULL;
903
904 otsolver = NULL;
905 }
906
907 bool BVDisjoints(int, int) const
908 {
909 return false;
910 }
911
912 bool BVDisjoints(int, int, FCL_REAL&) const
913 {
914 return false;
915 }
916
917 void leafCollides(int, int, FCL_REAL&) const
918 {
919 otsolver->OcTreeIntersect(model1, model2, tf1, tf2, request, *result);
920 }
921
922 const OcTree* model1;
923 const OcTree* model2;
924
925 Transform3f tf1, tf2;
926
927 const OcTreeSolver* otsolver;
928 };
929
931 template<typename S>
932 class HPP_FCL_DLLAPI ShapeOcTreeCollisionTraversalNode : public CollisionTraversalNodeBase
933 {
934 public:
935 ShapeOcTreeCollisionTraversalNode(const CollisionRequest& request) :
936 CollisionTraversalNodeBase (request)
937 {
938 model1 = NULL;
939 model2 = NULL;
940
941 otsolver = NULL;
942 }
943
944 bool BVDisjoints(int, int) const
945 {
946 return false;
947 }
948
949 bool BVDisjoints(int, int, FCL_REAL&) const
950 {
951 return false;
952 }
953
954 void leafCollides(int, int, FCL_REAL&) const
955 {
956 otsolver->OcTreeShapeIntersect(model2, *model1, tf2, tf1, request, *result);
957 }
958
959 const S* model1;
960 const OcTree* model2;
961
962 Transform3f tf1, tf2;
963
964 const OcTreeSolver* otsolver;
965 };
966
968
969 template<typename S>
970 class HPP_FCL_DLLAPI OcTreeShapeCollisionTraversalNode : public CollisionTraversalNodeBase
971 {
972 public:
973 OcTreeShapeCollisionTraversalNode(const CollisionRequest& request) :
974 CollisionTraversalNodeBase (request)
975 {
976 model1 = NULL;
977 model2 = NULL;
978
979 otsolver = NULL;
980 }
981
982 bool BVDisjoints(int, int) const
983 {
984 return false;
985 }
986
987 bool BVDisjoints(int, int, fcl::FCL_REAL&) const
988 {
989 return false;
990 }
991
992 void leafCollides(int, int, FCL_REAL&) const
993 {
994 otsolver->OcTreeShapeIntersect(model1, *model2, tf1, tf2, request, *result);
995 }
996
997 const OcTree* model1;
998 const S* model2;
999
1000 Transform3f tf1, tf2;
1001
1002 const OcTreeSolver* otsolver;
1003 };
1004
1006 template<typename BV>
1007 class HPP_FCL_DLLAPI MeshOcTreeCollisionTraversalNode : public CollisionTraversalNodeBase
1008 {
1009 public:
1010 MeshOcTreeCollisionTraversalNode(const CollisionRequest& request) :
1011 CollisionTraversalNodeBase (request)
1012 {
1013 model1 = NULL;
1014 model2 = NULL;
1015
1016 otsolver = NULL;
1017 }
1018
1019 bool BVDisjoints(int, int) const
1020 {
1021 return false;
1022 }
1023
1024 bool BVDisjoints(int, int, FCL_REAL&) const
1025 {
1026 return false;
1027 }
1028
1029 void leafCollides(int, int, FCL_REAL&) const
1030 {
1031 otsolver->OcTreeMeshIntersect(model2, model1, tf2, tf1, request, *result);
1032 }
1033
1034 const BVHModel<BV>* model1;
1035 const OcTree* model2;
1036
1037 Transform3f tf1, tf2;
1038
1039 const OcTreeSolver* otsolver;
1040 };
1041
1043 template<typename BV>
1044 class HPP_FCL_DLLAPI OcTreeMeshCollisionTraversalNode : public CollisionTraversalNodeBase
1045 {
1046 public:
1047 OcTreeMeshCollisionTraversalNode(const CollisionRequest& request) :
1048 CollisionTraversalNodeBase (request)
1049 {
1050 model1 = NULL;
1051 model2 = NULL;
1052
1053 otsolver = NULL;
1054 }
1055
1056 bool BVDisjoints(int, int) const
1057 {
1058 return false;
1059 }
1060
1061 bool BVDisjoints(int, int, FCL_REAL&) const
1062 {
1063 return false;
1064 }
1065
1066 void leafCollides(int, int, FCL_REAL&) const
1067 {
1068 otsolver->OcTreeMeshIntersect(model1, model2, tf1, tf2, request, *result);
1069 }
1070
1071 const OcTree* model1;
1072 const BVHModel<BV>* model2;
1073
1074 Transform3f tf1, tf2;
1075
1076 const OcTreeSolver* otsolver;
1077 };
1078
1080
1083
1085 class HPP_FCL_DLLAPI OcTreeDistanceTraversalNode : public DistanceTraversalNodeBase
1086 {
1087 public:
1088 OcTreeDistanceTraversalNode()
1089 {
1090 model1 = NULL;
1091 model2 = NULL;
1092
1093 otsolver = NULL;
1094 }
1095
1096
1097 FCL_REAL BVDistanceLowerBound(int, int) const
1098 {
1099 return -1;
1100 }
1101
1102 bool BVDistanceLowerBound(int, int, FCL_REAL&) const
1103 {
1104 return false;
1105 }
1106
1107 void leafComputeDistance(int, int) const
1108 {
1109 otsolver->OcTreeDistance(model1, model2, tf1, tf2, request, *result);
1110 }
1111
1112 const OcTree* model1;
1113 const OcTree* model2;
1114
1115 const OcTreeSolver* otsolver;
1116 };
1117
1118
1119
1121 template<typename S>
1122 class HPP_FCL_DLLAPI ShapeOcTreeDistanceTraversalNode : public DistanceTraversalNodeBase
1123 {
1124 public:
1125 ShapeOcTreeDistanceTraversalNode()
1126 {
1127 model1 = NULL;
1128 model2 = NULL;
1129
1130 otsolver = NULL;
1131 }
1132
1133 FCL_REAL BVDistanceLowerBound(int, int) const
1134 {
1135 return -1;
1136 }
1137
1138 void leafComputeDistance(int, int) const
1139 {
1140 otsolver->OcTreeShapeDistance(model2, *model1, tf2, tf1, request, *result);
1141 }
1142
1143 const S* model1;
1144 const OcTree* model2;
1145
1146 const OcTreeSolver* otsolver;
1147 };
1148
1150 template<typename S>
1151 class HPP_FCL_DLLAPI OcTreeShapeDistanceTraversalNode : public DistanceTraversalNodeBase
1152 {
1153 public:
1154 OcTreeShapeDistanceTraversalNode()
1155 {
1156 model1 = NULL;
1157 model2 = NULL;
1158
1159 otsolver = NULL;
1160 }
1161
1162 FCL_REAL BVDistanceLowerBound(int, int) const
1163 {
1164 return -1;
1165 }
1166
1167 void leafComputeDistance(int, int) const
1168 {
1169 otsolver->OcTreeShapeDistance(model1, *model2, tf1, tf2, request, *result);
1170 }
1171
1172 const OcTree* model1;
1173 const S* model2;
1174
1175 const OcTreeSolver* otsolver;
1176 };
1177
1179 template<typename BV>
1180 class HPP_FCL_DLLAPI MeshOcTreeDistanceTraversalNode : public DistanceTraversalNodeBase
1181 {
1182 public:
1183 MeshOcTreeDistanceTraversalNode()
1184 {
1185 model1 = NULL;
1186 model2 = NULL;
1187
1188 otsolver = NULL;
1189 }
1190
1191 FCL_REAL BVDistanceLowerBound(int, int) const
1192 {
1193 return -1;
1194 }
1195
1196 void leafComputeDistance(int, int) const
1197 {
1198 otsolver->OcTreeMeshDistance(model2, model1, tf2, tf1, request, *result);
1199 }
1200
1201 const BVHModel<BV>* model1;
1202 const OcTree* model2;
1203
1204 const OcTreeSolver* otsolver;
1205
1206 };
1207
1209 template<typename BV>
1210 class HPP_FCL_DLLAPI OcTreeMeshDistanceTraversalNode : public DistanceTraversalNodeBase
1211 {
1212 public:
1213 OcTreeMeshDistanceTraversalNode()
1214 {
1215 model1 = NULL;
1216 model2 = NULL;
1217
1218 otsolver = NULL;
1219 }
1220
1221 FCL_REAL BVDistanceLowerBound(int, int) const
1222 {
1223 return -1;
1224 }
1225
1226 void leafComputeDistance(int, int) const
1227 {
1228 otsolver->OcTreeMeshDistance(model1, model2, tf1, tf2, request, *result);
1229 }
1230
1231 const OcTree* model1;
1232 const BVHModel<BV>* model2;
1233
1234 const OcTreeSolver* otsolver;
1235
1236 };
1237
1239
1240 }
1241
1242 } // namespace hpp
1243
1245
1246 #endif
BVH_model.h
hpp::fcl::OcTree::OcTreeNode
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef octomap::OcTreeNode OcTreeNode
Definition: octree.h:70
collision_data.h
geometric_shapes_utility.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::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:67
hpp::fcl::constructBox
void constructBox(const AABB &bv, Box &box, Transform3f &tf)
construct a box shape (with a configuration) from a given bounding volume
hpp::fcl::computeBV< AABB, Box >
void computeBV< AABB, Box >(const Box &s, const Transform3f &tf, AABB &bv)
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:66
hpp
Main namespace.
Definition: AABB.h:44
narrowphase.h
octree.h
hpp::fcl::Contact::NONE
static const int NONE
invalid contact primitive information
Definition: collision_data.h:87
hpp::fcl::DistanceResult::NONE
static const int NONE
invalid contact primitive information
Definition: collision_data.h:428
traversal_node_base.h