1/**************************************************************************/
2/* bvh_tree.h */
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#ifndef BVH_TREE_H
32#define BVH_TREE_H
33
34// BVH Tree
35// This is an implementation of a dynamic BVH with templated leaf size.
36// This differs from most dynamic BVH in that it can handle more than 1 object
37// in leaf nodes. This can make it far more efficient in certain circumstances.
38// It also means that the splitting logic etc have to be completely different
39// to a simpler tree.
40// Note that MAX_CHILDREN should be fixed at 2 for now.
41
42#include "core/math/aabb.h"
43#include "core/math/bvh_abb.h"
44#include "core/math/geometry_3d.h"
45#include "core/math/vector3.h"
46#include "core/templates/local_vector.h"
47#include "core/templates/pooled_list.h"
48#include <limits.h>
49
50#define BVHABB_CLASS BVH_ABB<BOUNDS, POINT>
51
52// not sure if this is better yet so making optional
53#define BVH_EXPAND_LEAF_AABBS
54
55// never do these checks in release
56#ifdef DEV_ENABLED
57//#define BVH_VERBOSE
58//#define BVH_VERBOSE_TREE
59//#define BVH_VERBOSE_PAIRING
60//#define BVH_VERBOSE_MOVES
61
62//#define BVH_VERBOSE_FRAME
63//#define BVH_CHECKS
64//#define BVH_INTEGRITY_CHECKS
65#endif
66
67// debug only assert
68#ifdef BVH_CHECKS
69#define BVH_ASSERT(a) CRASH_COND((a) == false)
70#else
71#define BVH_ASSERT(a)
72#endif
73
74#ifdef BVH_VERBOSE
75#define VERBOSE_PRINT print_line
76#else
77#define VERBOSE_PRINT(a)
78#endif
79
80// really just a namespace
81struct BVHCommon {
82 // these could possibly also be the same constant,
83 // although this may be useful for debugging.
84 // or use zero for invalid and +1 based indices.
85 static const uint32_t INVALID = (0xffffffff);
86 static const uint32_t INACTIVE = (0xfffffffe);
87};
88
89// really a handle, can be anything
90// note that zero is a valid reference for the BVH .. this may involve using
91// a plus one based ID for clients that expect 0 to be invalid.
92struct BVHHandle {
93 // conversion operator
94 operator uint32_t() const { return _data; }
95 void set(uint32_t p_value) { _data = p_value; }
96
97 uint32_t _data;
98
99 void set_invalid() { _data = BVHCommon::INVALID; }
100 bool is_invalid() const { return _data == BVHCommon::INVALID; }
101 uint32_t id() const { return _data; }
102 void set_id(uint32_t p_id) { _data = p_id; }
103
104 bool operator==(const BVHHandle &p_h) const { return _data == p_h._data; }
105 bool operator!=(const BVHHandle &p_h) const { return (*this == p_h) == false; }
106};
107
108// helper class to make iterative versions of recursive functions
109template <class T>
110class BVH_IterativeInfo {
111public:
112 enum {
113 ALLOCA_STACK_SIZE = 128
114 };
115
116 int32_t depth = 1;
117 int32_t threshold = ALLOCA_STACK_SIZE - 2;
118 T *stack;
119 //only used in rare occasions when you run out of alloca memory
120 // because tree is too unbalanced.
121 LocalVector<T> aux_stack;
122 int32_t get_alloca_stacksize() const { return ALLOCA_STACK_SIZE * sizeof(T); }
123
124 T *get_first() const {
125 return &stack[0];
126 }
127
128 // pop the last member of the stack, or return false
129 bool pop(T &r_value) {
130 if (!depth) {
131 return false;
132 }
133
134 depth--;
135 r_value = stack[depth];
136 return true;
137 }
138
139 // request new addition to stack
140 T *request() {
141 if (depth > threshold) {
142 if (aux_stack.is_empty()) {
143 aux_stack.resize(ALLOCA_STACK_SIZE * 2);
144 memcpy(aux_stack.ptr(), stack, get_alloca_stacksize());
145 } else {
146 aux_stack.resize(aux_stack.size() * 2);
147 }
148 stack = aux_stack.ptr();
149 threshold = aux_stack.size() - 2;
150 }
151 return &stack[depth++];
152 }
153};
154
155template <class T>
156class BVH_DummyPairTestFunction {
157public:
158 static bool user_collision_check(T *p_a, T *p_b) {
159 // return false if no collision, decided by masks etc
160 return true;
161 }
162};
163
164template <class T>
165class BVH_DummyCullTestFunction {
166public:
167 static bool user_cull_check(T *p_a, T *p_b) {
168 // return false if no collision
169 return true;
170 }
171};
172
173template <class T, int NUM_TREES, int MAX_CHILDREN, int MAX_ITEMS, class USER_PAIR_TEST_FUNCTION = BVH_DummyPairTestFunction<T>, class USER_CULL_TEST_FUNCTION = BVH_DummyCullTestFunction<T>, bool USE_PAIRS = false, class BOUNDS = AABB, class POINT = Vector3>
174class BVH_Tree {
175 friend class BVH;
176
177#include "bvh_pair.inc"
178#include "bvh_structs.inc"
179
180public:
181 BVH_Tree() {
182 for (int n = 0; n < NUM_TREES; n++) {
183 _root_node_id[n] = BVHCommon::INVALID;
184 }
185
186 // disallow zero leaf ids
187 // (as these ids are stored as negative numbers in the node)
188 uint32_t dummy_leaf_id;
189 _leaves.request(dummy_leaf_id);
190
191 // In many cases you may want to change this default in the client code,
192 // or expose this value to the user.
193 // This default may make sense for a typically scaled 3d game, but maybe not for 2d on a pixel scale.
194 params_set_pairing_expansion(0.1);
195 }
196
197private:
198 bool node_add_child(uint32_t p_node_id, uint32_t p_child_node_id) {
199 TNode &tnode = _nodes[p_node_id];
200 if (tnode.is_full_of_children()) {
201 return false;
202 }
203
204 tnode.children[tnode.num_children] = p_child_node_id;
205 tnode.num_children += 1;
206
207 // back link in the child to the parent
208 TNode &tnode_child = _nodes[p_child_node_id];
209 tnode_child.parent_id = p_node_id;
210
211 return true;
212 }
213
214 void node_replace_child(uint32_t p_parent_id, uint32_t p_old_child_id, uint32_t p_new_child_id) {
215 TNode &parent = _nodes[p_parent_id];
216 BVH_ASSERT(!parent.is_leaf());
217
218 int child_num = parent.find_child(p_old_child_id);
219 BVH_ASSERT(child_num != -1);
220 parent.children[child_num] = p_new_child_id;
221
222 TNode &new_child = _nodes[p_new_child_id];
223 new_child.parent_id = p_parent_id;
224 }
225
226 void node_remove_child(uint32_t p_parent_id, uint32_t p_child_id, uint32_t p_tree_id, bool p_prevent_sibling = false) {
227 TNode &parent = _nodes[p_parent_id];
228 BVH_ASSERT(!parent.is_leaf());
229
230 int child_num = parent.find_child(p_child_id);
231 BVH_ASSERT(child_num != -1);
232
233 parent.remove_child_internal(child_num);
234
235 // no need to keep back references for children at the moment
236
237 uint32_t sibling_id = 0; // always a node id, as tnode is never a leaf
238 bool sibling_present = false;
239
240 // if there are more children, or this is the root node, don't try and delete
241 if (parent.num_children > 1) {
242 return;
243 }
244
245 // if there is 1 sibling, it can be moved to be a child of the
246 if (parent.num_children == 1) {
247 // else there is now a redundant node with one child, which can be removed
248 sibling_id = parent.children[0];
249 sibling_present = true;
250 }
251
252 // now there may be no children in this node .. in which case it can be deleted
253 // remove node if empty
254 // remove link from parent
255 uint32_t grandparent_id = parent.parent_id;
256
257 // special case for root node
258 if (grandparent_id == BVHCommon::INVALID) {
259 if (sibling_present) {
260 // change the root node
261 change_root_node(sibling_id, p_tree_id);
262
263 // delete the old root node as no longer needed
264 node_free_node_and_leaf(p_parent_id);
265 }
266
267 return;
268 }
269
270 if (sibling_present) {
271 node_replace_child(grandparent_id, p_parent_id, sibling_id);
272 } else {
273 node_remove_child(grandparent_id, p_parent_id, p_tree_id, true);
274 }
275
276 // put the node on the free list to recycle
277 node_free_node_and_leaf(p_parent_id);
278 }
279
280 // A node can either be a node, or a node AND a leaf combo.
281 // Both must be deleted to prevent a leak.
282 void node_free_node_and_leaf(uint32_t p_node_id) {
283 TNode &node = _nodes[p_node_id];
284 if (node.is_leaf()) {
285 int leaf_id = node.get_leaf_id();
286 _leaves.free(leaf_id);
287 }
288
289 _nodes.free(p_node_id);
290 }
291
292 void change_root_node(uint32_t p_new_root_id, uint32_t p_tree_id) {
293 _root_node_id[p_tree_id] = p_new_root_id;
294 TNode &root = _nodes[p_new_root_id];
295
296 // mark no parent
297 root.parent_id = BVHCommon::INVALID;
298 }
299
300 void node_make_leaf(uint32_t p_node_id) {
301 uint32_t child_leaf_id;
302 TLeaf *child_leaf = _leaves.request(child_leaf_id);
303 child_leaf->clear();
304
305 // zero is reserved at startup, to prevent this id being used
306 // (as they are stored as negative values in the node, and zero is already taken)
307 BVH_ASSERT(child_leaf_id != 0);
308
309 TNode &node = _nodes[p_node_id];
310 node.neg_leaf_id = -(int)child_leaf_id;
311 }
312
313 void node_remove_item(uint32_t p_ref_id, uint32_t p_tree_id, BVHABB_CLASS *r_old_aabb = nullptr) {
314 // get the reference
315 ItemRef &ref = _refs[p_ref_id];
316 uint32_t owner_node_id = ref.tnode_id;
317
318 // debug draw special
319 // This may not be needed
320 if (owner_node_id == BVHCommon::INVALID) {
321 return;
322 }
323
324 TNode &tnode = _nodes[owner_node_id];
325 CRASH_COND(!tnode.is_leaf());
326
327 TLeaf &leaf = _node_get_leaf(tnode);
328
329 // if the aabb is not determining the corner size, then there is no need to refit!
330 // (optimization, as merging AABBs takes a lot of time)
331 const BVHABB_CLASS &old_aabb = leaf.get_aabb(ref.item_id);
332
333 // shrink a little to prevent using corner aabbs
334 // in order to miss the corners first we shrink by node_expansion
335 // (which is added to the overall bound of the leaf), then we also
336 // shrink by an epsilon, in order to miss out the very corner aabbs
337 // which are important in determining the bound. Any other aabb
338 // within this can be removed and not affect the overall bound.
339 BVHABB_CLASS node_bound = tnode.aabb;
340 node_bound.expand(-_node_expansion - 0.001f);
341 bool refit = true;
342
343 if (node_bound.is_other_within(old_aabb)) {
344 refit = false;
345 }
346
347 // record the old aabb if required (for incremental remove_and_reinsert)
348 if (r_old_aabb) {
349 *r_old_aabb = old_aabb;
350 }
351
352 leaf.remove_item_unordered(ref.item_id);
353
354 if (leaf.num_items) {
355 // the swapped item has to have its reference changed to, to point to the new item id
356 uint32_t swapped_ref_id = leaf.get_item_ref_id(ref.item_id);
357
358 ItemRef &swapped_ref = _refs[swapped_ref_id];
359
360 swapped_ref.item_id = ref.item_id;
361
362 // only have to refit if it is an edge item
363 // This is a VERY EXPENSIVE STEP
364 // we defer the refit updates until the update function is called once per frame
365 if (refit) {
366 leaf.set_dirty(true);
367 }
368 } else {
369 // remove node if empty
370 // remove link from parent
371 if (tnode.parent_id != BVHCommon::INVALID) {
372 // DANGER .. this can potentially end up with root node with 1 child ...
373 // we don't want this and must check for it
374
375 uint32_t parent_id = tnode.parent_id;
376
377 node_remove_child(parent_id, owner_node_id, p_tree_id);
378 refit_upward(parent_id);
379
380 // put the node on the free list to recycle
381 node_free_node_and_leaf(owner_node_id);
382 }
383
384 // else if no parent, it is the root node. Do not delete
385 }
386
387 ref.tnode_id = BVHCommon::INVALID;
388 ref.item_id = BVHCommon::INVALID; // unset
389 }
390
391 // returns true if needs refit of PARENT tree only, the node itself AABB is calculated
392 // within this routine
393 bool _node_add_item(uint32_t p_node_id, uint32_t p_ref_id, const BVHABB_CLASS &p_aabb) {
394 ItemRef &ref = _refs[p_ref_id];
395 ref.tnode_id = p_node_id;
396
397 TNode &node = _nodes[p_node_id];
398 BVH_ASSERT(node.is_leaf());
399 TLeaf &leaf = _node_get_leaf(node);
400
401 // optimization - we only need to do a refit
402 // if the added item is changing the AABB of the node.
403 // in most cases it won't.
404 bool needs_refit = true;
405
406 // expand bound now
407 BVHABB_CLASS expanded = p_aabb;
408 expanded.expand(_node_expansion);
409
410 // the bound will only be valid if there is an item in there already
411 if (leaf.num_items) {
412 if (node.aabb.is_other_within(expanded)) {
413 // no change to node AABBs
414 needs_refit = false;
415 } else {
416 node.aabb.merge(expanded);
417 }
418 } else {
419 // bound of the node = the new aabb
420 node.aabb = expanded;
421 }
422
423 ref.item_id = leaf.request_item();
424 BVH_ASSERT(ref.item_id != BVHCommon::INVALID);
425
426 // set the aabb of the new item
427 leaf.get_aabb(ref.item_id) = p_aabb;
428
429 // back reference on the item back to the item reference
430 leaf.get_item_ref_id(ref.item_id) = p_ref_id;
431
432 return needs_refit;
433 }
434
435 uint32_t _node_create_another_child(uint32_t p_node_id, const BVHABB_CLASS &p_aabb) {
436 uint32_t child_node_id;
437 TNode *child_node = _nodes.request(child_node_id);
438 child_node->clear();
439
440 // may not be necessary
441 child_node->aabb = p_aabb;
442
443 node_add_child(p_node_id, child_node_id);
444
445 return child_node_id;
446 }
447
448#include "bvh_cull.inc"
449#include "bvh_debug.inc"
450#include "bvh_integrity.inc"
451#include "bvh_logic.inc"
452#include "bvh_misc.inc"
453#include "bvh_public.inc"
454#include "bvh_refit.inc"
455#include "bvh_split.inc"
456};
457
458#undef VERBOSE_PRINT
459
460#endif // BVH_TREE_H
461