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_ARRAY_INL_H |
||
39 |
#define HPP_FCL_HIERARCHY_TREE_ARRAY_INL_H |
||
40 |
|||
41 |
#include "hpp/fcl/broadphase/detail/hierarchy_tree_array.h" |
||
42 |
|||
43 |
#include <algorithm> |
||
44 |
#include <iostream> |
||
45 |
|||
46 |
namespace hpp { |
||
47 |
namespace fcl { |
||
48 |
|||
49 |
namespace detail { |
||
50 |
|||
51 |
namespace implementation_array { |
||
52 |
|||
53 |
//============================================================================== |
||
54 |
template <typename BV> |
||
55 |
102 |
HierarchyTree<BV>::HierarchyTree(int bu_threshold_, int topdown_level_) { |
|
56 |
102 |
root_node = NULL_NODE; |
|
57 |
102 |
n_nodes = 0; |
|
58 |
102 |
n_nodes_alloc = 16; |
|
59 |
✓✗✓✓ ✓✗ |
1734 |
nodes = new Node[n_nodes_alloc]; |
60 |
✓✓ | 1632 |
for (size_t i = 0; i < n_nodes_alloc - 1; ++i) nodes[i].next = i + 1; |
61 |
102 |
nodes[n_nodes_alloc - 1].next = NULL_NODE; |
|
62 |
102 |
n_leaves = 0; |
|
63 |
102 |
freelist = 0; |
|
64 |
102 |
opath = 0; |
|
65 |
102 |
max_lookahead_level = -1; |
|
66 |
102 |
bu_threshold = bu_threshold_; |
|
67 |
102 |
topdown_level = topdown_level_; |
|
68 |
102 |
} |
|
69 |
|||
70 |
//============================================================================== |
||
71 |
template <typename BV> |
||
72 |
100 |
HierarchyTree<BV>::~HierarchyTree() { |
|
73 |
✓✗ | 100 |
delete[] nodes; |
74 |
100 |
} |
|
75 |
|||
76 |
//============================================================================== |
||
77 |
template <typename BV> |
||
78 |
86 |
void HierarchyTree<BV>::init(Node* leaves, int n_leaves_, int level) { |
|
79 |
✓✗✓✗ ✗ |
86 |
switch (level) { |
80 |
43 |
case 0: |
|
81 |
43 |
init_0(leaves, n_leaves_); |
|
82 |
43 |
break; |
|
83 |
case 1: |
||
84 |
init_1(leaves, n_leaves_); |
||
85 |
break; |
||
86 |
43 |
case 2: |
|
87 |
43 |
init_2(leaves, n_leaves_); |
|
88 |
43 |
break; |
|
89 |
case 3: |
||
90 |
init_3(leaves, n_leaves_); |
||
91 |
break; |
||
92 |
default: |
||
93 |
init_0(leaves, n_leaves_); |
||
94 |
} |
||
95 |
86 |
} |
|
96 |
|||
97 |
//============================================================================== |
||
98 |
template <typename BV> |
||
99 |
43 |
void HierarchyTree<BV>::init_0(Node* leaves, int n_leaves_) { |
|
100 |
43 |
clear(); |
|
101 |
|||
102 |
43 |
n_leaves = (size_t)n_leaves_; |
|
103 |
43 |
root_node = NULL_NODE; |
|
104 |
✓✗✓✓ ✓✗ |
25385 |
nodes = new Node[n_leaves * 2]; |
105 |
43 |
std::copy(leaves, leaves + n_leaves, nodes); |
|
106 |
43 |
freelist = n_leaves; |
|
107 |
43 |
n_nodes = n_leaves; |
|
108 |
43 |
n_nodes_alloc = 2 * n_leaves; |
|
109 |
✓✓ | 12714 |
for (size_t i = n_leaves; i < n_nodes_alloc; ++i) nodes[i].next = i + 1; |
110 |
43 |
nodes[n_nodes_alloc - 1].next = NULL_NODE; |
|
111 |
|||
112 |
✓✗ | 43 |
size_t* ids = new size_t[n_leaves]; |
113 |
✓✓ | 12714 |
for (size_t i = 0; i < n_leaves; ++i) ids[i] = i; |
114 |
|||
115 |
43 |
root_node = topdown(ids, ids + n_leaves); |
|
116 |
✓✗ | 43 |
delete[] ids; |
117 |
|||
118 |
43 |
opath = 0; |
|
119 |
43 |
max_lookahead_level = -1; |
|
120 |
43 |
} |
|
121 |
|||
122 |
//============================================================================== |
||
123 |
template <typename BV> |
||
124 |
void HierarchyTree<BV>::init_1(Node* leaves, int n_leaves_) { |
||
125 |
clear(); |
||
126 |
|||
127 |
n_leaves = (size_t)n_leaves_; |
||
128 |
root_node = NULL_NODE; |
||
129 |
nodes = new Node[n_leaves * 2]; |
||
130 |
std::copy(leaves, leaves + n_leaves, nodes); |
||
131 |
freelist = n_leaves; |
||
132 |
n_nodes = n_leaves; |
||
133 |
n_nodes_alloc = 2 * n_leaves; |
||
134 |
for (size_t i = n_leaves; i < n_nodes_alloc; ++i) nodes[i].next = i + 1; |
||
135 |
nodes[n_nodes_alloc - 1].next = NULL_NODE; |
||
136 |
|||
137 |
BV bound_bv; |
||
138 |
if (n_leaves > 0) bound_bv = nodes[0].bv; |
||
139 |
for (size_t i = 1; i < n_leaves; ++i) bound_bv += nodes[i].bv; |
||
140 |
|||
141 |
morton_functor<FCL_REAL, uint32_t> coder(bound_bv); |
||
142 |
for (size_t i = 0; i < n_leaves; ++i) |
||
143 |
nodes[i].code = coder(nodes[i].bv.center()); |
||
144 |
|||
145 |
size_t* ids = new size_t[n_leaves]; |
||
146 |
for (size_t i = 0; i < n_leaves; ++i) ids[i] = i; |
||
147 |
|||
148 |
const SortByMorton comp{nodes}; |
||
149 |
std::sort(ids, ids + n_leaves, comp); |
||
150 |
root_node = mortonRecurse_0(ids, ids + n_leaves, (1 << (coder.bits() - 1)), |
||
151 |
coder.bits() - 1); |
||
152 |
delete[] ids; |
||
153 |
|||
154 |
refit(); |
||
155 |
|||
156 |
opath = 0; |
||
157 |
max_lookahead_level = -1; |
||
158 |
} |
||
159 |
|||
160 |
//============================================================================== |
||
161 |
template <typename BV> |
||
162 |
43 |
void HierarchyTree<BV>::init_2(Node* leaves, int n_leaves_) { |
|
163 |
✓✗ | 43 |
clear(); |
164 |
|||
165 |
43 |
n_leaves = (size_t)n_leaves_; |
|
166 |
43 |
root_node = NULL_NODE; |
|
167 |
✓✗✓✗ ✓✓✓✗ |
25385 |
nodes = new Node[n_leaves * 2]; |
168 |
✓✗ | 43 |
std::copy(leaves, leaves + n_leaves, nodes); |
169 |
43 |
freelist = n_leaves; |
|
170 |
43 |
n_nodes = n_leaves; |
|
171 |
43 |
n_nodes_alloc = 2 * n_leaves; |
|
172 |
✓✓ | 12714 |
for (size_t i = n_leaves; i < n_nodes_alloc; ++i) nodes[i].next = i + 1; |
173 |
43 |
nodes[n_nodes_alloc - 1].next = NULL_NODE; |
|
174 |
|||
175 |
✓✗ | 43 |
BV bound_bv; |
176 |
✓✗✓✗ |
43 |
if (n_leaves > 0) bound_bv = nodes[0].bv; |
177 |
✓✓✓✗ |
12671 |
for (size_t i = 1; i < n_leaves; ++i) bound_bv += nodes[i].bv; |
178 |
|||
179 |
✓✗ | 43 |
morton_functor<FCL_REAL, uint32_t> coder(bound_bv); |
180 |
✓✓ | 12714 |
for (size_t i = 0; i < n_leaves; ++i) |
181 |
✓✗✓✗ |
12671 |
nodes[i].code = coder(nodes[i].bv.center()); |
182 |
|||
183 |
✓✗✓✗ |
43 |
size_t* ids = new size_t[n_leaves]; |
184 |
✓✓ | 12714 |
for (size_t i = 0; i < n_leaves; ++i) ids[i] = i; |
185 |
|||
186 |
43 |
const SortByMorton comp{nodes}; |
|
187 |
✓✗ | 43 |
std::sort(ids, ids + n_leaves, comp); |
188 |
✓✗ | 43 |
root_node = mortonRecurse_1(ids, ids + n_leaves, (1 << (coder.bits() - 1)), |
189 |
43 |
coder.bits() - 1); |
|
190 |
✓✗ | 43 |
delete[] ids; |
191 |
|||
192 |
✓✗ | 43 |
refit(); |
193 |
|||
194 |
43 |
opath = 0; |
|
195 |
43 |
max_lookahead_level = -1; |
|
196 |
43 |
} |
|
197 |
|||
198 |
//============================================================================== |
||
199 |
template <typename BV> |
||
200 |
void HierarchyTree<BV>::init_3(Node* leaves, int n_leaves_) { |
||
201 |
clear(); |
||
202 |
|||
203 |
n_leaves = (size_t)n_leaves_; |
||
204 |
root_node = NULL_NODE; |
||
205 |
nodes = new Node[n_leaves * 2]; |
||
206 |
std::copy(leaves, leaves + n_leaves, nodes); |
||
207 |
freelist = n_leaves; |
||
208 |
n_nodes = n_leaves; |
||
209 |
n_nodes_alloc = 2 * n_leaves; |
||
210 |
for (size_t i = n_leaves; i < n_nodes_alloc; ++i) nodes[i].next = i + 1; |
||
211 |
nodes[n_nodes_alloc - 1].next = NULL_NODE; |
||
212 |
|||
213 |
BV bound_bv; |
||
214 |
if (n_leaves > 0) bound_bv = nodes[0].bv; |
||
215 |
for (size_t i = 1; i < n_leaves; ++i) bound_bv += nodes[i].bv; |
||
216 |
|||
217 |
morton_functor<FCL_REAL, uint32_t> coder(bound_bv); |
||
218 |
for (size_t i = 0; i < n_leaves; ++i) |
||
219 |
nodes[i].code = coder(nodes[i].bv.center()); |
||
220 |
|||
221 |
size_t* ids = new size_t[n_leaves]; |
||
222 |
for (size_t i = 0; i < n_leaves; ++i) ids[i] = i; |
||
223 |
|||
224 |
const SortByMorton comp{nodes}; |
||
225 |
std::sort(ids, ids + n_leaves, comp); |
||
226 |
root_node = mortonRecurse_2(ids, ids + n_leaves); |
||
227 |
delete[] ids; |
||
228 |
|||
229 |
refit(); |
||
230 |
|||
231 |
opath = 0; |
||
232 |
max_lookahead_level = -1; |
||
233 |
} |
||
234 |
|||
235 |
//============================================================================== |
||
236 |
template <typename BV> |
||
237 |
size_t HierarchyTree<BV>::insert(const BV& bv, void* data) { |
||
238 |
size_t node = createNode(NULL_NODE, bv, data); |
||
239 |
insertLeaf(root_node, node); |
||
240 |
++n_leaves; |
||
241 |
return node; |
||
242 |
} |
||
243 |
|||
244 |
//============================================================================== |
||
245 |
template <typename BV> |
||
246 |
void HierarchyTree<BV>::remove(size_t leaf) { |
||
247 |
removeLeaf(leaf); |
||
248 |
deleteNode(leaf); |
||
249 |
--n_leaves; |
||
250 |
} |
||
251 |
|||
252 |
//============================================================================== |
||
253 |
template <typename BV> |
||
254 |
86 |
void HierarchyTree<BV>::clear() { |
|
255 |
✓✗ | 86 |
delete[] nodes; |
256 |
86 |
root_node = NULL_NODE; |
|
257 |
86 |
n_nodes = 0; |
|
258 |
86 |
n_nodes_alloc = 16; |
|
259 |
✓✗✓✓ ✓✗ |
1462 |
nodes = new Node[n_nodes_alloc]; |
260 |
✓✓ | 1462 |
for (size_t i = 0; i < n_nodes_alloc; ++i) nodes[i].next = i + 1; |
261 |
86 |
nodes[n_nodes_alloc - 1].next = NULL_NODE; |
|
262 |
86 |
n_leaves = 0; |
|
263 |
86 |
freelist = 0; |
|
264 |
86 |
opath = 0; |
|
265 |
86 |
max_lookahead_level = -1; |
|
266 |
86 |
} |
|
267 |
|||
268 |
//============================================================================== |
||
269 |
template <typename BV> |
||
270 |
bool HierarchyTree<BV>::empty() const { |
||
271 |
return (n_nodes == 0); |
||
272 |
} |
||
273 |
|||
274 |
//============================================================================== |
||
275 |
template <typename BV> |
||
276 |
260 |
void HierarchyTree<BV>::update(size_t leaf, int lookahead_level) { |
|
277 |
260 |
size_t root = removeLeaf(leaf); |
|
278 |
✓✗ | 260 |
if (root != NULL_NODE) { |
279 |
✗✓ | 260 |
if (lookahead_level > 0) { |
280 |
for (int i = 0; |
||
281 |
(i < lookahead_level) && (nodes[root].parent != NULL_NODE); ++i) |
||
282 |
root = nodes[root].parent; |
||
283 |
} else |
||
284 |
260 |
root = root_node; |
|
285 |
} |
||
286 |
260 |
insertLeaf(root, leaf); |
|
287 |
260 |
} |
|
288 |
|||
289 |
//============================================================================== |
||
290 |
template <typename BV> |
||
291 |
bool HierarchyTree<BV>::update(size_t leaf, const BV& bv) { |
||
292 |
if (nodes[leaf].bv.contain(bv)) return false; |
||
293 |
update_(leaf, bv); |
||
294 |
return true; |
||
295 |
} |
||
296 |
|||
297 |
//============================================================================== |
||
298 |
template <typename BV> |
||
299 |
bool HierarchyTree<BV>::update(size_t leaf, const BV& bv, const Vec3f& vel, |
||
300 |
FCL_REAL margin) { |
||
301 |
HPP_FCL_UNUSED_VARIABLE(bv); |
||
302 |
HPP_FCL_UNUSED_VARIABLE(vel); |
||
303 |
HPP_FCL_UNUSED_VARIABLE(margin); |
||
304 |
|||
305 |
if (nodes[leaf].bv.contain(bv)) return false; |
||
306 |
update_(leaf, bv); |
||
307 |
return true; |
||
308 |
} |
||
309 |
|||
310 |
//============================================================================== |
||
311 |
template <typename BV> |
||
312 |
bool HierarchyTree<BV>::update(size_t leaf, const BV& bv, const Vec3f& vel) { |
||
313 |
HPP_FCL_UNUSED_VARIABLE(vel); |
||
314 |
|||
315 |
if (nodes[leaf].bv.contain(bv)) return false; |
||
316 |
update_(leaf, bv); |
||
317 |
return true; |
||
318 |
} |
||
319 |
|||
320 |
//============================================================================== |
||
321 |
template <typename BV> |
||
322 |
26 |
size_t HierarchyTree<BV>::getMaxHeight() const { |
|
323 |
✗✓ | 26 |
if (root_node == NULL_NODE) return 0; |
324 |
|||
325 |
26 |
return getMaxHeight(root_node); |
|
326 |
} |
||
327 |
|||
328 |
//============================================================================== |
||
329 |
template <typename BV> |
||
330 |
size_t HierarchyTree<BV>::getMaxDepth() const { |
||
331 |
if (root_node == NULL_NODE) return 0; |
||
332 |
|||
333 |
size_t max_depth; |
||
334 |
getMaxDepth(root_node, 0, max_depth); |
||
335 |
return max_depth; |
||
336 |
} |
||
337 |
|||
338 |
//============================================================================== |
||
339 |
template <typename BV> |
||
340 |
void HierarchyTree<BV>::balanceBottomup() { |
||
341 |
if (root_node != NULL_NODE) { |
||
342 |
Node* leaves = new Node[n_leaves]; |
||
343 |
Node* leaves_ = leaves; |
||
344 |
extractLeaves(root_node, leaves_); |
||
345 |
root_node = NULL_NODE; |
||
346 |
std::copy(leaves, leaves + n_leaves, nodes); |
||
347 |
freelist = n_leaves; |
||
348 |
n_nodes = n_leaves; |
||
349 |
for (size_t i = n_leaves; i < n_nodes_alloc; ++i) nodes[i].next = i + 1; |
||
350 |
nodes[n_nodes_alloc - 1].next = NULL_NODE; |
||
351 |
|||
352 |
size_t* ids = new size_t[n_leaves]; |
||
353 |
for (size_t i = 0; i < n_leaves; ++i) ids[i] = i; |
||
354 |
|||
355 |
bottomup(ids, ids + n_leaves); |
||
356 |
root_node = *ids; |
||
357 |
|||
358 |
delete[] ids; |
||
359 |
} |
||
360 |
} |
||
361 |
|||
362 |
//============================================================================== |
||
363 |
template <typename BV> |
||
364 |
void HierarchyTree<BV>::balanceTopdown() { |
||
365 |
if (root_node != NULL_NODE) { |
||
366 |
Node* leaves = new Node[n_leaves]; |
||
367 |
Node* leaves_ = leaves; |
||
368 |
extractLeaves(root_node, leaves_); |
||
369 |
root_node = NULL_NODE; |
||
370 |
std::copy(leaves, leaves + n_leaves, nodes); |
||
371 |
freelist = n_leaves; |
||
372 |
n_nodes = n_leaves; |
||
373 |
for (size_t i = n_leaves; i < n_nodes_alloc; ++i) nodes[i].next = i + 1; |
||
374 |
nodes[n_nodes_alloc - 1].next = NULL_NODE; |
||
375 |
|||
376 |
size_t* ids = new size_t[n_leaves]; |
||
377 |
for (size_t i = 0; i < n_leaves; ++i) ids[i] = i; |
||
378 |
|||
379 |
root_node = topdown(ids, ids + n_leaves); |
||
380 |
delete[] ids; |
||
381 |
} |
||
382 |
} |
||
383 |
|||
384 |
//============================================================================== |
||
385 |
template <typename BV> |
||
386 |
26 |
void HierarchyTree<BV>::balanceIncremental(int iterations) { |
|
387 |
✗✓ | 26 |
if (iterations < 0) iterations = (int)n_leaves; |
388 |
✓✗✓✗ |
26 |
if ((root_node != NULL_NODE) && (iterations > 0)) { |
389 |
✓✓ | 286 |
for (int i = 0; i < iterations; ++i) { |
390 |
260 |
size_t node = root_node; |
|
391 |
260 |
unsigned int bit = 0; |
|
392 |
✓✓ | 1530 |
while (!nodes[node].isLeaf()) { |
393 |
1270 |
node = nodes[node].children[(opath >> bit) & 1]; |
|
394 |
1270 |
bit = (bit + 1) & (sizeof(unsigned int) * 8 - 1); |
|
395 |
} |
||
396 |
260 |
update(node); |
|
397 |
260 |
++opath; |
|
398 |
} |
||
399 |
} |
||
400 |
26 |
} |
|
401 |
|||
402 |
//============================================================================== |
||
403 |
template <typename BV> |
||
404 |
69 |
void HierarchyTree<BV>::refit() { |
|
405 |
✓✗ | 69 |
if (root_node != NULL_NODE) recurseRefit(root_node); |
406 |
69 |
} |
|
407 |
|||
408 |
//============================================================================== |
||
409 |
template <typename BV> |
||
410 |
void HierarchyTree<BV>::extractLeaves(size_t root, Node*& leaves) const { |
||
411 |
if (!nodes[root].isLeaf()) { |
||
412 |
extractLeaves(nodes[root].children[0], leaves); |
||
413 |
extractLeaves(nodes[root].children[1], leaves); |
||
414 |
} else { |
||
415 |
*leaves = nodes[root]; |
||
416 |
leaves++; |
||
417 |
} |
||
418 |
} |
||
419 |
|||
420 |
//============================================================================== |
||
421 |
template <typename BV> |
||
422 |
8018 |
size_t HierarchyTree<BV>::size() const { |
|
423 |
8018 |
return n_leaves; |
|
424 |
} |
||
425 |
|||
426 |
//============================================================================== |
||
427 |
template <typename BV> |
||
428 |
7754 |
size_t HierarchyTree<BV>::getRoot() const { |
|
429 |
7754 |
return root_node; |
|
430 |
} |
||
431 |
|||
432 |
//============================================================================== |
||
433 |
template <typename BV> |
||
434 |
10442 |
typename HierarchyTree<BV>::Node* HierarchyTree<BV>::getNodes() const { |
|
435 |
10442 |
return nodes; |
|
436 |
} |
||
437 |
|||
438 |
//============================================================================== |
||
439 |
template <typename BV> |
||
440 |
void HierarchyTree<BV>::print(size_t root, int depth) { |
||
441 |
for (int i = 0; i < depth; ++i) std::cout << " "; |
||
442 |
Node* n = nodes + root; |
||
443 |
std::cout << " (" << n->bv.min_[0] << ", " << n->bv.min_[1] << ", " |
||
444 |
<< n->bv.min_[2] << "; " << n->bv.max_[0] << ", " << n->bv.max_[1] |
||
445 |
<< ", " << n->bv.max_[2] << ")" << std::endl; |
||
446 |
if (n->isLeaf()) { |
||
447 |
} else { |
||
448 |
print(n->children[0], depth + 1); |
||
449 |
print(n->children[1], depth + 1); |
||
450 |
} |
||
451 |
} |
||
452 |
|||
453 |
//============================================================================== |
||
454 |
template <typename BV> |
||
455 |
5350 |
size_t HierarchyTree<BV>::getMaxHeight(size_t node) const { |
|
456 |
✓✓ | 5350 |
if (!nodes[node].isLeaf()) { |
457 |
✓✗ | 2662 |
size_t height1 = getMaxHeight(nodes[node].children[0]); |
458 |
✓✗ | 2662 |
size_t height2 = getMaxHeight(nodes[node].children[1]); |
459 |
2662 |
return std::max(height1, height2) + 1; |
|
460 |
} else |
||
461 |
2688 |
return 0; |
|
462 |
} |
||
463 |
|||
464 |
//============================================================================== |
||
465 |
template <typename BV> |
||
466 |
void HierarchyTree<BV>::getMaxDepth(size_t node, size_t depth, |
||
467 |
size_t& max_depth) const { |
||
468 |
if (!nodes[node].isLeaf()) { |
||
469 |
getMaxDepth(nodes[node].children[0], depth + 1, max_depth); |
||
470 |
getmaxDepth(nodes[node].children[1], depth + 1, max_depth); |
||
471 |
} else |
||
472 |
max_depth = std::max(max_depth, depth); |
||
473 |
} |
||
474 |
|||
475 |
//============================================================================== |
||
476 |
template <typename BV> |
||
477 |
4837 |
void HierarchyTree<BV>::bottomup(size_t* lbeg, size_t* lend) { |
|
478 |
4837 |
size_t* lcur_end = lend; |
|
479 |
✓✓ | 9674 |
while (lbeg < lcur_end - 1) { |
480 |
4837 |
size_t *min_it1 = nullptr, *min_it2 = nullptr; |
|
481 |
4837 |
FCL_REAL min_size = (std::numeric_limits<FCL_REAL>::max)(); |
|
482 |
✓✓ | 14511 |
for (size_t* it1 = lbeg; it1 < lcur_end; ++it1) { |
483 |
✓✓ | 14511 |
for (size_t* it2 = it1 + 1; it2 < lcur_end; ++it2) { |
484 |
✓✗ | 4837 |
FCL_REAL cur_size = (nodes[*it1].bv + nodes[*it2].bv).size(); |
485 |
✓✗ | 4837 |
if (cur_size < min_size) { |
486 |
4837 |
min_size = cur_size; |
|
487 |
4837 |
min_it1 = it1; |
|
488 |
4837 |
min_it2 = it2; |
|
489 |
} |
||
490 |
} |
||
491 |
} |
||
492 |
|||
493 |
9674 |
size_t p = |
|
494 |
4837 |
createNode(NULL_NODE, nodes[*min_it1].bv, nodes[*min_it2].bv, nullptr); |
|
495 |
4837 |
nodes[p].children[0] = *min_it1; |
|
496 |
4837 |
nodes[p].children[1] = *min_it2; |
|
497 |
4837 |
nodes[*min_it1].parent = p; |
|
498 |
4837 |
nodes[*min_it2].parent = p; |
|
499 |
4837 |
*min_it1 = p; |
|
500 |
4837 |
size_t tmp = *min_it2; |
|
501 |
4837 |
lcur_end--; |
|
502 |
4837 |
*min_it2 = *lcur_end; |
|
503 |
4837 |
*lcur_end = tmp; |
|
504 |
} |
||
505 |
4837 |
} |
|
506 |
|||
507 |
//============================================================================== |
||
508 |
template <typename BV> |
||
509 |
43 |
size_t HierarchyTree<BV>::topdown(size_t* lbeg, size_t* lend) { |
|
510 |
✓✗✗ | 43 |
switch (topdown_level) { |
511 |
43 |
case 0: |
|
512 |
43 |
return topdown_0(lbeg, lend); |
|
513 |
break; |
||
514 |
case 1: |
||
515 |
return topdown_1(lbeg, lend); |
||
516 |
break; |
||
517 |
default: |
||
518 |
return topdown_0(lbeg, lend); |
||
519 |
} |
||
520 |
} |
||
521 |
|||
522 |
//============================================================================== |
||
523 |
template <typename BV> |
||
524 |
15625 |
size_t HierarchyTree<BV>::topdown_0(size_t* lbeg, size_t* lend) { |
|
525 |
15625 |
long num_leaves = lend - lbeg; |
|
526 |
✓✓ | 15625 |
if (num_leaves > 1) { |
527 |
✓✓ | 12628 |
if (num_leaves > bu_threshold) { |
528 |
✓✗ | 7791 |
BV vol = nodes[*lbeg].bv; |
529 |
✓✓✓✗ |
118399 |
for (size_t* i = lbeg + 1; i < lend; ++i) vol += nodes[*i].bv; |
530 |
|||
531 |
7791 |
size_t best_axis = 0; |
|
532 |
✓✗✓✗ ✓✗ |
7791 |
FCL_REAL extent[3] = {vol.width(), vol.height(), vol.depth()}; |
533 |
✓✓ | 7791 |
if (extent[1] > extent[0]) best_axis = 1; |
534 |
✓✓ | 7791 |
if (extent[2] > extent[best_axis]) best_axis = 2; |
535 |
|||
536 |
7791 |
nodeBaseLess<BV> comp(nodes, best_axis); |
|
537 |
7791 |
size_t* lcenter = lbeg + num_leaves / 2; |
|
538 |
✓✗ | 7791 |
std::nth_element(lbeg, lcenter, lend, comp); |
539 |
|||
540 |
✓✗ | 7791 |
size_t node = createNode(NULL_NODE, vol, nullptr); |
541 |
✓✗ | 7791 |
nodes[node].children[0] = topdown_0(lbeg, lcenter); |
542 |
✓✗ | 7791 |
nodes[node].children[1] = topdown_0(lcenter, lend); |
543 |
7791 |
nodes[nodes[node].children[0]].parent = node; |
|
544 |
7791 |
nodes[nodes[node].children[1]].parent = node; |
|
545 |
7791 |
return node; |
|
546 |
} else { |
||
547 |
4837 |
bottomup(lbeg, lend); |
|
548 |
4837 |
return *lbeg; |
|
549 |
} |
||
550 |
} |
||
551 |
2997 |
return *lbeg; |
|
552 |
} |
||
553 |
|||
554 |
//============================================================================== |
||
555 |
template <typename BV> |
||
556 |
size_t HierarchyTree<BV>::topdown_1(size_t* lbeg, size_t* lend) { |
||
557 |
long num_leaves = lend - lbeg; |
||
558 |
if (num_leaves > 1) { |
||
559 |
if (num_leaves > bu_threshold) { |
||
560 |
Vec3f split_p = nodes[*lbeg].bv.center(); |
||
561 |
BV vol = nodes[*lbeg].bv; |
||
562 |
for (size_t* i = lbeg + 1; i < lend; ++i) { |
||
563 |
split_p += nodes[*i].bv.center(); |
||
564 |
vol += nodes[*i].bv; |
||
565 |
} |
||
566 |
split_p /= static_cast<FCL_REAL>(num_leaves); |
||
567 |
int best_axis = -1; |
||
568 |
int bestmidp = (int)num_leaves; |
||
569 |
int splitcount[3][2] = {{0, 0}, {0, 0}, {0, 0}}; |
||
570 |
for (size_t* i = lbeg; i < lend; ++i) { |
||
571 |
Vec3f x = nodes[*i].bv.center() - split_p; |
||
572 |
for (int j = 0; j < 3; ++j) ++splitcount[j][x[j] > 0 ? 1 : 0]; |
||
573 |
} |
||
574 |
|||
575 |
for (size_t i = 0; i < 3; ++i) { |
||
576 |
if ((splitcount[i][0] > 0) && (splitcount[i][1] > 0)) { |
||
577 |
int midp = std::abs(splitcount[i][0] - splitcount[i][1]); |
||
578 |
if (midp < bestmidp) { |
||
579 |
best_axis = (int)i; |
||
580 |
bestmidp = midp; |
||
581 |
} |
||
582 |
} |
||
583 |
} |
||
584 |
|||
585 |
if (best_axis < 0) best_axis = 0; |
||
586 |
|||
587 |
FCL_REAL split_value = split_p[best_axis]; |
||
588 |
size_t* lcenter = lbeg; |
||
589 |
for (size_t* i = lbeg; i < lend; ++i) { |
||
590 |
if (nodes[*i].bv.center()[best_axis] < split_value) { |
||
591 |
size_t temp = *i; |
||
592 |
*i = *lcenter; |
||
593 |
*lcenter = temp; |
||
594 |
++lcenter; |
||
595 |
} |
||
596 |
} |
||
597 |
|||
598 |
size_t node = createNode(NULL_NODE, vol, nullptr); |
||
599 |
nodes[node].children[0] = topdown_1(lbeg, lcenter); |
||
600 |
nodes[node].children[1] = topdown_1(lcenter, lend); |
||
601 |
nodes[nodes[node].children[0]].parent = node; |
||
602 |
nodes[nodes[node].children[1]].parent = node; |
||
603 |
return node; |
||
604 |
} else { |
||
605 |
bottomup(lbeg, lend); |
||
606 |
return *lbeg; |
||
607 |
} |
||
608 |
} |
||
609 |
return *lbeg; |
||
610 |
} |
||
611 |
|||
612 |
//============================================================================== |
||
613 |
template <typename BV> |
||
614 |
size_t HierarchyTree<BV>::mortonRecurse_0(size_t* lbeg, size_t* lend, |
||
615 |
const uint32_t& split, int bits) { |
||
616 |
long num_leaves = lend - lbeg; |
||
617 |
if (num_leaves > 1) { |
||
618 |
if (bits > 0) { |
||
619 |
const SortByMorton comp{nodes, split}; |
||
620 |
size_t* lcenter = std::lower_bound(lbeg, lend, NULL_NODE, comp); |
||
621 |
|||
622 |
if (lcenter == lbeg) { |
||
623 |
uint32_t split2 = split | (1 << (bits - 1)); |
||
624 |
return mortonRecurse_0(lbeg, lend, split2, bits - 1); |
||
625 |
} else if (lcenter == lend) { |
||
626 |
uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); |
||
627 |
return mortonRecurse_0(lbeg, lend, split1, bits - 1); |
||
628 |
} else { |
||
629 |
uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); |
||
630 |
uint32_t split2 = split | (1 << (bits - 1)); |
||
631 |
|||
632 |
size_t child1 = mortonRecurse_0(lbeg, lcenter, split1, bits - 1); |
||
633 |
size_t child2 = mortonRecurse_0(lcenter, lend, split2, bits - 1); |
||
634 |
size_t node = createNode(NULL_NODE, nullptr); |
||
635 |
nodes[node].children[0] = child1; |
||
636 |
nodes[node].children[1] = child2; |
||
637 |
nodes[child1].parent = node; |
||
638 |
nodes[child2].parent = node; |
||
639 |
return node; |
||
640 |
} |
||
641 |
} else { |
||
642 |
size_t node = topdown(lbeg, lend); |
||
643 |
return node; |
||
644 |
} |
||
645 |
} else |
||
646 |
return *lbeg; |
||
647 |
} |
||
648 |
|||
649 |
//============================================================================== |
||
650 |
template <typename BV> |
||
651 |
37671 |
size_t HierarchyTree<BV>::mortonRecurse_1(size_t* lbeg, size_t* lend, |
|
652 |
const uint32_t& split, int bits) { |
||
653 |
37671 |
long num_leaves = lend - lbeg; |
|
654 |
✓✓ | 37671 |
if (num_leaves > 1) { |
655 |
✓✗ | 25000 |
if (bits > 0) { |
656 |
25000 |
const SortByMorton comp{nodes, split}; |
|
657 |
✓✗ | 25000 |
size_t* lcenter = std::lower_bound(lbeg, lend, NULL_NODE, comp); |
658 |
|||
659 |
✓✓ | 25000 |
if (lcenter == lbeg) { |
660 |
6013 |
uint32_t split2 = split | (1 << (bits - 1)); |
|
661 |
✓✗ | 6013 |
return mortonRecurse_1(lbeg, lend, split2, bits - 1); |
662 |
✓✓ | 18987 |
} else if (lcenter == lend) { |
663 |
6359 |
uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); |
|
664 |
✓✗ | 6359 |
return mortonRecurse_1(lbeg, lend, split1, bits - 1); |
665 |
} else { |
||
666 |
12628 |
uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); |
|
667 |
12628 |
uint32_t split2 = split | (1 << (bits - 1)); |
|
668 |
|||
669 |
✓✗ | 12628 |
size_t child1 = mortonRecurse_1(lbeg, lcenter, split1, bits - 1); |
670 |
✓✗ | 12628 |
size_t child2 = mortonRecurse_1(lcenter, lend, split2, bits - 1); |
671 |
✓✗ | 12628 |
size_t node = createNode(NULL_NODE, nullptr); |
672 |
12628 |
nodes[node].children[0] = child1; |
|
673 |
12628 |
nodes[node].children[1] = child2; |
|
674 |
12628 |
nodes[child1].parent = node; |
|
675 |
12628 |
nodes[child2].parent = node; |
|
676 |
12628 |
return node; |
|
677 |
} |
||
678 |
} else { |
||
679 |
size_t child1 = mortonRecurse_1(lbeg, lbeg + num_leaves / 2, 0, bits - 1); |
||
680 |
size_t child2 = mortonRecurse_1(lbeg + num_leaves / 2, lend, 0, bits - 1); |
||
681 |
size_t node = createNode(NULL_NODE, nullptr); |
||
682 |
nodes[node].children[0] = child1; |
||
683 |
nodes[node].children[1] = child2; |
||
684 |
nodes[child1].parent = node; |
||
685 |
nodes[child2].parent = node; |
||
686 |
return node; |
||
687 |
} |
||
688 |
} else |
||
689 |
12671 |
return *lbeg; |
|
690 |
} |
||
691 |
|||
692 |
//============================================================================== |
||
693 |
template <typename BV> |
||
694 |
size_t HierarchyTree<BV>::mortonRecurse_2(size_t* lbeg, size_t* lend) { |
||
695 |
long num_leaves = lend - lbeg; |
||
696 |
if (num_leaves > 1) { |
||
697 |
size_t child1 = mortonRecurse_2(lbeg, lbeg + num_leaves / 2); |
||
698 |
size_t child2 = mortonRecurse_2(lbeg + num_leaves / 2, lend); |
||
699 |
size_t node = createNode(NULL_NODE, nullptr); |
||
700 |
nodes[node].children[0] = child1; |
||
701 |
nodes[node].children[1] = child2; |
||
702 |
nodes[child1].parent = node; |
||
703 |
nodes[child2].parent = node; |
||
704 |
return node; |
||
705 |
} else |
||
706 |
return *lbeg; |
||
707 |
} |
||
708 |
|||
709 |
//============================================================================== |
||
710 |
template <typename BV> |
||
711 |
260 |
void HierarchyTree<BV>::insertLeaf(size_t root, size_t leaf) { |
|
712 |
✗✓ | 260 |
if (root_node == NULL_NODE) { |
713 |
root_node = leaf; |
||
714 |
nodes[leaf].parent = NULL_NODE; |
||
715 |
} else { |
||
716 |
✓✗ | 260 |
if (!nodes[root].isLeaf()) { |
717 |
908 |
do { |
|
718 |
2336 |
root = nodes[root].children[select(leaf, nodes[root].children[0], |
|
719 |
1168 |
nodes[root].children[1], nodes)]; |
|
720 |
✓✓ | 1168 |
} while (!nodes[root].isLeaf()); |
721 |
} |
||
722 |
|||
723 |
260 |
size_t prev = nodes[root].parent; |
|
724 |
260 |
size_t node = createNode(prev, nodes[leaf].bv, nodes[root].bv, nullptr); |
|
725 |
✓✗ | 260 |
if (prev != NULL_NODE) { |
726 |
260 |
nodes[prev].children[indexOf(root)] = node; |
|
727 |
260 |
nodes[node].children[0] = root; |
|
728 |
260 |
nodes[root].parent = node; |
|
729 |
260 |
nodes[node].children[1] = leaf; |
|
730 |
260 |
nodes[leaf].parent = node; |
|
731 |
511 |
do { |
|
732 |
✓✓ | 771 |
if (!nodes[prev].bv.contain(nodes[node].bv)) |
733 |
✓✗ | 604 |
nodes[prev].bv = nodes[nodes[prev].children[0]].bv + |
734 |
604 |
nodes[nodes[prev].children[1]].bv; |
|
735 |
else |
||
736 |
167 |
break; |
|
737 |
604 |
node = prev; |
|
738 |
✓✓ | 604 |
} while (NULL_NODE != (prev = nodes[node].parent)); |
739 |
} else { |
||
740 |
nodes[node].children[0] = root; |
||
741 |
nodes[root].parent = node; |
||
742 |
nodes[node].children[1] = leaf; |
||
743 |
nodes[leaf].parent = node; |
||
744 |
root_node = node; |
||
745 |
} |
||
746 |
} |
||
747 |
260 |
} |
|
748 |
|||
749 |
//============================================================================== |
||
750 |
template <typename BV> |
||
751 |
260 |
size_t HierarchyTree<BV>::removeLeaf(size_t leaf) { |
|
752 |
✗✓ | 260 |
if (leaf == root_node) { |
753 |
root_node = NULL_NODE; |
||
754 |
return NULL_NODE; |
||
755 |
} else { |
||
756 |
260 |
size_t parent = nodes[leaf].parent; |
|
757 |
260 |
size_t prev = nodes[parent].parent; |
|
758 |
260 |
size_t sibling = nodes[parent].children[1 - indexOf(leaf)]; |
|
759 |
|||
760 |
✓✗ | 260 |
if (prev != NULL_NODE) { |
761 |
260 |
nodes[prev].children[indexOf(parent)] = sibling; |
|
762 |
260 |
nodes[sibling].parent = prev; |
|
763 |
260 |
deleteNode(parent); |
|
764 |
✓✓ | 736 |
while (prev != NULL_NODE) { |
765 |
643 |
BV new_bv = nodes[nodes[prev].children[0]].bv + |
|
766 |
✓✗ | 643 |
nodes[nodes[prev].children[1]].bv; |
767 |
✓✗✓✓ |
643 |
if (!(new_bv == nodes[prev].bv)) { |
768 |
✓✗ | 476 |
nodes[prev].bv = new_bv; |
769 |
476 |
prev = nodes[prev].parent; |
|
770 |
} else |
||
771 |
167 |
break; |
|
772 |
} |
||
773 |
|||
774 |
✓✓ | 260 |
return (prev != NULL_NODE) ? prev : root_node; |
775 |
} else { |
||
776 |
root_node = sibling; |
||
777 |
nodes[sibling].parent = NULL_NODE; |
||
778 |
deleteNode(parent); |
||
779 |
return root_node; |
||
780 |
} |
||
781 |
} |
||
782 |
} |
||
783 |
|||
784 |
//============================================================================== |
||
785 |
template <typename BV> |
||
786 |
void HierarchyTree<BV>::update_(size_t leaf, const BV& bv) { |
||
787 |
size_t root = removeLeaf(leaf); |
||
788 |
if (root != NULL_NODE) { |
||
789 |
if (max_lookahead_level >= 0) { |
||
790 |
for (int i = 0; |
||
791 |
(i < max_lookahead_level) && (nodes[root].parent != NULL_NODE); ++i) |
||
792 |
root = nodes[root].parent; |
||
793 |
} |
||
794 |
|||
795 |
nodes[leaf].bv = bv; |
||
796 |
insertLeaf(root, leaf); |
||
797 |
} |
||
798 |
} |
||
799 |
|||
800 |
//============================================================================== |
||
801 |
template <typename BV> |
||
802 |
780 |
size_t HierarchyTree<BV>::indexOf(size_t node) { |
|
803 |
780 |
return (nodes[nodes[node].parent].children[1] == node); |
|
804 |
} |
||
805 |
|||
806 |
//============================================================================== |
||
807 |
template <typename BV> |
||
808 |
25516 |
size_t HierarchyTree<BV>::allocateNode() { |
|
809 |
✗✓ | 25516 |
if (freelist == NULL_NODE) { |
810 |
Node* old_nodes = nodes; |
||
811 |
n_nodes_alloc *= 2; |
||
812 |
nodes = new Node[n_nodes_alloc]; |
||
813 |
std::copy(old_nodes, old_nodes + n_nodes, nodes); |
||
814 |
delete[] old_nodes; |
||
815 |
|||
816 |
for (size_t i = n_nodes; i < n_nodes_alloc - 1; ++i) nodes[i].next = i + 1; |
||
817 |
nodes[n_nodes_alloc - 1].next = NULL_NODE; |
||
818 |
freelist = n_nodes; |
||
819 |
} |
||
820 |
|||
821 |
25516 |
size_t node_id = freelist; |
|
822 |
25516 |
freelist = nodes[node_id].next; |
|
823 |
25516 |
nodes[node_id].parent = NULL_NODE; |
|
824 |
25516 |
nodes[node_id].children[0] = NULL_NODE; |
|
825 |
25516 |
nodes[node_id].children[1] = NULL_NODE; |
|
826 |
25516 |
++n_nodes; |
|
827 |
25516 |
return node_id; |
|
828 |
} |
||
829 |
|||
830 |
//============================================================================== |
||
831 |
template <typename BV> |
||
832 |
5097 |
size_t HierarchyTree<BV>::createNode(size_t parent, const BV& bv1, |
|
833 |
const BV& bv2, void* data) { |
||
834 |
5097 |
size_t node = allocateNode(); |
|
835 |
5097 |
nodes[node].parent = parent; |
|
836 |
5097 |
nodes[node].data = data; |
|
837 |
✓✗ | 5097 |
nodes[node].bv = bv1 + bv2; |
838 |
5097 |
return node; |
|
839 |
} |
||
840 |
|||
841 |
//============================================================================== |
||
842 |
template <typename BV> |
||
843 |
7791 |
size_t HierarchyTree<BV>::createNode(size_t parent, const BV& bv, void* data) { |
|
844 |
7791 |
size_t node = allocateNode(); |
|
845 |
7791 |
nodes[node].parent = parent; |
|
846 |
7791 |
nodes[node].data = data; |
|
847 |
7791 |
nodes[node].bv = bv; |
|
848 |
7791 |
return node; |
|
849 |
} |
||
850 |
|||
851 |
//============================================================================== |
||
852 |
template <typename BV> |
||
853 |
12628 |
size_t HierarchyTree<BV>::createNode(size_t parent, void* data) { |
|
854 |
12628 |
size_t node = allocateNode(); |
|
855 |
12628 |
nodes[node].parent = parent; |
|
856 |
12628 |
nodes[node].data = data; |
|
857 |
12628 |
return node; |
|
858 |
} |
||
859 |
|||
860 |
//============================================================================== |
||
861 |
template <typename BV> |
||
862 |
260 |
void HierarchyTree<BV>::deleteNode(size_t node) { |
|
863 |
260 |
nodes[node].next = freelist; |
|
864 |
260 |
freelist = node; |
|
865 |
260 |
--n_nodes; |
|
866 |
260 |
} |
|
867 |
|||
868 |
//============================================================================== |
||
869 |
template <typename BV> |
||
870 |
30649 |
void HierarchyTree<BV>::recurseRefit(size_t node) { |
|
871 |
✓✓ | 30649 |
if (!nodes[node].isLeaf()) { |
872 |
15290 |
recurseRefit(nodes[node].children[0]); |
|
873 |
15290 |
recurseRefit(nodes[node].children[1]); |
|
874 |
✓✗ | 15290 |
nodes[node].bv = |
875 |
15290 |
nodes[nodes[node].children[0]].bv + nodes[nodes[node].children[1]].bv; |
|
876 |
} else |
||
877 |
15359 |
return; |
|
878 |
} |
||
879 |
|||
880 |
//============================================================================== |
||
881 |
template <typename BV> |
||
882 |
void HierarchyTree<BV>::fetchLeaves(size_t root, Node*& leaves, int depth) { |
||
883 |
if ((!nodes[root].isLeaf()) && depth) { |
||
884 |
fetchLeaves(nodes[root].children[0], leaves, depth - 1); |
||
885 |
fetchLeaves(nodes[root].children[1], leaves, depth - 1); |
||
886 |
deleteNode(root); |
||
887 |
} else { |
||
888 |
*leaves = nodes[root]; |
||
889 |
leaves++; |
||
890 |
} |
||
891 |
} |
||
892 |
|||
893 |
//============================================================================== |
||
894 |
template <typename BV> |
||
895 |
7791 |
nodeBaseLess<BV>::nodeBaseLess(const NodeBase<BV>* nodes_, size_t d_) |
|
896 |
7791 |
: nodes(nodes_), d(d_) { |
|
897 |
// Do nothing |
||
898 |
7791 |
} |
|
899 |
|||
900 |
//============================================================================== |
||
901 |
template <typename BV> |
||
902 |
329602 |
bool nodeBaseLess<BV>::operator()(size_t i, size_t j) const { |
|
903 |
✓✗✓✗ ✓✗✓✓ |
329602 |
if (nodes[i].bv.center()[(int)d] < nodes[j].bv.center()[(int)d]) return true; |
904 |
|||
905 |
142487 |
return false; |
|
906 |
} |
||
907 |
|||
908 |
//============================================================================== |
||
909 |
template <typename S, typename BV> |
||
910 |
struct SelectImpl { |
||
911 |
static bool run(size_t query, size_t node1, size_t node2, |
||
912 |
NodeBase<BV>* nodes) { |
||
913 |
HPP_FCL_UNUSED_VARIABLE(query); |
||
914 |
HPP_FCL_UNUSED_VARIABLE(node1); |
||
915 |
HPP_FCL_UNUSED_VARIABLE(node2); |
||
916 |
HPP_FCL_UNUSED_VARIABLE(nodes); |
||
917 |
|||
918 |
return 0; |
||
919 |
} |
||
920 |
|||
921 |
static bool run(const BV& query, size_t node1, size_t node2, |
||
922 |
NodeBase<BV>* nodes) { |
||
923 |
HPP_FCL_UNUSED_VARIABLE(query); |
||
924 |
HPP_FCL_UNUSED_VARIABLE(node1); |
||
925 |
HPP_FCL_UNUSED_VARIABLE(node2); |
||
926 |
HPP_FCL_UNUSED_VARIABLE(nodes); |
||
927 |
|||
928 |
return 0; |
||
929 |
} |
||
930 |
}; |
||
931 |
|||
932 |
//============================================================================== |
||
933 |
template <typename BV> |
||
934 |
1168 |
size_t select(size_t query, size_t node1, size_t node2, NodeBase<BV>* nodes) { |
|
935 |
1168 |
return SelectImpl<FCL_REAL, BV>::run(query, node1, node2, nodes); |
|
936 |
} |
||
937 |
|||
938 |
//============================================================================== |
||
939 |
template <typename BV> |
||
940 |
33246 |
size_t select(const BV& query, size_t node1, size_t node2, |
|
941 |
NodeBase<BV>* nodes) { |
||
942 |
33246 |
return SelectImpl<FCL_REAL, BV>::run(query, node1, node2, nodes); |
|
943 |
} |
||
944 |
|||
945 |
//============================================================================== |
||
946 |
template <typename S> |
||
947 |
struct SelectImpl<S, AABB> { |
||
948 |
1168 |
static bool run(size_t query, size_t node1, size_t node2, |
|
949 |
NodeBase<AABB>* nodes) { |
||
950 |
1168 |
const AABB& bv = nodes[query].bv; |
|
951 |
1168 |
const AABB& bv1 = nodes[node1].bv; |
|
952 |
1168 |
const AABB& bv2 = nodes[node2].bv; |
|
953 |
✓✗✓✗ |
1168 |
Vec3f v = bv.min_ + bv.max_; |
954 |
✓✗✓✗ ✓✗ |
1168 |
Vec3f v1 = v - (bv1.min_ + bv1.max_); |
955 |
✓✗✓✗ ✓✗ |
1168 |
Vec3f v2 = v - (bv2.min_ + bv2.max_); |
956 |
✓✗✓✗ ✓✗ |
1168 |
FCL_REAL d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]); |
957 |
✓✗✓✗ ✓✗ |
1168 |
FCL_REAL d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]); |
958 |
1168 |
return (d1 < d2) ? 0 : 1; |
|
959 |
} |
||
960 |
|||
961 |
33246 |
static bool run(const AABB& query, size_t node1, size_t node2, |
|
962 |
NodeBase<AABB>* nodes) { |
||
963 |
33246 |
const AABB& bv = query; |
|
964 |
33246 |
const AABB& bv1 = nodes[node1].bv; |
|
965 |
33246 |
const AABB& bv2 = nodes[node2].bv; |
|
966 |
✓✗✓✗ |
33246 |
Vec3f v = bv.min_ + bv.max_; |
967 |
✓✗✓✗ ✓✗ |
33246 |
Vec3f v1 = v - (bv1.min_ + bv1.max_); |
968 |
✓✗✓✗ ✓✗ |
33246 |
Vec3f v2 = v - (bv2.min_ + bv2.max_); |
969 |
✓✗✓✗ ✓✗ |
33246 |
FCL_REAL d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]); |
970 |
✓✗✓✗ ✓✗ |
33246 |
FCL_REAL d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]); |
971 |
33246 |
return (d1 < d2) ? 0 : 1; |
|
972 |
} |
||
973 |
}; |
||
974 |
|||
975 |
} // namespace implementation_array |
||
976 |
} // namespace detail |
||
977 |
} // namespace fcl |
||
978 |
} // namespace hpp |
||
979 |
|||
980 |
#endif |
Generated by: GCOVR (Version 4.2) |