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 #include <hpp/fcl/internal/shape_shape_func.h>
50 
51 namespace hpp {
52 namespace fcl {
53 
55 class HPP_FCL_DLLAPI OcTreeSolver {
56  private:
57  const GJKSolver* solver;
58 
59  mutable const CollisionRequest* crequest;
60  mutable const DistanceRequest* drequest;
61 
62  mutable CollisionResult* cresult;
63  mutable DistanceResult* dresult;
64 
65  public:
66  OcTreeSolver(const GJKSolver* solver_)
67  : solver(solver_),
68  crequest(NULL),
69  drequest(NULL),
70  cresult(NULL),
71  dresult(NULL) {}
72 
74  void OcTreeIntersect(const OcTree* tree1, const OcTree* tree2,
75  const Transform3f& tf1, const Transform3f& tf2,
76  const CollisionRequest& request_,
77  CollisionResult& result_) const {
78  crequest = &request_;
79  cresult = &result_;
80 
81  OcTreeIntersectRecurse(tree1, tree1->getRoot(), tree1->getRootBV(), tree2,
82  tree2->getRoot(), tree2->getRootBV(), tf1, tf2);
83  }
84 
86  void OcTreeDistance(const OcTree* tree1, const OcTree* tree2,
87  const Transform3f& tf1, const Transform3f& tf2,
88  const DistanceRequest& request_,
89  DistanceResult& result_) const {
90  drequest = &request_;
91  dresult = &result_;
92 
93  OcTreeDistanceRecurse(tree1, tree1->getRoot(), tree1->getRootBV(), tree2,
94  tree2->getRoot(), tree2->getRootBV(), tf1, tf2);
95  }
96 
98  template <typename BV>
99  void OcTreeMeshIntersect(const OcTree* tree1, const BVHModel<BV>* tree2,
100  const Transform3f& tf1, const Transform3f& tf2,
101  const CollisionRequest& request_,
102  CollisionResult& result_) const {
103  crequest = &request_;
104  cresult = &result_;
105 
106  OcTreeMeshIntersectRecurse(tree1, tree1->getRoot(), tree1->getRootBV(),
107  tree2, 0, tf1, tf2);
108  }
109 
111  template <typename BV>
112  void OcTreeMeshDistance(const OcTree* tree1, const BVHModel<BV>* tree2,
113  const Transform3f& tf1, const Transform3f& tf2,
114  const DistanceRequest& request_,
115  DistanceResult& result_) const {
116  drequest = &request_;
117  dresult = &result_;
118 
119  OcTreeMeshDistanceRecurse(tree1, tree1->getRoot(), tree1->getRootBV(),
120  tree2, 0, tf1, tf2);
121  }
122 
124  template <typename BV>
125  void MeshOcTreeIntersect(const BVHModel<BV>* tree1, const OcTree* tree2,
126  const Transform3f& tf1, const Transform3f& tf2,
127  const CollisionRequest& request_,
128  CollisionResult& result_) const
129 
130  {
131  crequest = &request_;
132  cresult = &result_;
133 
134  OcTreeMeshIntersectRecurse(tree2, tree2->getRoot(), tree2->getRootBV(),
135  tree1, 0, tf2, tf1);
136  }
137 
139  template <typename BV>
140  void MeshOcTreeDistance(const BVHModel<BV>* tree1, const OcTree* tree2,
141  const Transform3f& tf1, const Transform3f& tf2,
142  const DistanceRequest& request_,
143  DistanceResult& result_) const {
144  drequest = &request_;
145  dresult = &result_;
146 
147  OcTreeMeshDistanceRecurse(tree1, 0, tree2, tree2->getRoot(),
148  tree2->getRootBV(), tf1, tf2);
149  }
150 
152  template <typename S>
153  void OcTreeShapeIntersect(const OcTree* tree, const S& s,
154  const Transform3f& tf1, const Transform3f& tf2,
155  const CollisionRequest& request_,
156  CollisionResult& result_) const {
157  crequest = &request_;
158  cresult = &result_;
159 
160  AABB bv2;
161  computeBV<AABB>(s, Transform3f(), bv2);
162  OBB obb2;
163  convertBV(bv2, tf2, obb2);
164  OcTreeShapeIntersectRecurse(tree, tree->getRoot(), tree->getRootBV(), s,
165  obb2, tf1, tf2);
166  }
167 
169  template <typename S>
170  void ShapeOcTreeIntersect(const S& s, const OcTree* tree,
171  const Transform3f& tf1, const Transform3f& tf2,
172  const CollisionRequest& request_,
173  CollisionResult& result_) const {
174  crequest = &request_;
175  cresult = &result_;
176 
177  AABB bv1;
178  computeBV<AABB>(s, Transform3f(), bv1);
179  OBB obb1;
180  convertBV(bv1, tf1, obb1);
181  OcTreeShapeIntersectRecurse(tree, tree->getRoot(), tree->getRootBV(), s,
182  obb1, tf2, tf1);
183  }
184 
186  template <typename S>
187  void OcTreeShapeDistance(const OcTree* tree, const S& s,
188  const Transform3f& tf1, const Transform3f& tf2,
189  const DistanceRequest& request_,
190  DistanceResult& result_) const {
191  drequest = &request_;
192  dresult = &result_;
193 
194  AABB aabb2;
195  computeBV<AABB>(s, tf2, aabb2);
196  OcTreeShapeDistanceRecurse(tree, tree->getRoot(), tree->getRootBV(), s,
197  aabb2, tf1, tf2);
198  }
199 
201  template <typename S>
202  void ShapeOcTreeDistance(const S& s, const OcTree* tree,
203  const Transform3f& tf1, const Transform3f& tf2,
204  const DistanceRequest& request_,
205  DistanceResult& result_) const {
206  drequest = &request_;
207  dresult = &result_;
208 
209  AABB aabb1;
210  computeBV<AABB>(s, tf1, aabb1);
211  OcTreeShapeDistanceRecurse(tree, tree->getRoot(), tree->getRootBV(), s,
212  aabb1, tf2, tf1);
213  }
214 
215  private:
216  template <typename S>
217  bool OcTreeShapeDistanceRecurse(const OcTree* tree1,
218  const OcTree::OcTreeNode* root1,
219  const AABB& bv1, const S& s,
220  const AABB& aabb2, const Transform3f& tf1,
221  const Transform3f& tf2) const {
222  if (!tree1->nodeHasChildren(root1)) {
223  if (tree1->isNodeOccupied(root1)) {
224  Box box;
225  Transform3f box_tf;
226  constructBox(bv1, tf1, box, box_tf);
227 
228  FCL_REAL dist;
229  Vec3f closest_p1, closest_p2, normal;
230  solver->shapeDistance(box, box_tf, s, tf2, dist, closest_p1, closest_p2,
231  normal);
232 
233  dresult->update(dist, tree1, &s, (int)(root1 - tree1->getRoot()),
234  DistanceResult::NONE, closest_p1, closest_p2, normal);
235 
236  return drequest->isSatisfied(*dresult);
237  } else
238  return false;
239  }
240 
241  if (!tree1->isNodeOccupied(root1)) return false;
242 
243  for (unsigned int i = 0; i < 8; ++i) {
244  if (tree1->nodeChildExists(root1, i)) {
245  const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
246  AABB child_bv;
247  computeChildBV(bv1, i, child_bv);
248 
249  AABB aabb1;
250  convertBV(child_bv, tf1, aabb1);
251  FCL_REAL d = aabb1.distance(aabb2);
252  if (d < dresult->min_distance) {
253  if (OcTreeShapeDistanceRecurse(tree1, child, child_bv, s, aabb2, tf1,
254  tf2))
255  return true;
256  }
257  }
258  }
259 
260  return false;
261  }
262 
263  template <typename S>
264  bool OcTreeShapeIntersectRecurse(const OcTree* tree1,
265  const OcTree::OcTreeNode* root1,
266  const AABB& bv1, const S& s, const OBB& obb2,
267  const Transform3f& tf1,
268  const Transform3f& tf2) const {
269  // Empty OcTree is considered free.
270  if (!root1) return false;
271 
276  if (tree1->isNodeFree(root1))
277  return false;
278  else if ((tree1->isNodeUncertain(root1) || s.isUncertain()))
279  return false;
280  else {
281  OBB obb1;
282  convertBV(bv1, tf1, obb1);
283  FCL_REAL sqrDistLowerBound;
284  if (!obb1.overlap(obb2, *crequest, sqrDistLowerBound)) {
285  internal::updateDistanceLowerBoundFromBV(*crequest, *cresult,
286  sqrDistLowerBound);
287  return false;
288  }
289  }
290 
291  if (!tree1->nodeHasChildren(root1)) {
292  assert(tree1->isNodeOccupied(root1)); // it isn't free nor uncertain.
293 
294  Box box;
295  Transform3f box_tf;
296  constructBox(bv1, tf1, box, box_tf);
297 
298  bool contactNotAdded =
299  (cresult->numContacts() >= crequest->num_max_contacts);
300  std::size_t ncontact = ShapeShapeCollide<Box, S>(
301  &box, box_tf, &s, tf2, solver, *crequest, *cresult);
302  assert(ncontact == 0 || ncontact == 1);
303  if (!contactNotAdded && ncontact == 1) {
304  // Update contact information.
305  const Contact& c = cresult->getContact(cresult->numContacts() - 1);
306  cresult->setContact(
307  cresult->numContacts() - 1,
308  Contact(tree1, c.o2, static_cast<int>(root1 - tree1->getRoot()),
309  c.b2, c.pos, c.normal, c.penetration_depth));
310  }
311 
312  return crequest->isSatisfied(*cresult);
313  }
314 
315  for (unsigned int i = 0; i < 8; ++i) {
316  if (tree1->nodeChildExists(root1, i)) {
317  const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
318  AABB child_bv;
319  computeChildBV(bv1, i, child_bv);
320 
321  if (OcTreeShapeIntersectRecurse(tree1, child, child_bv, s, obb2, tf1,
322  tf2))
323  return true;
324  }
325  }
326 
327  return false;
328  }
329 
330  template <typename BV>
331  bool OcTreeMeshDistanceRecurse(const OcTree* tree1,
332  const OcTree::OcTreeNode* root1,
333  const AABB& bv1, const BVHModel<BV>* tree2,
334  unsigned int root2, const Transform3f& tf1,
335  const Transform3f& tf2) const {
336  if (!tree1->nodeHasChildren(root1) && tree2->getBV(root2).isLeaf()) {
337  if (tree1->isNodeOccupied(root1)) {
338  Box box;
339  Transform3f box_tf;
340  constructBox(bv1, tf1, box, box_tf);
341 
342  int primitive_id = tree2->getBV(root2).primitiveId();
343  const Triangle& tri_id = tree2->tri_indices[primitive_id];
344  const Vec3f& p1 = tree2->vertices[tri_id[0]];
345  const Vec3f& p2 = tree2->vertices[tri_id[1]];
346  const Vec3f& p3 = tree2->vertices[tri_id[2]];
347 
348  FCL_REAL dist;
349  Vec3f closest_p1, closest_p2, normal;
350  solver->shapeTriangleInteraction(box, box_tf, p1, p2, p3, tf2, dist,
351  closest_p1, closest_p2, normal);
352 
353  dresult->update(dist, tree1, tree2, (int)(root1 - tree1->getRoot()),
354  primitive_id, closest_p1, closest_p2, normal);
355 
356  return drequest->isSatisfied(*dresult);
357  } else
358  return false;
359  }
360 
361  if (!tree1->isNodeOccupied(root1)) return false;
362 
363  if (tree2->getBV(root2).isLeaf() ||
364  (tree1->nodeHasChildren(root1) &&
365  (bv1.size() > tree2->getBV(root2).bv.size()))) {
366  for (unsigned int i = 0; i < 8; ++i) {
367  if (tree1->nodeChildExists(root1, i)) {
368  const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
369  AABB child_bv;
370  computeChildBV(bv1, i, child_bv);
371 
372  FCL_REAL d;
373  AABB aabb1, aabb2;
374  convertBV(child_bv, tf1, aabb1);
375  convertBV(tree2->getBV(root2).bv, tf2, aabb2);
376  d = aabb1.distance(aabb2);
377 
378  if (d < dresult->min_distance) {
379  if (OcTreeMeshDistanceRecurse(tree1, child, child_bv, tree2, root2,
380  tf1, tf2))
381  return true;
382  }
383  }
384  }
385  } else {
386  FCL_REAL d;
387  AABB aabb1, aabb2;
388  convertBV(bv1, tf1, aabb1);
389  unsigned int child = (unsigned int)tree2->getBV(root2).leftChild();
390  convertBV(tree2->getBV(child).bv, tf2, aabb2);
391  d = aabb1.distance(aabb2);
392 
393  if (d < dresult->min_distance) {
394  if (OcTreeMeshDistanceRecurse(tree1, root1, bv1, tree2, child, tf1,
395  tf2))
396  return true;
397  }
398 
399  child = (unsigned int)tree2->getBV(root2).rightChild();
400  convertBV(tree2->getBV(child).bv, tf2, aabb2);
401  d = aabb1.distance(aabb2);
402 
403  if (d < dresult->min_distance) {
404  if (OcTreeMeshDistanceRecurse(tree1, root1, bv1, tree2, child, tf1,
405  tf2))
406  return true;
407  }
408  }
409 
410  return false;
411  }
412 
414  template <typename BV>
415  bool OcTreeMeshIntersectRecurse(const OcTree* tree1,
416  const OcTree::OcTreeNode* root1,
417  const AABB& bv1, const BVHModel<BV>* tree2,
418  unsigned int root2, const Transform3f& tf1,
419  const Transform3f& tf2) const {
420  // FIXME(jmirabel) I do not understand why the BVHModel was traversed. The
421  // code in this if(!root1) did not output anything so the empty OcTree is
422  // considered free. Should an empty OcTree be considered free ?
423 
424  // Empty OcTree is considered free.
425  if (!root1) return false;
426  BVNode<BV> const& bvn2 = tree2->getBV(root2);
427 
432  if (tree1->isNodeFree(root1))
433  return false;
434  else if ((tree1->isNodeUncertain(root1) || tree2->isUncertain()))
435  return false;
436  else {
437  OBB obb1, obb2;
438  convertBV(bv1, tf1, obb1);
439  convertBV(bvn2.bv, tf2, obb2);
440  FCL_REAL sqrDistLowerBound;
441  if (!obb1.overlap(obb2, *crequest, sqrDistLowerBound)) {
442  internal::updateDistanceLowerBoundFromBV(*crequest, *cresult,
443  sqrDistLowerBound);
444  return false;
445  }
446  }
447 
448  // Check if leaf collides.
449  if (!tree1->nodeHasChildren(root1) && bvn2.isLeaf()) {
450  assert(tree1->isNodeOccupied(root1)); // it isn't free nor uncertain.
451  Box box;
452  Transform3f box_tf;
453  constructBox(bv1, tf1, box, box_tf);
454 
455  int primitive_id = bvn2.primitiveId();
456  const Triangle& tri_id = tree2->tri_indices[primitive_id];
457  const Vec3f& p1 = tree2->vertices[tri_id[0]];
458  const Vec3f& p2 = tree2->vertices[tri_id[1]];
459  const Vec3f& p3 = tree2->vertices[tri_id[2]];
460 
461  Vec3f c1, c2, normal;
462  FCL_REAL distance;
463 
464  bool collision = solver->shapeTriangleInteraction(
465  box, box_tf, p1, p2, p3, tf2, distance, c1, c2, normal);
466  FCL_REAL distToCollision = distance - crequest->security_margin;
467 
468  if (cresult->numContacts() < crequest->num_max_contacts) {
469  if (collision) {
470  cresult->addContact(Contact(tree1, tree2,
471  (int)(root1 - tree1->getRoot()),
472  primitive_id, c1, normal, -distance));
473  } else if (distToCollision < 0) {
474  cresult->addContact(Contact(
475  tree1, tree2, (int)(root1 - tree1->getRoot()), primitive_id,
476  .5 * (c1 + c2), (c2 - c1).normalized(), -distance));
477  }
478  }
479  internal::updateDistanceLowerBoundFromLeaf(*crequest, *cresult,
480  distToCollision, c1, c2);
481 
482  return crequest->isSatisfied(*cresult);
483  }
484 
485  // Determine which tree to traverse first.
486  if (bvn2.isLeaf() ||
487  (tree1->nodeHasChildren(root1) && (bv1.size() > bvn2.bv.size()))) {
488  for (unsigned int i = 0; i < 8; ++i) {
489  if (tree1->nodeChildExists(root1, i)) {
490  const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
491  AABB child_bv;
492  computeChildBV(bv1, i, child_bv);
493 
494  if (OcTreeMeshIntersectRecurse(tree1, child, child_bv, tree2, root2,
495  tf1, tf2))
496  return true;
497  }
498  }
499  } else {
500  if (OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2,
501  (unsigned int)bvn2.leftChild(), tf1, tf2))
502  return true;
503 
504  if (OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2,
505  (unsigned int)bvn2.rightChild(), tf1, tf2))
506  return true;
507  }
508 
509  return false;
510  }
511 
512  bool OcTreeDistanceRecurse(const OcTree* tree1,
513  const OcTree::OcTreeNode* root1, const AABB& bv1,
514  const OcTree* tree2,
515  const OcTree::OcTreeNode* root2, const AABB& bv2,
516  const Transform3f& tf1,
517  const Transform3f& tf2) const {
518  if (!tree1->nodeHasChildren(root1) && !tree2->nodeHasChildren(root2)) {
519  if (tree1->isNodeOccupied(root1) && tree2->isNodeOccupied(root2)) {
520  Box box1, box2;
521  Transform3f box1_tf, box2_tf;
522  constructBox(bv1, tf1, box1, box1_tf);
523  constructBox(bv2, tf2, box2, box2_tf);
524 
525  FCL_REAL dist;
526  Vec3f closest_p1, closest_p2, normal;
527  solver->shapeDistance(box1, box1_tf, box2, box2_tf, dist, closest_p1,
528  closest_p2, normal);
529 
530  dresult->update(dist, tree1, tree2, (int)(root1 - tree1->getRoot()),
531  (int)(root2 - tree2->getRoot()), closest_p1, closest_p2,
532  normal);
533 
534  return drequest->isSatisfied(*dresult);
535  } else
536  return false;
537  }
538 
539  if (!tree1->isNodeOccupied(root1) || !tree2->isNodeOccupied(root2))
540  return false;
541 
542  if (!tree2->nodeHasChildren(root2) ||
543  (tree1->nodeHasChildren(root1) && (bv1.size() > bv2.size()))) {
544  for (unsigned int i = 0; i < 8; ++i) {
545  if (tree1->nodeChildExists(root1, i)) {
546  const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
547  AABB child_bv;
548  computeChildBV(bv1, i, child_bv);
549 
550  FCL_REAL d;
551  AABB aabb1, aabb2;
552  convertBV(bv1, tf1, aabb1);
553  convertBV(bv2, tf2, aabb2);
554  d = aabb1.distance(aabb2);
555 
556  if (d < dresult->min_distance) {
557  if (OcTreeDistanceRecurse(tree1, child, child_bv, tree2, root2, bv2,
558  tf1, tf2))
559  return true;
560  }
561  }
562  }
563  } else {
564  for (unsigned int i = 0; i < 8; ++i) {
565  if (tree2->nodeChildExists(root2, i)) {
566  const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i);
567  AABB child_bv;
568  computeChildBV(bv2, i, child_bv);
569 
570  FCL_REAL d;
571  AABB aabb1, aabb2;
572  convertBV(bv1, tf1, aabb1);
573  convertBV(bv2, tf2, aabb2);
574  d = aabb1.distance(aabb2);
575 
576  if (d < dresult->min_distance) {
577  if (OcTreeDistanceRecurse(tree1, root1, bv1, tree2, child, child_bv,
578  tf1, tf2))
579  return true;
580  }
581  }
582  }
583  }
584 
585  return false;
586  }
587 
588  bool OcTreeIntersectRecurse(const OcTree* tree1,
589  const OcTree::OcTreeNode* root1, const AABB& bv1,
590  const OcTree* tree2,
591  const OcTree::OcTreeNode* root2, const AABB& bv2,
592  const Transform3f& tf1,
593  const Transform3f& tf2) const {
594  // Empty OcTree is considered free.
595  if (!root1) return false;
596  if (!root2) return false;
597 
602  if (tree1->isNodeFree(root1) || tree2->isNodeFree(root2))
603  return false;
604  else if ((tree1->isNodeUncertain(root1) || tree2->isNodeUncertain(root2)))
605  return false;
606 
607  bool bothAreLeaves =
608  (!tree1->nodeHasChildren(root1) && !tree2->nodeHasChildren(root2));
609  if (!bothAreLeaves || !crequest->enable_contact) {
610  OBB obb1, obb2;
611  convertBV(bv1, tf1, obb1);
612  convertBV(bv2, tf2, obb2);
613  FCL_REAL sqrDistLowerBound;
614  if (!obb1.overlap(obb2, *crequest, sqrDistLowerBound)) {
615  if (cresult->distance_lower_bound > 0 &&
616  sqrDistLowerBound <
617  cresult->distance_lower_bound * cresult->distance_lower_bound)
618  cresult->distance_lower_bound =
619  sqrt(sqrDistLowerBound) - crequest->security_margin;
620  return false;
621  }
622  if (!crequest->enable_contact) { // Overlap
623  if (cresult->numContacts() < crequest->num_max_contacts)
624  cresult->addContact(
625  Contact(tree1, tree2, static_cast<int>(root1 - tree1->getRoot()),
626  static_cast<int>(root2 - tree2->getRoot())));
627  return crequest->isSatisfied(*cresult);
628  }
629  }
630 
631  // Both node are leaves
632  if (bothAreLeaves) {
633  assert(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 distance;
641  Vec3f c1, c2, normal;
642  bool collision = solver->shapeDistance(box1, box1_tf, box2, box2_tf,
643  distance, c1, c2, normal);
644  FCL_REAL distToCollision = distance - crequest->security_margin;
645 
646  if (cresult->numContacts() < crequest->num_max_contacts) {
647  if (collision)
648  cresult->addContact(
649  Contact(tree1, tree2, static_cast<int>(root1 - tree1->getRoot()),
650  static_cast<int>(root2 - tree2->getRoot()), c1, normal,
651  -distance));
652  else if (distToCollision <= 0)
653  cresult->addContact(
654  Contact(tree1, tree2, static_cast<int>(root1 - tree1->getRoot()),
655  static_cast<int>(root2 - tree2->getRoot()),
656  .5 * (c1 + c2), (c2 - c1).normalized(), -distance));
657  }
658  internal::updateDistanceLowerBoundFromLeaf(*crequest, *cresult,
659  distToCollision, c1, c2);
660 
661  return crequest->isSatisfied(*cresult);
662  }
663 
664  // Determine which tree to traverse first.
665  if (!tree2->nodeHasChildren(root2) ||
666  (tree1->nodeHasChildren(root1) && (bv1.size() > bv2.size()))) {
667  for (unsigned int i = 0; i < 8; ++i) {
668  if (tree1->nodeChildExists(root1, i)) {
669  const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
670  AABB child_bv;
671  computeChildBV(bv1, i, child_bv);
672 
673  if (OcTreeIntersectRecurse(tree1, child, child_bv, tree2, root2, bv2,
674  tf1, tf2))
675  return true;
676  }
677  }
678  } else {
679  for (unsigned int i = 0; i < 8; ++i) {
680  if (tree2->nodeChildExists(root2, i)) {
681  const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i);
682  AABB child_bv;
683  computeChildBV(bv2, i, child_bv);
684 
685  if (OcTreeIntersectRecurse(tree1, root1, bv1, tree2, child, child_bv,
686  tf1, tf2))
687  return true;
688  }
689  }
690  }
691 
692  return false;
693  }
694 };
695 
698 
700 class HPP_FCL_DLLAPI OcTreeCollisionTraversalNode
701  : public CollisionTraversalNodeBase {
702  public:
703  OcTreeCollisionTraversalNode(const CollisionRequest& request)
704  : CollisionTraversalNodeBase(request) {
705  model1 = NULL;
706  model2 = NULL;
707 
708  otsolver = NULL;
709  }
710 
711  bool BVDisjoints(unsigned, unsigned, FCL_REAL&) const { return false; }
712 
713  void leafCollides(unsigned, unsigned, FCL_REAL& sqrDistLowerBound) const {
714  otsolver->OcTreeIntersect(model1, model2, tf1, tf2, request, *result);
715  sqrDistLowerBound = std::max((FCL_REAL)0, result->distance_lower_bound);
716  sqrDistLowerBound *= sqrDistLowerBound;
717  }
718 
719  const OcTree* model1;
720  const OcTree* model2;
721 
722  Transform3f tf1, tf2;
723 
724  const OcTreeSolver* otsolver;
725 };
726 
728 template <typename S>
729 class HPP_FCL_DLLAPI ShapeOcTreeCollisionTraversalNode
730  : public CollisionTraversalNodeBase {
731  public:
732  ShapeOcTreeCollisionTraversalNode(const CollisionRequest& request)
733  : CollisionTraversalNodeBase(request) {
734  model1 = NULL;
735  model2 = NULL;
736 
737  otsolver = NULL;
738  }
739 
740  bool BVDisjoints(unsigned int, unsigned int, FCL_REAL&) const {
741  return false;
742  }
743 
744  void leafCollides(unsigned int, unsigned int,
745  FCL_REAL& sqrDistLowerBound) const {
746  otsolver->OcTreeShapeIntersect(model2, *model1, tf2, tf1, request, *result);
747  sqrDistLowerBound = std::max((FCL_REAL)0, result->distance_lower_bound);
748  sqrDistLowerBound *= sqrDistLowerBound;
749  }
750 
751  const S* model1;
752  const OcTree* model2;
753 
754  Transform3f tf1, tf2;
755 
756  const OcTreeSolver* otsolver;
757 };
758 
760 
761 template <typename S>
762 class HPP_FCL_DLLAPI OcTreeShapeCollisionTraversalNode
763  : public CollisionTraversalNodeBase {
764  public:
765  OcTreeShapeCollisionTraversalNode(const CollisionRequest& request)
766  : CollisionTraversalNodeBase(request) {
767  model1 = NULL;
768  model2 = NULL;
769 
770  otsolver = NULL;
771  }
772 
773  bool BVDisjoints(unsigned int, unsigned int, fcl::FCL_REAL&) const {
774  return false;
775  }
776 
777  void leafCollides(unsigned int, unsigned int,
778  FCL_REAL& sqrDistLowerBound) const {
779  otsolver->OcTreeShapeIntersect(model1, *model2, tf1, tf2, request, *result);
780  sqrDistLowerBound = std::max((FCL_REAL)0, result->distance_lower_bound);
781  sqrDistLowerBound *= sqrDistLowerBound;
782  }
783 
784  const OcTree* model1;
785  const S* model2;
786 
787  Transform3f tf1, tf2;
788 
789  const OcTreeSolver* otsolver;
790 };
791 
793 template <typename BV>
794 class HPP_FCL_DLLAPI MeshOcTreeCollisionTraversalNode
795  : public CollisionTraversalNodeBase {
796  public:
797  MeshOcTreeCollisionTraversalNode(const CollisionRequest& request)
798  : CollisionTraversalNodeBase(request) {
799  model1 = NULL;
800  model2 = NULL;
801 
802  otsolver = NULL;
803  }
804 
805  bool BVDisjoints(unsigned int, unsigned int, FCL_REAL&) const {
806  return false;
807  }
808 
809  void leafCollides(unsigned int, unsigned int,
810  FCL_REAL& sqrDistLowerBound) const {
811  otsolver->OcTreeMeshIntersect(model2, model1, tf2, tf1, request, *result);
812  sqrDistLowerBound = std::max((FCL_REAL)0, result->distance_lower_bound);
813  sqrDistLowerBound *= sqrDistLowerBound;
814  }
815 
816  const BVHModel<BV>* model1;
817  const OcTree* model2;
818 
819  Transform3f tf1, tf2;
820 
821  const OcTreeSolver* otsolver;
822 };
823 
825 template <typename BV>
826 class HPP_FCL_DLLAPI OcTreeMeshCollisionTraversalNode
827  : public CollisionTraversalNodeBase {
828  public:
829  OcTreeMeshCollisionTraversalNode(const CollisionRequest& request)
830  : CollisionTraversalNodeBase(request) {
831  model1 = NULL;
832  model2 = NULL;
833 
834  otsolver = NULL;
835  }
836 
837  bool BVDisjoints(unsigned int, unsigned int, FCL_REAL&) const {
838  return false;
839  }
840 
841  void leafCollides(unsigned int, unsigned int,
842  FCL_REAL& sqrDistLowerBound) const {
843  std::cout << "leafCollides" << std::endl;
844  otsolver->OcTreeMeshIntersect(model1, model2, tf1, tf2, request, *result);
845  sqrDistLowerBound = std::max((FCL_REAL)0, result->distance_lower_bound);
846  sqrDistLowerBound *= sqrDistLowerBound;
847  }
848 
849  const OcTree* model1;
850  const BVHModel<BV>* model2;
851 
852  Transform3f tf1, tf2;
853 
854  const OcTreeSolver* otsolver;
855 };
856 
858 
861 
863 class HPP_FCL_DLLAPI OcTreeDistanceTraversalNode
864  : public DistanceTraversalNodeBase {
865  public:
866  OcTreeDistanceTraversalNode() {
867  model1 = NULL;
868  model2 = NULL;
869 
870  otsolver = NULL;
871  }
872 
873  FCL_REAL BVDistanceLowerBound(unsigned, unsigned) const { return -1; }
874 
875  bool BVDistanceLowerBound(unsigned, unsigned, FCL_REAL&) const {
876  return false;
877  }
878 
879  void leafComputeDistance(unsigned, unsigned int) const {
880  otsolver->OcTreeDistance(model1, model2, tf1, tf2, request, *result);
881  }
882 
883  const OcTree* model1;
884  const OcTree* model2;
885 
886  const OcTreeSolver* otsolver;
887 };
888 
890 template <typename Shape>
891 class HPP_FCL_DLLAPI ShapeOcTreeDistanceTraversalNode
892  : public DistanceTraversalNodeBase {
893  public:
894  ShapeOcTreeDistanceTraversalNode() {
895  model1 = NULL;
896  model2 = NULL;
897 
898  otsolver = NULL;
899  }
900 
901  FCL_REAL BVDistanceLowerBound(unsigned int, unsigned int) const { return -1; }
902 
903  void leafComputeDistance(unsigned int, unsigned int) const {
904  otsolver->OcTreeShapeDistance(model2, *model1, tf2, tf1, request, *result);
905  }
906 
907  const Shape* model1;
908  const OcTree* model2;
909 
910  const OcTreeSolver* otsolver;
911 };
912 
914 template <typename Shape>
915 class HPP_FCL_DLLAPI OcTreeShapeDistanceTraversalNode
916  : public DistanceTraversalNodeBase {
917  public:
918  OcTreeShapeDistanceTraversalNode() {
919  model1 = NULL;
920  model2 = NULL;
921 
922  otsolver = NULL;
923  }
924 
925  FCL_REAL BVDistanceLowerBound(unsigned int, unsigned int) const { return -1; }
926 
927  void leafComputeDistance(unsigned int, unsigned int) const {
928  otsolver->OcTreeShapeDistance(model1, *model2, tf1, tf2, request, *result);
929  }
930 
931  const OcTree* model1;
932  const Shape* model2;
933 
934  const OcTreeSolver* otsolver;
935 };
936 
938 template <typename BV>
939 class HPP_FCL_DLLAPI MeshOcTreeDistanceTraversalNode
940  : public DistanceTraversalNodeBase {
941  public:
942  MeshOcTreeDistanceTraversalNode() {
943  model1 = NULL;
944  model2 = NULL;
945 
946  otsolver = NULL;
947  }
948 
949  FCL_REAL BVDistanceLowerBound(unsigned int, unsigned int) const { return -1; }
950 
951  void leafComputeDistance(unsigned int, unsigned int) const {
952  otsolver->OcTreeMeshDistance(model2, model1, tf2, tf1, request, *result);
953  }
954 
955  const BVHModel<BV>* model1;
956  const OcTree* model2;
957 
958  const OcTreeSolver* otsolver;
959 };
960 
962 template <typename BV>
963 class HPP_FCL_DLLAPI OcTreeMeshDistanceTraversalNode
964  : public DistanceTraversalNodeBase {
965  public:
966  OcTreeMeshDistanceTraversalNode() {
967  model1 = NULL;
968  model2 = NULL;
969 
970  otsolver = NULL;
971  }
972 
973  FCL_REAL BVDistanceLowerBound(unsigned int, unsigned int) const { return -1; }
974 
975  void leafComputeDistance(unsigned int, unsigned int) const {
976  otsolver->OcTreeMeshDistance(model1, model2, tf1, tf2, request, *result);
977  }
978 
979  const OcTree* model1;
980  const BVHModel<BV>* model2;
981 
982  const OcTreeSolver* otsolver;
983 };
984 
986 
987 } // namespace fcl
988 
989 } // namespace hpp
990 
992 
993 #endif
shape_shape_func.h
hpp::fcl::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
hpp::fcl::internal::updateDistanceLowerBoundFromLeaf
void updateDistanceLowerBoundFromLeaf(const CollisionRequest &, CollisionResult &res, const FCL_REAL &distance, const Vec3f &p0, const Vec3f &p1)
Definition: collision_data.h:547
collision_manager.box
box
Definition: collision_manager.py:11
gjk.tf1
tuple tf1
Definition: test/scripts/gjk.py:27
hpp::fcl::distance
HPP_FCL_DLLAPI FCL_REAL distance(const Matrix3f &R0, const Vec3f &T0, const kIOS &b1, const kIOS &b2, Vec3f *P=NULL, Vec3f *Q=NULL)
Approximate distance between two kIOS bounding volumes.
Definition: kIOS.cpp:181
traversal_node_base.h
geometric_shapes_utility.h
narrowphase.h
hpp::fcl::OcTree::OcTreeNode
octomap::OcTreeNode OcTreeNode
Definition: octree.h:63
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:65
hpp::fcl::internal::updateDistanceLowerBoundFromBV
void updateDistanceLowerBoundFromBV(const CollisionRequest &, CollisionResult &res, const FCL_REAL &sqrDistLowerBound)
Definition: collision_data.h:538
hpp::fcl::computeChildBV
static void computeChildBV(const AABB &root_bv, unsigned int i, AABB &child_bv)
compute the bounding volume of an octree node's i-th child
Definition: octree.h:239
c
c
hpp::fcl::convertBV
static void convertBV(const BV1 &bv1, const Transform3f &tf1, BV2 &bv2)
Convert a bounding volume of type BV1 in configuration tf1 to bounding volume of type BV2 in identity...
Definition: BV.h:277
collision_data.h
hpp::fcl::constructBox
HPP_FCL_DLLAPI void constructBox(const AABB &bv, Box &box, Transform3f &tf)
construct a box shape (with a configuration) from a given bounding volume
Definition: geometric_shapes_utility.cpp:911
hpp
Main namespace.
Definition: broadphase_bruteforce.h:44
gjk.tf2
tuple tf2
Definition: test/scripts/gjk.py:36
octree.p1
tuple p1
Definition: octree.py:54
collision
Definition: python_unit/collision.py:1
BVH_model.h
c2
c2
octree.h
hpp::fcl::DistanceResult::NONE
static const int NONE
invalid contact primitive information
Definition: collision_data.h:451

hpp-fcl
Author(s):
autogenerated on Fri Aug 2 2024 02:45:15