FCL  0.6.0
Flexible Collision Library
octree_solver-inl.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011-2014, Willow Garage, Inc.
5  * Copyright (c) 2014-2016, 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 FCL_TRAVERSAL_OCTREE_OCTREESOLVER_INL_H
39 #define FCL_TRAVERSAL_OCTREE_OCTREESOLVER_INL_H
40 
41 #include "fcl/narrowphase/detail/traversal/octree/octree_solver.h"
42 
43 #include "fcl/geometry/shape/utility.h"
44 
45 namespace fcl
46 {
47 
48 namespace detail
49 {
50 
51 //==============================================================================
52 template <typename NarrowPhaseSolver>
53 OcTreeSolver<NarrowPhaseSolver>::OcTreeSolver(
54  const NarrowPhaseSolver* solver_)
55  : solver(solver_),
56  crequest(nullptr),
57  drequest(nullptr),
58  cresult(nullptr),
59  dresult(nullptr)
60 {
61  // Do nothing
62 }
63 
64 //==============================================================================
65 template <typename NarrowPhaseSolver>
67  const OcTree<S>* tree1,
68  const OcTree<S>* tree2,
69  const Transform3<S>& tf1,
70  const Transform3<S>& tf2,
71  const CollisionRequest<S>& request_,
72  CollisionResult<S>& result_) const
73 {
74  crequest = &request_;
75  cresult = &result_;
76 
77  OcTreeIntersectRecurse(tree1, tree1->getRoot(), tree1->getRootBV(),
78  tree2, tree2->getRoot(), tree2->getRootBV(),
79  tf1, tf2);
80 }
81 
82 //==============================================================================
83 template <typename NarrowPhaseSolver>
85  const OcTree<S>* tree1,
86  const OcTree<S>* tree2,
87  const Transform3<S>& tf1,
88  const Transform3<S>& tf2,
89  const DistanceRequest<S>& request_,
90  DistanceResult<S>& result_) const
91 {
92  drequest = &request_;
93  dresult = &result_;
94 
95  OcTreeDistanceRecurse(tree1, tree1->getRoot(), tree1->getRootBV(),
96  tree2, tree2->getRoot(), tree2->getRootBV(),
97  tf1, tf2);
98 }
99 
100 //==============================================================================
101 template <typename NarrowPhaseSolver>
102 template <typename BV>
104  const OcTree<S>* tree1,
105  const BVHModel<BV>* tree2,
106  const Transform3<S>& tf1,
107  const Transform3<S>& tf2,
108  const CollisionRequest<S>& request_,
109  CollisionResult<S>& 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 
119 //==============================================================================
120 template <typename NarrowPhaseSolver>
121 template <typename BV>
123  const OcTree<S>* tree1,
124  const BVHModel<BV>* tree2,
125  const Transform3<S>& tf1,
126  const Transform3<S>& tf2,
127  const DistanceRequest<S>& request_,
128  DistanceResult<S>& result_) const
129 {
130  drequest = &request_;
131  dresult = &result_;
132 
133  OcTreeMeshDistanceRecurse(tree1, tree1->getRoot(), tree1->getRootBV(),
134  tree2, 0,
135  tf1, tf2);
136 }
137 
138 //==============================================================================
139 template <typename NarrowPhaseSolver>
140 template <typename BV>
142  const BVHModel<BV>* tree1,
143  const OcTree<S>* tree2,
144  const Transform3<S>& tf1,
145  const Transform3<S>& tf2,
146  const CollisionRequest<S>& request_,
147  CollisionResult<S>& result_) const
148 
149 {
150  crequest = &request_;
151  cresult = &result_;
152 
153  OcTreeMeshIntersectRecurse(tree2, tree2->getRoot(), tree2->getRootBV(),
154  tree1, 0,
155  tf2, tf1);
156 }
157 
158 //==============================================================================
160 template <typename NarrowPhaseSolver>
161 template <typename BV>
163  const BVHModel<BV>* tree1,
164  const OcTree<S>* tree2,
165  const Transform3<S>& tf1,
166  const Transform3<S>& tf2,
167  const DistanceRequest<S>& request_,
168  DistanceResult<S>& result_) const
169 {
170  drequest = &request_;
171  dresult = &result_;
172 
173  OcTreeMeshDistanceRecurse(tree1, 0,
174  tree2, tree2->getRoot(), tree2->getRootBV(),
175  tf1, tf2);
176 }
177 
178 //==============================================================================
180 template <typename NarrowPhaseSolver>
181 template <typename Shape>
183  const OcTree<S>* tree,
184  const Shape& s,
185  const Transform3<S>& tf1,
186  const Transform3<S>& tf2,
187  const CollisionRequest<S>& request_,
188  CollisionResult<S>& result_) const
189 {
190  crequest = &request_;
191  cresult = &result_;
192 
193  AABB<S> bv2;
194  computeBV(s, Transform3<S>::Identity(), bv2);
195  OBB<S> obb2;
196  convertBV(bv2, tf2, obb2);
197  OcTreeShapeIntersectRecurse(tree, tree->getRoot(), tree->getRootBV(),
198  s, obb2,
199  tf1, tf2);
200 
201 }
202 
203 //==============================================================================
205 template <typename NarrowPhaseSolver>
206 template <typename Shape>
208  const Shape& s,
209  const OcTree<S>* tree,
210  const Transform3<S>& tf1,
211  const Transform3<S>& tf2,
212  const CollisionRequest<S>& request_,
213  CollisionResult<S>& result_) const
214 {
215  crequest = &request_;
216  cresult = &result_;
217 
218  AABB<S> bv1;
219  computeBV(s, Transform3<S>::Identity(), bv1);
220  OBB<S> obb1;
221  convertBV(bv1, tf1, obb1);
222  OcTreeShapeIntersectRecurse(tree, tree->getRoot(), tree->getRootBV(),
223  s, obb1,
224  tf2, tf1);
225 }
226 
227 //==============================================================================
229 template <typename NarrowPhaseSolver>
230 template <typename Shape>
232  const OcTree<S>* tree,
233  const Shape& s,
234  const Transform3<S>& tf1,
235  const Transform3<S>& tf2,
236  const DistanceRequest<S>& request_,
237  DistanceResult<S>& result_) const
238 {
239  drequest = &request_;
240  dresult = &result_;
241 
242  AABB<S> aabb2;
243  computeBV(s, tf2, aabb2);
244  OcTreeShapeDistanceRecurse(tree, tree->getRoot(), tree->getRootBV(),
245  s, aabb2,
246  tf1, tf2);
247 }
248 
249 //==============================================================================
251 template <typename NarrowPhaseSolver>
252 template <typename Shape>
254  const Shape& s,
255  const OcTree<S>* tree,
256  const Transform3<S>& tf1,
257  const Transform3<S>& tf2,
258  const DistanceRequest<S>& request_,
259  DistanceResult<S>& result_) const
260 {
261  drequest = &request_;
262  dresult = &result_;
263 
264  AABB<S> aabb1;
265  computeBV(s, tf1, aabb1);
266  OcTreeShapeDistanceRecurse(tree, tree->getRoot(), tree->getRootBV(),
267  s, aabb1,
268  tf2, tf1);
269 }
270 
271 //==============================================================================
272 template <typename NarrowPhaseSolver>
273 template <typename Shape>
274 bool OcTreeSolver<NarrowPhaseSolver>::OcTreeShapeDistanceRecurse(const OcTree<S>* tree1, const typename OcTree<S>::OcTreeNode* root1, const AABB<S>& bv1,
275  const Shape& s, const AABB<S>& aabb2,
276  const Transform3<S>& tf1, const Transform3<S>& tf2) const
277 {
278  if(!tree1->nodeHasChildren(root1))
279  {
280  if(tree1->isNodeOccupied(root1))
281  {
282  Box<S> box;
283  Transform3<S> box_tf;
284  constructBox(bv1, tf1, box, box_tf);
285 
286  S dist;
287  // NOTE(JS): The closest points are set to zeros in order to suppress the
288  // maybe-uninitialized warning. It seems the warnings occur since
289  // NarrowPhaseSolver::shapeDistance() conditionally set the closest points.
290  // If this wasn't intentional then please remove the initialization of the
291  // closest points, and change the function NarrowPhaseSolver::shapeDistance()
292  // to always set the closest points.
293  Vector3<S> closest_p1 = Vector3<S>::Zero();
294  Vector3<S> closest_p2 = Vector3<S>::Zero();
295  solver->shapeDistance(box, box_tf, s, tf2, &dist, &closest_p1, &closest_p2);
296 
297  dresult->update(dist, tree1, &s, root1 - tree1->getRoot(), DistanceResult<S>::NONE, closest_p1, closest_p2);
298 
299  return drequest->isSatisfied(*dresult);
300  }
301  else
302  return false;
303  }
304 
305  if(!tree1->isNodeOccupied(root1)) return false;
306 
307  for(unsigned int i = 0; i < 8; ++i)
308  {
309  if(tree1->nodeChildExists(root1, i))
310  {
311  const typename OcTree<S>::OcTreeNode* child = tree1->getNodeChild(root1, i);
312  AABB<S> child_bv;
313  computeChildBV(bv1, i, child_bv);
314 
315  AABB<S> aabb1;
316  convertBV(child_bv, tf1, aabb1);
317  S d = aabb1.distance(aabb2);
318  if(d < dresult->min_distance)
319  {
320  if(OcTreeShapeDistanceRecurse(tree1, child, child_bv, s, aabb2, tf1, tf2))
321  return true;
322  }
323  }
324  }
325 
326  return false;
327 }
328 
329 //==============================================================================
330 template <typename NarrowPhaseSolver>
331 template <typename Shape>
332 bool OcTreeSolver<NarrowPhaseSolver>::OcTreeShapeIntersectRecurse(const OcTree<S>* tree1, const typename OcTree<S>::OcTreeNode* root1, const AABB<S>& bv1,
333  const Shape& s, const OBB<S>& obb2,
334  const Transform3<S>& tf1, const Transform3<S>& tf2) const
335 {
336  if(!root1)
337  {
338  OBB<S> obb1;
339  convertBV(bv1, tf1, obb1);
340  if(obb1.overlap(obb2))
341  {
342  Box<S> box;
343  Transform3<S> box_tf;
344  constructBox(bv1, tf1, box, box_tf);
345 
346  if(solver->shapeIntersect(box, box_tf, s, tf2, nullptr))
347  {
348  AABB<S> overlap_part;
349  AABB<S> aabb1, aabb2;
350  computeBV(box, box_tf, aabb1);
351  computeBV(s, tf2, aabb2);
352  aabb1.overlap(aabb2, overlap_part);
353  cresult->addCostSource(CostSource<S>(overlap_part, tree1->getOccupancyThres() * s.cost_density), crequest->num_max_cost_sources);
354  }
355  }
356 
357  return false;
358  }
359  else if(!tree1->nodeHasChildren(root1))
360  {
361  if(tree1->isNodeOccupied(root1) && s.isOccupied()) // occupied area
362  {
363  OBB<S> obb1;
364  convertBV(bv1, tf1, obb1);
365  if(obb1.overlap(obb2))
366  {
367  Box<S> box;
368  Transform3<S> box_tf;
369  constructBox(bv1, tf1, box, box_tf);
370 
371  bool is_intersect = false;
372  if(!crequest->enable_contact)
373  {
374  if(solver->shapeIntersect(box, box_tf, s, tf2, nullptr))
375  {
376  is_intersect = true;
377  if(cresult->numContacts() < crequest->num_max_contacts)
378  cresult->addContact(Contact<S>(tree1, &s, root1 - tree1->getRoot(), Contact<S>::NONE));
379  }
380  }
381  else
382  {
383  std::vector<ContactPoint<S>> contacts;
384  if(solver->shapeIntersect(box, box_tf, s, tf2, &contacts))
385  {
386  is_intersect = true;
387  if(crequest->num_max_contacts > cresult->numContacts())
388  {
389  const size_t free_space = crequest->num_max_contacts - cresult->numContacts();
390  size_t num_adding_contacts;
391 
392  // If the free space is not enough to add all the new contacts, we add contacts in descent order of penetration depth.
393  if (free_space < contacts.size())
394  {
395  std::partial_sort(contacts.begin(), contacts.begin() + free_space, contacts.end(), std::bind(comparePenDepth<S>, std::placeholders::_2, std::placeholders::_1));
396  num_adding_contacts = free_space;
397  }
398  else
399  {
400  num_adding_contacts = contacts.size();
401  }
402 
403  for(size_t i = 0; i < num_adding_contacts; ++i)
404  cresult->addContact(Contact<S>(tree1, &s, root1 - tree1->getRoot(), Contact<S>::NONE, contacts[i].pos, contacts[i].normal, contacts[i].penetration_depth));
405  }
406  }
407  }
408 
409  if(is_intersect && crequest->enable_cost)
410  {
411  AABB<S> overlap_part;
412  AABB<S> aabb1, aabb2;
413  computeBV(box, box_tf, aabb1);
414  computeBV(s, tf2, aabb2);
415  aabb1.overlap(aabb2, overlap_part);
416  }
417 
418  return crequest->isSatisfied(*cresult);
419  }
420  else return false;
421  }
422  else if(!tree1->isNodeFree(root1) && !s.isFree() && crequest->enable_cost) // uncertain area
423  {
424  OBB<S> obb1;
425  convertBV(bv1, tf1, obb1);
426  if(obb1.overlap(obb2))
427  {
428  Box<S> box;
429  Transform3<S> box_tf;
430  constructBox(bv1, tf1, box, box_tf);
431 
432  if(solver->shapeIntersect(box, box_tf, s, tf2, nullptr))
433  {
434  AABB<S> overlap_part;
435  AABB<S> aabb1, aabb2;
436  computeBV(box, box_tf, aabb1);
437  computeBV(s, tf2, aabb2);
438  aabb1.overlap(aabb2, overlap_part);
439  }
440  }
441 
442  return false;
443  }
444  else // free area
445  return false;
446  }
447 
451  if(tree1->isNodeFree(root1) || s.isFree()) return false;
452  else if((tree1->isNodeUncertain(root1) || s.isUncertain()) && !crequest->enable_cost) return false;
453  else
454  {
455  OBB<S> obb1;
456  convertBV(bv1, tf1, obb1);
457  if(!obb1.overlap(obb2)) return false;
458  }
459 
460  for(unsigned int i = 0; i < 8; ++i)
461  {
462  if(tree1->nodeChildExists(root1, i))
463  {
464  const typename OcTree<S>::OcTreeNode* child = tree1->getNodeChild(root1, i);
465  AABB<S> child_bv;
466  computeChildBV(bv1, i, child_bv);
467 
468  if(OcTreeShapeIntersectRecurse(tree1, child, child_bv, s, obb2, tf1, tf2))
469  return true;
470  }
471  else if(!s.isFree() && crequest->enable_cost)
472  {
473  AABB<S> child_bv;
474  computeChildBV(bv1, i, child_bv);
475 
476  if(OcTreeShapeIntersectRecurse(tree1, nullptr, child_bv, s, obb2, tf1, tf2))
477  return true;
478  }
479  }
480 
481  return false;
482 }
483 
484 //==============================================================================
485 template <typename NarrowPhaseSolver>
486 template <typename BV>
487 bool OcTreeSolver<NarrowPhaseSolver>::OcTreeMeshDistanceRecurse(const OcTree<S>* tree1, const typename OcTree<S>::OcTreeNode* root1, const AABB<S>& bv1,
488  const BVHModel<BV>* tree2, int root2,
489  const Transform3<S>& tf1, const Transform3<S>& tf2) const
490 {
491  if(!tree1->nodeHasChildren(root1) && tree2->getBV(root2).isLeaf())
492  {
493  if(tree1->isNodeOccupied(root1))
494  {
495  Box<S> box;
496  Transform3<S> box_tf;
497  constructBox(bv1, tf1, box, box_tf);
498 
499  int primitive_id = tree2->getBV(root2).primitiveId();
500  const Triangle& tri_id = tree2->tri_indices[primitive_id];
501  const Vector3<S>& p1 = tree2->vertices[tri_id[0]];
502  const Vector3<S>& p2 = tree2->vertices[tri_id[1]];
503  const Vector3<S>& p3 = tree2->vertices[tri_id[2]];
504 
505  S dist;
506  Vector3<S> closest_p1, closest_p2;
507  solver->shapeTriangleDistance(box, box_tf, p1, p2, p3, tf2, &dist, &closest_p1, &closest_p2);
508 
509  dresult->update(dist, tree1, tree2, root1 - tree1->getRoot(), primitive_id);
510 
511  return drequest->isSatisfied(*dresult);
512  }
513  else
514  return false;
515  }
516 
517  if(!tree1->isNodeOccupied(root1)) return false;
518 
519  if(tree2->getBV(root2).isLeaf() || (tree1->nodeHasChildren(root1) && (bv1.size() > tree2->getBV(root2).bv.size())))
520  {
521  for(unsigned int i = 0; i < 8; ++i)
522  {
523  if(tree1->nodeChildExists(root1, i))
524  {
525  const typename OcTree<S>::OcTreeNode* child = tree1->getNodeChild(root1, i);
526  AABB<S> child_bv;
527  computeChildBV(bv1, i, child_bv);
528 
529  S d;
530  AABB<S> aabb1, aabb2;
531  convertBV(child_bv, tf1, aabb1);
532  convertBV(tree2->getBV(root2).bv, tf2, aabb2);
533  d = aabb1.distance(aabb2);
534 
535  if(d < dresult->min_distance)
536  {
537  if(OcTreeMeshDistanceRecurse(tree1, child, child_bv, tree2, root2, tf1, tf2))
538  return true;
539  }
540  }
541  }
542  }
543  else
544  {
545  S d;
546  AABB<S> aabb1, aabb2;
547  convertBV(bv1, tf1, aabb1);
548  int child = tree2->getBV(root2).leftChild();
549  convertBV(tree2->getBV(child).bv, tf2, aabb2);
550  d = aabb1.distance(aabb2);
551 
552  if(d < dresult->min_distance)
553  {
554  if(OcTreeMeshDistanceRecurse(tree1, root1, bv1, tree2, child, tf1, tf2))
555  return true;
556  }
557 
558  child = tree2->getBV(root2).rightChild();
559  convertBV(tree2->getBV(child).bv, tf2, aabb2);
560  d = aabb1.distance(aabb2);
561 
562  if(d < dresult->min_distance)
563  {
564  if(OcTreeMeshDistanceRecurse(tree1, root1, bv1, tree2, child, tf1, tf2))
565  return true;
566  }
567  }
568 
569  return false;
570 }
571 
572 //==============================================================================
573 template <typename NarrowPhaseSolver>
574 template <typename BV>
575 bool OcTreeSolver<NarrowPhaseSolver>::OcTreeMeshIntersectRecurse(const OcTree<S>* tree1, const typename OcTree<S>::OcTreeNode* root1, const AABB<S>& bv1,
576  const BVHModel<BV>* tree2, int root2,
577  const Transform3<S>& tf1, const Transform3<S>& tf2) const
578 {
579  if(!root1)
580  {
581  if(tree2->getBV(root2).isLeaf())
582  {
583  OBB<S> obb1, obb2;
584  convertBV(bv1, tf1, obb1);
585  convertBV(tree2->getBV(root2).bv, tf2, obb2);
586  if(obb1.overlap(obb2))
587  {
588  Box<S> box;
589  Transform3<S> box_tf;
590  constructBox(bv1, tf1, box, box_tf);
591 
592  int primitive_id = tree2->getBV(root2).primitiveId();
593  const Triangle& tri_id = tree2->tri_indices[primitive_id];
594  const Vector3<S>& p1 = tree2->vertices[tri_id[0]];
595  const Vector3<S>& p2 = tree2->vertices[tri_id[1]];
596  const Vector3<S>& p3 = tree2->vertices[tri_id[2]];
597 
598  if(solver->shapeTriangleIntersect(box, box_tf, p1, p2, p3, tf2, nullptr, nullptr, nullptr))
599  {
600  AABB<S> overlap_part;
601  AABB<S> aabb1;
602  computeBV(box, box_tf, aabb1);
603  AABB<S> aabb2(tf2 * p1, tf2 * p2, tf2 * p3);
604  aabb1.overlap(aabb2, overlap_part);
605  cresult->addCostSource(CostSource<S>(overlap_part, tree1->getOccupancyThres() * tree2->cost_density), crequest->num_max_cost_sources);
606  }
607  }
608 
609  return false;
610  }
611  else
612  {
613  if(OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2, tree2->getBV(root2).leftChild(), tf1, tf2))
614  return true;
615 
616  if(OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2, tree2->getBV(root2).rightChild(), tf1, tf2))
617  return true;
618 
619  return false;
620  }
621  }
622  else if(!tree1->nodeHasChildren(root1) && tree2->getBV(root2).isLeaf())
623  {
624  if(tree1->isNodeOccupied(root1) && tree2->isOccupied())
625  {
626  OBB<S> obb1, obb2;
627  convertBV(bv1, tf1, obb1);
628  convertBV(tree2->getBV(root2).bv, tf2, obb2);
629  if(obb1.overlap(obb2))
630  {
631  Box<S> box;
632  Transform3<S> box_tf;
633  constructBox(bv1, tf1, box, box_tf);
634 
635  int primitive_id = tree2->getBV(root2).primitiveId();
636  const Triangle& tri_id = tree2->tri_indices[primitive_id];
637  const Vector3<S>& p1 = tree2->vertices[tri_id[0]];
638  const Vector3<S>& p2 = tree2->vertices[tri_id[1]];
639  const Vector3<S>& p3 = tree2->vertices[tri_id[2]];
640 
641  bool is_intersect = false;
642  if(!crequest->enable_contact)
643  {
644  if(solver->shapeTriangleIntersect(box, box_tf, p1, p2, p3, tf2, nullptr, nullptr, nullptr))
645  {
646  is_intersect = true;
647  if(cresult->numContacts() < crequest->num_max_contacts)
648  cresult->addContact(Contact<S>(tree1, tree2, root1 - tree1->getRoot(), primitive_id));
649  }
650  }
651  else
652  {
653  Vector3<S> contact;
654  S depth;
655  Vector3<S> normal;
656 
657  if(solver->shapeTriangleIntersect(box, box_tf, p1, p2, p3, tf2, &contact, &depth, &normal))
658  {
659  is_intersect = true;
660  if(cresult->numContacts() < crequest->num_max_contacts)
661  cresult->addContact(Contact<S>(tree1, tree2, root1 - tree1->getRoot(), primitive_id, contact, normal, depth));
662  }
663  }
664 
665  if(is_intersect && crequest->enable_cost)
666  {
667  AABB<S> overlap_part;
668  AABB<S> aabb1;
669  computeBV(box, box_tf, aabb1);
670  AABB<S> aabb2(tf2 * p1, tf2 * p2, tf2 * p3);
671  aabb1.overlap(aabb2, overlap_part);
672  cresult->addCostSource(CostSource<S>(overlap_part, root1->getOccupancy() * tree2->cost_density), crequest->num_max_cost_sources);
673  }
674 
675  return crequest->isSatisfied(*cresult);
676  }
677  else
678  return false;
679  }
680  else if(!tree1->isNodeFree(root1) && !tree2->isFree() && crequest->enable_cost) // uncertain area
681  {
682  OBB<S> obb1, obb2;
683  convertBV(bv1, tf1, obb1);
684  convertBV(tree2->getBV(root2).bv, tf2, obb2);
685  if(obb1.overlap(obb2))
686  {
687  Box<S> box;
688  Transform3<S> box_tf;
689  constructBox(bv1, tf1, box, box_tf);
690 
691  int primitive_id = tree2->getBV(root2).primitiveId();
692  const Triangle& tri_id = tree2->tri_indices[primitive_id];
693  const Vector3<S>& p1 = tree2->vertices[tri_id[0]];
694  const Vector3<S>& p2 = tree2->vertices[tri_id[1]];
695  const Vector3<S>& p3 = tree2->vertices[tri_id[2]];
696 
697  if(solver->shapeTriangleIntersect(box, box_tf, p1, p2, p3, tf2, nullptr, nullptr, nullptr))
698  {
699  AABB<S> overlap_part;
700  AABB<S> aabb1;
701  computeBV(box, box_tf, aabb1);
702  AABB<S> aabb2(tf2 * p1, tf2 * p2, tf2 * p3);
703  aabb1.overlap(aabb2, overlap_part);
704  cresult->addCostSource(CostSource<S>(overlap_part, root1->getOccupancy() * tree2->cost_density), crequest->num_max_cost_sources);
705  }
706  }
707 
708  return false;
709  }
710  else // free area
711  return false;
712  }
713 
717  if(tree1->isNodeFree(root1) || tree2->isFree()) return false;
718  else if((tree1->isNodeUncertain(root1) || tree2->isUncertain()) && !crequest->enable_cost) return false;
719  else
720  {
721  OBB<S> obb1, obb2;
722  convertBV(bv1, tf1, obb1);
723  convertBV(tree2->getBV(root2).bv, tf2, obb2);
724  if(!obb1.overlap(obb2)) return false;
725  }
726 
727  if(tree2->getBV(root2).isLeaf() || (tree1->nodeHasChildren(root1) && (bv1.size() > tree2->getBV(root2).bv.size())))
728  {
729  for(unsigned int i = 0; i < 8; ++i)
730  {
731  if(tree1->nodeChildExists(root1, i))
732  {
733  const typename OcTree<S>::OcTreeNode* child = tree1->getNodeChild(root1, i);
734  AABB<S> child_bv;
735  computeChildBV(bv1, i, child_bv);
736 
737  if(OcTreeMeshIntersectRecurse(tree1, child, child_bv, tree2, root2, tf1, tf2))
738  return true;
739  }
740 else if(!tree2->isFree() && crequest->enable_cost)
741  {
742  AABB<S> child_bv;
743  computeChildBV(bv1, i, child_bv);
744 
745  if(OcTreeMeshIntersectRecurse(tree1, nullptr, child_bv, tree2, root2, tf1, tf2))
746  return true;
747  }
748  }
749  }
750  else
751  {
752  if(OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2, tree2->getBV(root2).leftChild(), tf1, tf2))
753  return true;
754 
755  if(OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2, tree2->getBV(root2).rightChild(), tf1, tf2))
756  return true;
757 
758  }
759 
760  return false;
761 }
762 
763 //==============================================================================
764 template <typename NarrowPhaseSolver>
765 bool OcTreeSolver<NarrowPhaseSolver>::OcTreeDistanceRecurse(const OcTree<S>* tree1, const typename OcTree<S>::OcTreeNode* root1, const AABB<S>& bv1, const OcTree<S>* tree2, const typename OcTree<S>::OcTreeNode* root2, const AABB<S>& bv2, const Transform3<S>& tf1, const Transform3<S>& tf2) const
766 {
767  if(!tree1->nodeHasChildren(root1) && !tree2->nodeHasChildren(root2))
768  {
769  if(tree1->isNodeOccupied(root1) && tree2->isNodeOccupied(root2))
770  {
771  Box<S> box1, box2;
772  Transform3<S> box1_tf, box2_tf;
773  constructBox(bv1, tf1, box1, box1_tf);
774  constructBox(bv2, tf2, box2, box2_tf);
775 
776  S dist;
777  // NOTE(JS): The closest points are set to zeros in order to suppress the
778  // maybe-uninitialized warning. It seems the warnings occur since
779  // NarrowPhaseSolver::shapeDistance() conditionally set the closest points.
780  // If this wasn't intentional then please remove the initialization of the
781  // closest points, and change the function NarrowPhaseSolver::shapeDistance()
782  // to always set the closest points.
783  Vector3<S> closest_p1 = Vector3<S>::Zero();
784  Vector3<S> closest_p2 = Vector3<S>::Zero();
785  solver->shapeDistance(box1, box1_tf, box2, box2_tf, &dist, &closest_p1, &closest_p2);
786 
787  dresult->update(dist, tree1, tree2, root1 - tree1->getRoot(), root2 - tree2->getRoot(), closest_p1, closest_p2);
788 
789  return drequest->isSatisfied(*dresult);
790  }
791  else
792  return false;
793  }
794 
795  if(!tree1->isNodeOccupied(root1) || !tree2->isNodeOccupied(root2)) return false;
796 
797  if(!tree2->nodeHasChildren(root2) || (tree1->nodeHasChildren(root1) && (bv1.size() > bv2.size())))
798  {
799  for(unsigned int i = 0; i < 8; ++i)
800  {
801  if(tree1->nodeChildExists(root1, i))
802  {
803  const typename OcTree<S>::OcTreeNode* child = tree1->getNodeChild(root1, i);
804  AABB<S> child_bv;
805  computeChildBV(bv1, i, child_bv);
806 
807  S d;
808  AABB<S> aabb1, aabb2;
809  convertBV(bv1, tf1, aabb1);
810  convertBV(bv2, tf2, aabb2);
811  d = aabb1.distance(aabb2);
812 
813  if(d < dresult->min_distance)
814  {
815 
816  if(OcTreeDistanceRecurse(tree1, child, child_bv, tree2, root2, bv2, tf1, tf2))
817  return true;
818  }
819  }
820  }
821  }
822  else
823  {
824  for(unsigned int i = 0; i < 8; ++i)
825  {
826  if(tree2->nodeChildExists(root2, i))
827  {
828  const typename OcTree<S>::OcTreeNode* child = tree2->getNodeChild(root2, i);
829  AABB<S> child_bv;
830  computeChildBV(bv2, i, child_bv);
831 
832  S d;
833  AABB<S> aabb1, aabb2;
834  convertBV(bv1, tf1, aabb1);
835  convertBV(bv2, tf2, aabb2);
836  d = aabb1.distance(aabb2);
837 
838  if(d < dresult->min_distance)
839  {
840  if(OcTreeDistanceRecurse(tree1, root1, bv1, tree2, child, child_bv, tf1, tf2))
841  return true;
842  }
843  }
844  }
845  }
846 
847  return false;
848 }
849 
850 //==============================================================================
851 template <typename NarrowPhaseSolver>
852 bool OcTreeSolver<NarrowPhaseSolver>::OcTreeIntersectRecurse(const OcTree<S>* tree1, const typename OcTree<S>::OcTreeNode* root1, const AABB<S>& bv1, const OcTree<S>* tree2, const typename OcTree<S>::OcTreeNode* root2, const AABB<S>& bv2, const Transform3<S>& tf1, const Transform3<S>& tf2) const
853 {
854  if(!root1 && !root2)
855  {
856  OBB<S> obb1, obb2;
857  convertBV(bv1, tf1, obb1);
858  convertBV(bv2, tf2, obb2);
859 
860  if(obb1.overlap(obb2))
861  {
862  Box<S> box1, box2;
863  Transform3<S> box1_tf, box2_tf;
864  constructBox(bv1, tf1, box1, box1_tf);
865  constructBox(bv2, tf2, box2, box2_tf);
866 
867  AABB<S> overlap_part;
868  AABB<S> aabb1, aabb2;
869  computeBV(box1, box1_tf, aabb1);
870  computeBV(box2, box2_tf, aabb2);
871  aabb1.overlap(aabb2, overlap_part);
872  cresult->addCostSource(CostSource<S>(overlap_part, tree1->getOccupancyThres() * tree2->getOccupancyThres()), crequest->num_max_cost_sources);
873  }
874 
875  return false;
876  }
877  else if(!root1 && root2)
878  {
879  if(tree2->nodeHasChildren(root2))
880  {
881  for(unsigned int i = 0; i < 8; ++i)
882  {
883  if(tree2->nodeChildExists(root2, i))
884  {
885  const typename OcTree<S>::OcTreeNode* child = tree2->getNodeChild(root2, i);
886  AABB<S> child_bv;
887  computeChildBV(bv2, i, child_bv);
888  if(OcTreeIntersectRecurse(tree1, nullptr, bv1, tree2, child, child_bv, tf1, tf2))
889  return true;
890  }
891  else
892  {
893  AABB<S> child_bv;
894  computeChildBV(bv2, i, child_bv);
895  if(OcTreeIntersectRecurse(tree1, nullptr, bv1, tree2, nullptr, child_bv, tf1, tf2))
896  return true;
897  }
898  }
899  }
900  else
901  {
902  if(OcTreeIntersectRecurse(tree1, nullptr, bv1, tree2, nullptr, bv2, tf1, tf2))
903  return true;
904  }
905 
906  return false;
907  }
908  else if(root1 && !root2)
909  {
910  if(tree1->nodeHasChildren(root1))
911  {
912  for(unsigned int i = 0; i < 8; ++i)
913  {
914  if(tree1->nodeChildExists(root1, i))
915  {
916  const typename OcTree<S>::OcTreeNode* child = tree1->getNodeChild(root1, i);
917  AABB<S> child_bv;
918  computeChildBV(bv1, i, child_bv);
919  if(OcTreeIntersectRecurse(tree1, child, child_bv, tree2, nullptr, bv2, tf1, tf2))
920  return true;
921  }
922  else
923  {
924  AABB<S> child_bv;
925  computeChildBV(bv1, i, child_bv);
926  if(OcTreeIntersectRecurse(tree1, nullptr, child_bv, tree2, nullptr, bv2, tf1, tf2))
927  return true;
928  }
929  }
930  }
931  else
932  {
933  if(OcTreeIntersectRecurse(tree1, nullptr, bv1, tree2, nullptr, bv2, tf1, tf2))
934  return true;
935  }
936 
937  return false;
938  }
939  else if(!tree1->nodeHasChildren(root1) && !tree2->nodeHasChildren(root2))
940  {
941  if(tree1->isNodeOccupied(root1) && tree2->isNodeOccupied(root2)) // occupied area
942  {
943  bool is_intersect = false;
944  if(!crequest->enable_contact)
945  {
946  OBB<S> obb1, obb2;
947  convertBV(bv1, tf1, obb1);
948  convertBV(bv2, tf2, obb2);
949 
950  if(obb1.overlap(obb2))
951  {
952  is_intersect = true;
953  if(cresult->numContacts() < crequest->num_max_contacts)
954  cresult->addContact(Contact<S>(tree1, tree2, root1 - tree1->getRoot(), root2 - tree2->getRoot()));
955  }
956  }
957  else
958  {
959  Box<S> box1, box2;
960  Transform3<S> box1_tf, box2_tf;
961  constructBox(bv1, tf1, box1, box1_tf);
962  constructBox(bv2, tf2, box2, box2_tf);
963 
964  std::vector<ContactPoint<S>> contacts;
965  if(solver->shapeIntersect(box1, box1_tf, box2, box2_tf, &contacts))
966  {
967  is_intersect = true;
968  if(crequest->num_max_contacts > cresult->numContacts())
969  {
970  const size_t free_space = crequest->num_max_contacts - cresult->numContacts();
971  size_t num_adding_contacts;
972 
973  // If the free space is not enough to add all the new contacts, we add contacts in descent order of penetration depth.
974  if (free_space < contacts.size())
975  {
976  std::partial_sort(contacts.begin(), contacts.begin() + free_space, contacts.end(), std::bind(comparePenDepth<S>, std::placeholders::_2, std::placeholders::_1));
977  num_adding_contacts = free_space;
978  }
979  else
980  {
981  num_adding_contacts = contacts.size();
982  }
983 
984  for(size_t i = 0; i < num_adding_contacts; ++i)
985  cresult->addContact(Contact<S>(tree1, tree2, root1 - tree1->getRoot(), root2 - tree2->getRoot(), contacts[i].pos, contacts[i].normal, contacts[i].penetration_depth));
986  }
987  }
988  }
989 
990  if(is_intersect && crequest->enable_cost)
991  {
992  Box<S> box1, box2;
993  Transform3<S> box1_tf, box2_tf;
994  constructBox(bv1, tf1, box1, box1_tf);
995  constructBox(bv2, tf2, box2, box2_tf);
996 
997  AABB<S> overlap_part;
998  AABB<S> aabb1, aabb2;
999  computeBV(box1, box1_tf, aabb1);
1000  computeBV(box2, box2_tf, aabb2);
1001  aabb1.overlap(aabb2, overlap_part);
1002  cresult->addCostSource(CostSource<S>(overlap_part, root1->getOccupancy() * root2->getOccupancy()), crequest->num_max_cost_sources);
1003  }
1004 
1005  return crequest->isSatisfied(*cresult);
1006  }
1007  else if(!tree1->isNodeFree(root1) && !tree2->isNodeFree(root2) && crequest->enable_cost) // uncertain area (here means both are uncertain or one uncertain and one occupied)
1008  {
1009  OBB<S> obb1, obb2;
1010  convertBV(bv1, tf1, obb1);
1011  convertBV(bv2, tf2, obb2);
1012 
1013  if(obb1.overlap(obb2))
1014  {
1015  Box<S> box1, box2;
1016  Transform3<S> box1_tf, box2_tf;
1017  constructBox(bv1, tf1, box1, box1_tf);
1018  constructBox(bv2, tf2, box2, box2_tf);
1019 
1020  AABB<S> overlap_part;
1021  AABB<S> aabb1, aabb2;
1022  computeBV(box1, box1_tf, aabb1);
1023  computeBV(box2, box2_tf, aabb2);
1024  aabb1.overlap(aabb2, overlap_part);
1025  cresult->addCostSource(CostSource<S>(overlap_part, root1->getOccupancy() * root2->getOccupancy()), crequest->num_max_cost_sources);
1026  }
1027 
1028  return false;
1029  }
1030  else // free area (at least one node is free)
1031  return false;
1032  }
1033 
1037  if(tree1->isNodeFree(root1) || tree2->isNodeFree(root2)) return false;
1038  else if((tree1->isNodeUncertain(root1) || tree2->isNodeUncertain(root2)) && !crequest->enable_cost) return false;
1039  else
1040  {
1041  OBB<S> obb1, obb2;
1042  convertBV(bv1, tf1, obb1);
1043  convertBV(bv2, tf2, obb2);
1044  if(!obb1.overlap(obb2)) return false;
1045  }
1046 
1047  if(!tree2->nodeHasChildren(root2) || (tree1->nodeHasChildren(root1) && (bv1.size() > bv2.size())))
1048  {
1049  for(unsigned int i = 0; i < 8; ++i)
1050  {
1051  if(tree1->nodeChildExists(root1, i))
1052  {
1053  const typename OcTree<S>::OcTreeNode* child = tree1->getNodeChild(root1, i);
1054  AABB<S> child_bv;
1055  computeChildBV(bv1, i, child_bv);
1056 
1057  if(OcTreeIntersectRecurse(tree1, child, child_bv,
1058  tree2, root2, bv2,
1059  tf1, tf2))
1060  return true;
1061  }
1062  else if(!tree2->isNodeFree(root2) && crequest->enable_cost)
1063  {
1064  AABB<S> child_bv;
1065  computeChildBV(bv1, i, child_bv);
1066 
1067  if(OcTreeIntersectRecurse(tree1, nullptr, child_bv,
1068  tree2, root2, bv2,
1069  tf1, tf2))
1070  return true;
1071  }
1072  }
1073  }
1074  else
1075  {
1076  for(unsigned int i = 0; i < 8; ++i)
1077  {
1078  if(tree2->nodeChildExists(root2, i))
1079  {
1080  const typename OcTree<S>::OcTreeNode* child = tree2->getNodeChild(root2, i);
1081  AABB<S> child_bv;
1082  computeChildBV(bv2, i, child_bv);
1083 
1084  if(OcTreeIntersectRecurse(tree1, root1, bv1,
1085  tree2, child, child_bv,
1086  tf1, tf2))
1087  return true;
1088  }
1089  else if(!tree1->isNodeFree(root1) && crequest->enable_cost)
1090  {
1091  AABB<S> child_bv;
1092  computeChildBV(bv2, i, child_bv);
1093 
1094  if(OcTreeIntersectRecurse(tree1, root1, bv1,
1095  tree2, nullptr, child_bv,
1096  tf1, tf2))
1097  return true;
1098  }
1099  }
1100  }
1101 
1102  return false;
1103 }
1104 
1105 } // namespace detail
1106 } // namespace fcl
1107 
1108 #endif
void convertBV(const BV1 &bv1, const Transform3< typename BV1::S > &tf1, BV2 &bv2)
Convert a bounding volume of type BV1 in configuration tf1 to bounding volume of type BV2 in identity...
Definition: utility-inl.h:934
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
void OcTreeIntersect(const OcTree< S > *tree1, const OcTree< S > *tree2, const Transform3< S > &tf1, const Transform3< S > &tf2, const CollisionRequest< S > &request_, CollisionResult< S > &result_) const
collision between two octrees
Definition: octree_solver-inl.h:66
collision result
Definition: collision_request.h:48
S size() const
Size of the AABB (used in BV_Splitter to order two AABBs)
Definition: AABB-inl.h:216
void OcTreeShapeDistance(const OcTree< S > *tree, const Shape &s, const Transform3< S > &tf1, const Transform3< S > &tf2, const DistanceRequest< S > &request_, DistanceResult< S > &result_) const
distance between octree and shape
Definition: octree_solver-inl.h:231
void ShapeOcTreeDistance(const Shape &s, const OcTree< S > *tree, const Transform3< S > &tf1, const Transform3< S > &tf2, const DistanceRequest< S > &request_, DistanceResult< S > &result_) const
distance between shape and octree
Definition: octree_solver-inl.h:253
distance result
Definition: distance_request.h:48
const BVNode< BV > & getBV(int id) const
We provide getBV() and getNumBVs() because BVH may be compressed (in future), so we must provide some...
Definition: BVH_model-inl.h:160
Transform3< Shape::S > tf2
configuration of second object
Definition: traversal_node_base.h:88
Contact information returned by collision.
Definition: contact.h:48
bool overlap(const AABB< S > &other) const
Check whether two AABB are overlap.
Definition: AABB-inl.h:98
Vector3< S > * vertices
Geometry point data.
Definition: BVH_model.h:162
bool isFree() const
whether the object is completely free
bool isUncertain() const
whether the object has some uncertainty
Triangle * tri_indices
Geometry triangle index data, will be nullptr for point clouds.
Definition: BVH_model.h:165
Triangle with 3 indices for points.
Definition: triangle.h:47
bool overlap(const OBB< S > &other) const
Check collision between two OBB, return true if collision happens.
Definition: OBB-inl.h:96
Vector3< S > pos
contact position, in world space
Definition: contact.h:72
request to the collision algorithm
Definition: collision_request.h:52
Algorithms for collision related with octree.
Definition: octree_solver.h:59
void computeBV(const Shape &s, const Transform3< typename BV::S > &tf, BV &bv)
calculate a bounding volume for a shape in a specific configuration
Definition: utility-inl.h:1049
Center at zero point, axis aligned box.
Definition: box.h:48
void OcTreeDistance(const OcTree< S > *tree1, const OcTree< S > *tree2, const Transform3< S > &tf1, const Transform3< S > &tf2, const DistanceRequest< S > &request_, DistanceResult< S > &result_) const
distance between two octrees
Definition: octree_solver-inl.h:84
void OcTreeMeshDistance(const OcTree< S > *tree1, const BVHModel< BV > *tree2, const Transform3< S > &tf1, const Transform3< S > &tf2, const DistanceRequest< S > &request_, DistanceResult< S > &result_) const
distance between octree and mesh
Definition: octree_solver-inl.h:122
void MeshOcTreeDistance(const BVHModel< BV > *tree1, const OcTree< S > *tree2, const Transform3< S > &tf1, const Transform3< S > &tf2, const DistanceRequest< S > &request_, DistanceResult< S > &result_) const
distance between mesh and octree
Definition: octree_solver-inl.h:162
A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as ...
Definition: BVH_model.h:57
void ShapeOcTreeIntersect(const Shape &s, const OcTree< S > *tree, const Transform3< S > &tf1, const Transform3< S > &tf2, const CollisionRequest< S > &request_, CollisionResult< S > &result_) const
collision between shape and octree
Definition: octree_solver-inl.h:207
BV::S cost_density
collision cost for unit volume
Definition: collision_geometry.h:103
void OcTreeMeshIntersect(const OcTree< S > *tree1, const BVHModel< BV > *tree2, const Transform3< S > &tf1, const Transform3< S > &tf2, const CollisionRequest< S > &request_, CollisionResult< S > &result_) const
collision between octree and mesh
Definition: octree_solver-inl.h:103
void OcTreeShapeIntersect(const OcTree< S > *tree, const Shape &s, const Transform3< S > &tf1, const Transform3< S > &tf2, const CollisionRequest< S > &request_, CollisionResult< S > &result_) const
collision between octree and shape
Definition: octree_solver-inl.h:182
Transform3< Shape::S > tf1
configuation of first object
Definition: traversal_node_base.h:85
Cost source describes an area with a cost. The area is described by an AABB<S> region.
Definition: cost_source.h:49
void MeshOcTreeIntersect(const BVHModel< BV > *tree1, const OcTree< S > *tree2, const Transform3< S > &tf1, const Transform3< S > &tf2, const CollisionRequest< S > &request_, CollisionResult< S > &result_) const
collision between mesh and octree
Definition: octree_solver-inl.h:141
S distance(const AABB< S > &other, Vector3< S > *P, Vector3< S > *Q) const
Distance between two AABBs; P and Q, should not be nullptr, return the nearest points.
Definition: AABB-inl.h:237
bool isOccupied() const
whether the object is completely occupied
request to the distance computation
Definition: distance_request.h:52