traversal_node_bvhs.h 15.1 KB
Newer Older
sachinc's avatar
sachinc 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
sachinc's avatar
sachinc 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
sachinc's avatar
sachinc committed
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
 *     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 */


#ifndef FCL_TRAVERSAL_NODE_MESHES_H
#define FCL_TRAVERSAL_NODE_MESHES_H

42
43
44
45
46
47
#include <hpp/fcl/collision_data.h>
#include <hpp/fcl/traversal/traversal_node_base.h>
#include <hpp/fcl/BV/BV_node.h>
#include <hpp/fcl/BV/BV.h>
#include <hpp/fcl/BVH/BVH_model.h>
#include <hpp/fcl/intersect.h>
sachinc's avatar
sachinc committed
48
49
50
51
52

#include <boost/shared_array.hpp>
#include <boost/shared_ptr.hpp>
#include <limits>
#include <vector>
jpan's avatar
   
jpan committed
53
#include <cassert>
sachinc's avatar
sachinc committed
54

55

sachinc's avatar
sachinc committed
56
57
58
namespace fcl
{

59
/// @brief Traversal node for collision between BVH models
sachinc's avatar
sachinc committed
60
61
62
63
template<typename BV>
class BVHCollisionTraversalNode : public CollisionTraversalNodeBase
{
public:
64
65
  BVHCollisionTraversalNode(bool enable_distance_lower_bound) :
  CollisionTraversalNodeBase (enable_distance_lower_bound)
sachinc's avatar
sachinc committed
66
67
68
69
70
71
72
73
74
  {
    model1 = NULL;
    model2 = NULL;

    num_bv_tests = 0;
    num_leaf_tests = 0;
    query_time_seconds = 0.0;
  }

75
  /// @brief Whether the BV node in the first BVH tree is leaf
sachinc's avatar
sachinc committed
76
77
78
79
80
  bool isFirstNodeLeaf(int b) const
  {
    return model1->getBV(b).isLeaf();
  }

81
  /// @brief Whether the BV node in the second BVH tree is leaf
sachinc's avatar
sachinc committed
82
83
84
85
86
  bool isSecondNodeLeaf(int b) const
  {
    return model2->getBV(b).isLeaf();
  }

87
  /// @brief Determine the traversal order, is the first BVTT subtree better
sachinc's avatar
sachinc committed
88
89
  bool firstOverSecond(int b1, int b2) const
  {
90
91
    FCL_REAL sz1 = model1->getBV(b1).bv.size();
    FCL_REAL sz2 = model2->getBV(b2).bv.size();
sachinc's avatar
sachinc committed
92
93
94
95
96
97
98
99
100

    bool l1 = model1->getBV(b1).isLeaf();
    bool l2 = model2->getBV(b2).isLeaf();

    if(l2 || (!l1 && (sz1 > sz2)))
      return true;
    return false;
  }

101
  /// @brief Obtain the left child of BV node in the first BVH
sachinc's avatar
sachinc committed
102
103
104
105
106
  int getFirstLeftChild(int b) const
  {
    return model1->getBV(b).leftChild();
  }

107
  /// @brief Obtain the right child of BV node in the first BVH
sachinc's avatar
sachinc committed
108
109
110
111
112
  int getFirstRightChild(int b) const
  {
    return model1->getBV(b).rightChild();
  }

113
  /// @brief Obtain the left child of BV node in the second BVH
sachinc's avatar
sachinc committed
114
115
116
117
118
  int getSecondLeftChild(int b) const
  {
    return model2->getBV(b).leftChild();
  }

119
  /// @brief Obtain the right child of BV node in the second BVH
sachinc's avatar
sachinc committed
120
121
122
123
124
  int getSecondRightChild(int b) const
  {
    return model2->getBV(b).rightChild();
  }

125
  /// @brief BV culling test in one BVTT node
sachinc's avatar
sachinc committed
126
127
128
129
130
  bool BVTesting(int b1, int b2) const
  {
    if(enable_statistics) num_bv_tests++;
    return !model1->getBV(b1).overlap(model2->getBV(b2));
  }
jpan's avatar
   
jpan committed
131
  
132
133
134
135
136
137
138
139
140
141
  /// BV test between b1 and b2
  /// \param b1, b2 Bounding volumes to test,
  /// \retval sqrDistLowerBound square of a lower bound of the minimal
  ///         distance between bounding volumes.
  bool BVTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const
  {
    if(enable_statistics) num_bv_tests++;
    return !model1->getBV(b1).overlap(model2->getBV(b2), sqrDistLowerBound);
  }
  
142
  /// @brief The first BVH model
sachinc's avatar
sachinc committed
143
  const BVHModel<BV>* model1;
144
  /// @brief The second BVH model
sachinc's avatar
sachinc committed
145
146
  const BVHModel<BV>* model2;

147
  /// @brief statistical information
sachinc's avatar
sachinc committed
148
149
  mutable int num_bv_tests;
  mutable int num_leaf_tests;
150
  mutable FCL_REAL query_time_seconds;
sachinc's avatar
sachinc committed
151
152
153
};


154
/// @brief Traversal node for collision between two meshes
sachinc's avatar
sachinc committed
155
156
157
158
template<typename BV>
class MeshCollisionTraversalNode : public BVHCollisionTraversalNode<BV>
{
public:
159
160
  MeshCollisionTraversalNode(bool enable_distance_lower_bound) :
  BVHCollisionTraversalNode<BV> (enable_distance_lower_bound)
sachinc's avatar
sachinc committed
161
162
163
164
165
166
167
  {
    vertices1 = NULL;
    vertices2 = NULL;
    tri_indices1 = NULL;
    tri_indices2 = NULL;
  }

168
  /// @brief Intersection testing between leaves (two triangles)
169
  void leafTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const
sachinc's avatar
sachinc committed
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
  {
    if(this->enable_statistics) this->num_leaf_tests++;

    const BVNode<BV>& node1 = this->model1->getBV(b1);
    const BVNode<BV>& node2 = this->model2->getBV(b2);

    int primitive_id1 = node1.primitiveId();
    int primitive_id2 = node2.primitiveId();

    const Triangle& tri_id1 = tri_indices1[primitive_id1];
    const Triangle& tri_id2 = tri_indices2[primitive_id2];

    const Vec3f& p1 = vertices1[tri_id1[0]];
    const Vec3f& p2 = vertices1[tri_id1[1]];
    const Vec3f& p3 = vertices1[tri_id1[2]];
    const Vec3f& q1 = vertices2[tri_id2[0]];
    const Vec3f& q2 = vertices2[tri_id2[1]];
    const Vec3f& q3 = vertices2[tri_id2[2]];
jpan's avatar
jpan committed
188
    
189
    if(this->model1->isOccupied() && this->model2->isOccupied())
sachinc's avatar
sachinc committed
190
    {
191
192
193
      bool is_intersect = false;

      if(!this->request.enable_contact) // only interested in collision or not
sachinc's avatar
sachinc committed
194
      {
195
196
197
        if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3))
        {
          is_intersect = true;
jpan's avatar
jpan committed
198
199
          if(this->result->numContacts() < this->request.num_max_contacts)
            this->result->addContact(Contact(this->model1, this->model2, primitive_id1, primitive_id2));
200
        }
sachinc's avatar
sachinc committed
201
      }
202
      else // need compute the contact information
sachinc's avatar
sachinc committed
203
      {
204
205
206
207
208
209
210
211
212
213
        FCL_REAL penetration;
        Vec3f normal;
        unsigned int n_contacts;
        Vec3f contacts[2];

        if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3,
                                         contacts,
                                         &n_contacts,
                                         &penetration,
                                         &normal))
jpan's avatar
jpan committed
214
        {
215
          is_intersect = true;
jpan's avatar
jpan committed
216
217
218
          
          if(this->request.num_max_contacts < n_contacts + this->result->numContacts())
            n_contacts = (this->request.num_max_contacts >= this->result->numContacts()) ? (this->request.num_max_contacts - this->result->numContacts()) : 0;
jpan's avatar
jpan committed
219
    
220
221
222
223
          for(unsigned int i = 0; i < n_contacts; ++i)
          {
            this->result->addContact(Contact(this->model1, this->model2, primitive_id1, primitive_id2, contacts[i], normal, penetration));
          }
sachinc's avatar
sachinc committed
224
225
        }
      }
jpan's avatar
jpan committed
226

227
228
229
230
      if(is_intersect && this->request.enable_cost)
      {
        AABB overlap_part;
        AABB(p1, p2, p3).overlap(AABB(q1, q2, q3), overlap_part);
231
        this->result->addCostSource(CostSource(overlap_part, cost_density), this->request.num_max_cost_sources);      
232
      }
jpan's avatar
jpan committed
233
    }   
234
235
236
237
238
239
    else if((!this->model1->isFree() && !this->model2->isFree()) && this->request.enable_cost)
    {
      if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3))
      {
        AABB overlap_part;
        AABB(p1, p2, p3).overlap(AABB(q1, q2, q3), overlap_part);
240
        this->result->addCostSource(CostSource(overlap_part, cost_density), this->request.num_max_cost_sources);      
241
242
      }
    }
sachinc's avatar
sachinc committed
243
244
  }

245
  /// @brief Whether the traversal process can stop early
sachinc's avatar
sachinc committed
246
247
  bool canStop() const
  {
248
    return this->request.isSatisfied(*(this->result));
sachinc's avatar
sachinc committed
249
250
251
252
253
254
255
256
  }

  Vec3f* vertices1;
  Vec3f* vertices2;

  Triangle* tri_indices1;
  Triangle* tri_indices2;

jpan's avatar
jpan committed
257
  FCL_REAL cost_density;
sachinc's avatar
sachinc committed
258
259
260
};


261
/// @brief Traversal node for collision between two meshes if their underlying BVH node is oriented node (OBB, RSS, OBBRSS, kIOS)
sachinc's avatar
sachinc committed
262
263
264
class MeshCollisionTraversalNodeOBB : public MeshCollisionTraversalNode<OBB>
{
public:
265
  MeshCollisionTraversalNodeOBB (bool enable_distance_lower_bound);
sachinc's avatar
sachinc committed
266
267
268

  bool BVTesting(int b1, int b2) const;

269
  void leafTesting(int b1, int b2, FCL_REAL&) const;
sachinc's avatar
sachinc committed
270

jpan's avatar
   
jpan committed
271
272
  bool BVTesting(int b1, int b2, const Matrix3f& Rc, const Vec3f& Tc) const;

273
274
  void leafTesting(int b1, int b2, const Matrix3f& Rc, const Vec3f& Tc,
		   FCL_REAL& sqrDistLowerBound) const;
jpan's avatar
   
jpan committed
275

276
  Matrix3f R;
sachinc's avatar
sachinc committed
277
278
279
280
281
282
  Vec3f T;
};

class MeshCollisionTraversalNodeRSS : public MeshCollisionTraversalNode<RSS>
{
public:
283
  MeshCollisionTraversalNodeRSS (bool enable_distance_lower_bound);
sachinc's avatar
sachinc committed
284
285
286

  bool BVTesting(int b1, int b2) const;

287
  void leafTesting(int b1, int b2, FCL_REAL&) const;
sachinc's avatar
sachinc committed
288

289
  Matrix3f R;
sachinc's avatar
sachinc committed
290
291
292
  Vec3f T;
};

293
294
295
class MeshCollisionTraversalNodekIOS : public MeshCollisionTraversalNode<kIOS>
{
public:
296
  MeshCollisionTraversalNodekIOS (bool enable_distance_lower_bound);
297
298
299
 
  bool BVTesting(int b1, int b2) const;

300
  void leafTesting(int b1, int b2, FCL_REAL&) const;
sachinc's avatar
sachinc committed
301

302
303
304
  Matrix3f R;
  Vec3f T;
};
sachinc's avatar
sachinc committed
305

jpan's avatar
   
jpan committed
306
307
308
class MeshCollisionTraversalNodeOBBRSS : public MeshCollisionTraversalNode<OBBRSS>
{
public:
309
  MeshCollisionTraversalNodeOBBRSS (bool enable_distance_lower_bound);
jpan's avatar
   
jpan committed
310
311
312
 
  bool BVTesting(int b1, int b2) const;

313
314
  bool BVTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const;

315
  void leafTesting(int b1, int b2, FCL_REAL&) const;
jpan's avatar
   
jpan committed
316
317
318
319
320

  Matrix3f R;
  Vec3f T;
};

321
/// @brief Traversal node for distance computation between BVH models
sachinc's avatar
sachinc committed
322
323
324
325
326
327
328
329
330
331
332
333
334
335
template<typename BV>
class BVHDistanceTraversalNode : public DistanceTraversalNodeBase
{
public:
  BVHDistanceTraversalNode() : DistanceTraversalNodeBase()
  {
    model1 = NULL;
    model2 = NULL;

    num_bv_tests = 0;
    num_leaf_tests = 0;
    query_time_seconds = 0.0;
  }

336
  /// @brief Whether the BV node in the first BVH tree is leaf
sachinc's avatar
sachinc committed
337
338
339
340
341
  bool isFirstNodeLeaf(int b) const
  {
    return model1->getBV(b).isLeaf();
  }

342
  /// @brief Whether the BV node in the second BVH tree is leaf
sachinc's avatar
sachinc committed
343
344
345
346
347
  bool isSecondNodeLeaf(int b) const
  {
    return model2->getBV(b).isLeaf();
  }

348
  /// @brief Determine the traversal order, is the first BVTT subtree better
sachinc's avatar
sachinc committed
349
350
  bool firstOverSecond(int b1, int b2) const
  {
351
352
    FCL_REAL sz1 = model1->getBV(b1).bv.size();
    FCL_REAL sz2 = model2->getBV(b2).bv.size();
sachinc's avatar
sachinc committed
353
354
355
356
357
358
359
360
361

    bool l1 = model1->getBV(b1).isLeaf();
    bool l2 = model2->getBV(b2).isLeaf();

    if(l2 || (!l1 && (sz1 > sz2)))
      return true;
    return false;
  }

362
  /// @brief Obtain the left child of BV node in the first BVH
sachinc's avatar
sachinc committed
363
364
365
366
367
  int getFirstLeftChild(int b) const
  {
    return model1->getBV(b).leftChild();
  }

368
  /// @brief Obtain the right child of BV node in the first BVH
sachinc's avatar
sachinc committed
369
370
371
372
373
  int getFirstRightChild(int b) const
  {
    return model1->getBV(b).rightChild();
  }

374
  /// @brief Obtain the left child of BV node in the second BVH
sachinc's avatar
sachinc committed
375
376
377
378
379
  int getSecondLeftChild(int b) const
  {
    return model2->getBV(b).leftChild();
  }

380
  /// @brief Obtain the right child of BV node in the second BVH
sachinc's avatar
sachinc committed
381
382
383
384
385
  int getSecondRightChild(int b) const
  {
    return model2->getBV(b).rightChild();
  }

386
  /// @brief BV culling test in one BVTT node
387
  FCL_REAL BVTesting(int b1, int b2) const
sachinc's avatar
sachinc committed
388
389
390
391
392
  {
    if(enable_statistics) num_bv_tests++;
    return model1->getBV(b1).distance(model2->getBV(b2));
  }

393
  /// @brief The first BVH model
sachinc's avatar
sachinc committed
394
  const BVHModel<BV>* model1;
395
  /// @brief The second BVH model
sachinc's avatar
sachinc committed
396
397
  const BVHModel<BV>* model2;

398
  /// @brief statistical information
sachinc's avatar
sachinc committed
399
400
  mutable int num_bv_tests;
  mutable int num_leaf_tests;
401
  mutable FCL_REAL query_time_seconds;
sachinc's avatar
sachinc committed
402
403
404
};


405
/// @brief Traversal node for distance computation between two meshes
sachinc's avatar
sachinc committed
406
407
408
409
410
411
412
413
414
415
416
template<typename BV>
class MeshDistanceTraversalNode : public BVHDistanceTraversalNode<BV>
{
public:
  MeshDistanceTraversalNode() : BVHDistanceTraversalNode<BV>()
  {
    vertices1 = NULL;
    vertices2 = NULL;
    tri_indices1 = NULL;
    tri_indices2 = NULL;

417
418
    rel_err = this->request.rel_err;
    abs_err = this->request.abs_err;
sachinc's avatar
sachinc committed
419
420
  }

421
  /// @brief Distance testing between leaves (two triangles)
sachinc's avatar
sachinc committed
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
  void leafTesting(int b1, int b2) const
  {
    if(this->enable_statistics) this->num_leaf_tests++;

    const BVNode<BV>& node1 = this->model1->getBV(b1);
    const BVNode<BV>& node2 = this->model2->getBV(b2);

    int primitive_id1 = node1.primitiveId();
    int primitive_id2 = node2.primitiveId();

    const Triangle& tri_id1 = tri_indices1[primitive_id1];
    const Triangle& tri_id2 = tri_indices2[primitive_id2];

    const Vec3f& t11 = vertices1[tri_id1[0]];
    const Vec3f& t12 = vertices1[tri_id1[1]];
    const Vec3f& t13 = vertices1[tri_id1[2]];

    const Vec3f& t21 = vertices2[tri_id2[0]];
    const Vec3f& t22 = vertices2[tri_id2[1]];
    const Vec3f& t23 = vertices2[tri_id2[2]];

    // nearest point pair
    Vec3f P1, P2;

446
447
    FCL_REAL d = sqrt (TriangleDistance::sqrTriDistance
		       (t11, t12, t13, t21, t22, t23, P1, P2));
sachinc's avatar
sachinc committed
448

449
    if(this->request.enable_nearest_points)
sachinc's avatar
sachinc committed
450
    {
451
      this->result->update(d, this->model1, this->model2, primitive_id1, primitive_id2, P1, P2);
sachinc's avatar
sachinc committed
452
    }
453
454
455
456
    else
    {
      this->result->update(d, this->model1, this->model2, primitive_id1, primitive_id2);
    }
457
  }
sachinc's avatar
sachinc committed
458

459
  /// @brief Whether the traversal process can stop early
460
  bool canStop(FCL_REAL c) const
sachinc's avatar
sachinc committed
461
  {
462
    if((c >= this->result->min_distance - abs_err) && (c * (1 + rel_err) >= this->result->min_distance))
sachinc's avatar
sachinc committed
463
464
465
466
467
468
469
470
471
472
      return true;
    return false;
  }

  Vec3f* vertices1;
  Vec3f* vertices2;

  Triangle* tri_indices1;
  Triangle* tri_indices2;

473
  /// @brief relative and absolute error, default value is 0.01 for both terms
474
475
  FCL_REAL rel_err;
  FCL_REAL abs_err;
sachinc's avatar
sachinc committed
476
477
};

478
/// @brief Traversal node for distance computation between two meshes if their underlying BVH node is oriented node (RSS, OBBRSS, kIOS)
sachinc's avatar
sachinc committed
479
480
481
482
483
class MeshDistanceTraversalNodeRSS : public MeshDistanceTraversalNode<RSS>
{
public:
  MeshDistanceTraversalNodeRSS();

jpan's avatar
   
jpan committed
484
485
486
487
  void preprocess();

  void postprocess();

488
  FCL_REAL BVTesting(int b1, int b2) const;
sachinc's avatar
sachinc committed
489
490
491

  void leafTesting(int b1, int b2) const;

492
  Matrix3f R;
sachinc's avatar
sachinc committed
493
494
495
  Vec3f T;
};

496

497
498
499
500
501
class MeshDistanceTraversalNodekIOS : public MeshDistanceTraversalNode<kIOS>
{
public:
  MeshDistanceTraversalNodekIOS();

jpan's avatar
   
jpan committed
502
503
504
505
  void preprocess();
  
  void postprocess();

506
  FCL_REAL BVTesting(int b1, int b2) const;
jpan's avatar
   
jpan committed
507
508
509
510
511
512
513
514
515
516
517
518

  void leafTesting(int b1, int b2) const;

  Matrix3f R;
  Vec3f T;
};

class MeshDistanceTraversalNodeOBBRSS : public MeshDistanceTraversalNode<OBBRSS>
{
public:
  MeshDistanceTraversalNodeOBBRSS();

jpan's avatar
   
jpan committed
519
520
521
522
  void preprocess();

  void postprocess();

523
  FCL_REAL BVTesting(int b1, int b2) const;
524

525
526
  FCL_REAL BVTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const;

527
528
529
530
531
532
  void leafTesting(int b1, int b2) const;

  Matrix3f R;
  Vec3f T;
};

533
/// @brief for OBB and RSS, there is local coordinate of BV, so normal need to be transformed
534
535
536
537
namespace details
{

template<typename BV>
Joseph Mirabel's avatar
Joseph Mirabel committed
538
inline const Matrix3f& getBVAxes(const BV& bv)
539
{
Joseph Mirabel's avatar
Joseph Mirabel committed
540
  return bv.axes;
541
542
}

sachinc's avatar
sachinc committed
543
template<>
Joseph Mirabel's avatar
Joseph Mirabel committed
544
inline const Matrix3f& getBVAxes<OBBRSS>(const OBBRSS& bv)
545
{
Joseph Mirabel's avatar
Joseph Mirabel committed
546
  return bv.obb.axes;
547
548
549
550
}


}
sachinc's avatar
sachinc committed
551

552
}
sachinc's avatar
sachinc committed
553
554

#endif