1/**************************************************************************/
2/* bvh.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_H
32#define BVH_H
33
34// BVH
35// This class provides a wrapper around BVH tree, which contains most of the functionality
36// for a dynamic BVH with templated leaf size.
37// However BVH also adds facilities for pairing, to maintain compatibility with Godot 3.2.
38// Pairing is a collision pairing system, on top of the basic BVH.
39
40// Some notes on the use of BVH / Octree from Godot 3.2.
41// This is not well explained elsewhere.
42// The rendering tree mask and types that are sent to the BVH are NOT layer masks.
43// They are INSTANCE_TYPES (defined in visual_server.h), e.g. MESH, MULTIMESH, PARTICLES etc.
44// Thus the lights do no cull by layer mask in the BVH.
45
46// Layer masks are implemented in the renderers as a later step, and light_cull_mask appears to be
47// implemented in GLES3 but not GLES2. Layer masks are not yet implemented for directional lights.
48
49// In the physics, the pairable_type is based on 1 << p_object->get_type() where:
50// TYPE_AREA,
51// TYPE_BODY
52// and pairable_mask is either 0 if static, or set to all if non static
53
54#include "bvh_tree.h"
55#include "core/os/mutex.h"
56
57#define BVHTREE_CLASS BVH_Tree<T, NUM_TREES, 2, MAX_ITEMS, USER_PAIR_TEST_FUNCTION, USER_CULL_TEST_FUNCTION, USE_PAIRS, BOUNDS, POINT>
58#define BVH_LOCKED_FUNCTION BVHLockedFunction _lock_guard(&_mutex, BVH_THREAD_SAFE &&_thread_safe);
59
60template <class T, int NUM_TREES = 1, bool USE_PAIRS = false, int MAX_ITEMS = 32, class USER_PAIR_TEST_FUNCTION = BVH_DummyPairTestFunction<T>, class USER_CULL_TEST_FUNCTION = BVH_DummyCullTestFunction<T>, class BOUNDS = AABB, class POINT = Vector3, bool BVH_THREAD_SAFE = true>
61class BVH_Manager {
62public:
63 // note we are using uint32_t instead of BVHHandle, losing type safety, but this
64 // is for compatibility with octree
65 typedef void *(*PairCallback)(void *, uint32_t, T *, int, uint32_t, T *, int);
66 typedef void (*UnpairCallback)(void *, uint32_t, T *, int, uint32_t, T *, int, void *);
67 typedef void *(*CheckPairCallback)(void *, uint32_t, T *, int, uint32_t, T *, int, void *);
68
69 // allow locally toggling thread safety if the template has been compiled with BVH_THREAD_SAFE
70 void params_set_thread_safe(bool p_enable) {
71 _thread_safe = p_enable;
72 }
73
74 // these 2 are crucial for fine tuning, and can be applied manually
75 // see the variable declarations for more info.
76 void params_set_node_expansion(real_t p_value) {
77 BVH_LOCKED_FUNCTION
78 if (p_value >= 0.0) {
79 tree._node_expansion = p_value;
80 tree._auto_node_expansion = false;
81 } else {
82 tree._auto_node_expansion = true;
83 }
84 }
85
86 void params_set_pairing_expansion(real_t p_value) {
87 BVH_LOCKED_FUNCTION
88 tree.params_set_pairing_expansion(p_value);
89 }
90
91 void set_pair_callback(PairCallback p_callback, void *p_userdata) {
92 BVH_LOCKED_FUNCTION
93 pair_callback = p_callback;
94 pair_callback_userdata = p_userdata;
95 }
96 void set_unpair_callback(UnpairCallback p_callback, void *p_userdata) {
97 BVH_LOCKED_FUNCTION
98 unpair_callback = p_callback;
99 unpair_callback_userdata = p_userdata;
100 }
101 void set_check_pair_callback(CheckPairCallback p_callback, void *p_userdata) {
102 BVH_LOCKED_FUNCTION
103 check_pair_callback = p_callback;
104 check_pair_callback_userdata = p_userdata;
105 }
106
107 BVHHandle create(T *p_userdata, bool p_active = true, uint32_t p_tree_id = 0, uint32_t p_tree_collision_mask = 1, const BOUNDS &p_aabb = BOUNDS(), int p_subindex = 0) {
108 BVH_LOCKED_FUNCTION
109
110 // not sure if absolutely necessary to flush collisions here. It will cost performance to, instead
111 // of waiting for update, so only uncomment this if there are bugs.
112 if (USE_PAIRS) {
113 //_check_for_collisions();
114 }
115
116 BVHHandle h = tree.item_add(p_userdata, p_active, p_aabb, p_subindex, p_tree_id, p_tree_collision_mask);
117
118 if (USE_PAIRS) {
119 // for safety initialize the expanded AABB
120 BOUNDS &expanded_aabb = tree._pairs[h.id()].expanded_aabb;
121 expanded_aabb = p_aabb;
122 expanded_aabb.grow_by(tree._pairing_expansion);
123
124 // force a collision check no matter the AABB
125 if (p_active) {
126 _add_changed_item(h, p_aabb, false);
127 _check_for_collisions(true);
128 }
129 }
130
131 return h;
132 }
133
134 ////////////////////////////////////////////////////
135 // wrapper versions that use uint32_t instead of handle
136 // for backward compatibility. Less type safe
137 void move(uint32_t p_handle, const BOUNDS &p_aabb) {
138 BVHHandle h;
139 h.set(p_handle);
140 move(h, p_aabb);
141 }
142
143 void recheck_pairs(uint32_t p_handle) {
144 BVHHandle h;
145 h.set(p_handle);
146 recheck_pairs(h);
147 }
148
149 void erase(uint32_t p_handle) {
150 BVHHandle h;
151 h.set(p_handle);
152 erase(h);
153 }
154
155 void force_collision_check(uint32_t p_handle) {
156 BVHHandle h;
157 h.set(p_handle);
158 force_collision_check(h);
159 }
160
161 bool activate(uint32_t p_handle, const BOUNDS &p_aabb, bool p_delay_collision_check = false) {
162 BVHHandle h;
163 h.set(p_handle);
164 return activate(h, p_aabb, p_delay_collision_check);
165 }
166
167 bool deactivate(uint32_t p_handle) {
168 BVHHandle h;
169 h.set(p_handle);
170 return deactivate(h);
171 }
172
173 void set_tree(uint32_t p_handle, uint32_t p_tree_id, uint32_t p_tree_collision_mask, bool p_force_collision_check = true) {
174 BVHHandle h;
175 h.set(p_handle);
176 set_tree(h, p_tree_id, p_tree_collision_mask, p_force_collision_check);
177 }
178
179 uint32_t get_tree_id(uint32_t p_handle) const {
180 BVHHandle h;
181 h.set(p_handle);
182 return item_get_tree_id(h);
183 }
184 int get_subindex(uint32_t p_handle) const {
185 BVHHandle h;
186 h.set(p_handle);
187 return item_get_subindex(h);
188 }
189
190 T *get(uint32_t p_handle) const {
191 BVHHandle h;
192 h.set(p_handle);
193 return item_get_userdata(h);
194 }
195
196 ////////////////////////////////////////////////////
197
198 void move(BVHHandle p_handle, const BOUNDS &p_aabb) {
199 DEV_ASSERT(!p_handle.is_invalid());
200 BVH_LOCKED_FUNCTION
201 if (tree.item_move(p_handle, p_aabb)) {
202 if (USE_PAIRS) {
203 _add_changed_item(p_handle, p_aabb);
204 }
205 }
206 }
207
208 void recheck_pairs(BVHHandle p_handle) {
209 DEV_ASSERT(!p_handle.is_invalid());
210 force_collision_check(p_handle);
211 }
212
213 void erase(BVHHandle p_handle) {
214 DEV_ASSERT(!p_handle.is_invalid());
215 BVH_LOCKED_FUNCTION
216 // call unpair and remove all references to the item
217 // before deleting from the tree
218 if (USE_PAIRS) {
219 _remove_changed_item(p_handle);
220 }
221
222 tree.item_remove(p_handle);
223
224 _check_for_collisions(true);
225 }
226
227 // use in conjunction with activate if you have deferred the collision check, and
228 // set pairable has never been called.
229 // (deferred collision checks are a workaround for visual server for historical reasons)
230 void force_collision_check(BVHHandle p_handle) {
231 DEV_ASSERT(!p_handle.is_invalid());
232 BVH_LOCKED_FUNCTION
233 if (USE_PAIRS) {
234 // the aabb should already be up to date in the BVH
235 BOUNDS aabb;
236 item_get_AABB(p_handle, aabb);
237
238 // add it as changed even if aabb not different
239 _add_changed_item(p_handle, aabb, false);
240
241 // force an immediate full collision check, much like calls to set_pairable
242 _check_for_collisions(true);
243 }
244 }
245
246 // these should be read as set_visible for render trees,
247 // but generically this makes items add or remove from the
248 // tree internally, to speed things up by ignoring inactive items
249 bool activate(BVHHandle p_handle, const BOUNDS &p_aabb, bool p_delay_collision_check = false) {
250 DEV_ASSERT(!p_handle.is_invalid());
251 BVH_LOCKED_FUNCTION
252 // sending the aabb here prevents the need for the BVH to maintain
253 // a redundant copy of the aabb.
254 // returns success
255 if (tree.item_activate(p_handle, p_aabb)) {
256 if (USE_PAIRS) {
257 // in the special case of the render tree, when setting visibility we are using the combination of
258 // activate then set_pairable. This would case 2 sets of collision checks. For efficiency here we allow
259 // deferring to have a single collision check at the set_pairable call.
260 // Watch for bugs! This may cause bugs if set_pairable is not called.
261 if (!p_delay_collision_check) {
262 _add_changed_item(p_handle, p_aabb, false);
263
264 // force an immediate collision check, much like calls to set_pairable
265 _check_for_collisions(true);
266 }
267 }
268 return true;
269 }
270
271 return false;
272 }
273
274 bool deactivate(BVHHandle p_handle) {
275 DEV_ASSERT(!p_handle.is_invalid());
276 BVH_LOCKED_FUNCTION
277 // returns success
278 if (tree.item_deactivate(p_handle)) {
279 // call unpair and remove all references to the item
280 // before deleting from the tree
281 if (USE_PAIRS) {
282 _remove_changed_item(p_handle);
283
284 // force check for collisions, much like an erase was called
285 _check_for_collisions(true);
286 }
287 return true;
288 }
289
290 return false;
291 }
292
293 bool get_active(BVHHandle p_handle) {
294 DEV_ASSERT(!p_handle.is_invalid());
295 BVH_LOCKED_FUNCTION
296 return tree.item_get_active(p_handle);
297 }
298
299 // call e.g. once per frame (this does a trickle optimize)
300 void update() {
301 BVH_LOCKED_FUNCTION
302 tree.update();
303 _check_for_collisions();
304#ifdef BVH_INTEGRITY_CHECKS
305 tree._integrity_check_all();
306#endif
307 }
308
309 // this can be called more frequently than per frame if necessary
310 void update_collisions() {
311 BVH_LOCKED_FUNCTION
312 _check_for_collisions();
313 }
314
315 // prefer calling this directly as type safe
316 void set_tree(const BVHHandle &p_handle, uint32_t p_tree_id, uint32_t p_tree_collision_mask, bool p_force_collision_check = true) {
317 DEV_ASSERT(!p_handle.is_invalid());
318 BVH_LOCKED_FUNCTION
319 // Returns true if the pairing state has changed.
320 bool state_changed = tree.item_set_tree(p_handle, p_tree_id, p_tree_collision_mask);
321
322 if (USE_PAIRS) {
323 // not sure if absolutely necessary to flush collisions here. It will cost performance to, instead
324 // of waiting for update, so only uncomment this if there are bugs.
325 //_check_for_collisions();
326
327 if ((p_force_collision_check || state_changed) && tree.item_get_active(p_handle)) {
328 // when the pairable state changes, we need to force a collision check because newly pairable
329 // items may be in collision, and unpairable items might move out of collision.
330 // We cannot depend on waiting for the next update, because that may come much later.
331 BOUNDS aabb;
332 item_get_AABB(p_handle, aabb);
333
334 // passing false disables the optimization which prevents collision checks if
335 // the aabb hasn't changed
336 _add_changed_item(p_handle, aabb, false);
337
338 // force an immediate collision check (probably just for this one item)
339 // but it must be a FULL collision check, also checking pairable state and masks.
340 // This is because AABB intersecting objects may have changed pairable state / mask
341 // such that they should no longer be paired. E.g. lights.
342 _check_for_collisions(true);
343 } // only if active
344 }
345 }
346
347 // cull tests
348 int cull_aabb(const BOUNDS &p_aabb, T **p_result_array, int p_result_max, const T *p_tester, uint32_t p_tree_collision_mask = 0xFFFFFFFF, int *p_subindex_array = nullptr) {
349 BVH_LOCKED_FUNCTION
350 typename BVHTREE_CLASS::CullParams params;
351
352 params.result_count_overall = 0;
353 params.result_max = p_result_max;
354 params.result_array = p_result_array;
355 params.subindex_array = p_subindex_array;
356 params.tree_collision_mask = p_tree_collision_mask;
357 params.abb.from(p_aabb);
358 params.tester = p_tester;
359
360 tree.cull_aabb(params);
361
362 return params.result_count_overall;
363 }
364
365 int cull_segment(const POINT &p_from, const POINT &p_to, T **p_result_array, int p_result_max, const T *p_tester, uint32_t p_tree_collision_mask = 0xFFFFFFFF, int *p_subindex_array = nullptr) {
366 BVH_LOCKED_FUNCTION
367 typename BVHTREE_CLASS::CullParams params;
368
369 params.result_count_overall = 0;
370 params.result_max = p_result_max;
371 params.result_array = p_result_array;
372 params.subindex_array = p_subindex_array;
373 params.tester = p_tester;
374 params.tree_collision_mask = p_tree_collision_mask;
375
376 params.segment.from = p_from;
377 params.segment.to = p_to;
378
379 tree.cull_segment(params);
380
381 return params.result_count_overall;
382 }
383
384 int cull_point(const POINT &p_point, T **p_result_array, int p_result_max, const T *p_tester, uint32_t p_tree_collision_mask = 0xFFFFFFFF, int *p_subindex_array = nullptr) {
385 BVH_LOCKED_FUNCTION
386 typename BVHTREE_CLASS::CullParams params;
387
388 params.result_count_overall = 0;
389 params.result_max = p_result_max;
390 params.result_array = p_result_array;
391 params.subindex_array = p_subindex_array;
392 params.tester = p_tester;
393 params.tree_collision_mask = p_tree_collision_mask;
394
395 params.point = p_point;
396
397 tree.cull_point(params);
398 return params.result_count_overall;
399 }
400
401 int cull_convex(const Vector<Plane> &p_convex, T **p_result_array, int p_result_max, const T *p_tester, uint32_t p_tree_collision_mask = 0xFFFFFFFF) {
402 BVH_LOCKED_FUNCTION
403 if (!p_convex.size()) {
404 return 0;
405 }
406
407 Vector<Vector3> convex_points = Geometry3D::compute_convex_mesh_points(&p_convex[0], p_convex.size());
408 if (convex_points.size() == 0) {
409 return 0;
410 }
411
412 typename BVHTREE_CLASS::CullParams params;
413 params.result_count_overall = 0;
414 params.result_max = p_result_max;
415 params.result_array = p_result_array;
416 params.subindex_array = nullptr;
417 params.tester = p_tester;
418 params.tree_collision_mask = p_tree_collision_mask;
419
420 params.hull.planes = &p_convex[0];
421 params.hull.num_planes = p_convex.size();
422 params.hull.points = &convex_points[0];
423 params.hull.num_points = convex_points.size();
424
425 tree.cull_convex(params);
426
427 return params.result_count_overall;
428 }
429
430private:
431 // do this after moving etc.
432 void _check_for_collisions(bool p_full_check = false) {
433 if (!changed_items.size()) {
434 // noop
435 return;
436 }
437
438 BOUNDS bb;
439
440 typename BVHTREE_CLASS::CullParams params;
441
442 params.result_count_overall = 0;
443 params.result_max = INT_MAX;
444 params.result_array = nullptr;
445 params.subindex_array = nullptr;
446
447 for (const BVHHandle &h : changed_items) {
448 // use the expanded aabb for pairing
449 const BOUNDS &expanded_aabb = tree._pairs[h.id()].expanded_aabb;
450 BVHABB_CLASS abb;
451 abb.from(expanded_aabb);
452
453 tree.item_fill_cullparams(h, params);
454
455 // find all the existing paired aabbs that are no longer
456 // paired, and send callbacks
457 _find_leavers(h, abb, p_full_check);
458
459 uint32_t changed_item_ref_id = h.id();
460
461 params.abb = abb;
462
463 params.result_count_overall = 0; // might not be needed
464 tree.cull_aabb(params, false);
465
466 for (const uint32_t ref_id : tree._cull_hits) {
467 // don't collide against ourself
468 if (ref_id == changed_item_ref_id) {
469 continue;
470 }
471
472 // checkmasks is already done in the cull routine.
473 BVHHandle h_collidee;
474 h_collidee.set_id(ref_id);
475
476 // find NEW enterers, and send callbacks for them only
477 _collide(h, h_collidee);
478 }
479 }
480 _reset();
481 }
482
483public:
484 void item_get_AABB(BVHHandle p_handle, BOUNDS &r_aabb) {
485 DEV_ASSERT(!p_handle.is_invalid());
486 BVHABB_CLASS abb;
487 tree.item_get_ABB(p_handle, abb);
488 abb.to(r_aabb);
489 }
490
491private:
492 // supplemental funcs
493 uint32_t item_get_tree_id(BVHHandle p_handle) const { return _get_extra(p_handle).tree_id; }
494 T *item_get_userdata(BVHHandle p_handle) const { return _get_extra(p_handle).userdata; }
495 int item_get_subindex(BVHHandle p_handle) const { return _get_extra(p_handle).subindex; }
496
497 void _unpair(BVHHandle p_from, BVHHandle p_to) {
498 tree._handle_sort(p_from, p_to);
499
500 typename BVHTREE_CLASS::ItemExtra &exa = tree._extra[p_from.id()];
501 typename BVHTREE_CLASS::ItemExtra &exb = tree._extra[p_to.id()];
502
503 // if the userdata is the same, no collisions should occur
504 if ((exa.userdata == exb.userdata) && exa.userdata) {
505 return;
506 }
507
508 typename BVHTREE_CLASS::ItemPairs &pairs_from = tree._pairs[p_from.id()];
509 typename BVHTREE_CLASS::ItemPairs &pairs_to = tree._pairs[p_to.id()];
510
511 void *ud_from = pairs_from.remove_pair_to(p_to);
512 pairs_to.remove_pair_to(p_from);
513
514#ifdef BVH_VERBOSE_PAIRING
515 print_line("_unpair " + itos(p_from.id()) + " from " + itos(p_to.id()));
516#endif
517
518 // callback
519 if (unpair_callback) {
520 unpair_callback(pair_callback_userdata, p_from, exa.userdata, exa.subindex, p_to, exb.userdata, exb.subindex, ud_from);
521 }
522 }
523
524 void *_recheck_pair(BVHHandle p_from, BVHHandle p_to, void *p_pair_data) {
525 tree._handle_sort(p_from, p_to);
526
527 typename BVHTREE_CLASS::ItemExtra &exa = tree._extra[p_from.id()];
528 typename BVHTREE_CLASS::ItemExtra &exb = tree._extra[p_to.id()];
529
530 // if the userdata is the same, no collisions should occur
531 if ((exa.userdata == exb.userdata) && exa.userdata) {
532 return p_pair_data;
533 }
534
535 // callback
536 if (check_pair_callback) {
537 return check_pair_callback(check_pair_callback_userdata, p_from, exa.userdata, exa.subindex, p_to, exb.userdata, exb.subindex, p_pair_data);
538 }
539
540 return p_pair_data;
541 }
542
543 // returns true if unpair
544 bool _find_leavers_process_pair(typename BVHTREE_CLASS::ItemPairs &p_pairs_from, const BVHABB_CLASS &p_abb_from, BVHHandle p_from, BVHHandle p_to, bool p_full_check) {
545 BVHABB_CLASS abb_to;
546 tree.item_get_ABB(p_to, abb_to);
547
548 // do they overlap?
549 if (p_abb_from.intersects(abb_to)) {
550 // the full check for pairable / non pairable (i.e. tree_id and tree_masks) and mask changes is extra expense
551 // this need not be done in most cases (for speed) except in the case where set_tree is called
552 // where the masks etc of the objects in question may have changed
553 if (!p_full_check) {
554 return false;
555 }
556 const typename BVHTREE_CLASS::ItemExtra &exa = _get_extra(p_from);
557 const typename BVHTREE_CLASS::ItemExtra &exb = _get_extra(p_to);
558
559 // Checking tree_ids and tree_collision_masks
560 if (exa.are_item_trees_compatible(exb)) {
561 bool pair_allowed = USER_PAIR_TEST_FUNCTION::user_pair_check(exa.userdata, exb.userdata);
562
563 // the masks must still be compatible to pair
564 // i.e. if there is a hit between the two and they intersect, then they should stay paired
565 if (pair_allowed) {
566 return false;
567 }
568 }
569 }
570
571 _unpair(p_from, p_to);
572 return true;
573 }
574
575 // find all the existing paired aabbs that are no longer
576 // paired, and send callbacks
577 void _find_leavers(BVHHandle p_handle, const BVHABB_CLASS &expanded_abb_from, bool p_full_check) {
578 typename BVHTREE_CLASS::ItemPairs &p_from = tree._pairs[p_handle.id()];
579
580 BVHABB_CLASS abb_from = expanded_abb_from;
581
582 // remove from pairing list for every partner
583 for (unsigned int n = 0; n < p_from.extended_pairs.size(); n++) {
584 BVHHandle h_to = p_from.extended_pairs[n].handle;
585 if (_find_leavers_process_pair(p_from, abb_from, p_handle, h_to, p_full_check)) {
586 // we need to keep the counter n up to date if we deleted a pair
587 // as the number of items in p_from.extended_pairs will have decreased by 1
588 // and we don't want to miss an item
589 n--;
590 }
591 }
592 }
593
594 // find NEW enterers, and send callbacks for them only
595 // handle a and b
596 void _collide(BVHHandle p_ha, BVHHandle p_hb) {
597 // only have to do this oneway, lower ID then higher ID
598 tree._handle_sort(p_ha, p_hb);
599
600 const typename BVHTREE_CLASS::ItemExtra &exa = _get_extra(p_ha);
601 const typename BVHTREE_CLASS::ItemExtra &exb = _get_extra(p_hb);
602
603 // user collision callback
604 if (!USER_PAIR_TEST_FUNCTION::user_pair_check(exa.userdata, exb.userdata)) {
605 return;
606 }
607
608 // if the userdata is the same, no collisions should occur
609 if ((exa.userdata == exb.userdata) && exa.userdata) {
610 return;
611 }
612
613 typename BVHTREE_CLASS::ItemPairs &p_from = tree._pairs[p_ha.id()];
614 typename BVHTREE_CLASS::ItemPairs &p_to = tree._pairs[p_hb.id()];
615
616 // does this pair exist already?
617 // or only check the one with lower number of pairs for greater speed
618 if (p_from.num_pairs <= p_to.num_pairs) {
619 if (p_from.contains_pair_to(p_hb)) {
620 return;
621 }
622 } else {
623 if (p_to.contains_pair_to(p_ha)) {
624 return;
625 }
626 }
627
628 // callback
629 void *callback_userdata = nullptr;
630
631#ifdef BVH_VERBOSE_PAIRING
632 print_line("_pair " + itos(p_ha.id()) + " to " + itos(p_hb.id()));
633#endif
634
635 if (pair_callback) {
636 callback_userdata = pair_callback(pair_callback_userdata, p_ha, exa.userdata, exa.subindex, p_hb, exb.userdata, exb.subindex);
637 }
638
639 // new pair! .. only really need to store the userdata on the lower handle, but both have storage so...
640 p_from.add_pair_to(p_hb, callback_userdata);
641 p_to.add_pair_to(p_ha, callback_userdata);
642 }
643
644 // if we remove an item, we need to immediately remove the pairs, to prevent reading the pair after deletion
645 void _remove_pairs_containing(BVHHandle p_handle) {
646 typename BVHTREE_CLASS::ItemPairs &p_from = tree._pairs[p_handle.id()];
647
648 // remove from pairing list for every partner.
649 // can't easily use a for loop here, because removing changes the size of the list
650 while (p_from.extended_pairs.size()) {
651 BVHHandle h_to = p_from.extended_pairs[0].handle;
652 _unpair(p_handle, h_to);
653 }
654 }
655
656 // Send pair callbacks again for all existing pairs for the given handle.
657 void _recheck_pairs(BVHHandle p_handle) {
658 typename BVHTREE_CLASS::ItemPairs &from = tree._pairs[p_handle.id()];
659
660 // checking pair for every partner.
661 for (unsigned int n = 0; n < from.extended_pairs.size(); n++) {
662 typename BVHTREE_CLASS::ItemPairs::Link &pair = from.extended_pairs[n];
663 BVHHandle h_to = pair.handle;
664 void *new_pair_data = _recheck_pair(p_handle, h_to, pair.userdata);
665
666 if (new_pair_data != pair.userdata) {
667 pair.userdata = new_pair_data;
668
669 // Update pair data for the second item.
670 typename BVHTREE_CLASS::ItemPairs &to = tree._pairs[h_to.id()];
671 for (unsigned int to_index = 0; to_index < to.extended_pairs.size(); to_index++) {
672 typename BVHTREE_CLASS::ItemPairs::Link &to_pair = to.extended_pairs[to_index];
673 if (to_pair.handle == p_handle) {
674 to_pair.userdata = new_pair_data;
675 break;
676 }
677 }
678 }
679 }
680 }
681
682private:
683 const typename BVHTREE_CLASS::ItemExtra &_get_extra(BVHHandle p_handle) const {
684 return tree._extra[p_handle.id()];
685 }
686 const typename BVHTREE_CLASS::ItemRef &_get_ref(BVHHandle p_handle) const {
687 return tree._refs[p_handle.id()];
688 }
689
690 void _reset() {
691 changed_items.clear();
692 _tick++;
693 }
694
695 void _add_changed_item(BVHHandle p_handle, const BOUNDS &aabb, bool p_check_aabb = true) {
696 // Note that non pairable items can pair with pairable,
697 // so all types must be added to the list
698
699#ifdef BVH_EXPAND_LEAF_AABBS
700 // if using expanded AABB in the leaf, the redundancy check will already have been made
701 BOUNDS &expanded_aabb = tree._pairs[p_handle.id()].expanded_aabb;
702 item_get_AABB(p_handle, expanded_aabb);
703#else
704 // aabb check with expanded aabb. This greatly decreases processing
705 // at the cost of slightly less accurate pairing checks
706 // Note this pairing AABB is separate from the AABB in the actual tree
707 BOUNDS &expanded_aabb = tree._pairs[p_handle.id()].expanded_aabb;
708
709 // passing p_check_aabb false disables the optimization which prevents collision checks if
710 // the aabb hasn't changed. This is needed where set_pairable has been called, but the position
711 // has not changed.
712 if (p_check_aabb && tree.expanded_aabb_encloses_not_shrink(expanded_aabb, aabb)) {
713 return;
714 }
715
716 // ALWAYS update the new expanded aabb, even if already changed once
717 // this tick, because it is vital that the AABB is kept up to date
718 expanded_aabb = aabb;
719 expanded_aabb.grow_by(tree._pairing_expansion);
720#endif
721
722 // this code is to ensure that changed items only appear once on the updated list
723 // collision checking them multiple times is not needed, and repeats the same thing
724 uint32_t &last_updated_tick = tree._extra[p_handle.id()].last_updated_tick;
725
726 if (last_updated_tick == _tick) {
727 return; // already on changed list
728 }
729
730 // mark as on list
731 last_updated_tick = _tick;
732
733 // add to the list
734 changed_items.push_back(p_handle);
735 }
736
737 void _remove_changed_item(BVHHandle p_handle) {
738 // Care has to be taken here for items that are deleted. The ref ID
739 // could be reused on the same tick for new items. This is probably
740 // rare but should be taken into consideration
741
742 // callbacks
743 _remove_pairs_containing(p_handle);
744
745 // remove from changed items (not very efficient yet)
746 for (int n = 0; n < (int)changed_items.size(); n++) {
747 if (changed_items[n] == p_handle) {
748 changed_items.remove_at_unordered(n);
749
750 // because we are using an unordered remove,
751 // the last changed item will now be at spot 'n',
752 // and we need to redo it, so we prevent moving on to
753 // the next n at the next for iteration.
754 n--;
755 }
756 }
757
758 // reset the last updated tick (may not be necessary but just in case)
759 tree._extra[p_handle.id()].last_updated_tick = 0;
760 }
761
762 PairCallback pair_callback = nullptr;
763 UnpairCallback unpair_callback = nullptr;
764 CheckPairCallback check_pair_callback = nullptr;
765 void *pair_callback_userdata = nullptr;
766 void *unpair_callback_userdata = nullptr;
767 void *check_pair_callback_userdata = nullptr;
768
769 BVHTREE_CLASS tree;
770
771 // for collision pairing,
772 // maintain a list of all items moved etc on each frame / tick
773 LocalVector<BVHHandle, uint32_t, true> changed_items;
774 uint32_t _tick = 1; // Start from 1 so items with 0 indicate never updated.
775
776 class BVHLockedFunction {
777 public:
778 BVHLockedFunction(Mutex *p_mutex, bool p_thread_safe) {
779 // will be compiled out if not set in template
780 if (p_thread_safe) {
781 _mutex = p_mutex;
782 _mutex->lock();
783
784 } else {
785 _mutex = nullptr;
786 }
787 }
788 ~BVHLockedFunction() {
789 // will be compiled out if not set in template
790 if (_mutex) {
791 _mutex->unlock();
792 }
793 }
794
795 private:
796 Mutex *_mutex = nullptr;
797 };
798
799 Mutex _mutex;
800
801 // local toggle for turning on and off thread safety in project settings
802 bool _thread_safe = BVH_THREAD_SAFE;
803
804public:
805 BVH_Manager() {}
806};
807
808#undef BVHTREE_CLASS
809
810#endif // BVH_H
811