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