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