traversal_node_octree.h 34.8 KB
Newer Older
jpan's avatar
jpan committed
1
2
3
/*
 * Software License Agreement (BSD License)
 *
4
5
 *  Copyright (c) 2011-2014, Willow Garage, Inc.
 *  Copyright (c) 2014-2015, Open Source Robotics Foundation
jpan's avatar
jpan committed
6
7
8
9
10
11
12
13
14
15
16
17
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
18
 *   * Neither the name of Open Source Robotics Foundation nor the names of its
jpan's avatar
jpan committed
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 */

/** \author Jia Pan */

Joseph Mirabel's avatar
Joseph Mirabel committed
38
39
#ifndef HPP_FCL_TRAVERSAL_NODE_OCTREE_H
#define HPP_FCL_TRAVERSAL_NODE_OCTREE_H
jpan's avatar
jpan committed
40

41
42
/// @cond INTERNAL

43
#include <hpp/fcl/collision_data.h>
44
#include "traversal_node_base.h"
45
46
47
#include <hpp/fcl/narrowphase/narrowphase.h>
#include <hpp/fcl/octree.h>
#include <hpp/fcl/BVH/BVH_model.h>
48
#include "../src/shape/geometric_shapes_utility.h"
jpan's avatar
jpan committed
49

50
51
namespace hpp
{
jpan's avatar
jpan committed
52
53
54
namespace fcl
{

55
/// @brief Algorithms for collision related with octree
jpan's avatar
jpan committed
56
57
58
class OcTreeSolver
{
private:
59
  const GJKSolver* solver;
60

jpan's avatar
   
jpan committed
61
62
63
64
65
  mutable const CollisionRequest* crequest;
  mutable const DistanceRequest* drequest;

  mutable CollisionResult* cresult;
  mutable DistanceResult* dresult;
jpan's avatar
jpan committed
66
67

public:
68
  OcTreeSolver(const GJKSolver* solver_) : solver(solver_),
jpan's avatar
   
jpan committed
69
70
71
72
73
74
                                                   crequest(NULL),
                                                   drequest(NULL),
                                                   cresult(NULL),
                                                   dresult(NULL)
  {
  }
jpan's avatar
jpan committed
75

76
  /// @brief collision between two octrees
77
  void OcTreeIntersect(const OcTree* tree1, const OcTree* tree2,
78
                       const Transform3f& tf1, const Transform3f& tf2,
jpan's avatar
   
jpan committed
79
80
                       const CollisionRequest& request_,
                       CollisionResult& result_) const
jpan's avatar
jpan committed
81
  {
jpan's avatar
   
jpan committed
82
83
    crequest = &request_;
    cresult = &result_;
84
    
jpan's avatar
jpan committed
85
86
    OcTreeIntersectRecurse(tree1, tree1->getRoot(), tree1->getRootBV(), 
                           tree2, tree2->getRoot(), tree2->getRootBV(), 
jpan's avatar
   
jpan committed
87
                           tf1, tf2);
jpan's avatar
jpan committed
88
89
  }

90
  /// @brief distance between two octrees
91
  void OcTreeDistance(const OcTree* tree1, const OcTree* tree2,
92
                      const Transform3f& tf1, const Transform3f& tf2,
93
94
                      const DistanceRequest& request_,
                      DistanceResult& result_) const
jpan's avatar
jpan committed
95
  {
96
97
    drequest = &request_;
    dresult = &result_;
jpan's avatar
jpan committed
98
99
100

    OcTreeDistanceRecurse(tree1, tree1->getRoot(), tree1->getRootBV(), 
                          tree2, tree2->getRoot(), tree2->getRootBV(),
101
                          tf1, tf2);
jpan's avatar
jpan committed
102
103
  }

104
  /// @brief collision between octree and mesh
jpan's avatar
jpan committed
105
  template<typename BV>
106
  void OcTreeMeshIntersect(const OcTree* tree1, const BVHModel<BV>* tree2,
107
                           const Transform3f& tf1, const Transform3f& tf2,
jpan's avatar
   
jpan committed
108
109
                           const CollisionRequest& request_,
                           CollisionResult& result_) const
jpan's avatar
jpan committed
110
  {
jpan's avatar
jpan committed
111
112
    crequest = &request_;
    cresult = &result_;
113

jpan's avatar
jpan committed
114
115
116
    OcTreeMeshIntersectRecurse(tree1, tree1->getRoot(), tree1->getRootBV(),
                               tree2, 0,
                               tf1, tf2);
jpan's avatar
jpan committed
117
118
  }

119
  /// @brief distance between octree and mesh
jpan's avatar
jpan committed
120
  template<typename BV>
121
  void OcTreeMeshDistance(const OcTree* tree1, const BVHModel<BV>* tree2,
122
                          const Transform3f& tf1, const Transform3f& tf2,
123
124
                          const DistanceRequest& request_,
                          DistanceResult& result_) const
jpan's avatar
jpan committed
125
  {
126
127
    drequest = &request_;
    dresult = &result_;
jpan's avatar
jpan committed
128
129

    OcTreeMeshDistanceRecurse(tree1, tree1->getRoot(), tree1->getRootBV(),
130
                              tree2, 0,
131
                              tf1, tf2);
jpan's avatar
jpan committed
132
133
  }

134
  /// @brief collision between mesh and octree
jpan's avatar
jpan committed
135
  template<typename BV>
136
  void MeshOcTreeIntersect(const BVHModel<BV>* tree1, const OcTree* tree2,
137
                           const Transform3f& tf1, const Transform3f& tf2,
jpan's avatar
   
jpan committed
138
139
                           const CollisionRequest& request_,
                           CollisionResult& result_) const
jpan's avatar
jpan committed
140
141
  
  {
jpan's avatar
jpan committed
142
143
    crequest = &request_;
    cresult = &result_;
144

jpan's avatar
jpan committed
145
146
147
    OcTreeMeshIntersectRecurse(tree2, tree2->getRoot(), tree2->getRootBV(),
                               tree1, 0,
                               tf2, tf1);
jpan's avatar
jpan committed
148
149
  }

150
  /// @brief distance between mesh and octree
jpan's avatar
jpan committed
151
  template<typename BV>
152
  void MeshOcTreeDistance(const BVHModel<BV>* tree1, const OcTree* tree2,
153
                          const Transform3f& tf1, const Transform3f& tf2,
154
155
                          const DistanceRequest& request_,
                          DistanceResult& result_) const
jpan's avatar
jpan committed
156
  {
157
158
    drequest = &request_;
    dresult = &result_;
jpan's avatar
jpan committed
159

160
    OcTreeMeshDistanceRecurse(tree1, 0,
jpan's avatar
jpan committed
161
                              tree2, tree2->getRoot(), tree2->getRootBV(),
162
                              tf1, tf2);
jpan's avatar
jpan committed
163
164
  }

165
  /// @brief collision between octree and shape
jpan's avatar
jpan committed
166
  template<typename S>
167
  void OcTreeShapeIntersect(const OcTree* tree, const S& s,
168
                            const Transform3f& tf1, const Transform3f& tf2,
jpan's avatar
   
jpan committed
169
170
                            const CollisionRequest& request_,
                            CollisionResult& result_) const
jpan's avatar
jpan committed
171
  {
jpan's avatar
   
jpan committed
172
173
    crequest = &request_;
    cresult = &result_;
174

jpan's avatar
jpan committed
175
    AABB bv2;
176
    computeBV<AABB>(s, Transform3f(), bv2);
jpan's avatar
   
jpan committed
177
178
    OBB obb2;
    convertBV(bv2, tf2, obb2);
jpan's avatar
jpan committed
179
    OcTreeShapeIntersectRecurse(tree, tree->getRoot(), tree->getRootBV(),
jpan's avatar
   
jpan committed
180
                                s, obb2,
jpan's avatar
   
jpan committed
181
                                tf1, tf2);
jpan's avatar
jpan committed
182
183
184
    
  }

185
  /// @brief collision between shape and octree
jpan's avatar
jpan committed
186
  template<typename S>
187
  void ShapeOcTreeIntersect(const S& s, const OcTree* tree,
188
                            const Transform3f& tf1, const Transform3f& tf2,
jpan's avatar
   
jpan committed
189
190
                            const CollisionRequest& request_,
                            CollisionResult& result_) const
jpan's avatar
jpan committed
191
  {
jpan's avatar
   
jpan committed
192
193
    crequest = &request_;
    cresult = &result_;
194

jpan's avatar
jpan committed
195
    AABB bv1;
196
    computeBV<AABB>(s, Transform3f(), bv1);
jpan's avatar
   
jpan committed
197
198
    OBB obb1;
    convertBV(bv1, tf1, obb1);
jpan's avatar
jpan committed
199
    OcTreeShapeIntersectRecurse(tree, tree->getRoot(), tree->getRootBV(),
jpan's avatar
   
jpan committed
200
                                s, obb1,
jpan's avatar
   
jpan committed
201
                                tf2, tf1);
jpan's avatar
jpan committed
202
203
  }

204
  /// @brief distance between octree and shape
jpan's avatar
jpan committed
205
  template<typename S>
206
  void OcTreeShapeDistance(const OcTree* tree, const S& s,
207
                           const Transform3f& tf1, const Transform3f& tf2,
208
209
                           const DistanceRequest& request_,
                           DistanceResult& result_) const
jpan's avatar
jpan committed
210
  {
211
212
213
    drequest = &request_;
    dresult = &result_;

jpan's avatar
   
jpan committed
214
215
    AABB aabb2;
    computeBV<AABB>(s, tf2, aabb2);
jpan's avatar
jpan committed
216
    OcTreeShapeDistanceRecurse(tree, tree->getRoot(), tree->getRootBV(),
jpan's avatar
   
jpan committed
217
                               s, aabb2,
218
                               tf1, tf2);
jpan's avatar
jpan committed
219
220
  }

221
  /// @brief distance between shape and octree
jpan's avatar
jpan committed
222
  template<typename S>
223
  void ShapeOcTreeDistance(const S& s, const OcTree* tree,
224
                           const Transform3f& tf1, const Transform3f& tf2,
225
226
                           const DistanceRequest& request_,
                           DistanceResult& result_) const
jpan's avatar
jpan committed
227
  {
228
229
230
    drequest = &request_;
    dresult = &result_;

jpan's avatar
   
jpan committed
231
232
    AABB aabb1;
    computeBV<AABB>(s, tf1, aabb1);
jpan's avatar
jpan committed
233
    OcTreeShapeDistanceRecurse(tree, tree->getRoot(), tree->getRootBV(),
jpan's avatar
   
jpan committed
234
                               s, aabb1,
235
                               tf2, tf1);
jpan's avatar
jpan committed
236
237
238
239
240
  }
  

private:
  template<typename S>
241
  bool OcTreeShapeDistanceRecurse(const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1,
jpan's avatar
   
jpan committed
242
                                  const S& s, const AABB& aabb2,
243
                                  const Transform3f& tf1, const Transform3f& tf2) const
jpan's avatar
jpan committed
244
  {
Gautham Manoharan's avatar
Gautham Manoharan committed
245
    if(!tree1->nodeHasChildren(root1))
jpan's avatar
jpan committed
246
247
248
249
    {
      if(tree1->isNodeOccupied(root1))
      {
        Box box;
250
        Transform3f box_tf;
jpan's avatar
jpan committed
251
        constructBox(bv1, tf1, box, box_tf);
jpan's avatar
   
jpan committed
252
 
jpan's avatar
jpan committed
253
        FCL_REAL dist;
254
255
256
        Vec3f closest_p1, closest_p2, normal;
        solver->shapeDistance(box, box_tf, s, tf2, dist, closest_p1,
                              closest_p2, normal);
jpan's avatar
jpan committed
257
        
258
        dresult->update(dist, tree1, &s, (int) (root1 - tree1->getRoot()),
259
260
                        DistanceResult::NONE, closest_p1, closest_p2,
                        normal);
261
        
262
        return drequest->isSatisfied(*dresult);
jpan's avatar
jpan committed
263
264
265
266
267
268
269
270
271
      }
      else
        return false;
    }

    if(!tree1->isNodeOccupied(root1)) return false;
    
    for(unsigned int i = 0; i < 8; ++i)
    {
Gautham Manoharan's avatar
Gautham Manoharan committed
272
      if(tree1->nodeChildExists(root1, i))
jpan's avatar
jpan committed
273
      {
Gautham Manoharan's avatar
Gautham Manoharan committed
274
        const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
jpan's avatar
jpan committed
275
276
        AABB child_bv;
        computeChildBV(bv1, i, child_bv);
jpan's avatar
   
jpan committed
277
278
279
280
        
        AABB aabb1;
        convertBV(child_bv, tf1, aabb1);
        FCL_REAL d = aabb1.distance(aabb2);
281
        if(d < dresult->min_distance)
jpan's avatar
jpan committed
282
        {
283
          if(OcTreeShapeDistanceRecurse(tree1, child, child_bv, s, aabb2, tf1, tf2))
jpan's avatar
jpan committed
284
285
286
287
288
289
290
291
292
            return true;
        }        
      }
    }

    return false;
  }

  template<typename S>
293
  bool OcTreeShapeIntersectRecurse(const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1,
jpan's avatar
   
jpan committed
294
                                   const S& s, const OBB& obb2,
295
                                   const Transform3f& tf1, const Transform3f& tf2) const
jpan's avatar
jpan committed
296
  {
jpan's avatar
jpan committed
297
298
299
300
    if(!root1)
    {
      OBB obb1;
      convertBV(bv1, tf1, obb1);
jpan's avatar
jpan committed
301
      if(obb1.overlap(obb2))
jpan's avatar
jpan committed
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
      {
        Box box;
        Transform3f box_tf;
        constructBox(bv1, tf1, box, box_tf);

        if(solver->shapeIntersect(box, box_tf, s, tf2, NULL, NULL, NULL))
        {
          AABB overlap_part;
          AABB aabb1, aabb2;
          computeBV<AABB, Box>(box, box_tf, aabb1);
          computeBV<AABB, S>(s, tf2, aabb2);
          aabb1.overlap(aabb2, overlap_part);
        }
      }

      return false;
    }
Gautham Manoharan's avatar
Gautham Manoharan committed
319
    else if(!tree1->nodeHasChildren(root1))
jpan's avatar
jpan committed
320
    {
321
      if(tree1->isNodeOccupied(root1)) // occupied area
jpan's avatar
jpan committed
322
      {
jpan's avatar
   
jpan committed
323
324
325
        OBB obb1;
        convertBV(bv1, tf1, obb1);
        if(obb1.overlap(obb2))
jpan's avatar
jpan committed
326
        {
jpan's avatar
   
jpan committed
327
          Box box;
328
          Transform3f box_tf;
jpan's avatar
   
jpan committed
329
          constructBox(bv1, tf1, box, box_tf);
jpan's avatar
jpan committed
330

jpan's avatar
   
jpan committed
331
          if(!crequest->enable_contact)
jpan's avatar
   
jpan committed
332
333
          {
            if(solver->shapeIntersect(box, box_tf, s, tf2, NULL, NULL, NULL))
jpan's avatar
jpan committed
334
            {
jpan's avatar
   
jpan committed
335
336
              if(cresult->numContacts() < crequest->num_max_contacts)
                cresult->addContact(Contact(tree1, &s, root1 - tree1->getRoot(), Contact::NONE));
jpan's avatar
jpan committed
337
            }
jpan's avatar
   
jpan committed
338
339
340
341
342
343
344
345
          }
          else
          {
            Vec3f contact;
            FCL_REAL depth;
            Vec3f normal;

            if(solver->shapeIntersect(box, box_tf, s, tf2, &contact, &depth, &normal))
jpan's avatar
jpan committed
346
            {
jpan's avatar
   
jpan committed
347
348
              if(cresult->numContacts() < crequest->num_max_contacts)
                cresult->addContact(Contact(tree1, &s, root1 - tree1->getRoot(), Contact::NONE, contact, normal, depth));
jpan's avatar
jpan committed
349
            }
jpan's avatar
   
jpan committed
350
          }
351

352
          return crequest->isSatisfied(*cresult);
jpan's avatar
   
jpan committed
353
354
        }
        else return false;
jpan's avatar
jpan committed
355
      }
356
      else // free area
jpan's avatar
jpan committed
357
358
359
        return false;
    }

360
    /// stop when 1) bounding boxes of two objects not overlap; OR
361
362
    ///           2) at least of one the nodes is free; OR
    ///           2) (two uncertain nodes or one node occupied and one node uncertain) AND cost not required
363
    if(tree1->isNodeFree(root1)) return false;
364
    else if((tree1->isNodeUncertain(root1) || s.isUncertain())) return false;
365
366
367
368
369
370
    else
    {
      OBB obb1;
      convertBV(bv1, tf1, obb1);
      if(!obb1.overlap(obb2)) return false;
    }
jpan's avatar
jpan committed
371
372
373

    for(unsigned int i = 0; i < 8; ++i)
    {
Gautham Manoharan's avatar
Gautham Manoharan committed
374
      if(tree1->nodeChildExists(root1, i))
jpan's avatar
jpan committed
375
      {
Gautham Manoharan's avatar
Gautham Manoharan committed
376
        const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
jpan's avatar
jpan committed
377
378
379
        AABB child_bv;
        computeChildBV(bv1, i, child_bv);
        
jpan's avatar
   
jpan committed
380
        if(OcTreeShapeIntersectRecurse(tree1, child, child_bv, s, obb2, tf1, tf2))
jpan's avatar
jpan committed
381
382
383
384
385
386
387
388
          return true;
      }
    }

    return false;    
  }

  template<typename BV>
389
390
  bool OcTreeMeshDistanceRecurse(const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1,
                                 const BVHModel<BV>* tree2, int root2,
391
                                 const Transform3f& tf1, const Transform3f& tf2) const
jpan's avatar
jpan committed
392
  {
Gautham Manoharan's avatar
Gautham Manoharan committed
393
    if(!tree1->nodeHasChildren(root1) && tree2->getBV(root2).isLeaf())
jpan's avatar
jpan committed
394
395
396
397
    {
      if(tree1->isNodeOccupied(root1))
      {
        Box box;
398
        Transform3f box_tf;
jpan's avatar
jpan committed
399
400
401
        constructBox(bv1, tf1, box, box_tf);

        int primitive_id = tree2->getBV(root2).primitiveId();
402
403
404
405
        const Triangle& tri_id = tree2->tri_indices[primitive_id];
        const Vec3f& p1 = tree2->vertices[tri_id[0]];
        const Vec3f& p2 = tree2->vertices[tri_id[1]];
        const Vec3f& p3 = tree2->vertices[tri_id[2]];
jpan's avatar
jpan committed
406
407
        
        FCL_REAL dist;
408
409
410
        Vec3f closest_p1, closest_p2, normal;
        solver->shapeTriangleInteraction(box, box_tf, p1, p2, p3, tf2, dist,
                                         closest_p1, closest_p2, normal);
411

412
        dresult->update(dist, tree1, tree2, (int) (root1 - tree1->getRoot()),
413
                        primitive_id, closest_p1, closest_p2, normal);
414

415
        return drequest->isSatisfied(*dresult);
jpan's avatar
jpan committed
416
417
418
419
420
421
422
      }
      else
        return false;
    }

    if(!tree1->isNodeOccupied(root1)) return false;

Gautham Manoharan's avatar
Gautham Manoharan committed
423
    if(tree2->getBV(root2).isLeaf() || (tree1->nodeHasChildren(root1) && (bv1.size() > tree2->getBV(root2).bv.size())))
jpan's avatar
jpan committed
424
425
426
    {
      for(unsigned int i = 0; i < 8; ++i)
      {
Gautham Manoharan's avatar
Gautham Manoharan committed
427
        if(tree1->nodeChildExists(root1, i))
jpan's avatar
jpan committed
428
        {
Gautham Manoharan's avatar
Gautham Manoharan committed
429
          const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
jpan's avatar
jpan committed
430
431
432
          AABB child_bv;
          computeChildBV(bv1, i, child_bv);

jpan's avatar
   
jpan committed
433
434
435
436
437
          FCL_REAL d;
          AABB aabb1, aabb2;
          convertBV(child_bv, tf1, aabb1);
          convertBV(tree2->getBV(root2).bv, tf2, aabb2);
          d = aabb1.distance(aabb2);
jpan's avatar
jpan committed
438
          
439
          if(d < dresult->min_distance)
jpan's avatar
jpan committed
440
          {
441
            if(OcTreeMeshDistanceRecurse(tree1, child, child_bv, tree2, root2, tf1, tf2))
jpan's avatar
jpan committed
442
443
444
445
446
447
448
              return true;
          }
        }
      }
    }
    else
    {
jpan's avatar
   
jpan committed
449
450
451
      FCL_REAL d;
      AABB aabb1, aabb2;
      convertBV(bv1, tf1, aabb1);
jpan's avatar
jpan committed
452
      int child = tree2->getBV(root2).leftChild();
jpan's avatar
   
jpan committed
453
454
455
      convertBV(tree2->getBV(child).bv, tf2, aabb2);
      d = aabb1.distance(aabb2);

456
      if(d < dresult->min_distance)
jpan's avatar
jpan committed
457
      {
458
        if(OcTreeMeshDistanceRecurse(tree1, root1, bv1, tree2, child, tf1, tf2))
jpan's avatar
jpan committed
459
460
461
462
          return true;
      }

      child = tree2->getBV(root2).rightChild();
jpan's avatar
   
jpan committed
463
464
465
      convertBV(tree2->getBV(child).bv, tf2, aabb2);
      d = aabb1.distance(aabb2);
      
466
      if(d < dresult->min_distance)
jpan's avatar
jpan committed
467
      {
468
        if(OcTreeMeshDistanceRecurse(tree1, root1, bv1, tree2, child, tf1, tf2))
jpan's avatar
jpan committed
469
470
471
472
473
474
475
476
477
          return true;      
      }
    }

    return false;
  }


  template<typename BV>
478
479
  bool OcTreeMeshIntersectRecurse(const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1,
                                  const BVHModel<BV>* tree2, int root2,
480
                                  const Transform3f& tf1, const Transform3f& tf2) const
jpan's avatar
jpan committed
481
  {
jpan's avatar
jpan committed
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
    if(!root1)
    {
      if(tree2->getBV(root2).isLeaf())
      {
        OBB obb1, obb2;
        convertBV(bv1, tf1, obb1);
        convertBV(tree2->getBV(root2).bv, tf2, obb2);
        if(obb1.overlap(obb2))
        {
          Box box;
          Transform3f box_tf;
          constructBox(bv1, tf1, box, box_tf);

          int primitive_id = tree2->getBV(root2).primitiveId();
          const Triangle& tri_id = tree2->tri_indices[primitive_id];
          const Vec3f& p1 = tree2->vertices[tri_id[0]];
          const Vec3f& p2 = tree2->vertices[tri_id[1]];
          const Vec3f& p3 = tree2->vertices[tri_id[2]];
500
501
          Vec3f c1, c2, normal;
          FCL_REAL distance;
502
          if(solver->shapeTriangleInteraction
503
             (box, box_tf, p1, p2, p3, tf2, distance, c1, c2, normal))
jpan's avatar
jpan committed
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
          {
            AABB overlap_part;
            AABB aabb1;
            computeBV<AABB, Box>(box, box_tf, aabb1);
            AABB aabb2(tf2.transform(p1), tf2.transform(p2), tf2.transform(p3));
            aabb1.overlap(aabb2, overlap_part);
          }
        }

        return false;
      }
      else
      {
        if(OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2, tree2->getBV(root2).leftChild(), tf1, tf2))
          return true;

        if(OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2, tree2->getBV(root2).rightChild(), tf1, tf2))
          return true;

        return false;
      }
    }
Gautham Manoharan's avatar
Gautham Manoharan committed
526
    else if(!tree1->nodeHasChildren(root1) && tree2->getBV(root2).isLeaf())
jpan's avatar
jpan committed
527
    {
528
      if(tree1->isNodeOccupied(root1))
jpan's avatar
jpan committed
529
      {
jpan's avatar
   
jpan committed
530
531
532
533
534
535
        OBB obb1, obb2;
        convertBV(bv1, tf1, obb1);
        convertBV(tree2->getBV(root2).bv, tf2, obb2);
        if(obb1.overlap(obb2))
        {
          Box box;
536
          Transform3f box_tf;
jpan's avatar
   
jpan committed
537
538
539
540
541
542
543
          constructBox(bv1, tf1, box, box_tf);

          int primitive_id = tree2->getBV(root2).primitiveId();
          const Triangle& tri_id = tree2->tri_indices[primitive_id];
          const Vec3f& p1 = tree2->vertices[tri_id[0]];
          const Vec3f& p2 = tree2->vertices[tri_id[1]];
          const Vec3f& p3 = tree2->vertices[tri_id[2]];
jpan's avatar
jpan committed
544
        
jpan's avatar
   
jpan committed
545
          if(!crequest->enable_contact)
jpan's avatar
   
jpan committed
546
          {
547
548
            Vec3f c1, c2, normal;
            FCL_REAL distance;
549
            if(solver->shapeTriangleInteraction
550
               (box, box_tf, p1, p2, p3, tf2, distance, c1, c2, normal))
551
552
            {
              if(cresult->numContacts() < crequest->num_max_contacts)
553
554
555
                cresult->addContact(Contact(tree1, tree2,
                                            (int)(root1 - tree1->getRoot()),
                                            primitive_id));
556
            }
jpan's avatar
   
jpan committed
557
558
559
          }
          else
          {
560
561
            Vec3f c1, c2;
            FCL_REAL distance;
jpan's avatar
   
jpan committed
562
            Vec3f normal;
jpan's avatar
jpan committed
563

564
565
            if(solver->shapeTriangleInteraction(box, box_tf, p1, p2, p3, tf2,
                                                distance, c1, c2, normal))
566
            {
567
              assert (crequest->security_margin == 0);
568
              if(cresult->numContacts() < crequest->num_max_contacts)
569
570
571
                cresult->addContact
                  (Contact(tree1, tree2, (int) (root1 - tree1->getRoot()),
                           primitive_id, c1, normal, -distance));
572
573
574
            }
          }

575
          return crequest->isSatisfied(*cresult);
jpan's avatar
jpan committed
576
        }
jpan's avatar
   
jpan committed
577
578
        else
          return false;
jpan's avatar
jpan committed
579
      }
580
      else // free area
jpan's avatar
jpan committed
581
582
583
        return false;
    }

584
    /// stop when 1) bounding boxes of two objects not overlap; OR
585
586
    ///           2) at least one of the nodes is free; OR
    ///           2) (two uncertain nodes OR one node occupied and one node uncertain) AND cost not required
587
    if(tree1->isNodeFree(root1)) return false;
588
589
    else if((tree1->isNodeUncertain(root1) || tree2->isUncertain()))
      return false;
590
591
592
593
594
    else
    {
      OBB obb1, obb2;
      convertBV(bv1, tf1, obb1);
      convertBV(tree2->getBV(root2).bv, tf2, obb2);
jpan's avatar
jpan committed
595
      if(!obb1.overlap(obb2)) return false;      
596
597
    }
   
Gautham Manoharan's avatar
Gautham Manoharan committed
598
    if(tree2->getBV(root2).isLeaf() || (tree1->nodeHasChildren(root1) && (bv1.size() > tree2->getBV(root2).bv.size())))
jpan's avatar
jpan committed
599
600
601
    {
      for(unsigned int i = 0; i < 8; ++i)
      {
Gautham Manoharan's avatar
Gautham Manoharan committed
602
        if(tree1->nodeChildExists(root1, i))
jpan's avatar
jpan committed
603
        {
Gautham Manoharan's avatar
Gautham Manoharan committed
604
          const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
jpan's avatar
jpan committed
605
606
607
          AABB child_bv;
          computeChildBV(bv1, i, child_bv);
          
jpan's avatar
   
jpan committed
608
          if(OcTreeMeshIntersectRecurse(tree1, child, child_bv, tree2, root2, tf1, tf2))
jpan's avatar
jpan committed
609
610
611
612
613
614
            return true;
        }
      }
    }
    else
    {
jpan's avatar
   
jpan committed
615
      if(OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2, tree2->getBV(root2).leftChild(), tf1, tf2))
jpan's avatar
jpan committed
616
617
        return true;

jpan's avatar
   
jpan committed
618
      if(OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2, tree2->getBV(root2).rightChild(), tf1, tf2))
jpan's avatar
jpan committed
619
620
621
622
623
624
625
        return true;      

    }

    return false;
  }

626
627
  bool OcTreeDistanceRecurse(const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1,
                             const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& bv2,
628
                             const Transform3f& tf1, const Transform3f& tf2) const
jpan's avatar
jpan committed
629
  {
Gautham Manoharan's avatar
Gautham Manoharan committed
630
    if(!tree1->nodeHasChildren(root1) && !tree2->nodeHasChildren(root2))
jpan's avatar
jpan committed
631
632
633
634
    {
      if(tree1->isNodeOccupied(root1) && tree2->isNodeOccupied(root2))
      {
        Box box1, box2;
635
        Transform3f box1_tf, box2_tf;
jpan's avatar
jpan committed
636
637
638
639
        constructBox(bv1, tf1, box1, box1_tf);
        constructBox(bv2, tf2, box2, box2_tf);

        FCL_REAL dist;
640
641
642
        Vec3f closest_p1, closest_p2, normal;
        solver->shapeDistance(box1, box1_tf, box2, box2_tf, dist, closest_p1,
                              closest_p2, normal);
643

644
645
646
        dresult->update(dist, tree1, tree2, (int) (root1 - tree1->getRoot()),
                        (int) (root2 - tree2->getRoot()),
                        closest_p1, closest_p2, normal);
jpan's avatar
jpan committed
647
        
648
        return drequest->isSatisfied(*dresult);
jpan's avatar
jpan committed
649
650
651
652
653
654
655
      }
      else
        return false;
    }

    if(!tree1->isNodeOccupied(root1) || !tree2->isNodeOccupied(root2)) return false;

Gautham Manoharan's avatar
Gautham Manoharan committed
656
    if(!tree2->nodeHasChildren(root2) || (tree1->nodeHasChildren(root1) && (bv1.size() > bv2.size())))
jpan's avatar
jpan committed
657
658
659
    {
      for(unsigned int i = 0; i < 8; ++i)
      {
Gautham Manoharan's avatar
Gautham Manoharan committed
660
        if(tree1->nodeChildExists(root1, i))
jpan's avatar
jpan committed
661
        {
Gautham Manoharan's avatar
Gautham Manoharan committed
662
          const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
jpan's avatar
jpan committed
663
664
665
666
          AABB child_bv;
          computeChildBV(bv1, i, child_bv);

          FCL_REAL d;
jpan's avatar
   
jpan committed
667
668
669
670
671
          AABB aabb1, aabb2;
          convertBV(bv1, tf1, aabb1);
          convertBV(bv2, tf2, aabb2);
          d = aabb1.distance(aabb2);

672
          if(d < dresult->min_distance)
jpan's avatar
jpan committed
673
674
          {
          
675
            if(OcTreeDistanceRecurse(tree1, child, child_bv, tree2, root2, bv2, tf1, tf2))
jpan's avatar
jpan committed
676
677
678
679
680
681
682
683
684
              return true;
          }
        }
      }
    }
    else
    {
      for(unsigned int i = 0; i < 8; ++i)
      {
Gautham Manoharan's avatar
Gautham Manoharan committed
685
        if(tree2->nodeChildExists(root2, i))
jpan's avatar
jpan committed
686
        {
Gautham Manoharan's avatar
Gautham Manoharan committed
687
          const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i);
jpan's avatar
jpan committed
688
689
690
691
          AABB child_bv;
          computeChildBV(bv2, i, child_bv);

          FCL_REAL d;
jpan's avatar
   
jpan committed
692
693
694
695
          AABB aabb1, aabb2;
          convertBV(bv1, tf1, aabb1);
          convertBV(bv2, tf2, aabb2);
          d = aabb1.distance(aabb2);
jpan's avatar
jpan committed
696

697
          if(d < dresult->min_distance)
jpan's avatar
jpan committed
698
          {
699
            if(OcTreeDistanceRecurse(tree1, root1, bv1, tree2, child, child_bv, tf1, tf2))
jpan's avatar
jpan committed
700
701
702
703
704
705
706
707
708
709
              return true;
          }
        }
      }
    }
    
    return false;
  }


710
711
  bool OcTreeIntersectRecurse(const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1,
                              const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& bv2,
712
                              const Transform3f& tf1, const Transform3f& tf2) const
jpan's avatar
jpan committed
713
  {
jpan's avatar
jpan committed
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
    if(!root1 && !root2)
    {
      OBB obb1, obb2;
      convertBV(bv1, tf1, obb1);
      convertBV(bv2, tf2, obb2);

      if(obb1.overlap(obb2))
      {
        Box box1, box2;
        Transform3f box1_tf, box2_tf;
        constructBox(bv1, tf1, box1, box1_tf);
        constructBox(bv2, tf2, box2, box2_tf);
        
        AABB overlap_part;
        AABB aabb1, aabb2;
        computeBV<AABB, Box>(box1, box1_tf, aabb1);
        computeBV<AABB, Box>(box2, box2_tf, aabb2);
        aabb1.overlap(aabb2, overlap_part);
      }

      return false;
    }
    else if(!root1 && root2)
    {
Gautham Manoharan's avatar
Gautham Manoharan committed
738
      if(tree2->nodeHasChildren(root2))
jpan's avatar
jpan committed
739
740
741
      {
        for(unsigned int i = 0; i < 8; ++i)
        {
Gautham Manoharan's avatar
Gautham Manoharan committed
742
          if(tree2->nodeChildExists(root2, i))
jpan's avatar
jpan committed
743
          {
Gautham Manoharan's avatar
Gautham Manoharan committed
744
            const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i);
jpan's avatar
jpan committed
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
            AABB child_bv;
            computeChildBV(bv2, i, child_bv);
            if(OcTreeIntersectRecurse(tree1, NULL, bv1, tree2, child, child_bv, tf1, tf2))
              return true;
          }
          else 
          {
            AABB child_bv;
            computeChildBV(bv2, i, child_bv);
            if(OcTreeIntersectRecurse(tree1, NULL, bv1, tree2, NULL, child_bv, tf1, tf2))
              return true;
          }
        }
      }
      else
      {
        if(OcTreeIntersectRecurse(tree1, NULL, bv1, tree2, NULL, bv2, tf1, tf2))
          return true;
      }
      
      return false;
    }
    else if(root1 && !root2)
    {
Gautham Manoharan's avatar
Gautham Manoharan committed
769
      if(tree1->nodeHasChildren(root1))
jpan's avatar
jpan committed
770
771
772
      {
        for(unsigned int i = 0; i < 8; ++i)
        {
Gautham Manoharan's avatar
Gautham Manoharan committed
773
          if(tree1->nodeChildExists(root1, i))
jpan's avatar
jpan committed
774
          {
Gautham Manoharan's avatar
Gautham Manoharan committed
775
            const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
jpan's avatar
jpan committed
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
            AABB child_bv;
            computeChildBV(bv1, i,  child_bv);
            if(OcTreeIntersectRecurse(tree1, child, child_bv, tree2, NULL, bv2, tf1, tf2))
              return true;
          }
          else
          {
            AABB child_bv;
            computeChildBV(bv1, i, child_bv);
            if(OcTreeIntersectRecurse(tree1, NULL, child_bv, tree2, NULL, bv2, tf1, tf2))
              return true;
          }
        }
      }
      else
      {
        if(OcTreeIntersectRecurse(tree1, NULL, bv1, tree2, NULL, bv2, tf1, tf2))
          return true;
      }
      
      return false;
    }
Gautham Manoharan's avatar
Gautham Manoharan committed
798
    else if(!tree1->nodeHasChildren(root1) && !tree2->nodeHasChildren(root2))
jpan's avatar
jpan committed
799
    {
800
      if(tree1->isNodeOccupied(root1) && tree2->isNodeOccupied(root2)) // occupied area
jpan's avatar
jpan committed
801
      {
jpan's avatar
   
jpan committed
802
        if(!crequest->enable_contact)
jpan's avatar
jpan committed
803
        {
jpan's avatar
   
jpan committed
804
805
806
807
808
          OBB obb1, obb2;
          convertBV(bv1, tf1, obb1);
          convertBV(bv2, tf2, obb2);
          
          if(obb1.overlap(obb2))
809
810
811
812
          {
            if(cresult->numContacts() < crequest->num_max_contacts)
              cresult->addContact(Contact(tree1, tree2, root1 - tree1->getRoot(), root2 - tree2->getRoot()));
          }
jpan's avatar
jpan committed
813
814
815
        }
        else
        {
jpan's avatar
   
jpan committed
816
          Box box1, box2;
817
          Transform3f box1_tf, box2_tf;
jpan's avatar
   
jpan committed
818
819
820
          constructBox(bv1, tf1, box1, box1_tf);
          constructBox(bv2, tf2, box2, box2_tf);

jpan's avatar
jpan committed
821
822
823
824
          Vec3f contact;
          FCL_REAL depth;
          Vec3f normal;
          if(solver->shapeIntersect(box1, box1_tf, box2, box2_tf, &contact, &depth, &normal))
825
826
827
828
          {
            if(cresult->numContacts() < crequest->num_max_contacts)
              cresult->addContact(Contact(tree1, tree2, root1 - tree1->getRoot(), root2 - tree2->getRoot(), contact, normal, depth));
          }
jpan's avatar
jpan committed
829
830
        }

831
        return crequest->isSatisfied(*cresult);
jpan's avatar
jpan committed
832
      }
833
      else // free area (at least one node is free)
jpan's avatar
jpan committed
834
835
836
        return false;
    }

837
838
    /// stop when 1) bounding boxes of two objects not overlap; OR
    ///           2) at least one of the nodes is free; OR
839
    ///           2) (two uncertain nodes OR one node occupied and one node uncertain) AND cost not required
840
    if(tree1->isNodeFree(root1) || tree2->isNodeFree(root2)) return false;
841
842
    else if((tree1->isNodeUncertain(root1) || tree2->isNodeUncertain(root2)))
      return false;
843
844
845
846
847
848
849
    else 
    {
      OBB obb1, obb2;
      convertBV(bv1, tf1, obb1);
      convertBV(bv2, tf2, obb2);
      if(!obb1.overlap(obb2)) return false;
    }
jpan's avatar
   
jpan committed
850

Gautham Manoharan's avatar
Gautham Manoharan committed
851
    if(!tree2->nodeHasChildren(root2) || (tree1->nodeHasChildren(root1) && (bv1.size() > bv2.size())))
jpan's avatar
jpan committed
852
853
854
    {
      for(unsigned int i = 0; i < 8; ++i)
      {
Gautham Manoharan's avatar
Gautham Manoharan committed
855
        if(tree1->nodeChildExists(root1, i))
jpan's avatar
jpan committed
856
        {
Gautham Manoharan's avatar
Gautham Manoharan committed
857
          const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
jpan's avatar
jpan committed
858
859
860
861
862
          AABB child_bv;
          computeChildBV(bv1, i, child_bv);
        
          if(OcTreeIntersectRecurse(tree1, child, child_bv, 
                                    tree2, root2, bv2,
jpan's avatar
   
jpan committed
863
                                    tf1, tf2))
jpan's avatar
jpan committed
864
865
866
867
868
869
870
871
            return true;
        }
      }
    }
    else
    {
      for(unsigned int i = 0; i < 8; ++i)
      {
Gautham Manoharan's avatar
Gautham Manoharan committed
872
        if(tree2->nodeChildExists(root2, i))
jpan's avatar
jpan committed
873
        {
Gautham Manoharan's avatar
Gautham Manoharan committed
874
          const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i);
jpan's avatar
jpan committed
875
876
877
878
879
          AABB child_bv;
          computeChildBV(bv2, i, child_bv);
          
          if(OcTreeIntersectRecurse(tree1, root1, bv1,
                                    tree2, child, child_bv,
jpan's avatar
   
jpan committed
880
                                    tf1, tf2))
jpan's avatar
jpan committed
881
882
883
884
885
886
887
888
889
            return true;
        }
      }
    }

    return false;
  }
};

890
891
/// @addtogroup Traversal_For_Collision
/// @{
jpan's avatar
jpan committed
892

893
/// @brief Traversal node for octree collision
jpan's avatar
jpan committed
894
895
896
class OcTreeCollisionTraversalNode : public CollisionTraversalNodeBase
{
public:
897
898
  OcTreeCollisionTraversalNode(const CollisionRequest& request) :
  CollisionTraversalNodeBase (request)
jpan's avatar
jpan committed
899
900
901
902
903
904
905
  {
    model1 = NULL;
    model2 = NULL;

    otsolver = NULL;
  }

906
  bool BVDisjoints(int, int) const
jpan's avatar
jpan committed
907
908
909
910
  {
    return false;
  }

911
  bool BVDisjoints(int, int, FCL_REAL&) const
Steve Tonneau's avatar
Steve Tonneau committed
912
913
914
915
  {
    return false;
  }

916
  void leafCollides(int, int, FCL_REAL&) const
jpan's avatar
jpan committed
917
  {
jpan's avatar
   
jpan committed
918
    otsolver->OcTreeIntersect(model1, model2, tf1, tf2, request, *result);
jpan's avatar
jpan committed
919
920
921
922
923
  }

  const OcTree* model1;
  const OcTree* model2;

924
  Transform3f tf1, tf2;
jpan's avatar
jpan committed
925

Lucile Remigy's avatar
Lucile Remigy committed
926
  const OcTreeSolver* otsolver;
jpan's avatar
jpan committed
927
928
};

929
/// @brief Traversal node for shape-octree collision
Lucile Remigy's avatar
Lucile Remigy committed
930
template<typename S>
jpan's avatar
jpan committed
931
932
933
class ShapeOcTreeCollisionTraversalNode : public CollisionTraversalNodeBase
{
public:
934
935
 ShapeOcTreeCollisionTraversalNode(const CollisionRequest& request) :
  CollisionTraversalNodeBase (request)
jpan's avatar
jpan committed
936
937
938
939
940
941
942
  {
    model1 = NULL;
    model2 = NULL;

    otsolver = NULL;
  }

943
  bool BVDisjoints(int, int) const
jpan's avatar
jpan committed
944
945
946
947
  {
    return false;
  }

948
  bool BVDisjoints(int, int, FCL_REAL&) const
Steve Tonneau's avatar
Steve Tonneau committed
949
950
951
952
  {
    return false;
  }

953
  void leafCollides(int, int, FCL_REAL&) const
jpan's avatar
jpan committed
954
  {
jpan's avatar
   
jpan committed
955
    otsolver->OcTreeShapeIntersect(model2, *model1, tf2, tf1, request, *result);
jpan's avatar
jpan committed
956
957
958
959
960
  }

  const S* model1;
  const OcTree* model2;

961
  Transform3f tf1, tf2;
jpan's avatar
jpan committed
962

Lucile Remigy's avatar
Lucile Remigy committed
963
  const OcTreeSolver* otsolver;
jpan's avatar
jpan committed
964
965
};

966
/// @brief Traversal node for octree-shape collision
967

Lucile Remigy's avatar
Lucile Remigy committed
968
template<typename S>
jpan's avatar
jpan committed
969
970
971
class OcTreeShapeCollisionTraversalNode : public CollisionTraversalNodeBase
{
public:
972
973
 OcTreeShapeCollisionTraversalNode(const CollisionRequest& request) :
  CollisionTraversalNodeBase (request)
jpan's avatar
jpan committed
974
975
976
977
978
979
980
  {
    model1 = NULL;
    model2 = NULL;

    otsolver = NULL;
  }

981
  bool BVDisjoints(int, int) const
jpan's avatar
jpan committed
982
983
984
985
  {
    return false;
  }

986
  bool BVDisjoints(int, int, fcl::FCL_REAL&) const
Steve Tonneau's avatar
Steve Tonneau committed
987
988
989
990
  {
    return false;
  }