GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: src/traversal/traversal_recurse.cpp Lines: 168 168 100.0 %
Date: 2024-02-09 12:57:42 Branches: 147 226 65.0 %

Line Branch Exec Source
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
36
/** \author Jia Pan */
37
38
#include <hpp/fcl/internal/traversal_recurse.h>
39
40
#include <vector>
41
42
namespace hpp {
43
namespace fcl {
44
50075725
void collisionRecurse(CollisionTraversalNodeBase* node, unsigned int b1,
45
                      unsigned int b2, BVHFrontList* front_list,
46
                      FCL_REAL& sqrDistLowerBound) {
47
50075725
  FCL_REAL sqrDistLowerBound1 = 0, sqrDistLowerBound2 = 0;
48
50075725
  bool l1 = node->isFirstNodeLeaf(b1);
49
50075725
  bool l2 = node->isSecondNodeLeaf(b2);
50

50075725
  if (l1 && l2) {
51
29204630
    updateFrontList(front_list, b1, b2);
52
53
    // if(node->BVDisjoints(b1, b2, sqrDistLowerBound)) return;
54
29204630
    node->leafCollides(b1, b2, sqrDistLowerBound);
55
29269117
    return;
56
  }
57
58

20871095
  if (node->BVDisjoints(b1, b2, sqrDistLowerBound)) {
59
62675
    updateFrontList(front_list, b1, b2);
60
62675
    return;
61
  }
62

20808420
  if (node->firstOverSecond(b1, b2)) {
63
12781102
    unsigned int c1 = (unsigned int)node->getFirstLeftChild(b1);
64
12781102
    unsigned int c2 = (unsigned int)node->getFirstRightChild(b1);
65
66
12781102
    collisionRecurse(node, c1, b2, front_list, sqrDistLowerBound1);
67
68
    // early stop is disabled is front_list is used
69


12781102
    if (node->canStop() && !front_list) return;
70
71
12780086
    collisionRecurse(node, c2, b2, front_list, sqrDistLowerBound2);
72
12780086
    sqrDistLowerBound = std::min(sqrDistLowerBound1, sqrDistLowerBound2);
73
  } else {
74
8027318
    unsigned int c1 = (unsigned int)node->getSecondLeftChild(b2);
75
8027318
    unsigned int c2 = (unsigned int)node->getSecondRightChild(b2);
76
77
8027318
    collisionRecurse(node, b1, c1, front_list, sqrDistLowerBound1);
78
79
    // early stop is disabled is front_list is used
80


8027318
    if (node->canStop() && !front_list) return;
81
82
8026522
    collisionRecurse(node, b1, c2, front_list, sqrDistLowerBound2);
83
8026522
    sqrDistLowerBound = std::min(sqrDistLowerBound1, sqrDistLowerBound2);
84
  }
85
}
86
87
12
void collisionNonRecurse(CollisionTraversalNodeBase* node,
88
                         BVHFrontList* front_list,
89
                         FCL_REAL& sqrDistLowerBound) {
90
  typedef std::pair<unsigned int, unsigned int> BVPair_t;
91
  // typedef std::stack<BVPair_t, std::vector<BVPair_t> > Stack_t;
92
  typedef std::vector<BVPair_t> Stack_t;
93
94
12
  Stack_t pairs;
95
12
  pairs.reserve(1000);
96
12
  sqrDistLowerBound = std::numeric_limits<FCL_REAL>::infinity();
97
12
  FCL_REAL sdlb = std::numeric_limits<FCL_REAL>::infinity();
98
99
12
  pairs.push_back(BVPair_t(0, 0));
100
101
4904
  while (!pairs.empty()) {
102
4892
    unsigned int a = pairs.back().first, b = pairs.back().second;
103
4892
    pairs.pop_back();
104
105
4892
    bool la = node->isFirstNodeLeaf(a);
106
4892
    bool lb = node->isSecondNodeLeaf(b);
107
108
    // Leaf / Leaf case
109

4892
    if (la && lb) {
110
1821
      updateFrontList(front_list, a, b);
111
112
      // TODO should we test the BVs ?
113
      // if(node->BVDijsoints(a, b, sdlb)) {
114
      // if (sdlb < sqrDistLowerBound) sqrDistLowerBound = sdlb;
115
      // continue;
116
      //}
117
1821
      node->leafCollides(a, b, sdlb);
118
1821
      if (sdlb < sqrDistLowerBound) sqrDistLowerBound = sdlb;
119


1821
      if (node->canStop() && !front_list) return;
120
2452
      continue;
121
    }
122
123
    // TODO shouldn't we test the leaf triangle against BV is la != lb
124
    // if (la && !lb) { // leaf triangle 1 against BV 2
125
    // } else if (!la && lb) { // BV 1 against leaf triangle 2
126
    // }
127
128
    // Check the BV
129

3071
    if (node->BVDisjoints(a, b, sdlb)) {
130
631
      if (sdlb < sqrDistLowerBound) sqrDistLowerBound = sdlb;
131
631
      updateFrontList(front_list, a, b);
132
631
      continue;
133
    }
134
135

2440
    if (node->firstOverSecond(a, b)) {
136
282
      unsigned int c1 = (unsigned int)node->getFirstLeftChild(a);
137
282
      unsigned int c2 = (unsigned int)node->getFirstRightChild(a);
138
282
      pairs.push_back(BVPair_t(c2, b));
139
282
      pairs.push_back(BVPair_t(c1, b));
140
    } else {
141
2158
      unsigned int c1 = (unsigned int)node->getSecondLeftChild(b);
142
2158
      unsigned int c2 = (unsigned int)node->getSecondRightChild(b);
143
2158
      pairs.push_back(BVPair_t(a, c2));
144
2158
      pairs.push_back(BVPair_t(a, c1));
145
    }
146
  }
147
}
148
149
/** Recurse function for self collision
150
 * Make sure node is set correctly so that the first and second tree are the
151
 * same
152
 */
153
1791457
void distanceRecurse(DistanceTraversalNodeBase* node, unsigned int b1,
154
                     unsigned int b2, BVHFrontList* front_list) {
155
1791457
  bool l1 = node->isFirstNodeLeaf(b1);
156
1791457
  bool l2 = node->isSecondNodeLeaf(b2);
157
158

1791457
  if (l1 && l2) {
159
1041065
    updateFrontList(front_list, b1, b2);
160
161
1041065
    node->leafComputeDistance(b1, b2);
162
1041065
    return;
163
  }
164
165
  unsigned int a1, a2, c1, c2;
166
167
750392
  if (node->firstOverSecond(b1, b2)) {
168
543872
    a1 = (unsigned int)node->getFirstLeftChild(b1);
169
543872
    a2 = b2;
170
543872
    c1 = (unsigned int)node->getFirstRightChild(b1);
171
543872
    c2 = b2;
172
  } else {
173
206520
    a1 = b1;
174
206520
    a2 = (unsigned int)node->getSecondLeftChild(b2);
175
206520
    c1 = b1;
176
206520
    c2 = (unsigned int)node->getSecondRightChild(b2);
177
  }
178
179
750392
  FCL_REAL d1 = node->BVDistanceLowerBound(a1, a2);
180
750392
  FCL_REAL d2 = node->BVDistanceLowerBound(c1, c2);
181
182
750392
  if (d2 < d1) {
183
368518
    if (!node->canStop(d2))
184
328293
      distanceRecurse(node, c1, c2, front_list);
185
    else
186
40225
      updateFrontList(front_list, c1, c2);
187
188
368518
    if (!node->canStop(d1))
189
181948
      distanceRecurse(node, a1, a2, front_list);
190
    else
191
186570
      updateFrontList(front_list, a1, a2);
192
  } else {
193
381874
    if (!node->canStop(d1))
194
346986
      distanceRecurse(node, a1, a2, front_list);
195
    else
196
34888
      updateFrontList(front_list, a1, a2);
197
198
381874
    if (!node->canStop(d2))
199
211582
      distanceRecurse(node, c1, c2, front_list);
200
    else
201
170292
      updateFrontList(front_list, c1, c2);
202
  }
203
}
204
205
/** @brief Bounding volume test structure */
206
struct HPP_FCL_LOCAL BVT {
207
  /** @brief distance between bvs */
208
  FCL_REAL d;
209
210
  /** @brief bv indices for a pair of bvs in two models */
211
  unsigned int b1, b2;
212
};
213
214
/** @brief Comparer between two BVT */
215
struct HPP_FCL_LOCAL BVT_Comparer {
216
39290
  bool operator()(const BVT& lhs, const BVT& rhs) const {
217
39290
    return lhs.d > rhs.d;
218
  }
219
};
220
221
struct HPP_FCL_LOCAL BVTQ {
222
609
  BVTQ() : qsize(2) {}
223
224
7936
  bool empty() const { return pq.empty(); }
225
226
  size_t size() const { return pq.size(); }
227
228
7859
  const BVT& top() const { return pq.top(); }
229
230
10908
  void push(const BVT& x) { pq.push(x); }
231
232
7859
  void pop() { pq.pop(); }
233
234
6042
  bool full() const { return (pq.size() + 1 >= qsize); }
235
236
  std::priority_queue<BVT, std::vector<BVT>, BVT_Comparer> pq;
237
238
  /** @brief Queue size */
239
  unsigned int qsize;
240
};
241
242
609
void distanceQueueRecurse(DistanceTraversalNodeBase* node, unsigned int b1,
243
                          unsigned int b2, BVHFrontList* front_list,
244
                          unsigned int qsize) {
245
1218
  BVTQ bvtq;
246
609
  bvtq.qsize = qsize;
247
248
  BVT min_test;
249
609
  min_test.b1 = b1;
250
609
  min_test.b2 = b2;
251
252
  while (1) {
253
7936
    bool l1 = node->isFirstNodeLeaf(min_test.b1);
254
7936
    bool l2 = node->isSecondNodeLeaf(min_test.b2);
255
256

7936
    if (l1 && l2) {
257
1894
      updateFrontList(front_list, min_test.b1, min_test.b2);
258
259
1894
      node->leafComputeDistance(min_test.b1, min_test.b2);
260

6042
    } else if (bvtq.full()) {
261
      // queue should not get two more tests, recur
262
263
588
      distanceQueueRecurse(node, min_test.b1, min_test.b2, front_list, qsize);
264
    } else {
265
      // queue capacity is not full yet
266
      BVT bvt1, bvt2;
267
268

5454
      if (node->firstOverSecond(min_test.b1, min_test.b2)) {
269
765
        unsigned int c1 = (unsigned int)node->getFirstLeftChild(min_test.b1);
270
765
        unsigned int c2 = (unsigned int)node->getFirstRightChild(min_test.b1);
271
765
        bvt1.b1 = c1;
272
765
        bvt1.b2 = min_test.b2;
273
765
        bvt1.d = node->BVDistanceLowerBound(bvt1.b1, bvt1.b2);
274
275
765
        bvt2.b1 = c2;
276
765
        bvt2.b2 = min_test.b2;
277
765
        bvt2.d = node->BVDistanceLowerBound(bvt2.b1, bvt2.b2);
278
      } else {
279
4689
        unsigned int c1 = (unsigned int)node->getSecondLeftChild(min_test.b2);
280
4689
        unsigned int c2 = (unsigned int)node->getSecondRightChild(min_test.b2);
281
4689
        bvt1.b1 = min_test.b1;
282
4689
        bvt1.b2 = c1;
283
4689
        bvt1.d = node->BVDistanceLowerBound(bvt1.b1, bvt1.b2);
284
285
4689
        bvt2.b1 = min_test.b1;
286
4689
        bvt2.b2 = c2;
287
4689
        bvt2.d = node->BVDistanceLowerBound(bvt2.b1, bvt2.b2);
288
      }
289
290
5454
      bvtq.push(bvt1);
291
5454
      bvtq.push(bvt2);
292
    }
293
294

7936
    if (bvtq.empty())
295
77
      break;
296
    else {
297
7859
      min_test = bvtq.top();
298
7859
      bvtq.pop();
299
300

7859
      if (node->canStop(min_test.d)) {
301
532
        updateFrontList(front_list, min_test.b1, min_test.b2);
302
532
        break;
303
      }
304
    }
305
7327
  }
306
609
}
307
308
46
void propagateBVHFrontListCollisionRecurse(CollisionTraversalNodeBase* node,
309
                                           const CollisionRequest& /*request*/,
310
                                           CollisionResult& result,
311
                                           BVHFrontList* front_list) {
312
46
  FCL_REAL sqrDistLowerBound = -1, sqrDistLowerBound1 = 0,
313
46
           sqrDistLowerBound2 = 0;
314
46
  BVHFrontList::iterator front_iter;
315
92
  BVHFrontList append;
316
8460521
  for (front_iter = front_list->begin(); front_iter != front_list->end();
317
8460475
       ++front_iter) {
318
8460475
    unsigned int b1 = front_iter->left;
319
8460475
    unsigned int b2 = front_iter->right;
320
8460475
    bool l1 = node->isFirstNodeLeaf(b1);
321
8460475
    bool l2 = node->isSecondNodeLeaf(b2);
322
323
8460475
    if (l1 & l2) {
324
8446667
      front_iter->valid = false;  // the front node is no longer valid, in
325
                                  // collideRecurse will add again.
326
8446667
      collisionRecurse(node, b1, b2, &append, sqrDistLowerBound);
327
    } else {
328

13808
      if (!node->BVDisjoints(b1, b2, sqrDistLowerBound)) {
329
1731
        front_iter->valid = false;
330

1731
        if (node->firstOverSecond(b1, b2)) {
331
1047
          unsigned int c1 = (unsigned int)node->getFirstLeftChild(b1);
332
1047
          unsigned int c2 = (unsigned int)node->getFirstRightChild(b1);
333
334
1047
          collisionRecurse(node, c1, b2, front_list, sqrDistLowerBound1);
335
1047
          collisionRecurse(node, c2, b2, front_list, sqrDistLowerBound2);
336
1047
          sqrDistLowerBound = std::min(sqrDistLowerBound1, sqrDistLowerBound2);
337
        } else {
338
684
          unsigned int c1 = (unsigned int)node->getSecondLeftChild(b2);
339
684
          unsigned int c2 = (unsigned int)node->getSecondRightChild(b2);
340
341
684
          collisionRecurse(node, b1, c1, front_list, sqrDistLowerBound1);
342
684
          collisionRecurse(node, b1, c2, front_list, sqrDistLowerBound2);
343
684
          sqrDistLowerBound = std::min(sqrDistLowerBound1, sqrDistLowerBound2);
344
        }
345
      }
346
    }
347
8460475
    result.updateDistanceLowerBound(sqrt(sqrDistLowerBound));
348
  }
349
350
  // clean the old front list (remove invalid node)
351
8460521
  for (front_iter = front_list->begin(); front_iter != front_list->end();) {
352
8460475
    if (!front_iter->valid)
353
8448398
      front_iter = front_list->erase(front_iter);
354
    else
355
12077
      ++front_iter;
356
  }
357
358
8446713
  for (front_iter = append.begin(); front_iter != append.end(); ++front_iter) {
359
8446667
    front_list->push_back(*front_iter);
360
  }
361
46
}
362
363
}  // namespace fcl
364
365
}  // namespace hpp