coal/internal/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_, CoalScalar& 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  CoalScalar& 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 CoalScalar 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  CoalScalar 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  CoalScalar 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 CoalScalar distance =
395  internal::ShapeShapeDistance<Box, TriangleP>(
396  &box, box_tf, &tri, tf2, this->solver,
397  this->drequest->enable_signed_distance, p1, p2, normal);
398 
399  this->dresult->update(distance, tree1, tree2,
400  (int)(root1 - tree1->getRoot()),
401  static_cast<int>(primitive_id), p1, p2, normal);
402 
403  return this->drequest->isSatisfied(*dresult);
404  } else
405  return false;
406  }
407 
408  if (!tree1->isNodeOccupied(root1)) return false;
409 
410  if (tree2->getBV(root2).isLeaf() ||
411  (tree1->nodeHasChildren(root1) &&
412  (bv1.size() > tree2->getBV(root2).bv.size()))) {
413  for (unsigned int i = 0; i < 8; ++i) {
414  if (tree1->nodeChildExists(root1, i)) {
415  const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
416  AABB child_bv;
417  computeChildBV(bv1, i, child_bv);
418 
419  CoalScalar d;
420  AABB aabb1, aabb2;
421  convertBV(child_bv, tf1, aabb1);
422  convertBV(tree2->getBV(root2).bv, tf2, aabb2);
423  d = aabb1.distance(aabb2);
424 
425  if (d < dresult->min_distance) {
426  if (OcTreeMeshDistanceRecurse(tree1, child, child_bv, tree2, root2,
427  tf1, tf2))
428  return true;
429  }
430  }
431  }
432  } else {
433  CoalScalar d;
434  AABB aabb1, aabb2;
435  convertBV(bv1, tf1, aabb1);
436  unsigned int child = (unsigned int)tree2->getBV(root2).leftChild();
437  convertBV(tree2->getBV(child).bv, tf2, aabb2);
438  d = aabb1.distance(aabb2);
439 
440  if (d < dresult->min_distance) {
441  if (OcTreeMeshDistanceRecurse(tree1, root1, bv1, tree2, child, tf1,
442  tf2))
443  return true;
444  }
445 
446  child = (unsigned int)tree2->getBV(root2).rightChild();
447  convertBV(tree2->getBV(child).bv, tf2, aabb2);
448  d = aabb1.distance(aabb2);
449 
450  if (d < dresult->min_distance) {
451  if (OcTreeMeshDistanceRecurse(tree1, root1, bv1, tree2, child, tf1,
452  tf2))
453  return true;
454  }
455  }
456 
457  return false;
458  }
459 
461  template <typename BV>
462  bool OcTreeMeshIntersectRecurse(const OcTree* tree1,
463  const OcTree::OcTreeNode* root1,
464  const AABB& bv1, const BVHModel<BV>* tree2,
465  unsigned int root2, const Transform3s& tf1,
466  const Transform3s& tf2) const {
467  // FIXME(jmirabel) I do not understand why the BVHModel was traversed. The
468  // code in this if(!root1) did not output anything so the empty OcTree is
469  // considered free. Should an empty OcTree be considered free ?
470 
471  // Empty OcTree is considered free.
472  if (!root1) return false;
473  BVNode<BV> const& bvn2 = tree2->getBV(root2);
474 
479  if (tree1->isNodeFree(root1))
480  return false;
481  else if ((tree1->isNodeUncertain(root1) || tree2->isUncertain()))
482  return false;
483  else {
484  OBB obb1, obb2;
485  convertBV(bv1, tf1, obb1);
486  convertBV(bvn2.bv, tf2, obb2);
487  CoalScalar sqrDistLowerBound;
488  if (!obb1.overlap(obb2, *crequest, sqrDistLowerBound)) {
489  internal::updateDistanceLowerBoundFromBV(*crequest, *cresult,
490  sqrDistLowerBound);
491  return false;
492  }
493  }
494 
495  // Check if leaf collides.
496  if (!tree1->nodeHasChildren(root1) && bvn2.isLeaf()) {
497  assert(tree1->isNodeOccupied(root1)); // it isn't free nor uncertain.
498  Box box;
499  Transform3s box_tf;
500  constructBox(bv1, tf1, box, box_tf);
501  if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
502  box.computeLocalAABB();
503  }
504 
505  size_t primitive_id = static_cast<size_t>(bvn2.primitiveId());
506  const Triangle& tri_id = (*(tree2->tri_indices))[primitive_id];
507  const TriangleP tri((*(tree2->vertices))[tri_id[0]],
508  (*(tree2->vertices))[tri_id[1]],
509  (*(tree2->vertices))[tri_id[2]]);
510 
511  // When reaching this point, `this->solver` has already been set up
512  // by the CollisionRequest `this->crequest`.
513  // The only thing we need to (and can) pass to `ShapeShapeDistance` is
514  // whether or not penetration information is should be computed in case of
515  // collision.
516  const bool compute_penetration = this->crequest->enable_contact ||
517  (this->crequest->security_margin < 0);
518  Vec3s c1, c2, normal;
519  const CoalScalar distance = internal::ShapeShapeDistance<Box, TriangleP>(
520  &box, box_tf, &tri, tf2, this->solver, compute_penetration, c1, c2,
521  normal);
522  const CoalScalar distToCollision =
523  distance - this->crequest->security_margin;
524 
525  internal::updateDistanceLowerBoundFromLeaf(
526  *(this->crequest), *(this->cresult), distToCollision, c1, c2, normal);
527 
528  if (cresult->numContacts() < crequest->num_max_contacts) {
529  if (distToCollision <= crequest->collision_distance_threshold) {
530  cresult->addContact(Contact(
531  tree1, tree2, (int)(root1 - tree1->getRoot()),
532  static_cast<int>(primitive_id), c1, c2, normal, distance));
533  }
534  }
535  return crequest->isSatisfied(*cresult);
536  }
537 
538  // Determine which tree to traverse first.
539  if (bvn2.isLeaf() ||
540  (tree1->nodeHasChildren(root1) && (bv1.size() > bvn2.bv.size()))) {
541  for (unsigned int i = 0; i < 8; ++i) {
542  if (tree1->nodeChildExists(root1, i)) {
543  const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
544  AABB child_bv;
545  computeChildBV(bv1, i, child_bv);
546 
547  if (OcTreeMeshIntersectRecurse(tree1, child, child_bv, tree2, root2,
548  tf1, tf2))
549  return true;
550  }
551  }
552  } else {
553  if (OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2,
554  (unsigned int)bvn2.leftChild(), tf1, tf2))
555  return true;
556 
557  if (OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2,
558  (unsigned int)bvn2.rightChild(), tf1, tf2))
559  return true;
560  }
561 
562  return false;
563  }
564 
566  template <typename BV>
567  bool OcTreeHeightFieldIntersectRecurse(
568  const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1,
569  const HeightField<BV>* tree2, unsigned int root2, const Transform3s& tf1,
570  const Transform3s& tf2, CoalScalar& sqrDistLowerBound) const {
571  // FIXME(jmirabel) I do not understand why the BVHModel was traversed. The
572  // code in this if(!root1) did not output anything so the empty OcTree is
573  // considered free. Should an empty OcTree be considered free ?
574 
575  // Empty OcTree is considered free.
576  if (!root1) return false;
577  HFNode<BV> const& bvn2 = tree2->getBV(root2);
578 
583  if (tree1->isNodeFree(root1))
584  return false;
585  else if ((tree1->isNodeUncertain(root1) || tree2->isUncertain()))
586  return false;
587  else {
588  OBB obb1, obb2;
589  convertBV(bv1, tf1, obb1);
590  convertBV(bvn2.bv, tf2, obb2);
591  CoalScalar sqrDistLowerBound_;
592  if (!obb1.overlap(obb2, *crequest, sqrDistLowerBound_)) {
593  if (sqrDistLowerBound_ < sqrDistLowerBound)
594  sqrDistLowerBound = sqrDistLowerBound_;
595  internal::updateDistanceLowerBoundFromBV(*crequest, *cresult,
596  sqrDistLowerBound);
597  return false;
598  }
599  }
600 
601  // Check if leaf collides.
602  if (!tree1->nodeHasChildren(root1) && bvn2.isLeaf()) {
603  assert(tree1->isNodeOccupied(root1)); // it isn't free nor uncertain.
604  Box box;
605  Transform3s box_tf;
606  constructBox(bv1, tf1, box, box_tf);
607  if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
608  box.computeLocalAABB();
609  }
610 
611  typedef Convex<Triangle> ConvexTriangle;
612  ConvexTriangle convex1, convex2;
613  int convex1_active_faces, convex2_active_faces;
614  details::buildConvexTriangles(bvn2, *tree2, convex1, convex1_active_faces,
615  convex2, convex2_active_faces);
616  if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
617  convex1.computeLocalAABB();
618  convex2.computeLocalAABB();
619  }
620 
621  Vec3s c1, c2, normal, normal_top;
622  CoalScalar distance;
623  bool hfield_witness_is_on_bin_side;
624 
625  bool collision = details::shapeDistance<Triangle, Box, 0>(
626  solver, *crequest, convex1, convex1_active_faces, convex2,
627  convex2_active_faces, tf2, box, box_tf, distance, c2, c1, normal,
628  normal_top, hfield_witness_is_on_bin_side);
629 
630  CoalScalar distToCollision =
631  distance - crequest->security_margin * (normal_top.dot(normal));
632 
633  if (distToCollision <= crequest->collision_distance_threshold) {
634  sqrDistLowerBound = 0;
635  if (crequest->num_max_contacts > cresult->numContacts()) {
636  if (normal_top.isApprox(normal) &&
637  (collision || !hfield_witness_is_on_bin_side)) {
638  cresult->addContact(
639  Contact(tree1, tree2, (int)(root1 - tree1->getRoot()),
640  (int)Contact::NONE, c1, c2, -normal, distance));
641  }
642  }
643  } else
644  sqrDistLowerBound = distToCollision * distToCollision;
645 
646  // const Vec3s c1 = contact_point - distance * 0.5 * normal;
647  // const Vec3s c2 = contact_point + distance * 0.5 * normal;
648  internal::updateDistanceLowerBoundFromLeaf(
649  *crequest, *cresult, distToCollision, c1, c2, -normal);
650 
651  assert(cresult->isCollision() || sqrDistLowerBound > 0);
652  return crequest->isSatisfied(*cresult);
653  }
654 
655  // Determine which tree to traverse first.
656  if (bvn2.isLeaf() ||
657  (tree1->nodeHasChildren(root1) && (bv1.size() > bvn2.bv.size()))) {
658  for (unsigned int i = 0; i < 8; ++i) {
659  if (tree1->nodeChildExists(root1, i)) {
660  const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
661  AABB child_bv;
662  computeChildBV(bv1, i, child_bv);
663 
664  if (OcTreeHeightFieldIntersectRecurse(tree1, child, child_bv, tree2,
665  root2, tf1, tf2,
666  sqrDistLowerBound))
667  return true;
668  }
669  }
670  } else {
671  if (OcTreeHeightFieldIntersectRecurse(tree1, root1, bv1, tree2,
672  (unsigned int)bvn2.leftChild(), tf1,
673  tf2, sqrDistLowerBound))
674  return true;
675 
676  if (OcTreeHeightFieldIntersectRecurse(tree1, root1, bv1, tree2,
677  (unsigned int)bvn2.rightChild(),
678  tf1, tf2, sqrDistLowerBound))
679  return true;
680  }
681 
682  return false;
683  }
684 
686  template <typename BV>
687  bool HeightFieldOcTreeIntersectRecurse(
688  const HeightField<BV>* tree1, unsigned int root1, const OcTree* tree2,
689  const OcTree::OcTreeNode* root2, const AABB& bv2, const Transform3s& tf1,
690  const Transform3s& tf2, CoalScalar& sqrDistLowerBound) const {
691  // FIXME(jmirabel) I do not understand why the BVHModel was traversed. The
692  // code in this if(!root1) did not output anything so the empty OcTree is
693  // considered free. Should an empty OcTree be considered free ?
694 
695  // Empty OcTree is considered free.
696  if (!root2) return false;
697  HFNode<BV> const& bvn1 = tree1->getBV(root1);
698 
703  if (tree2->isNodeFree(root2))
704  return false;
705  else if ((tree2->isNodeUncertain(root2) || tree1->isUncertain()))
706  return false;
707  else {
708  OBB obb1, obb2;
709  convertBV(bvn1.bv, tf1, obb1);
710  convertBV(bv2, tf2, obb2);
711  CoalScalar sqrDistLowerBound_;
712  if (!obb2.overlap(obb1, *crequest, sqrDistLowerBound_)) {
713  if (sqrDistLowerBound_ < sqrDistLowerBound)
714  sqrDistLowerBound = sqrDistLowerBound_;
715  internal::updateDistanceLowerBoundFromBV(*crequest, *cresult,
716  sqrDistLowerBound);
717  return false;
718  }
719  }
720 
721  // Check if leaf collides.
722  if (!tree2->nodeHasChildren(root2) && bvn1.isLeaf()) {
723  assert(tree2->isNodeOccupied(root2)); // it isn't free nor uncertain.
724  Box box;
725  Transform3s box_tf;
726  constructBox(bv2, tf2, box, box_tf);
727  if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
728  box.computeLocalAABB();
729  }
730 
731  typedef Convex<Triangle> ConvexTriangle;
732  ConvexTriangle convex1, convex2;
733  int convex1_active_faces, convex2_active_faces;
734  details::buildConvexTriangles(bvn1, *tree1, convex1, convex1_active_faces,
735  convex2, convex2_active_faces);
736  if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
737  convex1.computeLocalAABB();
738  convex2.computeLocalAABB();
739  }
740 
741  Vec3s c1, c2, normal, normal_top;
742  CoalScalar distance;
743  bool hfield_witness_is_on_bin_side;
744 
745  bool collision = details::shapeDistance<Triangle, Box, 0>(
746  solver, *crequest, convex1, convex1_active_faces, convex2,
747  convex2_active_faces, tf1, box, box_tf, distance, c1, c2, normal,
748  normal_top, hfield_witness_is_on_bin_side);
749 
750  CoalScalar distToCollision =
751  distance - crequest->security_margin * (normal_top.dot(normal));
752 
753  if (distToCollision <= crequest->collision_distance_threshold) {
754  sqrDistLowerBound = 0;
755  if (crequest->num_max_contacts > cresult->numContacts()) {
756  if (normal_top.isApprox(normal) &&
757  (collision || !hfield_witness_is_on_bin_side)) {
758  cresult->addContact(Contact(tree1, tree2, (int)Contact::NONE,
759  (int)(root2 - tree2->getRoot()), c1, c2,
760  normal, distance));
761  }
762  }
763  } else
764  sqrDistLowerBound = distToCollision * distToCollision;
765 
766  // const Vec3s c1 = contact_point - distance * 0.5 * normal;
767  // const Vec3s c2 = contact_point + distance * 0.5 * normal;
768  internal::updateDistanceLowerBoundFromLeaf(
769  *crequest, *cresult, distToCollision, c1, c2, normal);
770 
771  assert(cresult->isCollision() || sqrDistLowerBound > 0);
772  return crequest->isSatisfied(*cresult);
773  }
774 
775  // Determine which tree to traverse first.
776  if (bvn1.isLeaf() ||
777  (tree2->nodeHasChildren(root2) && (bv2.size() > bvn1.bv.size()))) {
778  for (unsigned int i = 0; i < 8; ++i) {
779  if (tree2->nodeChildExists(root2, i)) {
780  const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i);
781  AABB child_bv;
782  computeChildBV(bv2, i, child_bv);
783 
784  if (HeightFieldOcTreeIntersectRecurse(tree1, root1, tree2, child,
785  child_bv, tf1, tf2,
786  sqrDistLowerBound))
787  return true;
788  }
789  }
790  } else {
791  if (HeightFieldOcTreeIntersectRecurse(
792  tree1, (unsigned int)bvn1.leftChild(), tree2, root2, bv2, tf1,
793  tf2, sqrDistLowerBound))
794  return true;
795 
796  if (HeightFieldOcTreeIntersectRecurse(
797  tree1, (unsigned int)bvn1.rightChild(), tree2, root2, bv2, tf1,
798  tf2, sqrDistLowerBound))
799  return true;
800  }
801 
802  return false;
803  }
804 
805  bool OcTreeDistanceRecurse(const OcTree* tree1,
806  const OcTree::OcTreeNode* root1, const AABB& bv1,
807  const OcTree* tree2,
808  const OcTree::OcTreeNode* root2, const AABB& bv2,
809  const Transform3s& tf1,
810  const Transform3s& tf2) const {
811  if (!tree1->nodeHasChildren(root1) && !tree2->nodeHasChildren(root2)) {
812  if (tree1->isNodeOccupied(root1) && tree2->isNodeOccupied(root2)) {
813  Box box1, box2;
814  Transform3s box1_tf, box2_tf;
815  constructBox(bv1, tf1, box1, box1_tf);
816  constructBox(bv2, tf2, box2, box2_tf);
817 
818  if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
819  box1.computeLocalAABB();
820  box2.computeLocalAABB();
821  }
822 
823  Vec3s p1, p2, normal;
824  const CoalScalar distance = internal::ShapeShapeDistance<Box, Box>(
825  &box1, box1_tf, &box2, box2_tf, this->solver,
826  this->drequest->enable_signed_distance, p1, p2, normal);
827 
828  this->dresult->update(distance, tree1, tree2,
829  (int)(root1 - tree1->getRoot()),
830  (int)(root2 - tree2->getRoot()), p1, p2, normal);
831 
832  return drequest->isSatisfied(*dresult);
833  } else
834  return false;
835  }
836 
837  if (!tree1->isNodeOccupied(root1) || !tree2->isNodeOccupied(root2))
838  return false;
839 
840  if (!tree2->nodeHasChildren(root2) ||
841  (tree1->nodeHasChildren(root1) && (bv1.size() > bv2.size()))) {
842  for (unsigned int i = 0; i < 8; ++i) {
843  if (tree1->nodeChildExists(root1, i)) {
844  const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
845  AABB child_bv;
846  computeChildBV(bv1, i, child_bv);
847 
848  CoalScalar d;
849  AABB aabb1, aabb2;
850  convertBV(bv1, tf1, aabb1);
851  convertBV(bv2, tf2, aabb2);
852  d = aabb1.distance(aabb2);
853 
854  if (d < dresult->min_distance) {
855  if (OcTreeDistanceRecurse(tree1, child, child_bv, tree2, root2, bv2,
856  tf1, tf2))
857  return true;
858  }
859  }
860  }
861  } else {
862  for (unsigned int i = 0; i < 8; ++i) {
863  if (tree2->nodeChildExists(root2, i)) {
864  const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i);
865  AABB child_bv;
866  computeChildBV(bv2, i, child_bv);
867 
868  CoalScalar d;
869  AABB aabb1, aabb2;
870  convertBV(bv1, tf1, aabb1);
871  convertBV(bv2, tf2, aabb2);
872  d = aabb1.distance(aabb2);
873 
874  if (d < dresult->min_distance) {
875  if (OcTreeDistanceRecurse(tree1, root1, bv1, tree2, child, child_bv,
876  tf1, tf2))
877  return true;
878  }
879  }
880  }
881  }
882 
883  return false;
884  }
885 
886  bool OcTreeIntersectRecurse(const OcTree* tree1,
887  const OcTree::OcTreeNode* root1, const AABB& bv1,
888  const OcTree* tree2,
889  const OcTree::OcTreeNode* root2, const AABB& bv2,
890  const Transform3s& tf1,
891  const Transform3s& tf2) const {
892  // Empty OcTree is considered free.
893  if (!root1) return false;
894  if (!root2) return false;
895 
900  if (tree1->isNodeFree(root1) || tree2->isNodeFree(root2))
901  return false;
902  else if ((tree1->isNodeUncertain(root1) || tree2->isNodeUncertain(root2)))
903  return false;
904 
905  bool bothAreLeaves =
906  (!tree1->nodeHasChildren(root1) && !tree2->nodeHasChildren(root2));
907  if (!bothAreLeaves || !crequest->enable_contact) {
908  OBB obb1, obb2;
909  convertBV(bv1, tf1, obb1);
910  convertBV(bv2, tf2, obb2);
911  CoalScalar sqrDistLowerBound;
912  if (!obb1.overlap(obb2, *crequest, sqrDistLowerBound)) {
913  if (cresult->distance_lower_bound > 0 &&
914  sqrDistLowerBound <
915  cresult->distance_lower_bound * cresult->distance_lower_bound)
916  cresult->distance_lower_bound =
917  sqrt(sqrDistLowerBound) - crequest->security_margin;
918  return false;
919  }
920  if (crequest->enable_contact) { // Overlap
921  if (cresult->numContacts() < crequest->num_max_contacts)
922  cresult->addContact(
923  Contact(tree1, tree2, static_cast<int>(root1 - tree1->getRoot()),
924  static_cast<int>(root2 - tree2->getRoot())));
925  return crequest->isSatisfied(*cresult);
926  }
927  }
928 
929  // Both node are leaves
930  if (bothAreLeaves) {
931  assert(tree1->isNodeOccupied(root1) && tree2->isNodeOccupied(root2));
932 
933  Box box1, box2;
934  Transform3s box1_tf, box2_tf;
935  constructBox(bv1, tf1, box1, box1_tf);
936  constructBox(bv2, tf2, box2, box2_tf);
937 
938  if (this->solver->gjk_initial_guess ==
939  GJKInitialGuess::BoundingVolumeGuess) {
940  box1.computeLocalAABB();
941  box2.computeLocalAABB();
942  }
943 
944  // When reaching this point, `this->solver` has already been set up
945  // by the CollisionRequest `this->request`.
946  // The only thing we need to (and can) pass to `ShapeShapeDistance` is
947  // whether or not penetration information is should be computed in case of
948  // collision.
949  const bool compute_penetration = (this->crequest->enable_contact ||
950  (this->crequest->security_margin < 0));
951  Vec3s c1, c2, normal;
952  CoalScalar distance = internal::ShapeShapeDistance<Box, Box>(
953  &box1, box1_tf, &box2, box2_tf, this->solver, compute_penetration, c1,
954  c2, normal);
955 
956  const CoalScalar distToCollision =
957  distance - this->crequest->security_margin;
958 
959  internal::updateDistanceLowerBoundFromLeaf(
960  *(this->crequest), *(this->cresult), distToCollision, c1, c2, normal);
961 
962  if (this->cresult->numContacts() < this->crequest->num_max_contacts) {
963  if (distToCollision <= this->crequest->collision_distance_threshold)
964  this->cresult->addContact(
965  Contact(tree1, tree2, static_cast<int>(root1 - tree1->getRoot()),
966  static_cast<int>(root2 - tree2->getRoot()), c1, c2,
967  normal, distance));
968  }
969 
970  return crequest->isSatisfied(*cresult);
971  }
972 
973  // Determine which tree to traverse first.
974  if (!tree2->nodeHasChildren(root2) ||
975  (tree1->nodeHasChildren(root1) && (bv1.size() > bv2.size()))) {
976  for (unsigned int i = 0; i < 8; ++i) {
977  if (tree1->nodeChildExists(root1, i)) {
978  const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
979  AABB child_bv;
980  computeChildBV(bv1, i, child_bv);
981 
982  if (OcTreeIntersectRecurse(tree1, child, child_bv, tree2, root2, bv2,
983  tf1, tf2))
984  return true;
985  }
986  }
987  } else {
988  for (unsigned int i = 0; i < 8; ++i) {
989  if (tree2->nodeChildExists(root2, i)) {
990  const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i);
991  AABB child_bv;
992  computeChildBV(bv2, i, child_bv);
993 
994  if (OcTreeIntersectRecurse(tree1, root1, bv1, tree2, child, child_bv,
995  tf1, tf2))
996  return true;
997  }
998  }
999  }
1000 
1001  return false;
1002  }
1003 };
1004 
1007 
1009 class COAL_DLLAPI OcTreeCollisionTraversalNode
1010  : public CollisionTraversalNodeBase {
1011  public:
1012  OcTreeCollisionTraversalNode(const CollisionRequest& request)
1013  : CollisionTraversalNodeBase(request) {
1014  model1 = NULL;
1015  model2 = NULL;
1016 
1017  otsolver = NULL;
1018  }
1019 
1020  bool BVDisjoints(unsigned, unsigned, CoalScalar&) const { return false; }
1021 
1022  void leafCollides(unsigned, unsigned, CoalScalar& sqrDistLowerBound) const {
1023  otsolver->OcTreeIntersect(model1, model2, tf1, tf2, request, *result);
1024  sqrDistLowerBound = std::max((CoalScalar)0, result->distance_lower_bound);
1025  sqrDistLowerBound *= sqrDistLowerBound;
1026  }
1027 
1028  const OcTree* model1;
1029  const OcTree* model2;
1030 
1031  Transform3s tf1, tf2;
1032 
1033  const OcTreeSolver* otsolver;
1034 };
1035 
1037 template <typename S>
1038 class COAL_DLLAPI ShapeOcTreeCollisionTraversalNode
1039  : public CollisionTraversalNodeBase {
1040  public:
1041  ShapeOcTreeCollisionTraversalNode(const CollisionRequest& request)
1042  : CollisionTraversalNodeBase(request) {
1043  model1 = NULL;
1044  model2 = NULL;
1045 
1046  otsolver = NULL;
1047  }
1048 
1049  bool BVDisjoints(unsigned int, unsigned int, CoalScalar&) const {
1050  return false;
1051  }
1052 
1053  void leafCollides(unsigned int, unsigned int,
1054  CoalScalar& sqrDistLowerBound) const {
1055  otsolver->OcTreeShapeIntersect(model2, *model1, tf2, tf1, request, *result);
1056  sqrDistLowerBound = std::max((CoalScalar)0, result->distance_lower_bound);
1057  sqrDistLowerBound *= sqrDistLowerBound;
1058  }
1059 
1060  const S* model1;
1061  const OcTree* model2;
1062 
1063  Transform3s tf1, tf2;
1064 
1065  const OcTreeSolver* otsolver;
1066 };
1067 
1069 
1070 template <typename S>
1071 class COAL_DLLAPI OcTreeShapeCollisionTraversalNode
1072  : public CollisionTraversalNodeBase {
1073  public:
1074  OcTreeShapeCollisionTraversalNode(const CollisionRequest& request)
1075  : CollisionTraversalNodeBase(request) {
1076  model1 = NULL;
1077  model2 = NULL;
1078 
1079  otsolver = NULL;
1080  }
1081 
1082  bool BVDisjoints(unsigned int, unsigned int, coal::CoalScalar&) const {
1083  return false;
1084  }
1085 
1086  void leafCollides(unsigned int, unsigned int,
1087  CoalScalar& sqrDistLowerBound) const {
1088  otsolver->OcTreeShapeIntersect(model1, *model2, tf1, tf2, request, *result);
1089  sqrDistLowerBound = std::max((CoalScalar)0, result->distance_lower_bound);
1090  sqrDistLowerBound *= sqrDistLowerBound;
1091  }
1092 
1093  const OcTree* model1;
1094  const S* model2;
1095 
1096  Transform3s tf1, tf2;
1097 
1098  const OcTreeSolver* otsolver;
1099 };
1100 
1102 template <typename BV>
1103 class COAL_DLLAPI MeshOcTreeCollisionTraversalNode
1104  : public CollisionTraversalNodeBase {
1105  public:
1106  MeshOcTreeCollisionTraversalNode(const CollisionRequest& request)
1107  : CollisionTraversalNodeBase(request) {
1108  model1 = NULL;
1109  model2 = NULL;
1110 
1111  otsolver = NULL;
1112  }
1113 
1114  bool BVDisjoints(unsigned int, unsigned int, CoalScalar&) const {
1115  return false;
1116  }
1117 
1118  void leafCollides(unsigned int, unsigned int,
1119  CoalScalar& sqrDistLowerBound) const {
1120  otsolver->OcTreeMeshIntersect(model2, model1, tf2, tf1, request, *result);
1121  sqrDistLowerBound = std::max((CoalScalar)0, result->distance_lower_bound);
1122  sqrDistLowerBound *= sqrDistLowerBound;
1123  }
1124 
1125  const BVHModel<BV>* model1;
1126  const OcTree* model2;
1127 
1128  Transform3s tf1, tf2;
1129 
1130  const OcTreeSolver* otsolver;
1131 };
1132 
1134 template <typename BV>
1135 class COAL_DLLAPI OcTreeMeshCollisionTraversalNode
1136  : public CollisionTraversalNodeBase {
1137  public:
1138  OcTreeMeshCollisionTraversalNode(const CollisionRequest& request)
1139  : CollisionTraversalNodeBase(request) {
1140  model1 = NULL;
1141  model2 = NULL;
1142 
1143  otsolver = NULL;
1144  }
1145 
1146  bool BVDisjoints(unsigned int, unsigned int, CoalScalar&) const {
1147  return false;
1148  }
1149 
1150  void leafCollides(unsigned int, unsigned int,
1151  CoalScalar& sqrDistLowerBound) const {
1152  otsolver->OcTreeMeshIntersect(model1, model2, tf1, tf2, request, *result);
1153  sqrDistLowerBound = std::max((CoalScalar)0, result->distance_lower_bound);
1154  sqrDistLowerBound *= sqrDistLowerBound;
1155  }
1156 
1157  const OcTree* model1;
1158  const BVHModel<BV>* model2;
1159 
1160  Transform3s tf1, tf2;
1161 
1162  const OcTreeSolver* otsolver;
1163 };
1164 
1166 template <typename BV>
1167 class COAL_DLLAPI OcTreeHeightFieldCollisionTraversalNode
1168  : public CollisionTraversalNodeBase {
1169  public:
1170  OcTreeHeightFieldCollisionTraversalNode(const CollisionRequest& request)
1171  : CollisionTraversalNodeBase(request) {
1172  model1 = NULL;
1173  model2 = NULL;
1174 
1175  otsolver = NULL;
1176  }
1177 
1178  bool BVDisjoints(unsigned int, unsigned int, CoalScalar&) const {
1179  return false;
1180  }
1181 
1182  void leafCollides(unsigned int, unsigned int,
1183  CoalScalar& sqrDistLowerBound) const {
1184  otsolver->OcTreeHeightFieldIntersect(model1, model2, tf1, tf2, request,
1185  *result, sqrDistLowerBound);
1186  }
1187 
1188  const OcTree* model1;
1189  const HeightField<BV>* model2;
1190 
1191  Transform3s tf1, tf2;
1192 
1193  const OcTreeSolver* otsolver;
1194 };
1195 
1197 template <typename BV>
1198 class COAL_DLLAPI HeightFieldOcTreeCollisionTraversalNode
1199  : public CollisionTraversalNodeBase {
1200  public:
1201  HeightFieldOcTreeCollisionTraversalNode(const CollisionRequest& request)
1202  : CollisionTraversalNodeBase(request) {
1203  model1 = NULL;
1204  model2 = NULL;
1205 
1206  otsolver = NULL;
1207  }
1208 
1209  bool BVDisjoints(unsigned int, unsigned int, CoalScalar&) const {
1210  return false;
1211  }
1212 
1213  void leafCollides(unsigned int, unsigned int,
1214  CoalScalar& sqrDistLowerBound) const {
1215  otsolver->HeightFieldOcTreeIntersect(model1, model2, tf1, tf2, request,
1216  *result, sqrDistLowerBound);
1217  }
1218 
1219  const HeightField<BV>* model1;
1220  const OcTree* model2;
1221 
1222  Transform3s tf1, tf2;
1223 
1224  const OcTreeSolver* otsolver;
1225 };
1226 
1228 
1231 
1233 class COAL_DLLAPI OcTreeDistanceTraversalNode
1234  : public DistanceTraversalNodeBase {
1235  public:
1236  OcTreeDistanceTraversalNode() {
1237  model1 = NULL;
1238  model2 = NULL;
1239 
1240  otsolver = NULL;
1241  }
1242 
1243  CoalScalar BVDistanceLowerBound(unsigned, unsigned) const { return -1; }
1244 
1245  bool BVDistanceLowerBound(unsigned, unsigned, CoalScalar&) const {
1246  return false;
1247  }
1248 
1249  void leafComputeDistance(unsigned, unsigned int) const {
1250  otsolver->OcTreeDistance(model1, model2, tf1, tf2, request, *result);
1251  }
1252 
1253  const OcTree* model1;
1254  const OcTree* model2;
1255 
1256  const OcTreeSolver* otsolver;
1257 };
1258 
1260 template <typename Shape>
1261 class COAL_DLLAPI ShapeOcTreeDistanceTraversalNode
1262  : public DistanceTraversalNodeBase {
1263  public:
1264  ShapeOcTreeDistanceTraversalNode() {
1265  model1 = NULL;
1266  model2 = NULL;
1267 
1268  otsolver = NULL;
1269  }
1270 
1271  CoalScalar BVDistanceLowerBound(unsigned int, unsigned int) const {
1272  return -1;
1273  }
1274 
1275  void leafComputeDistance(unsigned int, unsigned int) const {
1276  otsolver->OcTreeShapeDistance(model2, *model1, tf2, tf1, request, *result);
1277  }
1278 
1279  const Shape* model1;
1280  const OcTree* model2;
1281 
1282  const OcTreeSolver* otsolver;
1283 };
1284 
1286 template <typename Shape>
1287 class COAL_DLLAPI OcTreeShapeDistanceTraversalNode
1288  : public DistanceTraversalNodeBase {
1289  public:
1290  OcTreeShapeDistanceTraversalNode() {
1291  model1 = NULL;
1292  model2 = NULL;
1293 
1294  otsolver = NULL;
1295  }
1296 
1297  CoalScalar BVDistanceLowerBound(unsigned int, unsigned int) const {
1298  return -1;
1299  }
1300 
1301  void leafComputeDistance(unsigned int, unsigned int) const {
1302  otsolver->OcTreeShapeDistance(model1, *model2, tf1, tf2, request, *result);
1303  }
1304 
1305  const OcTree* model1;
1306  const Shape* model2;
1307 
1308  const OcTreeSolver* otsolver;
1309 };
1310 
1312 template <typename BV>
1313 class COAL_DLLAPI MeshOcTreeDistanceTraversalNode
1314  : public DistanceTraversalNodeBase {
1315  public:
1316  MeshOcTreeDistanceTraversalNode() {
1317  model1 = NULL;
1318  model2 = NULL;
1319 
1320  otsolver = NULL;
1321  }
1322 
1323  CoalScalar BVDistanceLowerBound(unsigned int, unsigned int) const {
1324  return -1;
1325  }
1326 
1327  void leafComputeDistance(unsigned int, unsigned int) const {
1328  otsolver->OcTreeMeshDistance(model2, model1, tf2, tf1, request, *result);
1329  }
1330 
1331  const BVHModel<BV>* model1;
1332  const OcTree* model2;
1333 
1334  const OcTreeSolver* otsolver;
1335 };
1336 
1338 template <typename BV>
1339 class COAL_DLLAPI OcTreeMeshDistanceTraversalNode
1340  : public DistanceTraversalNodeBase {
1341  public:
1342  OcTreeMeshDistanceTraversalNode() {
1343  model1 = NULL;
1344  model2 = NULL;
1345 
1346  otsolver = NULL;
1347  }
1348 
1349  CoalScalar BVDistanceLowerBound(unsigned int, unsigned int) const {
1350  return -1;
1351  }
1352 
1353  void leafComputeDistance(unsigned int, unsigned int) const {
1354  otsolver->OcTreeMeshDistance(model1, model2, tf1, tf2, request, *result);
1355  }
1356 
1357  const OcTree* model1;
1358  const BVHModel<BV>* model2;
1359 
1360  const OcTreeSolver* otsolver;
1361 };
1362 
1364 
1365 } // namespace coal
1366 
1368 
1369 #endif
octree.h
coal::Vec3s
Eigen::Matrix< CoalScalar, 3, 1 > Vec3s
Definition: coal/data_types.h:77
traversal_node_base.h
collision_manager.box
box
Definition: collision_manager.py:11
traversal_node_hfield_shape.h
coal::internal::updateDistanceLowerBoundFromLeaf
void updateDistanceLowerBoundFromLeaf(const CollisionRequest &, CollisionResult &res, const CoalScalar &distance, const Vec3s &p0, const Vec3s &p1, const Vec3s &normal)
Definition: coal/collision_data.h:1184
gjk.tf1
tuple tf1
Definition: test/scripts/gjk.py:27
collision_data.h
coal
Main namespace.
Definition: coal/broadphase/broadphase_bruteforce.h:44
narrowphase.h
coal::distance
COAL_DLLAPI CoalScalar 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.
Definition: kIOS.cpp:180
coal::DistanceResult::NONE
static const int NONE
invalid contact primitive information
Definition: coal/collision_data.h:1086
coal::constructBox
COAL_DLLAPI void constructBox(const AABB &bv, Box &box, Transform3s &tf)
construct a box shape (with a configuration) from a given bounding volume
Definition: geometric_shapes_utility.cpp:1011
shape_shape_func.h
BVH_model.h
d
d
c
c
coal::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: coal/octree.h:299
gjk.tf2
tuple tf2
Definition: test/scripts/gjk.py:36
coal::BoundingVolumeGuess
@ BoundingVolumeGuess
Definition: coal/data_types.h:95
octree.p1
tuple p1
Definition: octree.py:54
collision
Definition: python_unit/collision.py:1
coal::internal::updateDistanceLowerBoundFromBV
void updateDistanceLowerBoundFromBV(const CollisionRequest &, CollisionResult &res, const CoalScalar sqrDistLowerBound)
Definition: coal/collision_data.h:1175
coal::OcTree::OcTreeNode
octomap::OcTreeNode OcTreeNode
Definition: coal/octree.h:63
hfield.h
geometric_shapes_utility.h
coal::convertBV
static void convertBV(const BV1 &bv1, const Transform3s &tf1, BV2 &bv2)
Convert a bounding volume of type BV1 in configuration tf1 to bounding volume of type BV2 in identity...
Definition: coal/BV/BV.h:276
c2
c2
coal::CoalScalar
double CoalScalar
Definition: coal/data_types.h:76
coal::Contact::NONE
static const int NONE
invalid contact primitive information
Definition: coal/collision_data.h:108

hpp-fcl
Author(s):
autogenerated on Sat Nov 23 2024 03:44:59