hpp-fcl  1.4.4
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 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 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 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 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 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 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 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 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 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 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 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
hpp::fcl::computeBV< AABB, Box >
void computeBV< AABB, Box >(const Box &s, const Transform3f &tf, AABB &bv)
hpp
Main namespace.
Definition: AABB.h:43
hpp::fcl::OcTree::OcTreeNode
octomap::OcTreeNode OcTreeNode
Definition: octree.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.
traversal_node_base.h
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:68
hpp::fcl::Contact::NONE
static const int NONE
invalid contact primitive information
Definition: collision_data.h:91
geometric_shapes_utility.h
collision_data.h
BVH_model.h
hpp::fcl::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:73
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::DistanceResult::NONE
static const int NONE
invalid contact primitive information
Definition: collision_data.h:366