/**************************************************************************/ /* dynamic_bvh.cpp */ /**************************************************************************/ /* This file is part of: */ /* GODOT ENGINE */ /* https://godotengine.org */ /**************************************************************************/ /* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ /* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ /* "Software"), to deal in the Software without restriction, including */ /* without limitation the rights to use, copy, modify, merge, publish, */ /* distribute, sublicense, and/or sell copies of the Software, and to */ /* permit persons to whom the Software is furnished to do so, subject to */ /* the following conditions: */ /* */ /* The above copyright notice and this permission notice shall be */ /* included in all copies or substantial portions of the Software. */ /* */ /* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ /* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ /* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ /* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ /* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /**************************************************************************/ #include "dynamic_bvh.h" void DynamicBVH::_delete_node(Node *p_node) { node_allocator.free(p_node); } void DynamicBVH::_recurse_delete_node(Node *p_node) { if (!p_node->is_leaf()) { _recurse_delete_node(p_node->children[0]); _recurse_delete_node(p_node->children[1]); } if (p_node == bvh_root) { bvh_root = nullptr; } _delete_node(p_node); } DynamicBVH::Node *DynamicBVH::_create_node(Node *p_parent, void *p_data) { Node *node = node_allocator.alloc(); node->parent = p_parent; node->data = p_data; return (node); } DynamicBVH::Node *DynamicBVH::_create_node_with_volume(Node *p_parent, const Volume &p_volume, void *p_data) { Node *node = _create_node(p_parent, p_data); node->volume = p_volume; return node; } void DynamicBVH::_insert_leaf(Node *p_root, Node *p_leaf) { if (!bvh_root) { bvh_root = p_leaf; p_leaf->parent = nullptr; } else { if (!p_root->is_leaf()) { do { p_root = p_root->children[p_leaf->volume.select_by_proximity( p_root->children[0]->volume, p_root->children[1]->volume)]; } while (!p_root->is_leaf()); } Node *prev = p_root->parent; Node *node = _create_node_with_volume(prev, p_leaf->volume.merge(p_root->volume), nullptr); if (prev) { prev->children[p_root->get_index_in_parent()] = node; node->children[0] = p_root; p_root->parent = node; node->children[1] = p_leaf; p_leaf->parent = node; do { if (!prev->volume.contains(node->volume)) { prev->volume = prev->children[0]->volume.merge(prev->children[1]->volume); } else { break; } node = prev; } while (nullptr != (prev = node->parent)); } else { node->children[0] = p_root; p_root->parent = node; node->children[1] = p_leaf; p_leaf->parent = node; bvh_root = node; } } } DynamicBVH::Node *DynamicBVH::_remove_leaf(Node *leaf) { if (leaf == bvh_root) { bvh_root = nullptr; return (nullptr); } else { Node *parent = leaf->parent; Node *prev = parent->parent; Node *sibling = parent->children[1 - leaf->get_index_in_parent()]; if (prev) { prev->children[parent->get_index_in_parent()] = sibling; sibling->parent = prev; _delete_node(parent); while (prev) { const Volume pb = prev->volume; prev->volume = prev->children[0]->volume.merge(prev->children[1]->volume); if (pb.is_not_equal_to(prev->volume)) { prev = prev->parent; } else { break; } } return (prev ? prev : bvh_root); } else { bvh_root = sibling; sibling->parent = nullptr; _delete_node(parent); return (bvh_root); } } } void DynamicBVH::_fetch_leaves(Node *p_root, LocalVector<Node *> &r_leaves, int p_depth) { if (p_root->is_internal() && p_depth) { _fetch_leaves(p_root->children[0], r_leaves, p_depth - 1); _fetch_leaves(p_root->children[1], r_leaves, p_depth - 1); _delete_node(p_root); } else { r_leaves.push_back(p_root); } } // Partitions leaves such that leaves[0, n) are on the // left of axis, and leaves[n, count) are on the right // of axis. returns N. int DynamicBVH::_split(Node **leaves, int p_count, const Vector3 &p_org, const Vector3 &p_axis) { int begin = 0; int end = p_count; for (;;) { while (begin != end && leaves[begin]->is_left_of_axis(p_org, p_axis)) { ++begin; } if (begin == end) { break; } while (begin != end && !leaves[end - 1]->is_left_of_axis(p_org, p_axis)) { --end; } if (begin == end) { break; } // swap out of place nodes --end; Node *temp = leaves[begin]; leaves[begin] = leaves[end]; leaves[end] = temp; ++begin; } return begin; } DynamicBVH::Volume DynamicBVH::_bounds(Node **leaves, int p_count) { Volume volume = leaves[0]->volume; for (int i = 1, ni = p_count; i < ni; ++i) { volume = volume.merge(leaves[i]->volume); } return (volume); } void DynamicBVH::_bottom_up(Node **leaves, int p_count) { while (p_count > 1) { real_t minsize = INFINITY; int minidx[2] = { -1, -1 }; for (int i = 0; i < p_count; ++i) { for (int j = i + 1; j < p_count; ++j) { const real_t sz = leaves[i]->volume.merge(leaves[j]->volume).get_size(); if (sz < minsize) { minsize = sz; minidx[0] = i; minidx[1] = j; } } } Node *n[] = { leaves[minidx[0]], leaves[minidx[1]] }; Node *p = _create_node_with_volume(nullptr, n[0]->volume.merge(n[1]->volume), nullptr); p->children[0] = n[0]; p->children[1] = n[1]; n[0]->parent = p; n[1]->parent = p; leaves[minidx[0]] = p; leaves[minidx[1]] = leaves[p_count - 1]; --p_count; } } DynamicBVH::Node *DynamicBVH::_top_down(Node **leaves, int p_count, int p_bu_threshold) { static const Vector3 axis[] = { Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1) }; ERR_FAIL_COND_V(p_bu_threshold <= 1, nullptr); if (p_count > 1) { if (p_count > p_bu_threshold) { const Volume vol = _bounds(leaves, p_count); const Vector3 org = vol.get_center(); int partition; int bestaxis = -1; int bestmidp = p_count; int splitcount[3][2] = { { 0, 0 }, { 0, 0 }, { 0, 0 } }; int i; for (i = 0; i < p_count; ++i) { const Vector3 x = leaves[i]->volume.get_center() - org; for (int j = 0; j < 3; ++j) { ++splitcount[j][x.dot(axis[j]) > 0 ? 1 : 0]; } } for (i = 0; i < 3; ++i) { if ((splitcount[i][0] > 0) && (splitcount[i][1] > 0)) { const int midp = (int)Math::abs(real_t(splitcount[i][0] - splitcount[i][1])); if (midp < bestmidp) { bestaxis = i; bestmidp = midp; } } } if (bestaxis >= 0) { partition = _split(leaves, p_count, org, axis[bestaxis]); ERR_FAIL_COND_V(partition == 0 || partition == p_count, nullptr); } else { partition = p_count / 2 + 1; } Node *node = _create_node_with_volume(nullptr, vol, nullptr); node->children[0] = _top_down(&leaves[0], partition, p_bu_threshold); node->children[1] = _top_down(&leaves[partition], p_count - partition, p_bu_threshold); node->children[0]->parent = node; node->children[1]->parent = node; return (node); } else { _bottom_up(leaves, p_count); return (leaves[0]); } } return (leaves[0]); } DynamicBVH::Node *DynamicBVH::_node_sort(Node *n, Node *&r) { Node *p = n->parent; ERR_FAIL_COND_V(!n->is_internal(), nullptr); if (p > n) { const int i = n->get_index_in_parent(); const int j = 1 - i; Node *s = p->children[j]; Node *q = p->parent; ERR_FAIL_COND_V(n != p->children[i], nullptr); if (q) { q->children[p->get_index_in_parent()] = n; } else { r = n; } s->parent = n; p->parent = n; n->parent = q; p->children[0] = n->children[0]; p->children[1] = n->children[1]; n->children[0]->parent = p; n->children[1]->parent = p; n->children[i] = p; n->children[j] = s; SWAP(p->volume, n->volume); return (p); } return (n); } void DynamicBVH::clear() { if (bvh_root) { _recurse_delete_node(bvh_root); } lkhd = -1; opath = 0; } void DynamicBVH::optimize_bottom_up() { if (bvh_root) { LocalVector<Node *> leaves; _fetch_leaves(bvh_root, leaves); _bottom_up(&leaves[0], leaves.size()); bvh_root = leaves[0]; } } void DynamicBVH::optimize_top_down(int bu_threshold) { if (bvh_root) { LocalVector<Node *> leaves; _fetch_leaves(bvh_root, leaves); bvh_root = _top_down(&leaves[0], leaves.size(), bu_threshold); } } void DynamicBVH::optimize_incremental(int passes) { if (passes < 0) { passes = total_leaves; } if (passes > 0) { do { if (!bvh_root) { break; } Node *node = bvh_root; unsigned bit = 0; while (node->is_internal()) { node = _node_sort(node, bvh_root)->children[(opath >> bit) & 1]; bit = (bit + 1) & (sizeof(unsigned) * 8 - 1); } _update(node); ++opath; } while (--passes); } } DynamicBVH::ID DynamicBVH::insert(const AABB &p_box, void *p_userdata) { Volume volume; volume.min = p_box.position; volume.max = p_box.position + p_box.size; Node *leaf = _create_node_with_volume(nullptr, volume, p_userdata); _insert_leaf(bvh_root, leaf); ++total_leaves; ID id; id.node = leaf; return id; } void DynamicBVH::_update(Node *leaf, int lookahead) { Node *root = _remove_leaf(leaf); if (root) { if (lookahead >= 0) { for (int i = 0; (i < lookahead) && root->parent; ++i) { root = root->parent; } } else { root = bvh_root; } } _insert_leaf(root, leaf); } bool DynamicBVH::update(const ID &p_id, const AABB &p_box) { ERR_FAIL_COND_V(!p_id.is_valid(), false); Node *leaf = p_id.node; Volume volume; volume.min = p_box.position; volume.max = p_box.position + p_box.size; if (leaf->volume.min.is_equal_approx(volume.min) && leaf->volume.max.is_equal_approx(volume.max)) { // noop return false; } Node *base = _remove_leaf(leaf); if (base) { if (lkhd >= 0) { for (int i = 0; (i < lkhd) && base->parent; ++i) { base = base->parent; } } else { base = bvh_root; } } leaf->volume = volume; _insert_leaf(base, leaf); return true; } void DynamicBVH::remove(const ID &p_id) { ERR_FAIL_COND(!p_id.is_valid()); Node *leaf = p_id.node; _remove_leaf(leaf); _delete_node(leaf); --total_leaves; } void DynamicBVH::_extract_leaves(Node *p_node, List<ID> *r_elements) { if (p_node->is_internal()) { _extract_leaves(p_node->children[0], r_elements); _extract_leaves(p_node->children[1], r_elements); } else { ID id; id.node = p_node; r_elements->push_back(id); } } void DynamicBVH::set_index(uint32_t p_index) { ERR_FAIL_COND(bvh_root != nullptr); index = p_index; } uint32_t DynamicBVH::get_index() const { return index; } void DynamicBVH::get_elements(List<ID> *r_elements) { if (bvh_root) { _extract_leaves(bvh_root, r_elements); } } int DynamicBVH::get_leaf_count() const { return total_leaves; } int DynamicBVH::get_max_depth() const { if (bvh_root) { int depth = 1; int max_depth = 0; bvh_root->get_max_depth(depth, max_depth); return max_depth; } else { return 0; } } DynamicBVH::~DynamicBVH() { clear(); }