1/**************************************************************************/
2/* dynamic_bvh.cpp */
3/**************************************************************************/
4/* This file is part of: */
5/* GODOT ENGINE */
6/* https://godotengine.org */
7/**************************************************************************/
8/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
9/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
10/* */
11/* Permission is hereby granted, free of charge, to any person obtaining */
12/* a copy of this software and associated documentation files (the */
13/* "Software"), to deal in the Software without restriction, including */
14/* without limitation the rights to use, copy, modify, merge, publish, */
15/* distribute, sublicense, and/or sell copies of the Software, and to */
16/* permit persons to whom the Software is furnished to do so, subject to */
17/* the following conditions: */
18/* */
19/* The above copyright notice and this permission notice shall be */
20/* included in all copies or substantial portions of the Software. */
21/* */
22/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
23/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
24/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
25/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
26/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
27/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
28/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
29/**************************************************************************/
30
31#include "dynamic_bvh.h"
32
33void DynamicBVH::_delete_node(Node *p_node) {
34 node_allocator.free(p_node);
35}
36
37void DynamicBVH::_recurse_delete_node(Node *p_node) {
38 if (!p_node->is_leaf()) {
39 _recurse_delete_node(p_node->children[0]);
40 _recurse_delete_node(p_node->children[1]);
41 }
42 if (p_node == bvh_root) {
43 bvh_root = nullptr;
44 }
45 _delete_node(p_node);
46}
47
48DynamicBVH::Node *DynamicBVH::_create_node(Node *p_parent, void *p_data) {
49 Node *node = node_allocator.alloc();
50 node->parent = p_parent;
51 node->data = p_data;
52 return (node);
53}
54
55DynamicBVH::Node *DynamicBVH::_create_node_with_volume(Node *p_parent, const Volume &p_volume, void *p_data) {
56 Node *node = _create_node(p_parent, p_data);
57 node->volume = p_volume;
58 return node;
59}
60
61void DynamicBVH::_insert_leaf(Node *p_root, Node *p_leaf) {
62 if (!bvh_root) {
63 bvh_root = p_leaf;
64 p_leaf->parent = nullptr;
65 } else {
66 if (!p_root->is_leaf()) {
67 do {
68 p_root = p_root->children[p_leaf->volume.select_by_proximity(
69 p_root->children[0]->volume,
70 p_root->children[1]->volume)];
71 } while (!p_root->is_leaf());
72 }
73 Node *prev = p_root->parent;
74 Node *node = _create_node_with_volume(prev, p_leaf->volume.merge(p_root->volume), nullptr);
75 if (prev) {
76 prev->children[p_root->get_index_in_parent()] = node;
77 node->children[0] = p_root;
78 p_root->parent = node;
79 node->children[1] = p_leaf;
80 p_leaf->parent = node;
81 do {
82 if (!prev->volume.contains(node->volume)) {
83 prev->volume = prev->children[0]->volume.merge(prev->children[1]->volume);
84 } else {
85 break;
86 }
87 node = prev;
88 } while (nullptr != (prev = node->parent));
89 } else {
90 node->children[0] = p_root;
91 p_root->parent = node;
92 node->children[1] = p_leaf;
93 p_leaf->parent = node;
94 bvh_root = node;
95 }
96 }
97}
98
99DynamicBVH::Node *DynamicBVH::_remove_leaf(Node *leaf) {
100 if (leaf == bvh_root) {
101 bvh_root = nullptr;
102 return (nullptr);
103 } else {
104 Node *parent = leaf->parent;
105 Node *prev = parent->parent;
106 Node *sibling = parent->children[1 - leaf->get_index_in_parent()];
107 if (prev) {
108 prev->children[parent->get_index_in_parent()] = sibling;
109 sibling->parent = prev;
110 _delete_node(parent);
111 while (prev) {
112 const Volume pb = prev->volume;
113 prev->volume = prev->children[0]->volume.merge(prev->children[1]->volume);
114 if (pb.is_not_equal_to(prev->volume)) {
115 prev = prev->parent;
116 } else {
117 break;
118 }
119 }
120 return (prev ? prev : bvh_root);
121 } else {
122 bvh_root = sibling;
123 sibling->parent = nullptr;
124 _delete_node(parent);
125 return (bvh_root);
126 }
127 }
128}
129
130void DynamicBVH::_fetch_leaves(Node *p_root, LocalVector<Node *> &r_leaves, int p_depth) {
131 if (p_root->is_internal() && p_depth) {
132 _fetch_leaves(p_root->children[0], r_leaves, p_depth - 1);
133 _fetch_leaves(p_root->children[1], r_leaves, p_depth - 1);
134 _delete_node(p_root);
135 } else {
136 r_leaves.push_back(p_root);
137 }
138}
139
140// Partitions leaves such that leaves[0, n) are on the
141// left of axis, and leaves[n, count) are on the right
142// of axis. returns N.
143int DynamicBVH::_split(Node **leaves, int p_count, const Vector3 &p_org, const Vector3 &p_axis) {
144 int begin = 0;
145 int end = p_count;
146 for (;;) {
147 while (begin != end && leaves[begin]->is_left_of_axis(p_org, p_axis)) {
148 ++begin;
149 }
150
151 if (begin == end) {
152 break;
153 }
154
155 while (begin != end && !leaves[end - 1]->is_left_of_axis(p_org, p_axis)) {
156 --end;
157 }
158
159 if (begin == end) {
160 break;
161 }
162
163 // swap out of place nodes
164 --end;
165 Node *temp = leaves[begin];
166 leaves[begin] = leaves[end];
167 leaves[end] = temp;
168 ++begin;
169 }
170
171 return begin;
172}
173
174DynamicBVH::Volume DynamicBVH::_bounds(Node **leaves, int p_count) {
175 Volume volume = leaves[0]->volume;
176 for (int i = 1, ni = p_count; i < ni; ++i) {
177 volume = volume.merge(leaves[i]->volume);
178 }
179 return (volume);
180}
181
182void DynamicBVH::_bottom_up(Node **leaves, int p_count) {
183 while (p_count > 1) {
184 real_t minsize = INFINITY;
185 int minidx[2] = { -1, -1 };
186 for (int i = 0; i < p_count; ++i) {
187 for (int j = i + 1; j < p_count; ++j) {
188 const real_t sz = leaves[i]->volume.merge(leaves[j]->volume).get_size();
189 if (sz < minsize) {
190 minsize = sz;
191 minidx[0] = i;
192 minidx[1] = j;
193 }
194 }
195 }
196 Node *n[] = { leaves[minidx[0]], leaves[minidx[1]] };
197 Node *p = _create_node_with_volume(nullptr, n[0]->volume.merge(n[1]->volume), nullptr);
198 p->children[0] = n[0];
199 p->children[1] = n[1];
200 n[0]->parent = p;
201 n[1]->parent = p;
202 leaves[minidx[0]] = p;
203 leaves[minidx[1]] = leaves[p_count - 1];
204 --p_count;
205 }
206}
207
208DynamicBVH::Node *DynamicBVH::_top_down(Node **leaves, int p_count, int p_bu_threshold) {
209 static const Vector3 axis[] = { Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1) };
210
211 ERR_FAIL_COND_V(p_bu_threshold <= 1, nullptr);
212 if (p_count > 1) {
213 if (p_count > p_bu_threshold) {
214 const Volume vol = _bounds(leaves, p_count);
215 const Vector3 org = vol.get_center();
216 int partition;
217 int bestaxis = -1;
218 int bestmidp = p_count;
219 int splitcount[3][2] = { { 0, 0 }, { 0, 0 }, { 0, 0 } };
220 int i;
221 for (i = 0; i < p_count; ++i) {
222 const Vector3 x = leaves[i]->volume.get_center() - org;
223 for (int j = 0; j < 3; ++j) {
224 ++splitcount[j][x.dot(axis[j]) > 0 ? 1 : 0];
225 }
226 }
227 for (i = 0; i < 3; ++i) {
228 if ((splitcount[i][0] > 0) && (splitcount[i][1] > 0)) {
229 const int midp = (int)Math::abs(real_t(splitcount[i][0] - splitcount[i][1]));
230 if (midp < bestmidp) {
231 bestaxis = i;
232 bestmidp = midp;
233 }
234 }
235 }
236 if (bestaxis >= 0) {
237 partition = _split(leaves, p_count, org, axis[bestaxis]);
238 ERR_FAIL_COND_V(partition == 0 || partition == p_count, nullptr);
239 } else {
240 partition = p_count / 2 + 1;
241 }
242
243 Node *node = _create_node_with_volume(nullptr, vol, nullptr);
244 node->children[0] = _top_down(&leaves[0], partition, p_bu_threshold);
245 node->children[1] = _top_down(&leaves[partition], p_count - partition, p_bu_threshold);
246 node->children[0]->parent = node;
247 node->children[1]->parent = node;
248 return (node);
249 } else {
250 _bottom_up(leaves, p_count);
251 return (leaves[0]);
252 }
253 }
254 return (leaves[0]);
255}
256
257DynamicBVH::Node *DynamicBVH::_node_sort(Node *n, Node *&r) {
258 Node *p = n->parent;
259 ERR_FAIL_COND_V(!n->is_internal(), nullptr);
260 if (p > n) {
261 const int i = n->get_index_in_parent();
262 const int j = 1 - i;
263 Node *s = p->children[j];
264 Node *q = p->parent;
265 ERR_FAIL_COND_V(n != p->children[i], nullptr);
266 if (q) {
267 q->children[p->get_index_in_parent()] = n;
268 } else {
269 r = n;
270 }
271 s->parent = n;
272 p->parent = n;
273 n->parent = q;
274 p->children[0] = n->children[0];
275 p->children[1] = n->children[1];
276 n->children[0]->parent = p;
277 n->children[1]->parent = p;
278 n->children[i] = p;
279 n->children[j] = s;
280 SWAP(p->volume, n->volume);
281 return (p);
282 }
283 return (n);
284}
285
286void DynamicBVH::clear() {
287 if (bvh_root) {
288 _recurse_delete_node(bvh_root);
289 }
290 lkhd = -1;
291 opath = 0;
292}
293
294void DynamicBVH::optimize_bottom_up() {
295 if (bvh_root) {
296 LocalVector<Node *> leaves;
297 _fetch_leaves(bvh_root, leaves);
298 _bottom_up(&leaves[0], leaves.size());
299 bvh_root = leaves[0];
300 }
301}
302
303void DynamicBVH::optimize_top_down(int bu_threshold) {
304 if (bvh_root) {
305 LocalVector<Node *> leaves;
306 _fetch_leaves(bvh_root, leaves);
307 bvh_root = _top_down(&leaves[0], leaves.size(), bu_threshold);
308 }
309}
310
311void DynamicBVH::optimize_incremental(int passes) {
312 if (passes < 0) {
313 passes = total_leaves;
314 }
315 if (passes > 0) {
316 do {
317 if (!bvh_root) {
318 break;
319 }
320 Node *node = bvh_root;
321 unsigned bit = 0;
322 while (node->is_internal()) {
323 node = _node_sort(node, bvh_root)->children[(opath >> bit) & 1];
324 bit = (bit + 1) & (sizeof(unsigned) * 8 - 1);
325 }
326 _update(node);
327 ++opath;
328 } while (--passes);
329 }
330}
331
332DynamicBVH::ID DynamicBVH::insert(const AABB &p_box, void *p_userdata) {
333 Volume volume;
334 volume.min = p_box.position;
335 volume.max = p_box.position + p_box.size;
336
337 Node *leaf = _create_node_with_volume(nullptr, volume, p_userdata);
338 _insert_leaf(bvh_root, leaf);
339 ++total_leaves;
340
341 ID id;
342 id.node = leaf;
343
344 return id;
345}
346
347void DynamicBVH::_update(Node *leaf, int lookahead) {
348 Node *root = _remove_leaf(leaf);
349 if (root) {
350 if (lookahead >= 0) {
351 for (int i = 0; (i < lookahead) && root->parent; ++i) {
352 root = root->parent;
353 }
354 } else {
355 root = bvh_root;
356 }
357 }
358 _insert_leaf(root, leaf);
359}
360
361bool DynamicBVH::update(const ID &p_id, const AABB &p_box) {
362 ERR_FAIL_COND_V(!p_id.is_valid(), false);
363 Node *leaf = p_id.node;
364
365 Volume volume;
366 volume.min = p_box.position;
367 volume.max = p_box.position + p_box.size;
368
369 if (leaf->volume.min.is_equal_approx(volume.min) && leaf->volume.max.is_equal_approx(volume.max)) {
370 // noop
371 return false;
372 }
373
374 Node *base = _remove_leaf(leaf);
375 if (base) {
376 if (lkhd >= 0) {
377 for (int i = 0; (i < lkhd) && base->parent; ++i) {
378 base = base->parent;
379 }
380 } else {
381 base = bvh_root;
382 }
383 }
384 leaf->volume = volume;
385 _insert_leaf(base, leaf);
386 return true;
387}
388
389void DynamicBVH::remove(const ID &p_id) {
390 ERR_FAIL_COND(!p_id.is_valid());
391 Node *leaf = p_id.node;
392 _remove_leaf(leaf);
393 _delete_node(leaf);
394 --total_leaves;
395}
396
397void DynamicBVH::_extract_leaves(Node *p_node, List<ID> *r_elements) {
398 if (p_node->is_internal()) {
399 _extract_leaves(p_node->children[0], r_elements);
400 _extract_leaves(p_node->children[1], r_elements);
401 } else {
402 ID id;
403 id.node = p_node;
404 r_elements->push_back(id);
405 }
406}
407
408void DynamicBVH::set_index(uint32_t p_index) {
409 ERR_FAIL_COND(bvh_root != nullptr);
410 index = p_index;
411}
412
413uint32_t DynamicBVH::get_index() const {
414 return index;
415}
416
417void DynamicBVH::get_elements(List<ID> *r_elements) {
418 if (bvh_root) {
419 _extract_leaves(bvh_root, r_elements);
420 }
421}
422
423int DynamicBVH::get_leaf_count() const {
424 return total_leaves;
425}
426int DynamicBVH::get_max_depth() const {
427 if (bvh_root) {
428 int depth = 1;
429 int max_depth = 0;
430 bvh_root->get_max_depth(depth, max_depth);
431 return max_depth;
432 } else {
433 return 0;
434 }
435}
436
437DynamicBVH::~DynamicBVH() {
438 clear();
439}
440