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 |