GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
Line | Branch | Exec | Source |
1 |
/* |
||
2 |
* Software License Agreement (BSD License) |
||
3 |
* |
||
4 |
* Copyright (c) 2011-2014, Willow Garage, Inc. |
||
5 |
* Copyright (c) 2014-2016, 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 |
#ifndef HPP_FCL_HIERARCHY_TREE_INL_H |
||
39 |
#define HPP_FCL_HIERARCHY_TREE_INL_H |
||
40 |
|||
41 |
#include "hpp/fcl/broadphase/detail/hierarchy_tree.h" |
||
42 |
|||
43 |
namespace hpp { |
||
44 |
namespace fcl { |
||
45 |
|||
46 |
namespace detail { |
||
47 |
|||
48 |
//============================================================================== |
||
49 |
template <typename BV> |
||
50 |
104 |
HierarchyTree<BV>::HierarchyTree(int bu_threshold_, int topdown_level_) { |
|
51 |
104 |
root_node = nullptr; |
|
52 |
104 |
n_leaves = 0; |
|
53 |
104 |
free_node = nullptr; |
|
54 |
104 |
max_lookahead_level = -1; |
|
55 |
104 |
opath = 0; |
|
56 |
104 |
bu_threshold = bu_threshold_; |
|
57 |
104 |
topdown_level = topdown_level_; |
|
58 |
104 |
} |
|
59 |
|||
60 |
//============================================================================== |
||
61 |
template <typename BV> |
||
62 |
102 |
HierarchyTree<BV>::~HierarchyTree() { |
|
63 |
102 |
clear(); |
|
64 |
102 |
} |
|
65 |
|||
66 |
//============================================================================== |
||
67 |
template <typename BV> |
||
68 |
86 |
void HierarchyTree<BV>::init(std::vector<Node*>& leaves, int level) { |
|
69 |
✓✗✓✗ ✗ |
86 |
switch (level) { |
70 |
43 |
case 0: |
|
71 |
43 |
init_0(leaves); |
|
72 |
43 |
break; |
|
73 |
case 1: |
||
74 |
init_1(leaves); |
||
75 |
break; |
||
76 |
43 |
case 2: |
|
77 |
43 |
init_2(leaves); |
|
78 |
43 |
break; |
|
79 |
case 3: |
||
80 |
init_3(leaves); |
||
81 |
break; |
||
82 |
default: |
||
83 |
init_0(leaves); |
||
84 |
} |
||
85 |
86 |
} |
|
86 |
|||
87 |
//============================================================================== |
||
88 |
template <typename BV> |
||
89 |
4 |
typename HierarchyTree<BV>::Node* HierarchyTree<BV>::insert(const BV& bv, |
|
90 |
void* data) { |
||
91 |
4 |
Node* leaf = createNode(nullptr, bv, data); |
|
92 |
4 |
insertLeaf(root_node, leaf); |
|
93 |
4 |
++n_leaves; |
|
94 |
4 |
return leaf; |
|
95 |
} |
||
96 |
|||
97 |
//============================================================================== |
||
98 |
template <typename BV> |
||
99 |
void HierarchyTree<BV>::remove(Node* leaf) { |
||
100 |
removeLeaf(leaf); |
||
101 |
deleteNode(leaf); |
||
102 |
--n_leaves; |
||
103 |
} |
||
104 |
|||
105 |
//============================================================================== |
||
106 |
template <typename BV> |
||
107 |
188 |
void HierarchyTree<BV>::clear() { |
|
108 |
✓✓ | 188 |
if (root_node) recurseDeleteNode(root_node); |
109 |
188 |
n_leaves = 0; |
|
110 |
✓✓ | 188 |
delete free_node; |
111 |
188 |
free_node = nullptr; |
|
112 |
188 |
max_lookahead_level = -1; |
|
113 |
188 |
opath = 0; |
|
114 |
188 |
} |
|
115 |
|||
116 |
//============================================================================== |
||
117 |
template <typename BV> |
||
118 |
bool HierarchyTree<BV>::empty() const { |
||
119 |
return (nullptr == root_node); |
||
120 |
} |
||
121 |
|||
122 |
//============================================================================== |
||
123 |
template <typename BV> |
||
124 |
350 |
void HierarchyTree<BV>::update(Node* leaf, int lookahead_level) { |
|
125 |
// TODO(DamrongGuoy): Since we update a leaf node by removing and |
||
126 |
// inserting the same leaf node, it is likely to change the structure of |
||
127 |
// the tree even if no object's pose has changed. In the future, |
||
128 |
// find a way to preserve the structure of the tree to solve this issue: |
||
129 |
// https://github.com/flexible-collision-library/fcl/issues/368 |
||
130 |
|||
131 |
// First we remove the leaf node pointed by `leaf` variable from the tree. |
||
132 |
// The `sub_root` variable is the root of the subtree containing nodes |
||
133 |
// whose BVs were adjusted due to node removal. |
||
134 |
350 |
typename HierarchyTree<BV>::Node* sub_root = removeLeaf(leaf); |
|
135 |
✓✗ | 350 |
if (sub_root) { |
136 |
✗✓ | 350 |
if (lookahead_level > 0) { |
137 |
// For positive `lookahead_level`, we move the `sub_root` variable up. |
||
138 |
for (int i = 0; (i < lookahead_level) && sub_root->parent; ++i) |
||
139 |
sub_root = sub_root->parent; |
||
140 |
} else |
||
141 |
// By default, lookahead_level = -1, and we reset the `sub_root` variable |
||
142 |
// to the root of the entire tree. |
||
143 |
350 |
sub_root = root_node; |
|
144 |
} |
||
145 |
// Then we insert the node pointed by `leaf` variable back into the |
||
146 |
// subtree rooted at `sub_root` variable. |
||
147 |
350 |
insertLeaf(sub_root, leaf); |
|
148 |
350 |
} |
|
149 |
|||
150 |
//============================================================================== |
||
151 |
template <typename BV> |
||
152 |
bool HierarchyTree<BV>::update(Node* leaf, const BV& bv) { |
||
153 |
if (leaf->bv.contain(bv)) return false; |
||
154 |
update_(leaf, bv); |
||
155 |
return true; |
||
156 |
} |
||
157 |
|||
158 |
//============================================================================== |
||
159 |
template <typename S, typename BV> |
||
160 |
struct UpdateImpl { |
||
161 |
static bool run(const HierarchyTree<BV>& tree, |
||
162 |
typename HierarchyTree<BV>::Node* leaf, const BV& bv, |
||
163 |
const Vec3f& /*vel*/, FCL_REAL /*margin*/) { |
||
164 |
if (leaf->bv.contain(bv)) return false; |
||
165 |
tree.update_(leaf, bv); |
||
166 |
return true; |
||
167 |
} |
||
168 |
|||
169 |
static bool run(const HierarchyTree<BV>& tree, |
||
170 |
typename HierarchyTree<BV>::Node* leaf, const BV& bv, |
||
171 |
const Vec3f& /*vel*/) { |
||
172 |
if (leaf->bv.contain(bv)) return false; |
||
173 |
tree.update_(leaf, bv); |
||
174 |
return true; |
||
175 |
} |
||
176 |
}; |
||
177 |
|||
178 |
//============================================================================== |
||
179 |
template <typename BV> |
||
180 |
bool HierarchyTree<BV>::update(Node* leaf, const BV& bv, const Vec3f& vel, |
||
181 |
FCL_REAL margin) { |
||
182 |
return UpdateImpl<FCL_REAL, BV>::run(*this, leaf, bv, vel, margin); |
||
183 |
} |
||
184 |
|||
185 |
//============================================================================== |
||
186 |
template <typename BV> |
||
187 |
bool HierarchyTree<BV>::update(Node* leaf, const BV& bv, const Vec3f& vel) { |
||
188 |
return UpdateImpl<FCL_REAL, BV>::run(*this, leaf, bv, vel); |
||
189 |
} |
||
190 |
|||
191 |
//============================================================================== |
||
192 |
template <typename BV> |
||
193 |
35 |
size_t HierarchyTree<BV>::getMaxHeight() const { |
|
194 |
✗✓ | 35 |
if (!root_node) return 0; |
195 |
35 |
return getMaxHeight(root_node); |
|
196 |
} |
||
197 |
|||
198 |
//============================================================================== |
||
199 |
template <typename BV> |
||
200 |
size_t HierarchyTree<BV>::getMaxDepth() const { |
||
201 |
if (!root_node) return 0; |
||
202 |
|||
203 |
size_t max_depth; |
||
204 |
getMaxDepth(root_node, 0, max_depth); |
||
205 |
return max_depth; |
||
206 |
} |
||
207 |
|||
208 |
//============================================================================== |
||
209 |
template <typename BV> |
||
210 |
void HierarchyTree<BV>::balanceBottomup() { |
||
211 |
if (root_node) { |
||
212 |
std::vector<Node*> leaves; |
||
213 |
leaves.reserve(n_leaves); |
||
214 |
fetchLeaves(root_node, leaves); |
||
215 |
bottomup(leaves.begin(), leaves.end()); |
||
216 |
root_node = leaves[0]; |
||
217 |
} |
||
218 |
} |
||
219 |
|||
220 |
//============================================================================== |
||
221 |
template <typename BV> |
||
222 |
void HierarchyTree<BV>::balanceTopdown() { |
||
223 |
if (root_node) { |
||
224 |
std::vector<Node*> leaves; |
||
225 |
leaves.reserve(n_leaves); |
||
226 |
fetchLeaves(root_node, leaves); |
||
227 |
root_node = topdown(leaves.begin(), leaves.end()); |
||
228 |
} |
||
229 |
} |
||
230 |
|||
231 |
//============================================================================== |
||
232 |
template <typename BV> |
||
233 |
35 |
void HierarchyTree<BV>::balanceIncremental(int iterations) { |
|
234 |
✗✓ | 35 |
if (iterations < 0) iterations = (int)n_leaves; |
235 |
✓✗✓✗ |
35 |
if (root_node && (iterations > 0)) { |
236 |
✓✓ | 385 |
for (int i = 0; i < iterations; ++i) { |
237 |
350 |
Node* node = root_node; |
|
238 |
350 |
unsigned int bit = 0; |
|
239 |
✓✓ | 1710 |
while (!node->isLeaf()) { |
240 |
1360 |
node = sort(node, root_node)->children[(opath >> bit) & 1]; |
|
241 |
1360 |
bit = (bit + 1) & (sizeof(unsigned int) * 8 - 1); |
|
242 |
} |
||
243 |
350 |
update(node); |
|
244 |
350 |
++opath; |
|
245 |
} |
||
246 |
} |
||
247 |
35 |
} |
|
248 |
|||
249 |
//============================================================================== |
||
250 |
template <typename BV> |
||
251 |
77 |
void HierarchyTree<BV>::refit() { |
|
252 |
✓✗ | 77 |
if (root_node) recurseRefit(root_node); |
253 |
77 |
} |
|
254 |
|||
255 |
//============================================================================== |
||
256 |
template <typename BV> |
||
257 |
void HierarchyTree<BV>::extractLeaves(const Node* root, |
||
258 |
std::vector<Node*>& leaves) const { |
||
259 |
if (!root->isLeaf()) { |
||
260 |
extractLeaves(root->children[0], leaves); |
||
261 |
extractLeaves(root->children[1], leaves); |
||
262 |
} else |
||
263 |
leaves.push_back(root); |
||
264 |
} |
||
265 |
|||
266 |
//============================================================================== |
||
267 |
template <typename BV> |
||
268 |
8037 |
size_t HierarchyTree<BV>::size() const { |
|
269 |
8037 |
return n_leaves; |
|
270 |
} |
||
271 |
|||
272 |
//============================================================================== |
||
273 |
template <typename BV> |
||
274 |
7763 |
typename HierarchyTree<BV>::Node* HierarchyTree<BV>::getRoot() const { |
|
275 |
7763 |
return root_node; |
|
276 |
} |
||
277 |
|||
278 |
//============================================================================== |
||
279 |
template <typename BV> |
||
280 |
typename HierarchyTree<BV>::Node*& HierarchyTree<BV>::getRoot() { |
||
281 |
return root_node; |
||
282 |
} |
||
283 |
|||
284 |
//============================================================================== |
||
285 |
template <typename BV> |
||
286 |
void HierarchyTree<BV>::print(Node* root, int depth) { |
||
287 |
for (int i = 0; i < depth; ++i) std::cout << " "; |
||
288 |
std::cout << " (" << root->bv.min_[0] << ", " << root->bv.min_[1] << ", " |
||
289 |
<< root->bv.min_[2] << "; " << root->bv.max_[0] << ", " |
||
290 |
<< root->bv.max_[1] << ", " << root->bv.max_[2] << ")" << std::endl; |
||
291 |
if (root->isLeaf()) { |
||
292 |
} else { |
||
293 |
print(root->children[0], depth + 1); |
||
294 |
print(root->children[1], depth + 1); |
||
295 |
} |
||
296 |
} |
||
297 |
|||
298 |
//============================================================================== |
||
299 |
template <typename BV> |
||
300 |
4837 |
void HierarchyTree<BV>::bottomup(const NodeVecIterator lbeg, |
|
301 |
const NodeVecIterator lend) { |
||
302 |
4837 |
NodeVecIterator lcur_end = lend; |
|
303 |
✓✓ | 9674 |
while (lbeg < lcur_end - 1) { |
304 |
4837 |
NodeVecIterator min_it1 = lbeg; |
|
305 |
4837 |
NodeVecIterator min_it2 = lbeg + 1; |
|
306 |
4837 |
FCL_REAL min_size = (std::numeric_limits<FCL_REAL>::max)(); |
|
307 |
✓✓ | 14511 |
for (NodeVecIterator it1 = lbeg; it1 < lcur_end; ++it1) { |
308 |
✓✓ | 14511 |
for (NodeVecIterator it2 = it1 + 1; it2 < lcur_end; ++it2) { |
309 |
✓✗✓✗ |
4837 |
FCL_REAL cur_size = ((*it1)->bv + (*it2)->bv).size(); |
310 |
✓✗ | 4837 |
if (cur_size < min_size) { |
311 |
4837 |
min_size = cur_size; |
|
312 |
4837 |
min_it1 = it1; |
|
313 |
4837 |
min_it2 = it2; |
|
314 |
} |
||
315 |
} |
||
316 |
} |
||
317 |
|||
318 |
4837 |
Node* n[2] = {*min_it1, *min_it2}; |
|
319 |
✓✗ | 4837 |
Node* p = createNode(nullptr, n[0]->bv, n[1]->bv, nullptr); |
320 |
4837 |
p->children[0] = n[0]; |
|
321 |
4837 |
p->children[1] = n[1]; |
|
322 |
4837 |
n[0]->parent = p; |
|
323 |
4837 |
n[1]->parent = p; |
|
324 |
4837 |
*min_it1 = p; |
|
325 |
4837 |
Node* tmp = *min_it2; |
|
326 |
4837 |
lcur_end--; |
|
327 |
4837 |
*min_it2 = *lcur_end; |
|
328 |
4837 |
*lcur_end = tmp; |
|
329 |
} |
||
330 |
4837 |
} |
|
331 |
|||
332 |
//============================================================================== |
||
333 |
template <typename BV> |
||
334 |
43 |
typename HierarchyTree<BV>::Node* HierarchyTree<BV>::topdown( |
|
335 |
const NodeVecIterator lbeg, const NodeVecIterator lend) { |
||
336 |
✓✗✗ | 43 |
switch (topdown_level) { |
337 |
43 |
case 0: |
|
338 |
43 |
return topdown_0(lbeg, lend); |
|
339 |
break; |
||
340 |
case 1: |
||
341 |
return topdown_1(lbeg, lend); |
||
342 |
break; |
||
343 |
default: |
||
344 |
return topdown_0(lbeg, lend); |
||
345 |
} |
||
346 |
} |
||
347 |
|||
348 |
//============================================================================== |
||
349 |
template <typename BV> |
||
350 |
5377 |
size_t HierarchyTree<BV>::getMaxHeight(Node* node) const { |
|
351 |
✓✓ | 5377 |
if (!node->isLeaf()) { |
352 |
✓✗ | 2671 |
size_t height1 = getMaxHeight(node->children[0]); |
353 |
✓✗ | 2671 |
size_t height2 = getMaxHeight(node->children[1]); |
354 |
2671 |
return std::max(height1, height2) + 1; |
|
355 |
} else |
||
356 |
2706 |
return 0; |
|
357 |
} |
||
358 |
|||
359 |
//============================================================================== |
||
360 |
template <typename BV> |
||
361 |
void HierarchyTree<BV>::getMaxDepth(Node* node, size_t depth, |
||
362 |
size_t& max_depth) const { |
||
363 |
if (!node->isLeaf()) { |
||
364 |
getMaxDepth(node->children[0], depth + 1, max_depth); |
||
365 |
getMaxDepth(node->children[1], depth + 1, max_depth); |
||
366 |
} else |
||
367 |
max_depth = std::max(max_depth, depth); |
||
368 |
} |
||
369 |
|||
370 |
//============================================================================== |
||
371 |
template <typename BV> |
||
372 |
15625 |
typename HierarchyTree<BV>::Node* HierarchyTree<BV>::topdown_0( |
|
373 |
const NodeVecIterator lbeg, const NodeVecIterator lend) { |
||
374 |
15625 |
long num_leaves = lend - lbeg; |
|
375 |
✓✓ | 15625 |
if (num_leaves > 1) { |
376 |
✓✓ | 12628 |
if (num_leaves > bu_threshold) { |
377 |
✓✗ | 7791 |
BV vol = (*lbeg)->bv; |
378 |
✓✓✓✗ |
118399 |
for (NodeVecIterator it = lbeg + 1; it < lend; ++it) vol += (*it)->bv; |
379 |
|||
380 |
7791 |
int best_axis = 0; |
|
381 |
✓✗✓✗ ✓✗ |
7791 |
FCL_REAL extent[3] = {vol.width(), vol.height(), vol.depth()}; |
382 |
✓✓ | 7791 |
if (extent[1] > extent[0]) best_axis = 1; |
383 |
✓✓ | 7791 |
if (extent[2] > extent[best_axis]) best_axis = 2; |
384 |
|||
385 |
// compute median |
||
386 |
7791 |
NodeVecIterator lcenter = lbeg + num_leaves / 2; |
|
387 |
✓✗✓✗ |
7791 |
std::nth_element(lbeg, lcenter, lend, |
388 |
std::bind(&nodeBaseLess<BV>, std::placeholders::_1, |
||
389 |
7791 |
std::placeholders::_2, std::ref(best_axis))); |
|
390 |
|||
391 |
✓✗ | 7791 |
Node* node = createNode(nullptr, vol, nullptr); |
392 |
✓✗ | 7791 |
node->children[0] = topdown_0(lbeg, lcenter); |
393 |
✓✗ | 7791 |
node->children[1] = topdown_0(lcenter, lend); |
394 |
7791 |
node->children[0]->parent = node; |
|
395 |
7791 |
node->children[1]->parent = node; |
|
396 |
7791 |
return node; |
|
397 |
} else { |
||
398 |
4837 |
bottomup(lbeg, lend); |
|
399 |
4837 |
return *lbeg; |
|
400 |
} |
||
401 |
} |
||
402 |
2997 |
return *lbeg; |
|
403 |
} |
||
404 |
|||
405 |
//============================================================================== |
||
406 |
template <typename BV> |
||
407 |
typename HierarchyTree<BV>::Node* HierarchyTree<BV>::topdown_1( |
||
408 |
const NodeVecIterator lbeg, const NodeVecIterator lend) { |
||
409 |
long num_leaves = lend - lbeg; |
||
410 |
if (num_leaves > 1) { |
||
411 |
if (num_leaves > bu_threshold) { |
||
412 |
Vec3f split_p = (*lbeg)->bv.center(); |
||
413 |
BV vol = (*lbeg)->bv; |
||
414 |
NodeVecIterator it; |
||
415 |
for (it = lbeg + 1; it < lend; ++it) { |
||
416 |
split_p += (*it)->bv.center(); |
||
417 |
vol += (*it)->bv; |
||
418 |
} |
||
419 |
split_p /= static_cast<FCL_REAL>(num_leaves); |
||
420 |
int best_axis = -1; |
||
421 |
long bestmidp = num_leaves; |
||
422 |
int splitcount[3][2] = {{0, 0}, {0, 0}, {0, 0}}; |
||
423 |
for (it = lbeg; it < lend; ++it) { |
||
424 |
Vec3f x = (*it)->bv.center() - split_p; |
||
425 |
for (int j = 0; j < 3; ++j) ++splitcount[j][x[j] > 0 ? 1 : 0]; |
||
426 |
} |
||
427 |
|||
428 |
for (int i = 0; i < 3; ++i) { |
||
429 |
if ((splitcount[i][0] > 0) && (splitcount[i][1] > 0)) { |
||
430 |
long midp = std::abs(splitcount[i][0] - splitcount[i][1]); |
||
431 |
if (midp < bestmidp) { |
||
432 |
best_axis = i; |
||
433 |
bestmidp = midp; |
||
434 |
} |
||
435 |
} |
||
436 |
} |
||
437 |
|||
438 |
if (best_axis < 0) best_axis = 0; |
||
439 |
|||
440 |
FCL_REAL split_value = split_p[best_axis]; |
||
441 |
NodeVecIterator lcenter = lbeg; |
||
442 |
for (it = lbeg; it < lend; ++it) { |
||
443 |
if ((*it)->bv.center()[best_axis] < split_value) { |
||
444 |
Node* temp = *it; |
||
445 |
*it = *lcenter; |
||
446 |
*lcenter = temp; |
||
447 |
++lcenter; |
||
448 |
} |
||
449 |
} |
||
450 |
|||
451 |
Node* node = createNode(nullptr, vol, nullptr); |
||
452 |
node->children[0] = topdown_1(lbeg, lcenter); |
||
453 |
node->children[1] = topdown_1(lcenter, lend); |
||
454 |
node->children[0]->parent = node; |
||
455 |
node->children[1]->parent = node; |
||
456 |
return node; |
||
457 |
} else { |
||
458 |
bottomup(lbeg, lend); |
||
459 |
return *lbeg; |
||
460 |
} |
||
461 |
} |
||
462 |
return *lbeg; |
||
463 |
} |
||
464 |
|||
465 |
//============================================================================== |
||
466 |
template <typename BV> |
||
467 |
43 |
void HierarchyTree<BV>::init_0(std::vector<Node*>& leaves) { |
|
468 |
43 |
clear(); |
|
469 |
43 |
root_node = topdown(leaves.begin(), leaves.end()); |
|
470 |
43 |
n_leaves = leaves.size(); |
|
471 |
43 |
max_lookahead_level = -1; |
|
472 |
43 |
opath = 0; |
|
473 |
43 |
} |
|
474 |
|||
475 |
//============================================================================== |
||
476 |
template <typename BV> |
||
477 |
void HierarchyTree<BV>::init_1(std::vector<Node*>& leaves) { |
||
478 |
clear(); |
||
479 |
|||
480 |
BV bound_bv; |
||
481 |
if (leaves.size() > 0) bound_bv = leaves[0]->bv; |
||
482 |
for (size_t i = 1; i < leaves.size(); ++i) bound_bv += leaves[i]->bv; |
||
483 |
|||
484 |
morton_functor<FCL_REAL, uint32_t> coder(bound_bv); |
||
485 |
for (size_t i = 0; i < leaves.size(); ++i) |
||
486 |
leaves[i]->code = coder(leaves[i]->bv.center()); |
||
487 |
|||
488 |
std::sort(leaves.begin(), leaves.end(), SortByMorton()); |
||
489 |
|||
490 |
root_node = mortonRecurse_0(leaves.begin(), leaves.end(), |
||
491 |
(1 << (coder.bits() - 1)), coder.bits() - 1); |
||
492 |
|||
493 |
refit(); |
||
494 |
n_leaves = leaves.size(); |
||
495 |
max_lookahead_level = -1; |
||
496 |
opath = 0; |
||
497 |
} |
||
498 |
|||
499 |
//============================================================================== |
||
500 |
template <typename BV> |
||
501 |
43 |
void HierarchyTree<BV>::init_2(std::vector<Node*>& leaves) { |
|
502 |
✓✗ | 43 |
clear(); |
503 |
|||
504 |
✓✗ | 43 |
BV bound_bv; |
505 |
✓✗✓✗ |
43 |
if (leaves.size() > 0) bound_bv = leaves[0]->bv; |
506 |
✓✓✓✗ |
12671 |
for (size_t i = 1; i < leaves.size(); ++i) bound_bv += leaves[i]->bv; |
507 |
|||
508 |
✓✗ | 43 |
morton_functor<FCL_REAL, uint32_t> coder(bound_bv); |
509 |
✓✓ | 12714 |
for (size_t i = 0; i < leaves.size(); ++i) |
510 |
✓✗✓✗ |
12671 |
leaves[i]->code = coder(leaves[i]->bv.center()); |
511 |
|||
512 |
✓✗ | 43 |
std::sort(leaves.begin(), leaves.end(), SortByMorton()); |
513 |
|||
514 |
✓✗ | 43 |
root_node = mortonRecurse_1(leaves.begin(), leaves.end(), |
515 |
43 |
(1 << (coder.bits() - 1)), coder.bits() - 1); |
|
516 |
|||
517 |
✓✗ | 43 |
refit(); |
518 |
43 |
n_leaves = leaves.size(); |
|
519 |
43 |
max_lookahead_level = -1; |
|
520 |
43 |
opath = 0; |
|
521 |
43 |
} |
|
522 |
|||
523 |
//============================================================================== |
||
524 |
template <typename BV> |
||
525 |
void HierarchyTree<BV>::init_3(std::vector<Node*>& leaves) { |
||
526 |
clear(); |
||
527 |
|||
528 |
BV bound_bv; |
||
529 |
if (leaves.size() > 0) bound_bv = leaves[0]->bv; |
||
530 |
for (size_t i = 1; i < leaves.size(); ++i) bound_bv += leaves[i]->bv; |
||
531 |
|||
532 |
morton_functor<FCL_REAL, uint32_t> coder(bound_bv); |
||
533 |
for (size_t i = 0; i < leaves.size(); ++i) |
||
534 |
leaves[i]->code = coder(leaves[i]->bv.center()); |
||
535 |
|||
536 |
std::sort(leaves.begin(), leaves.end(), SortByMorton()); |
||
537 |
|||
538 |
root_node = mortonRecurse_2(leaves.begin(), leaves.end()); |
||
539 |
|||
540 |
refit(); |
||
541 |
n_leaves = leaves.size(); |
||
542 |
max_lookahead_level = -1; |
||
543 |
opath = 0; |
||
544 |
} |
||
545 |
|||
546 |
//============================================================================== |
||
547 |
template <typename BV> |
||
548 |
typename HierarchyTree<BV>::Node* HierarchyTree<BV>::mortonRecurse_0( |
||
549 |
const NodeVecIterator lbeg, const NodeVecIterator lend, |
||
550 |
const uint32_t& split, int bits) { |
||
551 |
long num_leaves = lend - lbeg; |
||
552 |
if (num_leaves > 1) { |
||
553 |
if (bits > 0) { |
||
554 |
Node dummy; |
||
555 |
dummy.code = split; |
||
556 |
NodeVecIterator lcenter = |
||
557 |
std::lower_bound(lbeg, lend, &dummy, SortByMorton()); |
||
558 |
|||
559 |
if (lcenter == lbeg) { |
||
560 |
uint32_t split2 = split | (1 << (bits - 1)); |
||
561 |
return mortonRecurse_0(lbeg, lend, split2, bits - 1); |
||
562 |
} else if (lcenter == lend) { |
||
563 |
uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); |
||
564 |
return mortonRecurse_0(lbeg, lend, split1, bits - 1); |
||
565 |
} else { |
||
566 |
uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); |
||
567 |
uint32_t split2 = split | (1 << (bits - 1)); |
||
568 |
|||
569 |
Node* child1 = mortonRecurse_0(lbeg, lcenter, split1, bits - 1); |
||
570 |
Node* child2 = mortonRecurse_0(lcenter, lend, split2, bits - 1); |
||
571 |
Node* node = createNode(nullptr, nullptr); |
||
572 |
node->children[0] = child1; |
||
573 |
node->children[1] = child2; |
||
574 |
child1->parent = node; |
||
575 |
child2->parent = node; |
||
576 |
return node; |
||
577 |
} |
||
578 |
} else { |
||
579 |
Node* node = topdown(lbeg, lend); |
||
580 |
return node; |
||
581 |
} |
||
582 |
} else |
||
583 |
return *lbeg; |
||
584 |
} |
||
585 |
|||
586 |
//============================================================================== |
||
587 |
template <typename BV> |
||
588 |
37671 |
typename HierarchyTree<BV>::Node* HierarchyTree<BV>::mortonRecurse_1( |
|
589 |
const NodeVecIterator lbeg, const NodeVecIterator lend, |
||
590 |
const uint32_t& split, int bits) { |
||
591 |
37671 |
long num_leaves = lend - lbeg; |
|
592 |
✓✓ | 37671 |
if (num_leaves > 1) { |
593 |
✓✗ | 25000 |
if (bits > 0) { |
594 |
✓✗ | 25000 |
Node dummy; |
595 |
25000 |
dummy.code = split; |
|
596 |
NodeVecIterator lcenter = |
||
597 |
✓✗ | 25000 |
std::lower_bound(lbeg, lend, &dummy, SortByMorton()); |
598 |
|||
599 |
✓✓ | 25000 |
if (lcenter == lbeg) { |
600 |
6013 |
uint32_t split2 = split | (1 << (bits - 1)); |
|
601 |
✓✗ | 6013 |
return mortonRecurse_1(lbeg, lend, split2, bits - 1); |
602 |
✓✓ | 18987 |
} else if (lcenter == lend) { |
603 |
6359 |
uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); |
|
604 |
✓✗ | 6359 |
return mortonRecurse_1(lbeg, lend, split1, bits - 1); |
605 |
} else { |
||
606 |
12628 |
uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); |
|
607 |
12628 |
uint32_t split2 = split | (1 << (bits - 1)); |
|
608 |
|||
609 |
✓✗ | 12628 |
Node* child1 = mortonRecurse_1(lbeg, lcenter, split1, bits - 1); |
610 |
✓✗ | 12628 |
Node* child2 = mortonRecurse_1(lcenter, lend, split2, bits - 1); |
611 |
✓✗ | 12628 |
Node* node = createNode(nullptr, nullptr); |
612 |
12628 |
node->children[0] = child1; |
|
613 |
12628 |
node->children[1] = child2; |
|
614 |
12628 |
child1->parent = node; |
|
615 |
12628 |
child2->parent = node; |
|
616 |
12628 |
return node; |
|
617 |
} |
||
618 |
} else { |
||
619 |
Node* child1 = mortonRecurse_1(lbeg, lbeg + num_leaves / 2, 0, bits - 1); |
||
620 |
Node* child2 = mortonRecurse_1(lbeg + num_leaves / 2, lend, 0, bits - 1); |
||
621 |
Node* node = createNode(nullptr, nullptr); |
||
622 |
node->children[0] = child1; |
||
623 |
node->children[1] = child2; |
||
624 |
child1->parent = node; |
||
625 |
child2->parent = node; |
||
626 |
return node; |
||
627 |
} |
||
628 |
} else |
||
629 |
12671 |
return *lbeg; |
|
630 |
} |
||
631 |
|||
632 |
//============================================================================== |
||
633 |
template <typename BV> |
||
634 |
typename HierarchyTree<BV>::Node* HierarchyTree<BV>::mortonRecurse_2( |
||
635 |
const NodeVecIterator lbeg, const NodeVecIterator lend) { |
||
636 |
long num_leaves = lend - lbeg; |
||
637 |
if (num_leaves > 1) { |
||
638 |
Node* child1 = mortonRecurse_2(lbeg, lbeg + num_leaves / 2); |
||
639 |
Node* child2 = mortonRecurse_2(lbeg + num_leaves / 2, lend); |
||
640 |
Node* node = createNode(nullptr, nullptr); |
||
641 |
node->children[0] = child1; |
||
642 |
node->children[1] = child2; |
||
643 |
child1->parent = node; |
||
644 |
child2->parent = node; |
||
645 |
return node; |
||
646 |
} else |
||
647 |
return *lbeg; |
||
648 |
} |
||
649 |
|||
650 |
//============================================================================== |
||
651 |
template <typename BV> |
||
652 |
void HierarchyTree<BV>::update_(Node* leaf, const BV& bv) { |
||
653 |
Node* root = removeLeaf(leaf); |
||
654 |
if (root) { |
||
655 |
if (max_lookahead_level >= 0) { |
||
656 |
for (int i = 0; (i < max_lookahead_level) && root->parent; ++i) |
||
657 |
root = root->parent; |
||
658 |
} else |
||
659 |
root = root_node; |
||
660 |
} |
||
661 |
|||
662 |
leaf->bv = bv; |
||
663 |
insertLeaf(root, leaf); |
||
664 |
} |
||
665 |
|||
666 |
//============================================================================== |
||
667 |
template <typename BV> |
||
668 |
1360 |
typename HierarchyTree<BV>::Node* HierarchyTree<BV>::sort(Node* n, Node*& r) { |
|
669 |
1360 |
Node* p = n->parent; |
|
670 |
✓✓ | 1360 |
if (p > n) { |
671 |
389 |
size_t i = indexOf(n); |
|
672 |
389 |
size_t j = 1 - i; |
|
673 |
389 |
Node* s = p->children[j]; |
|
674 |
389 |
Node* q = p->parent; |
|
675 |
✓✓ | 389 |
if (q) |
676 |
346 |
q->children[indexOf(p)] = n; |
|
677 |
else |
||
678 |
43 |
r = n; |
|
679 |
389 |
s->parent = n; |
|
680 |
389 |
p->parent = n; |
|
681 |
389 |
n->parent = q; |
|
682 |
389 |
p->children[0] = n->children[0]; |
|
683 |
389 |
p->children[1] = n->children[1]; |
|
684 |
389 |
n->children[0]->parent = p; |
|
685 |
389 |
n->children[1]->parent = p; |
|
686 |
389 |
n->children[i] = p; |
|
687 |
389 |
n->children[j] = s; |
|
688 |
389 |
std::swap(p->bv, n->bv); |
|
689 |
389 |
return p; |
|
690 |
} |
||
691 |
971 |
return n; |
|
692 |
} |
||
693 |
|||
694 |
//============================================================================== |
||
695 |
template <typename BV> |
||
696 |
354 |
void HierarchyTree<BV>::insertLeaf(Node* const sub_root, Node* const leaf) |
|
697 |
// Attempts to insert `leaf` into a subtree rooted at `sub_root`. |
||
698 |
// 1. If the whole tree is empty, then `leaf` simply becomes the tree. |
||
699 |
// 2. Otherwise, a leaf node called `sibling` of the subtree rooted at |
||
700 |
// `sub_root` is selected (the selection criteria is a black box for this |
||
701 |
// algorithm), and the `sibling` leaf is replaced by an internal node with |
||
702 |
// two children, `sibling` and `leaf`. The bounding volumes are updated as |
||
703 |
// necessary. |
||
704 |
{ |
||
705 |
✓✓ | 354 |
if (!root_node) { |
706 |
// If the entire tree is empty, the node pointed by `leaf` variable will |
||
707 |
// become the root of the tree. |
||
708 |
2 |
root_node = leaf; |
|
709 |
2 |
leaf->parent = nullptr; |
|
710 |
2 |
return; |
|
711 |
} |
||
712 |
// Traverse the tree from the given `sub_root` down to an existing leaf |
||
713 |
// node that we call `sibling`. The `sibling` node will eventually become |
||
714 |
// the sibling of the given `leaf` node. |
||
715 |
352 |
Node* sibling = sub_root; |
|
716 |
✓✓ | 1520 |
while (!sibling->isLeaf()) { |
717 |
1168 |
sibling = sibling->children[select(*leaf, *(sibling->children[0]), |
|
718 |
1168 |
*(sibling->children[1]))]; |
|
719 |
} |
||
720 |
352 |
Node* prev = sibling->parent; |
|
721 |
// Create a new `node` that later will become the new parent of both the |
||
722 |
// `sibling` and the given `leaf`. |
||
723 |
352 |
Node* node = createNode(prev, leaf->bv, sibling->bv, nullptr); |
|
724 |
✓✓ | 352 |
if (prev) |
725 |
// If the parent `prev` of the `sibling` is an interior node, we will |
||
726 |
// replace the `sibling` with the subtree {node {`sibling`, leaf}} like |
||
727 |
// this: |
||
728 |
// Before After |
||
729 |
// |
||
730 |
// ╱ ╱ |
||
731 |
// prev prev |
||
732 |
// ╱ ╲ ─> ╱ ╲ |
||
733 |
// sibling ... node ... |
||
734 |
// ╱ ╲ |
||
735 |
// sibling leaf |
||
736 |
{ |
||
737 |
260 |
prev->children[indexOf(sibling)] = node; |
|
738 |
260 |
node->children[0] = sibling; |
|
739 |
260 |
sibling->parent = node; |
|
740 |
260 |
node->children[1] = leaf; |
|
741 |
260 |
leaf->parent = node; |
|
742 |
// Now that we've inserted `leaf` some of the existing bounding |
||
743 |
// volumes might not fully enclose their children. Walk up the tree |
||
744 |
// looking for parents that don't already enclose their children |
||
745 |
// and create a new tight-fitting bounding volume for those. |
||
746 |
511 |
do { |
|
747 |
✓✓ | 771 |
if (!prev->bv.contain(node->bv)) |
748 |
✓✗ | 604 |
prev->bv = prev->children[0]->bv + prev->children[1]->bv; |
749 |
else |
||
750 |
167 |
break; |
|
751 |
604 |
node = prev; |
|
752 |
✓✓ | 604 |
} while (nullptr != (prev = node->parent)); |
753 |
} else |
||
754 |
// If the `sibling` has no parent, i.e., the tree is a singleton, |
||
755 |
// we will replace it with the 3-node tree {node {`sibling`, `leaf`}} like |
||
756 |
// this: |
||
757 |
// |
||
758 |
// node |
||
759 |
// ╱ ╲ |
||
760 |
// sibling leaf |
||
761 |
{ |
||
762 |
92 |
node->children[0] = sibling; |
|
763 |
92 |
sibling->parent = node; |
|
764 |
92 |
node->children[1] = leaf; |
|
765 |
92 |
leaf->parent = node; |
|
766 |
92 |
root_node = node; |
|
767 |
} |
||
768 |
|||
769 |
// Note that the above algorithm always adds the new `leaf` node as the right |
||
770 |
// child, i.e., children[1]. Calling removeLeaf(l) followed by calling |
||
771 |
// this function insertLeaf(l) where l is a left child will result in |
||
772 |
// switching l and its sibling even if no object's pose has changed. |
||
773 |
} |
||
774 |
|||
775 |
//============================================================================== |
||
776 |
template <typename BV> |
||
777 |
350 |
typename HierarchyTree<BV>::Node* HierarchyTree<BV>::removeLeaf( |
|
778 |
Node* const leaf) { |
||
779 |
// Deletes `leaf` by replacing the subtree consisting of `leaf`, its sibling, |
||
780 |
// and its parent with just its sibling. It then "tightens" the ancestor |
||
781 |
// bounding volumes. Returns a pointer to the parent of the highest node whose |
||
782 |
// BV changed due to the removal. |
||
783 |
✗✓ | 350 |
if (leaf == root_node) { |
784 |
// If the `leaf` node is the only node in the tree, the tree becomes empty. |
||
785 |
root_node = nullptr; |
||
786 |
return nullptr; |
||
787 |
} |
||
788 |
350 |
Node* parent = leaf->parent; |
|
789 |
350 |
Node* prev = parent->parent; |
|
790 |
350 |
Node* sibling = parent->children[1 - indexOf(leaf)]; |
|
791 |
✓✓ | 350 |
if (prev) { |
792 |
// If the parent node is not the root node, the sibling node will |
||
793 |
// replace the parent node like this: |
||
794 |
// |
||
795 |
// Before After |
||
796 |
// ... ... |
||
797 |
// ╱ ╱ |
||
798 |
// prev prev |
||
799 |
// ╱ ╲ ╱ ╲ |
||
800 |
// parent ... ─> sibling ... |
||
801 |
// ╱ ╲ ╱╲ |
||
802 |
// leaf sibling ... |
||
803 |
// ╱╲ |
||
804 |
// ... |
||
805 |
// |
||
806 |
// Step 1: replace the subtree {parent {leaf, sibling {...}}} with |
||
807 |
// {sibling {...}}. |
||
808 |
260 |
prev->children[indexOf(parent)] = sibling; |
|
809 |
260 |
sibling->parent = prev; |
|
810 |
260 |
deleteNode(parent); |
|
811 |
// Step 2: tighten up the BVs of the ancestor nodes. |
||
812 |
✓✓ | 736 |
while (prev) { |
813 |
✓✗ | 643 |
BV new_bv = prev->children[0]->bv + prev->children[1]->bv; |
814 |
✓✗✓✓ |
643 |
if (!(new_bv == prev->bv)) { |
815 |
✓✗ | 476 |
prev->bv = new_bv; |
816 |
476 |
prev = prev->parent; |
|
817 |
} else |
||
818 |
167 |
break; |
|
819 |
} |
||
820 |
|||
821 |
✓✓ | 260 |
return prev ? prev : root_node; |
822 |
} else { |
||
823 |
// If the parent node is the root node, the sibling node will become the |
||
824 |
// root of the whole tree like this: |
||
825 |
// |
||
826 |
// Before After |
||
827 |
// |
||
828 |
// parent |
||
829 |
// ╱ ╲ |
||
830 |
// leaf sibling ─> sibling |
||
831 |
// ╱╲ ╱╲ |
||
832 |
// ... ... |
||
833 |
90 |
root_node = sibling; |
|
834 |
90 |
sibling->parent = nullptr; |
|
835 |
90 |
deleteNode(parent); |
|
836 |
90 |
return root_node; |
|
837 |
} |
||
838 |
} |
||
839 |
|||
840 |
//============================================================================== |
||
841 |
template <typename BV> |
||
842 |
void HierarchyTree<BV>::fetchLeaves(Node* root, std::vector<Node*>& leaves, |
||
843 |
int depth) { |
||
844 |
if ((!root->isLeaf()) && depth) { |
||
845 |
fetchLeaves(root->children[0], leaves, depth - 1); |
||
846 |
fetchLeaves(root->children[1], leaves, depth - 1); |
||
847 |
deleteNode(root); |
||
848 |
} else { |
||
849 |
leaves.push_back(root); |
||
850 |
} |
||
851 |
} |
||
852 |
|||
853 |
//============================================================================== |
||
854 |
template <typename BV> |
||
855 |
1605 |
size_t HierarchyTree<BV>::indexOf(Node* node) { |
|
856 |
// node cannot be nullptr |
||
857 |
1605 |
return (node->parent->children[1] == node); |
|
858 |
} |
||
859 |
|||
860 |
//============================================================================== |
||
861 |
template <typename BV> |
||
862 |
7795 |
typename HierarchyTree<BV>::Node* HierarchyTree<BV>::createNode(Node* parent, |
|
863 |
const BV& bv, |
||
864 |
void* data) { |
||
865 |
7795 |
Node* node = createNode(parent, data); |
|
866 |
7795 |
node->bv = bv; |
|
867 |
7795 |
return node; |
|
868 |
} |
||
869 |
|||
870 |
//============================================================================== |
||
871 |
template <typename BV> |
||
872 |
5189 |
typename HierarchyTree<BV>::Node* HierarchyTree<BV>::createNode(Node* parent, |
|
873 |
const BV& bv1, |
||
874 |
const BV& bv2, |
||
875 |
void* data) { |
||
876 |
5189 |
Node* node = createNode(parent, data); |
|
877 |
✓✗ | 5189 |
node->bv = bv1 + bv2; |
878 |
5189 |
return node; |
|
879 |
} |
||
880 |
|||
881 |
//============================================================================== |
||
882 |
template <typename BV> |
||
883 |
25612 |
typename HierarchyTree<BV>::Node* HierarchyTree<BV>::createNode(Node* parent, |
|
884 |
void* data) { |
||
885 |
25612 |
Node* node = nullptr; |
|
886 |
✓✓ | 25612 |
if (free_node) { |
887 |
350 |
node = free_node; |
|
888 |
350 |
free_node = nullptr; |
|
889 |
} else |
||
890 |
✓✗ | 25262 |
node = new Node(); |
891 |
25612 |
node->parent = parent; |
|
892 |
25612 |
node->data = data; |
|
893 |
25612 |
node->children[1] = 0; |
|
894 |
25612 |
return node; |
|
895 |
} |
||
896 |
|||
897 |
//============================================================================== |
||
898 |
template <typename BV> |
||
899 |
49756 |
void HierarchyTree<BV>::deleteNode(Node* node) { |
|
900 |
✓✗ | 49756 |
if (free_node != node) { |
901 |
✓✓ | 49756 |
delete free_node; |
902 |
49756 |
free_node = node; |
|
903 |
} |
||
904 |
49756 |
} |
|
905 |
|||
906 |
//============================================================================== |
||
907 |
template <typename BV> |
||
908 |
49406 |
void HierarchyTree<BV>::recurseDeleteNode(Node* node) { |
|
909 |
✓✓ | 49406 |
if (!node->isLeaf()) { |
910 |
24660 |
recurseDeleteNode(node->children[0]); |
|
911 |
24660 |
recurseDeleteNode(node->children[1]); |
|
912 |
} |
||
913 |
|||
914 |
✓✓ | 49406 |
if (node == root_node) root_node = nullptr; |
915 |
49406 |
deleteNode(node); |
|
916 |
49406 |
} |
|
917 |
|||
918 |
//============================================================================== |
||
919 |
template <typename BV> |
||
920 |
30673 |
void HierarchyTree<BV>::recurseRefit(Node* node) { |
|
921 |
✓✓ | 30673 |
if (!node->isLeaf()) { |
922 |
15298 |
recurseRefit(node->children[0]); |
|
923 |
15298 |
recurseRefit(node->children[1]); |
|
924 |
✓✗ | 15298 |
node->bv = node->children[0]->bv + node->children[1]->bv; |
925 |
} else |
||
926 |
15375 |
return; |
|
927 |
} |
||
928 |
|||
929 |
//============================================================================== |
||
930 |
template <typename BV> |
||
931 |
329602 |
bool nodeBaseLess(NodeBase<BV>* a, NodeBase<BV>* b, int d) { |
|
932 |
✓✗✓✗ ✓✗✓✓ |
329602 |
if (a->bv.center()[d] < b->bv.center()[d]) return true; |
933 |
142487 |
return false; |
|
934 |
} |
||
935 |
|||
936 |
//============================================================================== |
||
937 |
template <typename S, typename BV> |
||
938 |
struct SelectImpl { |
||
939 |
static std::size_t run(const NodeBase<BV>& /*query*/, |
||
940 |
const NodeBase<BV>& /*node1*/, |
||
941 |
const NodeBase<BV>& /*node2*/) { |
||
942 |
return 0; |
||
943 |
} |
||
944 |
|||
945 |
static std::size_t run(const BV& /*query*/, const NodeBase<BV>& /*node1*/, |
||
946 |
const NodeBase<BV>& /*node2*/) { |
||
947 |
return 0; |
||
948 |
} |
||
949 |
}; |
||
950 |
|||
951 |
//============================================================================== |
||
952 |
template <typename BV> |
||
953 |
1168 |
size_t select(const NodeBase<BV>& query, const NodeBase<BV>& node1, |
|
954 |
const NodeBase<BV>& node2) { |
||
955 |
1168 |
return SelectImpl<FCL_REAL, BV>::run(query, node1, node2); |
|
956 |
} |
||
957 |
|||
958 |
//============================================================================== |
||
959 |
template <typename BV> |
||
960 |
33247 |
size_t select(const BV& query, const NodeBase<BV>& node1, |
|
961 |
const NodeBase<BV>& node2) { |
||
962 |
33247 |
return SelectImpl<FCL_REAL, BV>::run(query, node1, node2); |
|
963 |
} |
||
964 |
|||
965 |
//============================================================================== |
||
966 |
template <typename S> |
||
967 |
struct SelectImpl<S, AABB> { |
||
968 |
1168 |
static std::size_t run(const NodeBase<AABB>& node, |
|
969 |
const NodeBase<AABB>& node1, |
||
970 |
const NodeBase<AABB>& node2) { |
||
971 |
1168 |
const AABB& bv = node.bv; |
|
972 |
1168 |
const AABB& bv1 = node1.bv; |
|
973 |
1168 |
const AABB& bv2 = node2.bv; |
|
974 |
✓✗✓✗ |
1168 |
Vec3f v = bv.min_ + bv.max_; |
975 |
✓✗✓✗ ✓✗ |
1168 |
Vec3f v1 = v - (bv1.min_ + bv1.max_); |
976 |
✓✗✓✗ ✓✗ |
1168 |
Vec3f v2 = v - (bv2.min_ + bv2.max_); |
977 |
✓✗✓✗ ✓✗ |
1168 |
FCL_REAL d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]); |
978 |
✓✗✓✗ ✓✗ |
1168 |
FCL_REAL d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]); |
979 |
1168 |
return (d1 < d2) ? 0 : 1; |
|
980 |
} |
||
981 |
|||
982 |
33247 |
static std::size_t run(const AABB& query, const NodeBase<AABB>& node1, |
|
983 |
const NodeBase<AABB>& node2) { |
||
984 |
33247 |
const AABB& bv = query; |
|
985 |
33247 |
const AABB& bv1 = node1.bv; |
|
986 |
33247 |
const AABB& bv2 = node2.bv; |
|
987 |
✓✗✓✗ |
33247 |
Vec3f v = bv.min_ + bv.max_; |
988 |
✓✗✓✗ ✓✗ |
33247 |
Vec3f v1 = v - (bv1.min_ + bv1.max_); |
989 |
✓✗✓✗ ✓✗ |
33247 |
Vec3f v2 = v - (bv2.min_ + bv2.max_); |
990 |
✓✗✓✗ ✓✗ |
33247 |
FCL_REAL d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]); |
991 |
✓✗✓✗ ✓✗ |
33247 |
FCL_REAL d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]); |
992 |
33247 |
return (d1 < d2) ? 0 : 1; |
|
993 |
} |
||
994 |
}; |
||
995 |
|||
996 |
} // namespace detail |
||
997 |
} // namespace fcl |
||
998 |
} // namespace hpp |
||
999 |
|||
1000 |
#endif |
Generated by: GCOVR (Version 4.2) |