coal  3.0.1
Coal, The Collision Detection Library. Previously known as HPP-FCL, 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  * Copyright (c) 2022-2023, INRIA
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of Open Source Robotics Foundation nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  */
36 
39 #ifndef COAL_TRAVERSAL_NODE_OCTREE_H
40 #define COAL_TRAVERSAL_NODE_OCTREE_H
41 
43 
44 #include "coal/collision_data.h"
45 #include "coal/internal/traversal_node_base.h"
46 #include "coal/internal/traversal_node_hfield_shape.h"
47 #include "coal/narrowphase/narrowphase.h"
48 #include "coal/hfield.h"
49 #include "coal/octree.h"
50 #include "coal/BVH/BVH_model.h"
51 #include "coal/shape/geometric_shapes_utility.h"
52 #include "coal/internal/shape_shape_func.h"
53 
54 namespace coal {
55 
57 class COAL_DLLAPI OcTreeSolver {
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_)
69  : solver(solver_),
70  crequest(NULL),
71  drequest(NULL),
72  cresult(NULL),
73  dresult(NULL) {}
74 
76  void OcTreeIntersect(const OcTree* tree1, const OcTree* tree2,
77  const Transform3s& tf1, const Transform3s& tf2,
78  const CollisionRequest& request_,
79  CollisionResult& result_) const {
80  crequest = &request_;
81  cresult = &result_;
82 
83  OcTreeIntersectRecurse(tree1, tree1->getRoot(), tree1->getRootBV(), tree2,
84  tree2->getRoot(), tree2->getRootBV(), tf1, tf2);
85  }
86 
88  void OcTreeDistance(const OcTree* tree1, const OcTree* tree2,
89  const Transform3s& tf1, const Transform3s& tf2,
90  const DistanceRequest& request_,
91  DistanceResult& result_) const {
92  drequest = &request_;
93  dresult = &result_;
94 
95  OcTreeDistanceRecurse(tree1, tree1->getRoot(), tree1->getRootBV(), tree2,
96  tree2->getRoot(), tree2->getRootBV(), tf1, tf2);
97  }
98 
100  template <typename BV>
101  void OcTreeMeshIntersect(const OcTree* tree1, const BVHModel<BV>* tree2,
102  const Transform3s& tf1, const Transform3s& tf2,
103  const CollisionRequest& request_,
104  CollisionResult& result_) const {
105  crequest = &request_;
106  cresult = &result_;
107 
108  OcTreeMeshIntersectRecurse(tree1, tree1->getRoot(), tree1->getRootBV(),
109  tree2, 0, tf1, tf2);
110  }
111 
113  template <typename BV>
114  void OcTreeMeshDistance(const OcTree* tree1, const BVHModel<BV>* tree2,
115  const Transform3s& tf1, const Transform3s& tf2,
116  const DistanceRequest& request_,
117  DistanceResult& result_) const {
118  drequest = &request_;
119  dresult = &result_;
120 
121  OcTreeMeshDistanceRecurse(tree1, tree1->getRoot(), tree1->getRootBV(),
122  tree2, 0, tf1, tf2);
123  }
124 
126  template <typename BV>
127  void MeshOcTreeIntersect(const BVHModel<BV>* tree1, const OcTree* tree2,
128  const Transform3s& tf1, const Transform3s& tf2,
129  const CollisionRequest& request_,
130  CollisionResult& result_) const
131 
132  {
133  crequest = &request_;
134  cresult = &result_;
135 
136  OcTreeMeshIntersectRecurse(tree2, tree2->getRoot(), tree2->getRootBV(),
137  tree1, 0, tf2, tf1);
138  }
139 
141  template <typename BV>
142  void MeshOcTreeDistance(const BVHModel<BV>* tree1, const OcTree* tree2,
143  const Transform3s& tf1, const Transform3s& tf2,
144  const DistanceRequest& request_,
145  DistanceResult& result_) const {
146  drequest = &request_;
147  dresult = &result_;
148 
149  OcTreeMeshDistanceRecurse(tree1, 0, tree2, tree2->getRoot(),
150  tree2->getRootBV(), tf1, tf2);
151  }
152 
153  template <typename BV>
154  void OcTreeHeightFieldIntersect(
155  const OcTree* tree1, const HeightField<BV>* tree2, const Transform3s& tf1,
156  const Transform3s& tf2, const CollisionRequest& request_,
157  CollisionResult& result_, Scalar& sqrDistLowerBound) const {
158  crequest = &request_;
159  cresult = &result_;
160 
161  OcTreeHeightFieldIntersectRecurse(tree1, tree1->getRoot(),
162  tree1->getRootBV(), tree2, 0, tf1, tf2,
163  sqrDistLowerBound);
164  }
165 
166  template <typename BV>
167  void HeightFieldOcTreeIntersect(const HeightField<BV>* tree1,
168  const OcTree* tree2, const Transform3s& tf1,
169  const Transform3s& tf2,
170  const CollisionRequest& request_,
171  CollisionResult& result_,
172  Scalar& sqrDistLowerBound) const {
173  crequest = &request_;
174  cresult = &result_;
175 
176  HeightFieldOcTreeIntersectRecurse(tree1, 0, tree2, tree2->getRoot(),
177  tree2->getRootBV(), tf1, tf2,
178  sqrDistLowerBound);
179  }
180 
182  template <typename S>
183  void OcTreeShapeIntersect(const OcTree* tree, const S& s,
184  const Transform3s& tf1, const Transform3s& tf2,
185  const CollisionRequest& request_,
186  CollisionResult& result_) const {
187  crequest = &request_;
188  cresult = &result_;
189 
190  AABB bv2;
191  computeBV<AABB>(s, Transform3s(), bv2);
192  OBB obb2;
193  convertBV(bv2, tf2, obb2);
194  OcTreeShapeIntersectRecurse(tree, tree->getRoot(), tree->getRootBV(), s,
195  obb2, tf1, tf2);
196  }
197 
199  template <typename S>
200  void ShapeOcTreeIntersect(const S& s, const OcTree* tree,
201  const Transform3s& tf1, const Transform3s& tf2,
202  const CollisionRequest& request_,
203  CollisionResult& result_) const {
204  crequest = &request_;
205  cresult = &result_;
206 
207  AABB bv1;
208  computeBV<AABB>(s, Transform3s(), bv1);
209  OBB obb1;
210  convertBV(bv1, tf1, obb1);
211  OcTreeShapeIntersectRecurse(tree, tree->getRoot(), tree->getRootBV(), s,
212  obb1, tf2, tf1);
213  }
214 
216  template <typename S>
217  void OcTreeShapeDistance(const OcTree* tree, const S& s,
218  const Transform3s& tf1, const Transform3s& tf2,
219  const DistanceRequest& request_,
220  DistanceResult& result_) const {
221  drequest = &request_;
222  dresult = &result_;
223 
224  AABB aabb2;
225  computeBV<AABB>(s, tf2, aabb2);
226  OcTreeShapeDistanceRecurse(tree, tree->getRoot(), tree->getRootBV(), s,
227  aabb2, tf1, tf2);
228  }
229 
231  template <typename S>
232  void ShapeOcTreeDistance(const S& s, const OcTree* tree,
233  const Transform3s& tf1, const Transform3s& tf2,
234  const DistanceRequest& request_,
235  DistanceResult& result_) const {
236  drequest = &request_;
237  dresult = &result_;
238 
239  AABB aabb1;
240  computeBV<AABB>(s, tf1, aabb1);
241  OcTreeShapeDistanceRecurse(tree, tree->getRoot(), tree->getRootBV(), s,
242  aabb1, tf2, tf1);
243  }
244 
245  private:
246  template <typename S>
247  bool OcTreeShapeDistanceRecurse(const OcTree* tree1,
248  const OcTree::OcTreeNode* root1,
249  const AABB& bv1, const S& s,
250  const AABB& aabb2, const Transform3s& tf1,
251  const Transform3s& tf2) const {
252  if (!tree1->nodeHasChildren(root1)) {
253  if (tree1->isNodeOccupied(root1)) {
254  Box box;
255  Transform3s box_tf;
256  constructBox(bv1, tf1, box, box_tf);
257 
258  if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
259  box.computeLocalAABB();
260  }
261 
262  Vec3s p1, p2, normal;
263  const Scalar distance = internal::ShapeShapeDistance<Box, S>(
264  &box, box_tf, &s, tf2, this->solver,
265  this->drequest->enable_signed_distance, p1, p2, normal);
266 
267  this->dresult->update(distance, tree1, &s,
268  (int)(root1 - tree1->getRoot()),
269  DistanceResult::NONE, p1, p2, normal);
270 
271  return drequest->isSatisfied(*dresult);
272  } else
273  return false;
274  }
275 
276  if (!tree1->isNodeOccupied(root1)) return false;
277 
278  for (unsigned int i = 0; i < 8; ++i) {
279  if (tree1->nodeChildExists(root1, i)) {
280  const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
281  AABB child_bv;
282  computeChildBV(bv1, i, child_bv);
283 
284  AABB aabb1;
285  convertBV(child_bv, tf1, aabb1);
286  Scalar d = aabb1.distance(aabb2);
287  if (d < dresult->min_distance) {
288  if (OcTreeShapeDistanceRecurse(tree1, child, child_bv, s, aabb2, tf1,
289  tf2))
290  return true;
291  }
292  }
293  }
294 
295  return false;
296  }
297 
298  template <typename S>
299  bool OcTreeShapeIntersectRecurse(const OcTree* tree1,
300  const OcTree::OcTreeNode* root1,
301  const AABB& bv1, const S& s, const OBB& obb2,
302  const Transform3s& tf1,
303  const Transform3s& tf2) const {
304  // Empty OcTree is considered free.
305  if (!root1) return false;
306 
311  if (tree1->isNodeFree(root1))
312  return false;
313  else if ((tree1->isNodeUncertain(root1) || s.isUncertain()))
314  return false;
315  else {
316  OBB obb1;
317  convertBV(bv1, tf1, obb1);
318  Scalar sqrDistLowerBound;
319  if (!obb1.overlap(obb2, *crequest, sqrDistLowerBound)) {
320  internal::updateDistanceLowerBoundFromBV(*crequest, *cresult,
321  sqrDistLowerBound);
322  return false;
323  }
324  }
325 
326  if (!tree1->nodeHasChildren(root1)) {
327  assert(tree1->isNodeOccupied(root1)); // it isn't free nor uncertain.
328 
329  Box box;
330  Transform3s box_tf;
331  constructBox(bv1, tf1, box, box_tf);
332  if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
333  box.computeLocalAABB();
334  }
335 
336  bool contactNotAdded =
337  (cresult->numContacts() >= crequest->num_max_contacts);
338  std::size_t ncontact = ShapeShapeCollide<Box, S>(
339  &box, box_tf, &s, tf2, solver, *crequest, *cresult);
340  assert(ncontact == 0 || ncontact == 1);
341  if (!contactNotAdded && ncontact == 1) {
342  // Update contact information.
343  const Contact& c = cresult->getContact(cresult->numContacts() - 1);
344  cresult->setContact(
345  cresult->numContacts() - 1,
346  Contact(tree1, c.o2, static_cast<int>(root1 - tree1->getRoot()),
347  c.b2, c.pos, c.normal, c.penetration_depth));
348  }
349 
350  // no need to call `internal::updateDistanceLowerBoundFromLeaf` here
351  // as it is already done internally in `ShapeShapeCollide` above.
352  return crequest->isSatisfied(*cresult);
353  }
354 
355  for (unsigned int i = 0; i < 8; ++i) {
356  if (tree1->nodeChildExists(root1, i)) {
357  const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
358  AABB child_bv;
359  computeChildBV(bv1, i, child_bv);
360 
361  if (OcTreeShapeIntersectRecurse(tree1, child, child_bv, s, obb2, tf1,
362  tf2))
363  return true;
364  }
365  }
366 
367  return false;
368  }
369 
370  template <typename BV>
371  bool OcTreeMeshDistanceRecurse(const OcTree* tree1,
372  const OcTree::OcTreeNode* root1,
373  const AABB& bv1, const BVHModel<BV>* tree2,
374  unsigned int root2, const Transform3s& tf1,
375  const Transform3s& tf2) const {
376  if (!tree1->nodeHasChildren(root1) && tree2->getBV(root2).isLeaf()) {
377  if (tree1->isNodeOccupied(root1)) {
378  Box box;
379  Transform3s box_tf;
380  constructBox(bv1, tf1, box, box_tf);
381 
382  if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
383  box.computeLocalAABB();
384  }
385 
386  size_t primitive_id =
387  static_cast<size_t>(tree2->getBV(root2).primitiveId());
388  const Triangle& tri_id = (*(tree2->tri_indices))[primitive_id];
389  const TriangleP tri((*(tree2->vertices))[tri_id[0]],
390  (*(tree2->vertices))[tri_id[1]],
391  (*(tree2->vertices))[tri_id[2]]);
392 
393  Vec3s p1, p2, normal;
394  const Scalar distance = internal::ShapeShapeDistance<Box, TriangleP>(
395  &box, box_tf, &tri, tf2, this->solver,
396  this->drequest->enable_signed_distance, p1, p2, normal);
397 
398  this->dresult->update(distance, tree1, tree2,
399  (int)(root1 - tree1->getRoot()),
400  static_cast<int>(primitive_id), p1, p2, normal);
401 
402  return this->drequest->isSatisfied(*dresult);
403  } else
404  return false;
405  }
406 
407  if (!tree1->isNodeOccupied(root1)) return false;
408 
409  if (tree2->getBV(root2).isLeaf() ||
410  (tree1->nodeHasChildren(root1) &&
411  (bv1.size() > tree2->getBV(root2).bv.size()))) {
412  for (unsigned int i = 0; i < 8; ++i) {
413  if (tree1->nodeChildExists(root1, i)) {
414  const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
415  AABB child_bv;
416  computeChildBV(bv1, i, child_bv);
417 
418  Scalar d;
419  AABB aabb1, aabb2;
420  convertBV(child_bv, tf1, aabb1);
421  convertBV(tree2->getBV(root2).bv, tf2, aabb2);
422  d = aabb1.distance(aabb2);
423 
424  if (d < dresult->min_distance) {
425  if (OcTreeMeshDistanceRecurse(tree1, child, child_bv, tree2, root2,
426  tf1, tf2))
427  return true;
428  }
429  }
430  }
431  } else {
432  Scalar d;
433  AABB aabb1, aabb2;
434  convertBV(bv1, tf1, aabb1);
435  unsigned int child = (unsigned int)tree2->getBV(root2).leftChild();
436  convertBV(tree2->getBV(child).bv, tf2, aabb2);
437  d = aabb1.distance(aabb2);
438 
439  if (d < dresult->min_distance) {
440  if (OcTreeMeshDistanceRecurse(tree1, root1, bv1, tree2, child, tf1,
441  tf2))
442  return true;
443  }
444 
445  child = (unsigned int)tree2->getBV(root2).rightChild();
446  convertBV(tree2->getBV(child).bv, tf2, aabb2);
447  d = aabb1.distance(aabb2);
448 
449  if (d < dresult->min_distance) {
450  if (OcTreeMeshDistanceRecurse(tree1, root1, bv1, tree2, child, tf1,
451  tf2))
452  return true;
453  }
454  }
455 
456  return false;
457  }
458 
460  template <typename BV>
461  bool OcTreeMeshIntersectRecurse(const OcTree* tree1,
462  const OcTree::OcTreeNode* root1,
463  const AABB& bv1, const BVHModel<BV>* tree2,
464  unsigned int root2, const Transform3s& tf1,
465  const Transform3s& tf2) const {
466  // FIXME(jmirabel) I do not understand why the BVHModel was traversed. The
467  // code in this if(!root1) did not output anything so the empty OcTree is
468  // considered free. Should an empty OcTree be considered free ?
469 
470  // Empty OcTree is considered free.
471  if (!root1) return false;
472  BVNode<BV> const& bvn2 = tree2->getBV(root2);
473 
478  if (tree1->isNodeFree(root1))
479  return false;
480  else if ((tree1->isNodeUncertain(root1) || tree2->isUncertain()))
481  return false;
482  else {
483  OBB obb1, obb2;
484  convertBV(bv1, tf1, obb1);
485  convertBV(bvn2.bv, tf2, obb2);
486  Scalar sqrDistLowerBound;
487  if (!obb1.overlap(obb2, *crequest, sqrDistLowerBound)) {
488  internal::updateDistanceLowerBoundFromBV(*crequest, *cresult,
489  sqrDistLowerBound);
490  return false;
491  }
492  }
493 
494  // Check if leaf collides.
495  if (!tree1->nodeHasChildren(root1) && bvn2.isLeaf()) {
496  assert(tree1->isNodeOccupied(root1)); // it isn't free nor uncertain.
497  Box box;
498  Transform3s box_tf;
499  constructBox(bv1, tf1, box, box_tf);
500  if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
501  box.computeLocalAABB();
502  }
503 
504  size_t primitive_id = static_cast<size_t>(bvn2.primitiveId());
505  const Triangle& tri_id = (*(tree2->tri_indices))[primitive_id];
506  const TriangleP tri((*(tree2->vertices))[tri_id[0]],
507  (*(tree2->vertices))[tri_id[1]],
508  (*(tree2->vertices))[tri_id[2]]);
509 
510  // When reaching this point, `this->solver` has already been set up
511  // by the CollisionRequest `this->crequest`.
512  // The only thing we need to (and can) pass to `ShapeShapeDistance` is
513  // whether or not penetration information is should be computed in case of
514  // collision.
515  const bool compute_penetration = this->crequest->enable_contact ||
516  (this->crequest->security_margin < 0);
517  Vec3s c1, c2, normal;
518  const Scalar distance = internal::ShapeShapeDistance<Box, TriangleP>(
519  &box, box_tf, &tri, tf2, this->solver, compute_penetration, c1, c2,
520  normal);
521  const Scalar distToCollision = distance - this->crequest->security_margin;
522 
523  internal::updateDistanceLowerBoundFromLeaf(
524  *(this->crequest), *(this->cresult), distToCollision, c1, c2, normal);
525 
526  if (cresult->numContacts() < crequest->num_max_contacts) {
527  if (distToCollision <= crequest->collision_distance_threshold) {
528  cresult->addContact(Contact(
529  tree1, tree2, (int)(root1 - tree1->getRoot()),
530  static_cast<int>(primitive_id), c1, c2, normal, distance));
531  }
532  }
533  return crequest->isSatisfied(*cresult);
534  }
535 
536  // Determine which tree to traverse first.
537  if (bvn2.isLeaf() ||
538  (tree1->nodeHasChildren(root1) && (bv1.size() > bvn2.bv.size()))) {
539  for (unsigned int i = 0; i < 8; ++i) {
540  if (tree1->nodeChildExists(root1, i)) {
541  const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
542  AABB child_bv;
543  computeChildBV(bv1, i, child_bv);
544 
545  if (OcTreeMeshIntersectRecurse(tree1, child, child_bv, tree2, root2,
546  tf1, tf2))
547  return true;
548  }
549  }
550  } else {
551  if (OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2,
552  (unsigned int)bvn2.leftChild(), tf1, tf2))
553  return true;
554 
555  if (OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2,
556  (unsigned int)bvn2.rightChild(), tf1, tf2))
557  return true;
558  }
559 
560  return false;
561  }
562 
564  template <typename BV>
565  bool OcTreeHeightFieldIntersectRecurse(
566  const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1,
567  const HeightField<BV>* tree2, unsigned int root2, const Transform3s& tf1,
568  const Transform3s& tf2, Scalar& sqrDistLowerBound) const {
569  // FIXME(jmirabel) I do not understand why the BVHModel was traversed. The
570  // code in this if(!root1) did not output anything so the empty OcTree is
571  // considered free. Should an empty OcTree be considered free ?
572 
573  // Empty OcTree is considered free.
574  if (!root1) return false;
575  HFNode<BV> const& bvn2 = tree2->getBV(root2);
576 
581  if (tree1->isNodeFree(root1))
582  return false;
583  else if ((tree1->isNodeUncertain(root1) || tree2->isUncertain()))
584  return false;
585  else {
586  OBB obb1, obb2;
587  convertBV(bv1, tf1, obb1);
588  convertBV(bvn2.bv, tf2, obb2);
589  Scalar sqrDistLowerBound_;
590  if (!obb1.overlap(obb2, *crequest, sqrDistLowerBound_)) {
591  if (sqrDistLowerBound_ < sqrDistLowerBound)
592  sqrDistLowerBound = sqrDistLowerBound_;
593  internal::updateDistanceLowerBoundFromBV(*crequest, *cresult,
594  sqrDistLowerBound);
595  return false;
596  }
597  }
598 
599  // Check if leaf collides.
600  if (!tree1->nodeHasChildren(root1) && bvn2.isLeaf()) {
601  assert(tree1->isNodeOccupied(root1)); // it isn't free nor uncertain.
602  Box box;
603  Transform3s box_tf;
604  constructBox(bv1, tf1, box, box_tf);
605  if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
606  box.computeLocalAABB();
607  }
608 
609  typedef Convex<Triangle> ConvexTriangle;
610  ConvexTriangle convex1, convex2;
611  int convex1_active_faces, convex2_active_faces;
612  details::buildConvexTriangles(bvn2, *tree2, convex1, convex1_active_faces,
613  convex2, convex2_active_faces);
614  if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
615  convex1.computeLocalAABB();
616  convex2.computeLocalAABB();
617  }
618 
619  Vec3s c1, c2, normal, normal_top;
620  Scalar distance;
621  bool hfield_witness_is_on_bin_side;
622 
623  bool collision = details::shapeDistance<Triangle, Box, 0>(
624  solver, *crequest, convex1, convex1_active_faces, convex2,
625  convex2_active_faces, tf2, box, box_tf, distance, c2, c1, normal,
626  normal_top, hfield_witness_is_on_bin_side);
627 
628  Scalar distToCollision =
629  distance - crequest->security_margin * (normal_top.dot(normal));
630 
631  if (distToCollision <= crequest->collision_distance_threshold) {
632  sqrDistLowerBound = 0;
633  if (crequest->num_max_contacts > cresult->numContacts()) {
634  if (normal_top.isApprox(normal) &&
635  (collision || !hfield_witness_is_on_bin_side)) {
636  cresult->addContact(
637  Contact(tree1, tree2, (int)(root1 - tree1->getRoot()),
638  (int)Contact::NONE, c1, c2, -normal, distance));
639  }
640  }
641  } else
642  sqrDistLowerBound = distToCollision * distToCollision;
643 
644  // const Vec3s c1 = contact_point - distance * 0.5 * normal;
645  // const Vec3s c2 = contact_point + distance * 0.5 * normal;
646  internal::updateDistanceLowerBoundFromLeaf(
647  *crequest, *cresult, distToCollision, c1, c2, -normal);
648 
649  assert(cresult->isCollision() || sqrDistLowerBound > 0);
650  return crequest->isSatisfied(*cresult);
651  }
652 
653  // Determine which tree to traverse first.
654  if (bvn2.isLeaf() ||
655  (tree1->nodeHasChildren(root1) && (bv1.size() > bvn2.bv.size()))) {
656  for (unsigned int i = 0; i < 8; ++i) {
657  if (tree1->nodeChildExists(root1, i)) {
658  const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
659  AABB child_bv;
660  computeChildBV(bv1, i, child_bv);
661 
662  if (OcTreeHeightFieldIntersectRecurse(tree1, child, child_bv, tree2,
663  root2, tf1, tf2,
664  sqrDistLowerBound))
665  return true;
666  }
667  }
668  } else {
669  if (OcTreeHeightFieldIntersectRecurse(tree1, root1, bv1, tree2,
670  (unsigned int)bvn2.leftChild(), tf1,
671  tf2, sqrDistLowerBound))
672  return true;
673 
674  if (OcTreeHeightFieldIntersectRecurse(tree1, root1, bv1, tree2,
675  (unsigned int)bvn2.rightChild(),
676  tf1, tf2, sqrDistLowerBound))
677  return true;
678  }
679 
680  return false;
681  }
682 
684  template <typename BV>
685  bool HeightFieldOcTreeIntersectRecurse(
686  const HeightField<BV>* tree1, unsigned int root1, const OcTree* tree2,
687  const OcTree::OcTreeNode* root2, const AABB& bv2, const Transform3s& tf1,
688  const Transform3s& tf2, Scalar& sqrDistLowerBound) const {
689  // FIXME(jmirabel) I do not understand why the BVHModel was traversed. The
690  // code in this if(!root1) did not output anything so the empty OcTree is
691  // considered free. Should an empty OcTree be considered free ?
692 
693  // Empty OcTree is considered free.
694  if (!root2) return false;
695  HFNode<BV> const& bvn1 = tree1->getBV(root1);
696 
701  if (tree2->isNodeFree(root2))
702  return false;
703  else if ((tree2->isNodeUncertain(root2) || tree1->isUncertain()))
704  return false;
705  else {
706  OBB obb1, obb2;
707  convertBV(bvn1.bv, tf1, obb1);
708  convertBV(bv2, tf2, obb2);
709  Scalar sqrDistLowerBound_;
710  if (!obb2.overlap(obb1, *crequest, sqrDistLowerBound_)) {
711  if (sqrDistLowerBound_ < sqrDistLowerBound)
712  sqrDistLowerBound = sqrDistLowerBound_;
713  internal::updateDistanceLowerBoundFromBV(*crequest, *cresult,
714  sqrDistLowerBound);
715  return false;
716  }
717  }
718 
719  // Check if leaf collides.
720  if (!tree2->nodeHasChildren(root2) && bvn1.isLeaf()) {
721  assert(tree2->isNodeOccupied(root2)); // it isn't free nor uncertain.
722  Box box;
723  Transform3s box_tf;
724  constructBox(bv2, tf2, box, box_tf);
725  if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
726  box.computeLocalAABB();
727  }
728 
729  typedef Convex<Triangle> ConvexTriangle;
730  ConvexTriangle convex1, convex2;
731  int convex1_active_faces, convex2_active_faces;
732  details::buildConvexTriangles(bvn1, *tree1, convex1, convex1_active_faces,
733  convex2, convex2_active_faces);
734  if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
735  convex1.computeLocalAABB();
736  convex2.computeLocalAABB();
737  }
738 
739  Vec3s c1, c2, normal, normal_top;
740  Scalar distance;
741  bool hfield_witness_is_on_bin_side;
742 
743  bool collision = details::shapeDistance<Triangle, Box, 0>(
744  solver, *crequest, convex1, convex1_active_faces, convex2,
745  convex2_active_faces, tf1, box, box_tf, distance, c1, c2, normal,
746  normal_top, hfield_witness_is_on_bin_side);
747 
748  Scalar distToCollision =
749  distance - crequest->security_margin * (normal_top.dot(normal));
750 
751  if (distToCollision <= crequest->collision_distance_threshold) {
752  sqrDistLowerBound = 0;
753  if (crequest->num_max_contacts > cresult->numContacts()) {
754  if (normal_top.isApprox(normal) &&
755  (collision || !hfield_witness_is_on_bin_side)) {
756  cresult->addContact(Contact(tree1, tree2, (int)Contact::NONE,
757  (int)(root2 - tree2->getRoot()), c1, c2,
758  normal, distance));
759  }
760  }
761  } else
762  sqrDistLowerBound = distToCollision * distToCollision;
763 
764  // const Vec3s c1 = contact_point - distance * 0.5 * normal;
765  // const Vec3s c2 = contact_point + distance * 0.5 * normal;
766  internal::updateDistanceLowerBoundFromLeaf(
767  *crequest, *cresult, distToCollision, c1, c2, normal);
768 
769  assert(cresult->isCollision() || sqrDistLowerBound > 0);
770  return crequest->isSatisfied(*cresult);
771  }
772 
773  // Determine which tree to traverse first.
774  if (bvn1.isLeaf() ||
775  (tree2->nodeHasChildren(root2) && (bv2.size() > bvn1.bv.size()))) {
776  for (unsigned int i = 0; i < 8; ++i) {
777  if (tree2->nodeChildExists(root2, i)) {
778  const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i);
779  AABB child_bv;
780  computeChildBV(bv2, i, child_bv);
781 
782  if (HeightFieldOcTreeIntersectRecurse(tree1, root1, tree2, child,
783  child_bv, tf1, tf2,
784  sqrDistLowerBound))
785  return true;
786  }
787  }
788  } else {
789  if (HeightFieldOcTreeIntersectRecurse(
790  tree1, (unsigned int)bvn1.leftChild(), tree2, root2, bv2, tf1,
791  tf2, sqrDistLowerBound))
792  return true;
793 
794  if (HeightFieldOcTreeIntersectRecurse(
795  tree1, (unsigned int)bvn1.rightChild(), tree2, root2, bv2, tf1,
796  tf2, sqrDistLowerBound))
797  return true;
798  }
799 
800  return false;
801  }
802 
803  bool OcTreeDistanceRecurse(const OcTree* tree1,
804  const OcTree::OcTreeNode* root1, const AABB& bv1,
805  const OcTree* tree2,
806  const OcTree::OcTreeNode* root2, const AABB& bv2,
807  const Transform3s& tf1,
808  const Transform3s& tf2) const {
809  if (!tree1->nodeHasChildren(root1) && !tree2->nodeHasChildren(root2)) {
810  if (tree1->isNodeOccupied(root1) && tree2->isNodeOccupied(root2)) {
811  Box box1, box2;
812  Transform3s box1_tf, box2_tf;
813  constructBox(bv1, tf1, box1, box1_tf);
814  constructBox(bv2, tf2, box2, box2_tf);
815 
816  if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
817  box1.computeLocalAABB();
818  box2.computeLocalAABB();
819  }
820 
821  Vec3s p1, p2, normal;
822  const Scalar distance = internal::ShapeShapeDistance<Box, Box>(
823  &box1, box1_tf, &box2, box2_tf, this->solver,
824  this->drequest->enable_signed_distance, p1, p2, normal);
825 
826  this->dresult->update(distance, tree1, tree2,
827  (int)(root1 - tree1->getRoot()),
828  (int)(root2 - tree2->getRoot()), p1, p2, normal);
829 
830  return drequest->isSatisfied(*dresult);
831  } else
832  return false;
833  }
834 
835  if (!tree1->isNodeOccupied(root1) || !tree2->isNodeOccupied(root2))
836  return false;
837 
838  if (!tree2->nodeHasChildren(root2) ||
839  (tree1->nodeHasChildren(root1) && (bv1.size() > bv2.size()))) {
840  for (unsigned int i = 0; i < 8; ++i) {
841  if (tree1->nodeChildExists(root1, i)) {
842  const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
843  AABB child_bv;
844  computeChildBV(bv1, i, child_bv);
845 
846  Scalar d;
847  AABB aabb1, aabb2;
848  convertBV(bv1, tf1, aabb1);
849  convertBV(bv2, tf2, aabb2);
850  d = aabb1.distance(aabb2);
851 
852  if (d < dresult->min_distance) {
853  if (OcTreeDistanceRecurse(tree1, child, child_bv, tree2, root2, bv2,
854  tf1, tf2))
855  return true;
856  }
857  }
858  }
859  } else {
860  for (unsigned int i = 0; i < 8; ++i) {
861  if (tree2->nodeChildExists(root2, i)) {
862  const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i);
863  AABB child_bv;
864  computeChildBV(bv2, i, child_bv);
865 
866  Scalar d;
867  AABB aabb1, aabb2;
868  convertBV(bv1, tf1, aabb1);
869  convertBV(bv2, tf2, aabb2);
870  d = aabb1.distance(aabb2);
871 
872  if (d < dresult->min_distance) {
873  if (OcTreeDistanceRecurse(tree1, root1, bv1, tree2, child, child_bv,
874  tf1, tf2))
875  return true;
876  }
877  }
878  }
879  }
880 
881  return false;
882  }
883 
884  bool OcTreeIntersectRecurse(const OcTree* tree1,
885  const OcTree::OcTreeNode* root1, const AABB& bv1,
886  const OcTree* tree2,
887  const OcTree::OcTreeNode* root2, const AABB& bv2,
888  const Transform3s& tf1,
889  const Transform3s& tf2) const {
890  // Empty OcTree is considered free.
891  if (!root1) return false;
892  if (!root2) return false;
893 
898  if (tree1->isNodeFree(root1) || tree2->isNodeFree(root2))
899  return false;
900  else if ((tree1->isNodeUncertain(root1) || tree2->isNodeUncertain(root2)))
901  return false;
902 
903  bool bothAreLeaves =
904  (!tree1->nodeHasChildren(root1) && !tree2->nodeHasChildren(root2));
905  if (!bothAreLeaves || !crequest->enable_contact) {
906  OBB obb1, obb2;
907  convertBV(bv1, tf1, obb1);
908  convertBV(bv2, tf2, obb2);
909  Scalar sqrDistLowerBound;
910  if (!obb1.overlap(obb2, *crequest, sqrDistLowerBound)) {
911  if (cresult->distance_lower_bound > 0 &&
912  sqrDistLowerBound <
913  cresult->distance_lower_bound * cresult->distance_lower_bound)
914  cresult->distance_lower_bound =
915  sqrt(sqrDistLowerBound) - crequest->security_margin;
916  return false;
917  }
918  if (crequest->enable_contact) { // Overlap
919  if (cresult->numContacts() < crequest->num_max_contacts)
920  cresult->addContact(
921  Contact(tree1, tree2, static_cast<int>(root1 - tree1->getRoot()),
922  static_cast<int>(root2 - tree2->getRoot())));
923  return crequest->isSatisfied(*cresult);
924  }
925  }
926 
927  // Both node are leaves
928  if (bothAreLeaves) {
929  assert(tree1->isNodeOccupied(root1) && tree2->isNodeOccupied(root2));
930 
931  Box box1, box2;
932  Transform3s box1_tf, box2_tf;
933  constructBox(bv1, tf1, box1, box1_tf);
934  constructBox(bv2, tf2, box2, box2_tf);
935 
936  if (this->solver->gjk_initial_guess ==
937  GJKInitialGuess::BoundingVolumeGuess) {
938  box1.computeLocalAABB();
939  box2.computeLocalAABB();
940  }
941 
942  // When reaching this point, `this->solver` has already been set up
943  // by the CollisionRequest `this->request`.
944  // The only thing we need to (and can) pass to `ShapeShapeDistance` is
945  // whether or not penetration information is should be computed in case of
946  // collision.
947  const bool compute_penetration = (this->crequest->enable_contact ||
948  (this->crequest->security_margin < 0));
949  Vec3s c1, c2, normal;
950  Scalar distance = internal::ShapeShapeDistance<Box, Box>(
951  &box1, box1_tf, &box2, box2_tf, this->solver, compute_penetration, c1,
952  c2, normal);
953 
954  const Scalar distToCollision = distance - this->crequest->security_margin;
955 
956  internal::updateDistanceLowerBoundFromLeaf(
957  *(this->crequest), *(this->cresult), distToCollision, c1, c2, normal);
958 
959  if (this->cresult->numContacts() < this->crequest->num_max_contacts) {
960  if (distToCollision <= this->crequest->collision_distance_threshold)
961  this->cresult->addContact(
962  Contact(tree1, tree2, static_cast<int>(root1 - tree1->getRoot()),
963  static_cast<int>(root2 - tree2->getRoot()), c1, c2,
964  normal, distance));
965  }
966 
967  return crequest->isSatisfied(*cresult);
968  }
969 
970  // Determine which tree to traverse first.
971  if (!tree2->nodeHasChildren(root2) ||
972  (tree1->nodeHasChildren(root1) && (bv1.size() > bv2.size()))) {
973  for (unsigned int i = 0; i < 8; ++i) {
974  if (tree1->nodeChildExists(root1, i)) {
975  const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
976  AABB child_bv;
977  computeChildBV(bv1, i, child_bv);
978 
979  if (OcTreeIntersectRecurse(tree1, child, child_bv, tree2, root2, bv2,
980  tf1, tf2))
981  return true;
982  }
983  }
984  } else {
985  for (unsigned int i = 0; i < 8; ++i) {
986  if (tree2->nodeChildExists(root2, i)) {
987  const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i);
988  AABB child_bv;
989  computeChildBV(bv2, i, child_bv);
990 
991  if (OcTreeIntersectRecurse(tree1, root1, bv1, tree2, child, child_bv,
992  tf1, tf2))
993  return true;
994  }
995  }
996  }
997 
998  return false;
999  }
1000 };
1001 
1004 
1006 class COAL_DLLAPI OcTreeCollisionTraversalNode
1007  : public CollisionTraversalNodeBase {
1008  public:
1009  OcTreeCollisionTraversalNode(const CollisionRequest& request)
1010  : CollisionTraversalNodeBase(request) {
1011  model1 = NULL;
1012  model2 = NULL;
1013 
1014  otsolver = NULL;
1015  }
1016 
1017  bool BVDisjoints(unsigned, unsigned, Scalar&) const { return false; }
1018 
1019  void leafCollides(unsigned, unsigned, Scalar& sqrDistLowerBound) const {
1020  otsolver->OcTreeIntersect(model1, model2, tf1, tf2, request, *result);
1021  sqrDistLowerBound = std::max((Scalar)0, result->distance_lower_bound);
1022  sqrDistLowerBound *= sqrDistLowerBound;
1023  }
1024 
1025  const OcTree* model1;
1026  const OcTree* model2;
1027 
1028  Transform3s tf1, tf2;
1029 
1030  const OcTreeSolver* otsolver;
1031 };
1032 
1034 template <typename S>
1035 class COAL_DLLAPI ShapeOcTreeCollisionTraversalNode
1036  : public CollisionTraversalNodeBase {
1037  public:
1038  ShapeOcTreeCollisionTraversalNode(const CollisionRequest& request)
1039  : CollisionTraversalNodeBase(request) {
1040  model1 = NULL;
1041  model2 = NULL;
1042 
1043  otsolver = NULL;
1044  }
1045 
1046  bool BVDisjoints(unsigned int, unsigned int, Scalar&) const { return false; }
1047 
1048  void leafCollides(unsigned int, unsigned int,
1049  Scalar& sqrDistLowerBound) const {
1050  otsolver->OcTreeShapeIntersect(model2, *model1, tf2, tf1, request, *result);
1051  sqrDistLowerBound = std::max((Scalar)0, result->distance_lower_bound);
1052  sqrDistLowerBound *= sqrDistLowerBound;
1053  }
1054 
1055  const S* model1;
1056  const OcTree* model2;
1057 
1058  Transform3s tf1, tf2;
1059 
1060  const OcTreeSolver* otsolver;
1061 };
1062 
1064 
1065 template <typename S>
1066 class COAL_DLLAPI OcTreeShapeCollisionTraversalNode
1067  : public CollisionTraversalNodeBase {
1068  public:
1069  OcTreeShapeCollisionTraversalNode(const CollisionRequest& request)
1070  : CollisionTraversalNodeBase(request) {
1071  model1 = NULL;
1072  model2 = NULL;
1073 
1074  otsolver = NULL;
1075  }
1076 
1077  bool BVDisjoints(unsigned int, unsigned int, coal::Scalar&) const {
1078  return false;
1079  }
1080 
1081  void leafCollides(unsigned int, unsigned int,
1082  Scalar& sqrDistLowerBound) const {
1083  otsolver->OcTreeShapeIntersect(model1, *model2, tf1, tf2, request, *result);
1084  sqrDistLowerBound = std::max((Scalar)0, result->distance_lower_bound);
1085  sqrDistLowerBound *= sqrDistLowerBound;
1086  }
1087 
1088  const OcTree* model1;
1089  const S* model2;
1090 
1091  Transform3s tf1, tf2;
1092 
1093  const OcTreeSolver* otsolver;
1094 };
1095 
1097 template <typename BV>
1098 class COAL_DLLAPI MeshOcTreeCollisionTraversalNode
1099  : public CollisionTraversalNodeBase {
1100  public:
1101  MeshOcTreeCollisionTraversalNode(const CollisionRequest& request)
1102  : CollisionTraversalNodeBase(request) {
1103  model1 = NULL;
1104  model2 = NULL;
1105 
1106  otsolver = NULL;
1107  }
1108 
1109  bool BVDisjoints(unsigned int, unsigned int, Scalar&) const { return false; }
1110 
1111  void leafCollides(unsigned int, unsigned int,
1112  Scalar& sqrDistLowerBound) const {
1113  otsolver->OcTreeMeshIntersect(model2, model1, tf2, tf1, request, *result);
1114  sqrDistLowerBound = std::max((Scalar)0, result->distance_lower_bound);
1115  sqrDistLowerBound *= sqrDistLowerBound;
1116  }
1117 
1118  const BVHModel<BV>* model1;
1119  const OcTree* model2;
1120 
1121  Transform3s tf1, tf2;
1122 
1123  const OcTreeSolver* otsolver;
1124 };
1125 
1127 template <typename BV>
1128 class COAL_DLLAPI OcTreeMeshCollisionTraversalNode
1129  : public CollisionTraversalNodeBase {
1130  public:
1131  OcTreeMeshCollisionTraversalNode(const CollisionRequest& request)
1132  : CollisionTraversalNodeBase(request) {
1133  model1 = NULL;
1134  model2 = NULL;
1135 
1136  otsolver = NULL;
1137  }
1138 
1139  bool BVDisjoints(unsigned int, unsigned int, Scalar&) const { return false; }
1140 
1141  void leafCollides(unsigned int, unsigned int,
1142  Scalar& sqrDistLowerBound) const {
1143  otsolver->OcTreeMeshIntersect(model1, model2, tf1, tf2, request, *result);
1144  sqrDistLowerBound = std::max((Scalar)0, result->distance_lower_bound);
1145  sqrDistLowerBound *= sqrDistLowerBound;
1146  }
1147 
1148  const OcTree* model1;
1149  const BVHModel<BV>* model2;
1150 
1151  Transform3s tf1, tf2;
1152 
1153  const OcTreeSolver* otsolver;
1154 };
1155 
1157 template <typename BV>
1158 class COAL_DLLAPI OcTreeHeightFieldCollisionTraversalNode
1159  : public CollisionTraversalNodeBase {
1160  public:
1161  OcTreeHeightFieldCollisionTraversalNode(const CollisionRequest& request)
1162  : CollisionTraversalNodeBase(request) {
1163  model1 = NULL;
1164  model2 = NULL;
1165 
1166  otsolver = NULL;
1167  }
1168 
1169  bool BVDisjoints(unsigned int, unsigned int, Scalar&) const { return false; }
1170 
1171  void leafCollides(unsigned int, unsigned int,
1172  Scalar& sqrDistLowerBound) const {
1173  otsolver->OcTreeHeightFieldIntersect(model1, model2, tf1, tf2, request,
1174  *result, sqrDistLowerBound);
1175  }
1176 
1177  const OcTree* model1;
1178  const HeightField<BV>* model2;
1179 
1180  Transform3s tf1, tf2;
1181 
1182  const OcTreeSolver* otsolver;
1183 };
1184 
1186 template <typename BV>
1187 class COAL_DLLAPI HeightFieldOcTreeCollisionTraversalNode
1188  : public CollisionTraversalNodeBase {
1189  public:
1190  HeightFieldOcTreeCollisionTraversalNode(const CollisionRequest& request)
1191  : CollisionTraversalNodeBase(request) {
1192  model1 = NULL;
1193  model2 = NULL;
1194 
1195  otsolver = NULL;
1196  }
1197 
1198  bool BVDisjoints(unsigned int, unsigned int, Scalar&) const { return false; }
1199 
1200  void leafCollides(unsigned int, unsigned int,
1201  Scalar& sqrDistLowerBound) const {
1202  otsolver->HeightFieldOcTreeIntersect(model1, model2, tf1, tf2, request,
1203  *result, sqrDistLowerBound);
1204  }
1205 
1206  const HeightField<BV>* model1;
1207  const OcTree* model2;
1208 
1209  Transform3s tf1, tf2;
1210 
1211  const OcTreeSolver* otsolver;
1212 };
1213 
1215 
1218 
1220 class COAL_DLLAPI OcTreeDistanceTraversalNode
1221  : public DistanceTraversalNodeBase {
1222  public:
1223  OcTreeDistanceTraversalNode() {
1224  model1 = NULL;
1225  model2 = NULL;
1226 
1227  otsolver = NULL;
1228  }
1229 
1230  Scalar BVDistanceLowerBound(unsigned, unsigned) const { return -1; }
1231 
1232  bool BVDistanceLowerBound(unsigned, unsigned, Scalar&) const { return false; }
1233 
1234  void leafComputeDistance(unsigned, unsigned int) const {
1235  otsolver->OcTreeDistance(model1, model2, tf1, tf2, request, *result);
1236  }
1237 
1238  const OcTree* model1;
1239  const OcTree* model2;
1240 
1241  const OcTreeSolver* otsolver;
1242 };
1243 
1245 template <typename Shape>
1246 class COAL_DLLAPI ShapeOcTreeDistanceTraversalNode
1247  : public DistanceTraversalNodeBase {
1248  public:
1249  ShapeOcTreeDistanceTraversalNode() {
1250  model1 = NULL;
1251  model2 = NULL;
1252 
1253  otsolver = NULL;
1254  }
1255 
1256  Scalar BVDistanceLowerBound(unsigned int, unsigned int) const { return -1; }
1257 
1258  void leafComputeDistance(unsigned int, unsigned int) const {
1259  otsolver->OcTreeShapeDistance(model2, *model1, tf2, tf1, request, *result);
1260  }
1261 
1262  const Shape* model1;
1263  const OcTree* model2;
1264 
1265  const OcTreeSolver* otsolver;
1266 };
1267 
1269 template <typename Shape>
1270 class COAL_DLLAPI OcTreeShapeDistanceTraversalNode
1271  : public DistanceTraversalNodeBase {
1272  public:
1273  OcTreeShapeDistanceTraversalNode() {
1274  model1 = NULL;
1275  model2 = NULL;
1276 
1277  otsolver = NULL;
1278  }
1279 
1280  Scalar BVDistanceLowerBound(unsigned int, unsigned int) const { return -1; }
1281 
1282  void leafComputeDistance(unsigned int, unsigned int) const {
1283  otsolver->OcTreeShapeDistance(model1, *model2, tf1, tf2, request, *result);
1284  }
1285 
1286  const OcTree* model1;
1287  const Shape* model2;
1288 
1289  const OcTreeSolver* otsolver;
1290 };
1291 
1293 template <typename BV>
1294 class COAL_DLLAPI MeshOcTreeDistanceTraversalNode
1295  : public DistanceTraversalNodeBase {
1296  public:
1297  MeshOcTreeDistanceTraversalNode() {
1298  model1 = NULL;
1299  model2 = NULL;
1300 
1301  otsolver = NULL;
1302  }
1303 
1304  Scalar BVDistanceLowerBound(unsigned int, unsigned int) const { return -1; }
1305 
1306  void leafComputeDistance(unsigned int, unsigned int) const {
1307  otsolver->OcTreeMeshDistance(model2, model1, tf2, tf1, request, *result);
1308  }
1309 
1310  const BVHModel<BV>* model1;
1311  const OcTree* model2;
1312 
1313  const OcTreeSolver* otsolver;
1314 };
1315 
1317 template <typename BV>
1318 class COAL_DLLAPI OcTreeMeshDistanceTraversalNode
1319  : public DistanceTraversalNodeBase {
1320  public:
1321  OcTreeMeshDistanceTraversalNode() {
1322  model1 = NULL;
1323  model2 = NULL;
1324 
1325  otsolver = NULL;
1326  }
1327 
1328  Scalar BVDistanceLowerBound(unsigned int, unsigned int) const { return -1; }
1329 
1330  void leafComputeDistance(unsigned int, unsigned int) const {
1331  otsolver->OcTreeMeshDistance(model1, model2, tf1, tf2, request, *result);
1332  }
1333 
1334  const OcTree* model1;
1335  const BVHModel<BV>* model2;
1336 
1337  const OcTreeSolver* otsolver;
1338 };
1339 
1341 
1342 } // namespace coal
1343 
1345 
1346 #endif
BVH_model.h
coal::OcTree::OcTreeNode
octomap::OcTreeNode OcTreeNode
Definition: octree.h:63
collision_data.h
COAL_DLLAPI
#define COAL_DLLAPI
Definition: config.hh:88
geometric_shapes_utility.h
coal::distance
Scalar distance(const Matrix3s &R0, const Vec3s &T0, const kIOS &b1, const kIOS &b2, Vec3s *P=NULL, Vec3s *Q=NULL)
Approximate distance between two kIOS bounding volumes.
coal::internal::updateDistanceLowerBoundFromBV
void updateDistanceLowerBoundFromBV(const CollisionRequest &, CollisionResult &res, const Scalar sqrDistLowerBound)
Definition: collision_data.h:1171
coal::internal::updateDistanceLowerBoundFromLeaf
void updateDistanceLowerBoundFromLeaf(const CollisionRequest &, CollisionResult &res, const Scalar &distance, const Vec3s &p0, const Vec3s &p1, const Vec3s &normal)
Definition: collision_data.h:1180
coal
Main namespace.
Definition: broadphase_bruteforce.h:44
coal::constructBox
void constructBox(const AABB &bv, Box &box, Transform3s &tf)
construct a box shape (with a configuration) from a given bounding volume
coal::Vec3s
Eigen::Matrix< Scalar, 3, 1 > Vec3s
Definition: data_types.h:70
coal::Scalar
double Scalar
Definition: data_types.h:68
coal::BoundingVolumeGuess
@ BoundingVolumeGuess
Definition: data_types.h:105
shape_shape_func.h
coal::Contact::NONE
static const int NONE
invalid contact primitive information
Definition: collision_data.h:108
coal::DistanceResult::NONE
static const int NONE
invalid contact primitive information
Definition: collision_data.h:1085
traversal_node_base.h
traversal_node_hfield_shape.h