Add a Dynamic BVH implementation.
-Based on Bullet Dbvh, has style and functional changes. -Provides efficient pairing -Needed to optimize rendering -Needed to optimize physics This PR is up for others to review the implementation.
This commit is contained in:
reduz
parent
e9d12f9e4e
commit
5ff89931c7
2 changed files with 759 additions and 0 deletions
400
core/math/dynamic_bvh.cpp
Normal file
400
core/math/dynamic_bvh.cpp
Normal file
|
|
@ -0,0 +1,400 @@
|
|||
/*************************************************************************/
|
||||
/* dynamic_bvh.cpp */
|
||||
/*************************************************************************/
|
||||
/* This file is part of: */
|
||||
/* GODOT ENGINE */
|
||||
/* https://godotengine.org */
|
||||
/*************************************************************************/
|
||||
/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */
|
||||
/* Copyright (c) 2014-2020 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) {
|
||||
memdelete(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 = memnew(Node);
|
||||
node->parent = p_parent;
|
||||
node->data = p_data;
|
||||
node->childs[1] = 0;
|
||||
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 = 0;
|
||||
} 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), 0);
|
||||
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 (0 != (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 = 0;
|
||||
return (0);
|
||||
} 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 = 0;
|
||||
_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);
|
||||
}
|
||||
|
||||
void DynamicBVH::update(const ID &p_id, const AABB &p_box) {
|
||||
ERR_FAIL_COND(!p_id.is_valid());
|
||||
Node *leaf = p_id.node;
|
||||
Node *base = _remove_leaf(leaf);
|
||||
Volume volume;
|
||||
volume.min = p_box.position;
|
||||
volume.max = p_box.position + p_box.size;
|
||||
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);
|
||||
}
|
||||
|
||||
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::get_elements(List<ID> *r_elements) {
|
||||
if (bvh_root) {
|
||||
_extract_leaves(bvh_root, r_elements);
|
||||
}
|
||||
}
|
||||
|
||||
DynamicBVH::~DynamicBVH() {
|
||||
clear();
|
||||
}
|
||||
359
core/math/dynamic_bvh.h
Normal file
359
core/math/dynamic_bvh.h
Normal file
|
|
@ -0,0 +1,359 @@
|
|||
/*************************************************************************/
|
||||
/* dynamic_bvh.h */
|
||||
/*************************************************************************/
|
||||
/* This file is part of: */
|
||||
/* GODOT ENGINE */
|
||||
/* https://godotengine.org */
|
||||
/*************************************************************************/
|
||||
/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */
|
||||
/* Copyright (c) 2014-2020 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. */
|
||||
/*************************************************************************/
|
||||
|
||||
#ifndef DYNAMICBVH_H
|
||||
#define DYNAMICBVH_H
|
||||
|
||||
#include "core/math/aabb.h"
|
||||
#include "core/templates/list.h"
|
||||
#include "core/templates/local_vector.h"
|
||||
#include "core/typedefs.h"
|
||||
|
||||
// Based on bullet Dbvh
|
||||
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
///DynamicBVH implementation by Nathanael Presson
|
||||
// The DynamicBVH class implements a fast dynamic bounding volume tree based on axis aligned bounding boxes (aabb tree).
|
||||
|
||||
class DynamicBVH {
|
||||
struct Node;
|
||||
|
||||
public:
|
||||
struct ID {
|
||||
Node *node;
|
||||
|
||||
public:
|
||||
_FORCE_INLINE_ bool is_valid() const { return node != nullptr; }
|
||||
_FORCE_INLINE_ ID() {
|
||||
node = nullptr;
|
||||
}
|
||||
};
|
||||
|
||||
private:
|
||||
struct Volume {
|
||||
Vector3 min, max;
|
||||
|
||||
_FORCE_INLINE_ Vector3 get_center() const { return ((min + max) / 2); }
|
||||
_FORCE_INLINE_ Vector3 get_length() const { return (max - min); }
|
||||
|
||||
_FORCE_INLINE_ bool contains(const Volume &a) const {
|
||||
return ((min.x <= a.min.x) &&
|
||||
(min.y <= a.min.y) &&
|
||||
(min.z <= a.min.z) &&
|
||||
(max.x >= a.max.x) &&
|
||||
(max.y >= a.max.y) &&
|
||||
(max.z >= a.max.z));
|
||||
}
|
||||
|
||||
_FORCE_INLINE_ Volume merge(const Volume &b) const {
|
||||
Volume r;
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
if (min[i] < b.min[i])
|
||||
r.min[i] = min[i];
|
||||
else
|
||||
r.min[i] = b.min[i];
|
||||
if (max[i] > b.max[i])
|
||||
r.max[i] = max[i];
|
||||
else
|
||||
r.max[i] = b.max[i];
|
||||
}
|
||||
return r;
|
||||
}
|
||||
|
||||
_FORCE_INLINE_ real_t get_size() const {
|
||||
const Vector3 edges = get_length();
|
||||
return (edges.x * edges.y * edges.z +
|
||||
edges.x + edges.y + edges.z);
|
||||
}
|
||||
|
||||
_FORCE_INLINE_ bool is_not_equal_to(const Volume &b) const {
|
||||
return ((min.x != b.min.x) ||
|
||||
(min.y != b.min.y) ||
|
||||
(min.z != b.min.z) ||
|
||||
(max.x != b.max.x) ||
|
||||
(max.y != b.max.y) ||
|
||||
(max.z != b.max.z));
|
||||
}
|
||||
|
||||
_FORCE_INLINE_ real_t get_proximity_to(const Volume &b) const {
|
||||
const Vector3 d = (min + max) - (b.min + b.max);
|
||||
return (Math::abs(d.x) + Math::abs(d.y) + Math::abs(d.z));
|
||||
}
|
||||
|
||||
_FORCE_INLINE_ int select_by_proximity(const Volume &a, const Volume &b) const {
|
||||
return (get_proximity_to(a) < get_proximity_to(b) ? 0 : 1);
|
||||
}
|
||||
|
||||
//
|
||||
_FORCE_INLINE_ bool intersects(const Volume &b) const {
|
||||
return ((min.x <= b.max.x) &&
|
||||
(max.x >= b.min.x) &&
|
||||
(min.y <= b.max.y) &&
|
||||
(max.y >= b.min.y) &&
|
||||
(min.z <= b.max.z) &&
|
||||
(max.z >= b.min.z));
|
||||
}
|
||||
};
|
||||
|
||||
struct Node {
|
||||
Volume volume;
|
||||
Node *parent = nullptr;
|
||||
union {
|
||||
Node *childs[2];
|
||||
void *data;
|
||||
};
|
||||
|
||||
_FORCE_INLINE_ bool is_leaf() const { return data != nullptr; }
|
||||
_FORCE_INLINE_ bool is_internal() const { return (!is_leaf()); }
|
||||
|
||||
_FORCE_INLINE_ int get_index_in_parent() const {
|
||||
ERR_FAIL_COND_V(!parent, 0);
|
||||
return (parent->childs[1] == this) ? 1 : 0;
|
||||
}
|
||||
_FORCE_INLINE_ void get_max_depth(int depth, int &maxdepth) {
|
||||
if (is_internal()) {
|
||||
childs[0]->get_max_depth(depth + 1, maxdepth);
|
||||
childs[1]->get_max_depth(depth + 1, maxdepth);
|
||||
} else {
|
||||
maxdepth = MAX(maxdepth, depth);
|
||||
}
|
||||
}
|
||||
|
||||
//
|
||||
int count_leaves() const {
|
||||
if (is_internal())
|
||||
return childs[0]->count_leaves() + childs[1]->count_leaves();
|
||||
else
|
||||
return (1);
|
||||
}
|
||||
|
||||
bool is_left_of_axis(const Vector3 &org, const Vector3 &axis) const {
|
||||
return axis.dot(volume.get_center() - org) <= 0;
|
||||
}
|
||||
|
||||
Node() {
|
||||
childs[0] = nullptr;
|
||||
childs[1] = nullptr;
|
||||
}
|
||||
};
|
||||
|
||||
// Fields
|
||||
Node *bvh_root = nullptr;
|
||||
int lkhd = -1;
|
||||
int total_leaves = 0;
|
||||
uint32_t opath = 0;
|
||||
|
||||
enum {
|
||||
ALLOCA_STACK_SIZE = 128
|
||||
};
|
||||
|
||||
_FORCE_INLINE_ void _delete_node(Node *p_node);
|
||||
void _recurse_delete_node(Node *p_node);
|
||||
_FORCE_INLINE_ Node *_create_node(Node *p_parent, void *p_data);
|
||||
_FORCE_INLINE_ DynamicBVH::Node *_create_node_with_volume(Node *p_parent, const Volume &p_volume, void *p_data);
|
||||
_FORCE_INLINE_ void _insert_leaf(Node *p_root, Node *p_leaf);
|
||||
_FORCE_INLINE_ Node *_remove_leaf(Node *leaf);
|
||||
void _fetch_leaves(Node *p_root, LocalVector<Node *> &r_leaves, int p_depth = -1);
|
||||
static int _split(Node **leaves, int p_count, const Vector3 &p_org, const Vector3 &p_axis);
|
||||
static Volume _bounds(Node **leaves, int p_count);
|
||||
void _bottom_up(Node **leaves, int p_count);
|
||||
Node *_top_down(Node **leaves, int p_count, int p_bu_threshold);
|
||||
Node *_node_sort(Node *n, Node *&r);
|
||||
|
||||
_FORCE_INLINE_ void _update(Node *leaf, int lookahead = -1);
|
||||
|
||||
void _extract_leaves(Node *p_node, List<ID> *r_elements);
|
||||
|
||||
_FORCE_INLINE_ bool _ray_aabb(const Vector3 &rayFrom, const Vector3 &rayInvDirection, const unsigned int raySign[3], const Vector3 bounds[2], real_t &tmin, real_t lambda_min, real_t lambda_max) {
|
||||
real_t tmax, tymin, tymax, tzmin, tzmax;
|
||||
tmin = (bounds[raySign[0]].x - rayFrom.x) * rayInvDirection.x;
|
||||
tmax = (bounds[1 - raySign[0]].x - rayFrom.x) * rayInvDirection.x;
|
||||
tymin = (bounds[raySign[1]].y - rayFrom.y) * rayInvDirection.y;
|
||||
tymax = (bounds[1 - raySign[1]].y - rayFrom.y) * rayInvDirection.y;
|
||||
|
||||
if ((tmin > tymax) || (tymin > tmax))
|
||||
return false;
|
||||
|
||||
if (tymin > tmin)
|
||||
tmin = tymin;
|
||||
|
||||
if (tymax < tmax)
|
||||
tmax = tymax;
|
||||
|
||||
tzmin = (bounds[raySign[2]].z - rayFrom.z) * rayInvDirection.z;
|
||||
tzmax = (bounds[1 - raySign[2]].z - rayFrom.z) * rayInvDirection.z;
|
||||
|
||||
if ((tmin > tzmax) || (tzmin > tmax))
|
||||
return false;
|
||||
if (tzmin > tmin)
|
||||
tmin = tzmin;
|
||||
if (tzmax < tmax)
|
||||
tmax = tzmax;
|
||||
return ((tmin < lambda_max) && (tmax > lambda_min));
|
||||
}
|
||||
|
||||
public:
|
||||
// Methods
|
||||
void clear();
|
||||
bool empty() const { return (0 == bvh_root); }
|
||||
void optimize_bottom_up();
|
||||
void optimize_top_down(int bu_threshold = 128);
|
||||
void optimize_incremental(int passes);
|
||||
ID insert(const AABB &p_box, void *p_userdata);
|
||||
void update(const ID &p_id, const AABB &p_box);
|
||||
void remove(const ID &p_id);
|
||||
void get_elements(List<ID> *r_elements);
|
||||
|
||||
/* Discouraged, but works as a reference on how it must be used */
|
||||
struct DefaultQueryResult {
|
||||
virtual bool operator()(void *p_data) = 0; //return true whether you want to continue the query
|
||||
virtual ~DefaultQueryResult() {}
|
||||
};
|
||||
|
||||
template <class QueryResult>
|
||||
_FORCE_INLINE_ void aabb_query(const AABB &p_aabb, QueryResult &r_result);
|
||||
template <class QueryResult>
|
||||
_FORCE_INLINE_ void ray_query(const Vector3 &p_from, const Vector3 &p_to, QueryResult &r_result);
|
||||
|
||||
DynamicBVH();
|
||||
~DynamicBVH();
|
||||
};
|
||||
|
||||
template <class QueryResult>
|
||||
void DynamicBVH::aabb_query(const AABB &p_box, QueryResult &r_result) {
|
||||
if (!bvh_root) {
|
||||
return;
|
||||
}
|
||||
|
||||
Volume volume;
|
||||
volume.min = p_box.position;
|
||||
volume.max = p_box.position + p_box.size;
|
||||
|
||||
const Node **stack = (const Node **)alloca(ALLOCA_STACK_SIZE * sizeof(const Node *));
|
||||
stack[0] = bvh_root;
|
||||
int32_t depth = 1;
|
||||
int32_t threshold = ALLOCA_STACK_SIZE - 2;
|
||||
|
||||
LocalVector<const Node *> aux_stack; //only used in rare occasions when you run out of alloca memory because tree is too unbalanced. Should correct itself over time.
|
||||
|
||||
do {
|
||||
const Node *n = stack[depth - 1];
|
||||
depth--;
|
||||
if (n->volume.intersects(volume)) {
|
||||
if (n->is_internal()) {
|
||||
if (depth > threshold) {
|
||||
if (aux_stack.empty()) {
|
||||
aux_stack.resize(ALLOCA_STACK_SIZE * 2);
|
||||
copymem(aux_stack.ptr(), stack, ALLOCA_STACK_SIZE * sizeof(const Node *));
|
||||
} else {
|
||||
aux_stack.resize(aux_stack.size() * 2);
|
||||
}
|
||||
stack = aux_stack.ptr();
|
||||
threshold = aux_stack.size() - 2;
|
||||
}
|
||||
stack[depth++] = n->childs[0];
|
||||
stack[depth++] = n->childs[1];
|
||||
} else {
|
||||
if (r_result(n->data)) {
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
} while (depth > 0);
|
||||
}
|
||||
|
||||
template <class QueryResult>
|
||||
void DynamicBVH::ray_query(const Vector3 &p_from, const Vector3 &p_to, QueryResult &r_result) {
|
||||
Vector3 ray_dir = (p_to - p_from);
|
||||
ray_dir.normalize();
|
||||
|
||||
///what about division by zero? --> just set rayDirection[i] to INF/B3_LARGE_FLOAT
|
||||
Vector3 inv_dir;
|
||||
inv_dir[0] = ray_dir[0] == real_t(0.0) ? real_t(1e20) : real_t(1.0) / ray_dir[0];
|
||||
inv_dir[1] = ray_dir[1] == real_t(0.0) ? real_t(1e20) : real_t(1.0) / ray_dir[1];
|
||||
inv_dir[2] = ray_dir[2] == real_t(0.0) ? real_t(1e20) : real_t(1.0) / ray_dir[2];
|
||||
unsigned int signs[3] = { inv_dir[0] < 0.0, inv_dir[1] < 0.0, inv_dir[2] < 0.0 };
|
||||
|
||||
real_t lambda_max = ray_dir.dot(p_to - p_from);
|
||||
|
||||
Vector3 bounds[2];
|
||||
|
||||
const Node **stack = (const Node **)alloca(ALLOCA_STACK_SIZE * sizeof(const Node *));
|
||||
stack[0] = bvh_root;
|
||||
int32_t depth = 1;
|
||||
int32_t threshold = ALLOCA_STACK_SIZE - 2;
|
||||
|
||||
LocalVector<const Node *> aux_stack; //only used in rare occasions when you run out of alloca memory because tree is too unbalanced. Should correct itself over time.
|
||||
|
||||
do {
|
||||
const Node *node = stack[--depth];
|
||||
bounds[0] = node->volume.min;
|
||||
bounds[1] = node->volume.max;
|
||||
real_t tmin = 1.f, lambda_min = 0.f;
|
||||
unsigned int result1 = false;
|
||||
result1 = _ray_aabb(p_from, inv_dir, signs, bounds, tmin, lambda_min, lambda_max);
|
||||
if (result1) {
|
||||
if (node->is_internal()) {
|
||||
if (depth > threshold) {
|
||||
if (aux_stack.empty()) {
|
||||
aux_stack.resize(ALLOCA_STACK_SIZE * 2);
|
||||
copymem(aux_stack.ptr(), stack, ALLOCA_STACK_SIZE * sizeof(const Node *));
|
||||
} else {
|
||||
aux_stack.resize(aux_stack.size() * 2);
|
||||
}
|
||||
stack = aux_stack.ptr();
|
||||
threshold = aux_stack.size() - 2;
|
||||
}
|
||||
stack[depth++] = node->childs[0];
|
||||
stack[depth++] = node->childs[1];
|
||||
} else {
|
||||
if (r_result(node->data)) {
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
} while (depth > 0);
|
||||
}
|
||||
|
||||
#endif // DYNAMICBVH_H
|
||||
Loading…
Reference in a new issue