summaryrefslogtreecommitdiffstats
path: root/core/math/dynamic_bvh.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'core/math/dynamic_bvh.cpp')
-rw-r--r--core/math/dynamic_bvh.cpp436
1 files changed, 436 insertions, 0 deletions
diff --git a/core/math/dynamic_bvh.cpp b/core/math/dynamic_bvh.cpp
new file mode 100644
index 0000000000..200095d8cb
--- /dev/null
+++ b/core/math/dynamic_bvh.cpp
@@ -0,0 +1,436 @@
+/*************************************************************************/
+/* dynamic_bvh.cpp */
+/*************************************************************************/
+/* This file is part of: */
+/* GODOT ENGINE */
+/* https://godotengine.org */
+/*************************************************************************/
+/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */
+/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */
+/* */
+/* 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->childs[0]);
+ _recurse_delete_node(p_node->childs[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->childs[p_leaf->volume.select_by_proximity(
+ p_root->childs[0]->volume,
+ p_root->childs[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->childs[p_root->get_index_in_parent()] = node;
+ node->childs[0] = p_root;
+ p_root->parent = node;
+ node->childs[1] = p_leaf;
+ p_leaf->parent = node;
+ do {
+ if (!prev->volume.contains(node->volume)) {
+ prev->volume = prev->childs[0]->volume.merge(prev->childs[1]->volume);
+ } else {
+ break;
+ }
+ node = prev;
+ } while (nullptr != (prev = node->parent));
+ } else {
+ node->childs[0] = p_root;
+ p_root->parent = node;
+ node->childs[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->childs[1 - leaf->get_index_in_parent()];
+ if (prev) {
+ prev->childs[parent->get_index_in_parent()] = sibling;
+ sibling->parent = prev;
+ _delete_node(parent);
+ while (prev) {
+ const Volume pb = prev->volume;
+ prev->volume = prev->childs[0]->volume.merge(prev->childs[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->childs[0], r_leaves, p_depth - 1);
+ _fetch_leaves(p_root->childs[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 = Math_INF;
+ 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->childs[0] = n[0];
+ p->childs[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->childs[0] = _top_down(&leaves[0], partition, p_bu_threshold);
+ node->childs[1] = _top_down(&leaves[partition], p_count - partition, p_bu_threshold);
+ node->childs[0]->parent = node;
+ node->childs[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->childs[j];
+ Node *q = p->parent;
+ ERR_FAIL_COND_V(n != p->childs[i], nullptr);
+ if (q) {
+ q->childs[p->get_index_in_parent()] = n;
+ } else {
+ r = n;
+ }
+ s->parent = n;
+ p->parent = n;
+ n->parent = q;
+ p->childs[0] = n->childs[0];
+ p->childs[1] = n->childs[1];
+ n->childs[0]->parent = p;
+ n->childs[1]->parent = p;
+ n->childs[i] = p;
+ n->childs[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 (bvh_root && (passes > 0)) {
+ do {
+ Node *node = bvh_root;
+ unsigned bit = 0;
+ while (node->is_internal()) {
+ node = _node_sort(node, bvh_root)->childs[(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->childs[0], r_elements);
+ _extract_leaves(p_node->childs[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();
+}