Merge pull request #48629 from nekomatata/dynamic-bvh-broadphase-4.0
Dynamic BVH broadphase in 2D & 3D Godot Physics
This commit is contained in:
Rémi Verschelde
GitHub
commit
5f33951009
|
|
@ -0,0 +1,695 @@
|
|||
/*************************************************************************/
|
||||
/* bvh.h */
|
||||
/*************************************************************************/
|
||||
/* 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. */
|
||||
/*************************************************************************/
|
||||
|
||||
#ifndef BVH_H
|
||||
#define BVH_H
|
||||
|
||||
// BVH
|
||||
// This class provides a wrapper around BVH tree, which contains most of the functionality
|
||||
// for a dynamic BVH with templated leaf size.
|
||||
// However BVH also adds facilities for pairing, to maintain compatibility with Godot 3.2.
|
||||
// Pairing is a collision pairing system, on top of the basic BVH.
|
||||
|
||||
// Some notes on the use of BVH / Octree from Godot 3.2.
|
||||
// This is not well explained elsewhere.
|
||||
// The rendering tree mask and types that are sent to the BVH are NOT layer masks.
|
||||
// They are INSTANCE_TYPES (defined in visual_server.h), e.g. MESH, MULTIMESH, PARTICLES etc.
|
||||
// Thus the lights do no cull by layer mask in the BVH.
|
||||
|
||||
// Layer masks are implemented in the renderers as a later step, and light_cull_mask appears to be
|
||||
// implemented in GLES3 but not GLES2. Layer masks are not yet implemented for directional lights.
|
||||
|
||||
#include "bvh_tree.h"
|
||||
|
||||
#define BVHTREE_CLASS BVH_Tree<T, 2, MAX_ITEMS, USE_PAIRS, Bounds, Point>
|
||||
|
||||
template <class T, bool USE_PAIRS = false, int MAX_ITEMS = 32, class Bounds = AABB, class Point = Vector3>
|
||||
class BVH_Manager {
|
||||
public:
|
||||
// note we are using uint32_t instead of BVHHandle, losing type safety, but this
|
||||
// is for compatibility with octree
|
||||
typedef void *(*PairCallback)(void *, uint32_t, T *, int, uint32_t, T *, int);
|
||||
typedef void (*UnpairCallback)(void *, uint32_t, T *, int, uint32_t, T *, int, void *);
|
||||
|
||||
// these 2 are crucial for fine tuning, and can be applied manually
|
||||
// see the variable declarations for more info.
|
||||
void params_set_node_expansion(real_t p_value) {
|
||||
if (p_value >= 0.0) {
|
||||
tree._node_expansion = p_value;
|
||||
tree._auto_node_expansion = false;
|
||||
} else {
|
||||
tree._auto_node_expansion = true;
|
||||
}
|
||||
}
|
||||
|
||||
void params_set_pairing_expansion(real_t p_value) {
|
||||
if (p_value >= 0.0) {
|
||||
tree._pairing_expansion = p_value;
|
||||
tree._auto_pairing_expansion = false;
|
||||
} else {
|
||||
tree._auto_pairing_expansion = true;
|
||||
}
|
||||
}
|
||||
|
||||
void set_pair_callback(PairCallback p_callback, void *p_userdata) {
|
||||
pair_callback = p_callback;
|
||||
pair_callback_userdata = p_userdata;
|
||||
}
|
||||
void set_unpair_callback(UnpairCallback p_callback, void *p_userdata) {
|
||||
unpair_callback = p_callback;
|
||||
unpair_callback_userdata = p_userdata;
|
||||
}
|
||||
|
||||
BVHHandle create(T *p_userdata, bool p_active, const Bounds &p_aabb = Bounds(), int p_subindex = 0, bool p_pairable = false, uint32_t p_pairable_type = 0, uint32_t p_pairable_mask = 1) {
|
||||
// not sure if absolutely necessary to flush collisions here. It will cost performance to, instead
|
||||
// of waiting for update, so only uncomment this if there are bugs.
|
||||
if (USE_PAIRS) {
|
||||
//_check_for_collisions();
|
||||
}
|
||||
|
||||
#ifdef TOOLS_ENABLED
|
||||
if (!USE_PAIRS) {
|
||||
if (p_pairable) {
|
||||
WARN_PRINT_ONCE("creating pairable item in BVH with USE_PAIRS set to false");
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
BVHHandle h = tree.item_add(p_userdata, p_active, p_aabb, p_subindex, p_pairable, p_pairable_type, p_pairable_mask);
|
||||
|
||||
if (USE_PAIRS) {
|
||||
// for safety initialize the expanded AABB
|
||||
Bounds &expanded_aabb = tree._pairs[h.id()].expanded_aabb;
|
||||
expanded_aabb = p_aabb;
|
||||
expanded_aabb.grow_by(tree._pairing_expansion);
|
||||
|
||||
// force a collision check no matter the AABB
|
||||
if (p_active) {
|
||||
_add_changed_item(h, p_aabb, false);
|
||||
_check_for_collisions(true);
|
||||
}
|
||||
}
|
||||
|
||||
return h;
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////
|
||||
// wrapper versions that use uint32_t instead of handle
|
||||
// for backward compatibility. Less type safe
|
||||
void move(uint32_t p_handle, const Bounds &p_aabb) {
|
||||
BVHHandle h;
|
||||
h.set(p_handle);
|
||||
move(h, p_aabb);
|
||||
}
|
||||
|
||||
void erase(uint32_t p_handle) {
|
||||
BVHHandle h;
|
||||
h.set(p_handle);
|
||||
erase(h);
|
||||
}
|
||||
|
||||
void force_collision_check(uint32_t p_handle) {
|
||||
BVHHandle h;
|
||||
h.set(p_handle);
|
||||
force_collision_check(h);
|
||||
}
|
||||
|
||||
bool activate(uint32_t p_handle, const Bounds &p_aabb, bool p_delay_collision_check = false) {
|
||||
BVHHandle h;
|
||||
h.set(p_handle);
|
||||
return activate(h, p_aabb, p_delay_collision_check);
|
||||
}
|
||||
|
||||
bool deactivate(uint32_t p_handle) {
|
||||
BVHHandle h;
|
||||
h.set(p_handle);
|
||||
return deactivate(h);
|
||||
}
|
||||
|
||||
void set_pairable(uint32_t p_handle, bool p_pairable, uint32_t p_pairable_type, uint32_t p_pairable_mask, bool p_force_collision_check = true) {
|
||||
BVHHandle h;
|
||||
h.set(p_handle);
|
||||
set_pairable(h, p_pairable, p_pairable_type, p_pairable_mask, p_force_collision_check);
|
||||
}
|
||||
|
||||
bool is_pairable(uint32_t p_handle) const {
|
||||
BVHHandle h;
|
||||
h.set(p_handle);
|
||||
return item_is_pairable(h);
|
||||
}
|
||||
int get_subindex(uint32_t p_handle) const {
|
||||
BVHHandle h;
|
||||
h.set(p_handle);
|
||||
return item_get_subindex(h);
|
||||
}
|
||||
|
||||
T *get(uint32_t p_handle) const {
|
||||
BVHHandle h;
|
||||
h.set(p_handle);
|
||||
return item_get_userdata(h);
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////
|
||||
|
||||
void move(BVHHandle p_handle, const Bounds &p_aabb) {
|
||||
if (tree.item_move(p_handle, p_aabb)) {
|
||||
if (USE_PAIRS) {
|
||||
_add_changed_item(p_handle, p_aabb);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void erase(BVHHandle p_handle) {
|
||||
// call unpair and remove all references to the item
|
||||
// before deleting from the tree
|
||||
if (USE_PAIRS) {
|
||||
_remove_changed_item(p_handle);
|
||||
}
|
||||
|
||||
tree.item_remove(p_handle);
|
||||
|
||||
_check_for_collisions(true);
|
||||
}
|
||||
|
||||
// use in conjunction with activate if you have deferred the collision check, and
|
||||
// set pairable has never been called.
|
||||
// (deferred collision checks are a workaround for visual server for historical reasons)
|
||||
void force_collision_check(BVHHandle p_handle) {
|
||||
if (USE_PAIRS) {
|
||||
// the aabb should already be up to date in the BVH
|
||||
Bounds aabb;
|
||||
item_get_AABB(p_handle, aabb);
|
||||
|
||||
// add it as changed even if aabb not different
|
||||
_add_changed_item(p_handle, aabb, false);
|
||||
|
||||
// force an immediate full collision check, much like calls to set_pairable
|
||||
_check_for_collisions(true);
|
||||
}
|
||||
}
|
||||
|
||||
// these should be read as set_visible for render trees,
|
||||
// but generically this makes items add or remove from the
|
||||
// tree internally, to speed things up by ignoring inactive items
|
||||
bool activate(BVHHandle p_handle, const Bounds &p_aabb, bool p_delay_collision_check = false) {
|
||||
// sending the aabb here prevents the need for the BVH to maintain
|
||||
// a redundant copy of the aabb.
|
||||
// returns success
|
||||
if (tree.item_activate(p_handle, p_aabb)) {
|
||||
if (USE_PAIRS) {
|
||||
// in the special case of the render tree, when setting visibility we are using the combination of
|
||||
// activate then set_pairable. This would case 2 sets of collision checks. For efficiency here we allow
|
||||
// deferring to have a single collision check at the set_pairable call.
|
||||
// Watch for bugs! This may cause bugs if set_pairable is not called.
|
||||
if (!p_delay_collision_check) {
|
||||
_add_changed_item(p_handle, p_aabb, false);
|
||||
|
||||
// force an immediate collision check, much like calls to set_pairable
|
||||
_check_for_collisions(true);
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
bool deactivate(BVHHandle p_handle) {
|
||||
// returns success
|
||||
if (tree.item_deactivate(p_handle)) {
|
||||
// call unpair and remove all references to the item
|
||||
// before deleting from the tree
|
||||
if (USE_PAIRS) {
|
||||
_remove_changed_item(p_handle);
|
||||
|
||||
// force check for collisions, much like an erase was called
|
||||
_check_for_collisions(true);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
bool get_active(BVHHandle p_handle) const {
|
||||
return tree.item_get_active(p_handle);
|
||||
}
|
||||
|
||||
// call e.g. once per frame (this does a trickle optimize)
|
||||
void update() {
|
||||
tree.update();
|
||||
_check_for_collisions();
|
||||
#ifdef BVH_INTEGRITY_CHECKS
|
||||
tree.integrity_check_all();
|
||||
#endif
|
||||
}
|
||||
|
||||
// this can be called more frequently than per frame if necessary
|
||||
void update_collisions() {
|
||||
_check_for_collisions();
|
||||
}
|
||||
|
||||
// prefer calling this directly as type safe
|
||||
void set_pairable(const BVHHandle &p_handle, bool p_pairable, uint32_t p_pairable_type, uint32_t p_pairable_mask, bool p_force_collision_check = true) {
|
||||
// Returns true if the pairing state has changed.
|
||||
bool state_changed = tree.item_set_pairable(p_handle, p_pairable, p_pairable_type, p_pairable_mask);
|
||||
|
||||
if (USE_PAIRS) {
|
||||
// not sure if absolutely necessary to flush collisions here. It will cost performance to, instead
|
||||
// of waiting for update, so only uncomment this if there are bugs.
|
||||
//_check_for_collisions();
|
||||
|
||||
if ((p_force_collision_check || state_changed) && get_active(p_handle)) {
|
||||
// when the pairable state changes, we need to force a collision check because newly pairable
|
||||
// items may be in collision, and unpairable items might move out of collision.
|
||||
// We cannot depend on waiting for the next update, because that may come much later.
|
||||
Bounds aabb;
|
||||
item_get_AABB(p_handle, aabb);
|
||||
|
||||
// passing false disables the optimization which prevents collision checks if
|
||||
// the aabb hasn't changed
|
||||
_add_changed_item(p_handle, aabb, false);
|
||||
|
||||
// force an immediate collision check (probably just for this one item)
|
||||
// but it must be a FULL collision check, also checking pairable state and masks.
|
||||
// This is because AABB intersecting objects may have changed pairable state / mask
|
||||
// such that they should no longer be paired. E.g. lights.
|
||||
_check_for_collisions(true);
|
||||
} // only if active
|
||||
}
|
||||
}
|
||||
|
||||
// cull tests
|
||||
int cull_aabb(const Bounds &p_aabb, T **p_result_array, int p_result_max, int *p_subindex_array = nullptr, uint32_t p_mask = 0xFFFFFFFF) {
|
||||
typename BVHTREE_CLASS::CullParams params;
|
||||
|
||||
params.result_count_overall = 0;
|
||||
params.result_max = p_result_max;
|
||||
params.result_array = p_result_array;
|
||||
params.subindex_array = p_subindex_array;
|
||||
params.mask = p_mask;
|
||||
params.pairable_type = 0;
|
||||
params.test_pairable_only = false;
|
||||
params.abb.from(p_aabb);
|
||||
|
||||
tree.cull_aabb(params);
|
||||
|
||||
return params.result_count_overall;
|
||||
}
|
||||
|
||||
int cull_segment(const Point &p_from, const Point &p_to, T **p_result_array, int p_result_max, int *p_subindex_array = nullptr, uint32_t p_mask = 0xFFFFFFFF) {
|
||||
typename BVHTREE_CLASS::CullParams params;
|
||||
|
||||
params.result_count_overall = 0;
|
||||
params.result_max = p_result_max;
|
||||
params.result_array = p_result_array;
|
||||
params.subindex_array = p_subindex_array;
|
||||
params.mask = p_mask;
|
||||
params.pairable_type = 0;
|
||||
|
||||
params.segment.from = p_from;
|
||||
params.segment.to = p_to;
|
||||
|
||||
tree.cull_segment(params);
|
||||
|
||||
return params.result_count_overall;
|
||||
}
|
||||
|
||||
int cull_point(const Point &p_point, T **p_result_array, int p_result_max, int *p_subindex_array = nullptr, uint32_t p_mask = 0xFFFFFFFF) {
|
||||
typename BVHTREE_CLASS::CullParams params;
|
||||
|
||||
params.result_count_overall = 0;
|
||||
params.result_max = p_result_max;
|
||||
params.result_array = p_result_array;
|
||||
params.subindex_array = p_subindex_array;
|
||||
params.mask = p_mask;
|
||||
params.pairable_type = 0;
|
||||
|
||||
params.point = p_point;
|
||||
|
||||
tree.cull_point(params);
|
||||
return params.result_count_overall;
|
||||
}
|
||||
|
||||
int cull_convex(const Vector<Plane> &p_convex, T **p_result_array, int p_result_max, uint32_t p_mask = 0xFFFFFFFF) {
|
||||
if (!p_convex.size()) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
Vector<Vector3> convex_points = Geometry3D::compute_convex_mesh_points(&p_convex[0], p_convex.size());
|
||||
if (convex_points.size() == 0) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
typename BVHTREE_CLASS::CullParams params;
|
||||
params.result_count_overall = 0;
|
||||
params.result_max = p_result_max;
|
||||
params.result_array = p_result_array;
|
||||
params.subindex_array = nullptr;
|
||||
params.mask = p_mask;
|
||||
params.pairable_type = 0;
|
||||
|
||||
params.hull.planes = &p_convex[0];
|
||||
params.hull.num_planes = p_convex.size();
|
||||
params.hull.points = &convex_points[0];
|
||||
params.hull.num_points = convex_points.size();
|
||||
|
||||
tree.cull_convex(params);
|
||||
|
||||
return params.result_count_overall;
|
||||
}
|
||||
|
||||
private:
|
||||
// do this after moving etc.
|
||||
void _check_for_collisions(bool p_full_check = false) {
|
||||
if (!changed_items.size()) {
|
||||
// noop
|
||||
return;
|
||||
}
|
||||
|
||||
Bounds bb;
|
||||
|
||||
typename BVHTREE_CLASS::CullParams params;
|
||||
|
||||
params.result_count_overall = 0;
|
||||
params.result_max = INT_MAX;
|
||||
params.result_array = nullptr;
|
||||
params.subindex_array = nullptr;
|
||||
params.mask = 0xFFFFFFFF;
|
||||
params.pairable_type = 0;
|
||||
|
||||
for (unsigned int n = 0; n < changed_items.size(); n++) {
|
||||
const BVHHandle &h = changed_items[n];
|
||||
|
||||
// use the expanded aabb for pairing
|
||||
const Bounds &expanded_aabb = tree._pairs[h.id()].expanded_aabb;
|
||||
BVHABB_CLASS abb;
|
||||
abb.from(expanded_aabb);
|
||||
|
||||
// find all the existing paired aabbs that are no longer
|
||||
// paired, and send callbacks
|
||||
_find_leavers(h, abb, p_full_check);
|
||||
|
||||
uint32_t changed_item_ref_id = h.id();
|
||||
|
||||
// set up the test from this item.
|
||||
// this includes whether to test the non pairable tree,
|
||||
// and the item mask.
|
||||
tree.item_fill_cullparams(h, params);
|
||||
|
||||
params.abb = abb;
|
||||
|
||||
params.result_count_overall = 0; // might not be needed
|
||||
tree.cull_aabb(params, false);
|
||||
|
||||
for (unsigned int i = 0; i < tree._cull_hits.size(); i++) {
|
||||
uint32_t ref_id = tree._cull_hits[i];
|
||||
|
||||
// don't collide against ourself
|
||||
if (ref_id == changed_item_ref_id) {
|
||||
continue;
|
||||
}
|
||||
|
||||
#ifdef BVH_CHECKS
|
||||
// if neither are pairable, they should ignore each other
|
||||
// THIS SHOULD NEVER HAPPEN .. now we only test the pairable tree
|
||||
// if the changed item is not pairable
|
||||
CRASH_COND(params.test_pairable_only && !tree._extra[ref_id].pairable);
|
||||
#endif
|
||||
|
||||
// checkmasks is already done in the cull routine.
|
||||
BVHHandle h_collidee;
|
||||
h_collidee.set_id(ref_id);
|
||||
|
||||
// find NEW enterers, and send callbacks for them only
|
||||
_collide(h, h_collidee);
|
||||
}
|
||||
}
|
||||
_reset();
|
||||
}
|
||||
|
||||
public:
|
||||
void item_get_AABB(BVHHandle p_handle, Bounds &r_aabb) {
|
||||
BVHABB_CLASS abb;
|
||||
tree.item_get_ABB(p_handle, abb);
|
||||
abb.to(r_aabb);
|
||||
}
|
||||
|
||||
private:
|
||||
// supplemental funcs
|
||||
bool item_is_pairable(BVHHandle p_handle) const { return _get_extra(p_handle).pairable; }
|
||||
T *item_get_userdata(BVHHandle p_handle) const { return _get_extra(p_handle).userdata; }
|
||||
int item_get_subindex(BVHHandle p_handle) const { return _get_extra(p_handle).subindex; }
|
||||
|
||||
void _unpair(BVHHandle p_from, BVHHandle p_to) {
|
||||
tree._handle_sort(p_from, p_to);
|
||||
|
||||
typename BVHTREE_CLASS::ItemExtra &exa = tree._extra[p_from.id()];
|
||||
typename BVHTREE_CLASS::ItemExtra &exb = tree._extra[p_to.id()];
|
||||
|
||||
// if the userdata is the same, no collisions should occur
|
||||
if ((exa.userdata == exb.userdata) && exa.userdata) {
|
||||
return;
|
||||
}
|
||||
|
||||
typename BVHTREE_CLASS::ItemPairs &pairs_from = tree._pairs[p_from.id()];
|
||||
typename BVHTREE_CLASS::ItemPairs &pairs_to = tree._pairs[p_to.id()];
|
||||
|
||||
void *ud_from = pairs_from.remove_pair_to(p_to);
|
||||
pairs_to.remove_pair_to(p_from);
|
||||
|
||||
// callback
|
||||
if (unpair_callback) {
|
||||
unpair_callback(pair_callback_userdata, p_from, exa.userdata, exa.subindex, p_to, exb.userdata, exb.subindex, ud_from);
|
||||
}
|
||||
}
|
||||
|
||||
// returns true if unpair
|
||||
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) {
|
||||
BVHABB_CLASS abb_to;
|
||||
tree.item_get_ABB(p_to, abb_to);
|
||||
|
||||
// do they overlap?
|
||||
if (p_abb_from.intersects(abb_to)) {
|
||||
// the full check for pairable / non pairable and mask changes is extra expense
|
||||
// this need not be done in most cases (for speed) except in the case where set_pairable is called
|
||||
// where the masks etc of the objects in question may have changed
|
||||
if (!p_full_check) {
|
||||
return false;
|
||||
}
|
||||
const typename BVHTREE_CLASS::ItemExtra &exa = _get_extra(p_from);
|
||||
const typename BVHTREE_CLASS::ItemExtra &exb = _get_extra(p_to);
|
||||
|
||||
// one of the two must be pairable to still pair
|
||||
// if neither are pairable, we always unpair
|
||||
if (exa.pairable || exb.pairable) {
|
||||
// the masks must still be compatible to pair
|
||||
// i.e. if there is a hit between the two, then they should stay paired
|
||||
if (tree._cull_pairing_mask_test_hit(exa.pairable_mask, exa.pairable_type, exb.pairable_mask, exb.pairable_type)) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
_unpair(p_from, p_to);
|
||||
return true;
|
||||
}
|
||||
|
||||
// find all the existing paired aabbs that are no longer
|
||||
// paired, and send callbacks
|
||||
void _find_leavers(BVHHandle p_handle, const BVHABB_CLASS &expanded_abb_from, bool p_full_check) {
|
||||
typename BVHTREE_CLASS::ItemPairs &p_from = tree._pairs[p_handle.id()];
|
||||
|
||||
BVHABB_CLASS abb_from = expanded_abb_from;
|
||||
|
||||
// remove from pairing list for every partner
|
||||
for (unsigned int n = 0; n < p_from.extended_pairs.size(); n++) {
|
||||
BVHHandle h_to = p_from.extended_pairs[n].handle;
|
||||
if (_find_leavers_process_pair(p_from, abb_from, p_handle, h_to, p_full_check)) {
|
||||
// we need to keep the counter n up to date if we deleted a pair
|
||||
// as the number of items in p_from.extended_pairs will have decreased by 1
|
||||
// and we don't want to miss an item
|
||||
n--;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// find NEW enterers, and send callbacks for them only
|
||||
// handle a and b
|
||||
void _collide(BVHHandle p_ha, BVHHandle p_hb) {
|
||||
// only have to do this oneway, lower ID then higher ID
|
||||
tree._handle_sort(p_ha, p_hb);
|
||||
|
||||
const typename BVHTREE_CLASS::ItemExtra &exa = _get_extra(p_ha);
|
||||
const typename BVHTREE_CLASS::ItemExtra &exb = _get_extra(p_hb);
|
||||
|
||||
// if the userdata is the same, no collisions should occur
|
||||
if ((exa.userdata == exb.userdata) && exa.userdata) {
|
||||
return;
|
||||
}
|
||||
|
||||
typename BVHTREE_CLASS::ItemPairs &p_from = tree._pairs[p_ha.id()];
|
||||
typename BVHTREE_CLASS::ItemPairs &p_to = tree._pairs[p_hb.id()];
|
||||
|
||||
// does this pair exist already?
|
||||
// or only check the one with lower number of pairs for greater speed
|
||||
if (p_from.num_pairs <= p_to.num_pairs) {
|
||||
if (p_from.contains_pair_to(p_hb)) {
|
||||
return;
|
||||
}
|
||||
} else {
|
||||
if (p_to.contains_pair_to(p_ha)) {
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
// callback
|
||||
void *callback_userdata = nullptr;
|
||||
|
||||
if (pair_callback) {
|
||||
callback_userdata = pair_callback(pair_callback_userdata, p_ha, exa.userdata, exa.subindex, p_hb, exb.userdata, exb.subindex);
|
||||
}
|
||||
|
||||
// new pair! .. only really need to store the userdata on the lower handle, but both have storage so...
|
||||
p_from.add_pair_to(p_hb, callback_userdata);
|
||||
p_to.add_pair_to(p_ha, callback_userdata);
|
||||
}
|
||||
|
||||
// if we remove an item, we need to immediately remove the pairs, to prevent reading the pair after deletion
|
||||
void _remove_pairs_containing(BVHHandle p_handle) {
|
||||
typename BVHTREE_CLASS::ItemPairs &p_from = tree._pairs[p_handle.id()];
|
||||
|
||||
// remove from pairing list for every partner.
|
||||
// can't easily use a for loop here, because removing changes the size of the list
|
||||
while (p_from.extended_pairs.size()) {
|
||||
BVHHandle h_to = p_from.extended_pairs[0].handle;
|
||||
_unpair(p_handle, h_to);
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
const typename BVHTREE_CLASS::ItemExtra &_get_extra(BVHHandle p_handle) const {
|
||||
return tree._extra[p_handle.id()];
|
||||
}
|
||||
const typename BVHTREE_CLASS::ItemRef &_get_ref(BVHHandle p_handle) const {
|
||||
return tree._refs[p_handle.id()];
|
||||
}
|
||||
|
||||
void _reset() {
|
||||
changed_items.clear();
|
||||
_tick++;
|
||||
}
|
||||
|
||||
void _add_changed_item(BVHHandle p_handle, const Bounds &aabb, bool p_check_aabb = true) {
|
||||
// Note that non pairable items can pair with pairable,
|
||||
// so all types must be added to the list
|
||||
|
||||
// aabb check with expanded aabb. This greatly decreases processing
|
||||
// at the cost of slightly less accurate pairing checks
|
||||
// Note this pairing AABB is separate from the AABB in the actual tree
|
||||
Bounds &expanded_aabb = tree._pairs[p_handle.id()].expanded_aabb;
|
||||
|
||||
// passing p_check_aabb false disables the optimization which prevents collision checks if
|
||||
// the aabb hasn't changed. This is needed where set_pairable has been called, but the position
|
||||
// has not changed.
|
||||
if (p_check_aabb && expanded_aabb.encloses(aabb)) {
|
||||
return;
|
||||
}
|
||||
|
||||
// ALWAYS update the new expanded aabb, even if already changed once
|
||||
// this tick, because it is vital that the AABB is kept up to date
|
||||
expanded_aabb = aabb;
|
||||
expanded_aabb.grow_by(tree._pairing_expansion);
|
||||
|
||||
// this code is to ensure that changed items only appear once on the updated list
|
||||
// collision checking them multiple times is not needed, and repeats the same thing
|
||||
uint32_t &last_updated_tick = tree._extra[p_handle.id()].last_updated_tick;
|
||||
|
||||
if (last_updated_tick == _tick) {
|
||||
return; // already on changed list
|
||||
}
|
||||
|
||||
// mark as on list
|
||||
last_updated_tick = _tick;
|
||||
|
||||
// add to the list
|
||||
changed_items.push_back(p_handle);
|
||||
}
|
||||
|
||||
void _remove_changed_item(BVHHandle p_handle) {
|
||||
// Care has to be taken here for items that are deleted. The ref ID
|
||||
// could be reused on the same tick for new items. This is probably
|
||||
// rare but should be taken into consideration
|
||||
|
||||
// callbacks
|
||||
_remove_pairs_containing(p_handle);
|
||||
|
||||
// remove from changed items (not very efficient yet)
|
||||
for (int n = 0; n < (int)changed_items.size(); n++) {
|
||||
if (changed_items[n] == p_handle) {
|
||||
changed_items.remove_unordered(n);
|
||||
|
||||
// because we are using an unordered remove,
|
||||
// the last changed item will now be at spot 'n',
|
||||
// and we need to redo it, so we prevent moving on to
|
||||
// the next n at the next for iteration.
|
||||
n--;
|
||||
}
|
||||
}
|
||||
|
||||
// reset the last updated tick (may not be necessary but just in case)
|
||||
tree._extra[p_handle.id()].last_updated_tick = 0;
|
||||
}
|
||||
|
||||
PairCallback pair_callback;
|
||||
UnpairCallback unpair_callback;
|
||||
void *pair_callback_userdata;
|
||||
void *unpair_callback_userdata;
|
||||
|
||||
BVHTREE_CLASS tree;
|
||||
|
||||
// for collision pairing,
|
||||
// maintain a list of all items moved etc on each frame / tick
|
||||
LocalVector<BVHHandle, uint32_t, true> changed_items;
|
||||
uint32_t _tick;
|
||||
|
||||
public:
|
||||
BVH_Manager() {
|
||||
_tick = 1; // start from 1 so items with 0 indicate never updated
|
||||
pair_callback = nullptr;
|
||||
unpair_callback = nullptr;
|
||||
pair_callback_userdata = nullptr;
|
||||
unpair_callback_userdata = nullptr;
|
||||
}
|
||||
};
|
||||
|
||||
#undef BVHTREE_CLASS
|
||||
|
||||
#endif // BVH_H
|
||||
|
|
@ -0,0 +1,276 @@
|
|||
/*************************************************************************/
|
||||
/* bvh_abb.h */
|
||||
/*************************************************************************/
|
||||
/* 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. */
|
||||
/*************************************************************************/
|
||||
|
||||
#ifndef BVH_ABB_H
|
||||
#define BVH_ABB_H
|
||||
|
||||
// special optimized version of axis aligned bounding box
|
||||
template <class Bounds = AABB, class Point = Vector3>
|
||||
struct BVH_ABB {
|
||||
struct ConvexHull {
|
||||
// convex hulls (optional)
|
||||
const Plane *planes;
|
||||
int num_planes;
|
||||
const Vector3 *points;
|
||||
int num_points;
|
||||
};
|
||||
|
||||
struct Segment {
|
||||
Point from;
|
||||
Point to;
|
||||
};
|
||||
|
||||
enum IntersectResult {
|
||||
IR_MISS = 0,
|
||||
IR_PARTIAL,
|
||||
IR_FULL,
|
||||
};
|
||||
|
||||
// we store mins with a negative value in order to test them with SIMD
|
||||
Point min;
|
||||
Point neg_max;
|
||||
|
||||
bool operator==(const BVH_ABB &o) const { return (min == o.min) && (neg_max == o.neg_max); }
|
||||
bool operator!=(const BVH_ABB &o) const { return (*this == o) == false; }
|
||||
|
||||
void set(const Point &_min, const Point &_max) {
|
||||
min = _min;
|
||||
neg_max = -_max;
|
||||
}
|
||||
|
||||
// to and from standard AABB
|
||||
void from(const Bounds &p_aabb) {
|
||||
min = p_aabb.position;
|
||||
neg_max = -(p_aabb.position + p_aabb.size);
|
||||
}
|
||||
|
||||
void to(Bounds &r_aabb) const {
|
||||
r_aabb.position = min;
|
||||
r_aabb.size = calculate_size();
|
||||
}
|
||||
|
||||
void merge(const BVH_ABB &p_o) {
|
||||
for (int axis = 0; axis < Point::AXIS_COUNT; ++axis) {
|
||||
neg_max[axis] = MIN(neg_max[axis], p_o.neg_max[axis]);
|
||||
min[axis] = MIN(min[axis], p_o.min[axis]);
|
||||
}
|
||||
}
|
||||
|
||||
Point calculate_size() const {
|
||||
return -neg_max - min;
|
||||
}
|
||||
|
||||
Point calculate_centre() const {
|
||||
return Point((calculate_size() * 0.5) + min);
|
||||
}
|
||||
|
||||
real_t get_proximity_to(const BVH_ABB &p_b) const {
|
||||
const Point d = (min - neg_max) - (p_b.min - p_b.neg_max);
|
||||
real_t proximity = 0.0;
|
||||
for (int axis = 0; axis < Point::AXIS_COUNT; ++axis) {
|
||||
proximity += Math::abs(d[axis]);
|
||||
}
|
||||
return proximity;
|
||||
}
|
||||
|
||||
int select_by_proximity(const BVH_ABB &p_a, const BVH_ABB &p_b) const {
|
||||
return (get_proximity_to(p_a) < get_proximity_to(p_b) ? 0 : 1);
|
||||
}
|
||||
|
||||
uint32_t find_cutting_planes(const BVH_ABB::ConvexHull &p_hull, uint32_t *p_plane_ids) const {
|
||||
uint32_t count = 0;
|
||||
|
||||
for (int n = 0; n < p_hull.num_planes; n++) {
|
||||
const Plane &p = p_hull.planes[n];
|
||||
if (intersects_plane(p)) {
|
||||
p_plane_ids[count++] = n;
|
||||
}
|
||||
}
|
||||
|
||||
return count;
|
||||
}
|
||||
|
||||
bool intersects_plane(const Plane &p_p) const {
|
||||
Vector3 size = calculate_size();
|
||||
Vector3 half_extents = size * 0.5;
|
||||
Vector3 ofs = min + half_extents;
|
||||
|
||||
// forward side of plane?
|
||||
Vector3 point_offset(
|
||||
(p_p.normal.x < 0) ? -half_extents.x : half_extents.x,
|
||||
(p_p.normal.y < 0) ? -half_extents.y : half_extents.y,
|
||||
(p_p.normal.z < 0) ? -half_extents.z : half_extents.z);
|
||||
Vector3 point = point_offset + ofs;
|
||||
|
||||
if (!p_p.is_point_over(point)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
point = -point_offset + ofs;
|
||||
if (p_p.is_point_over(point)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool intersects_convex_optimized(const ConvexHull &p_hull, const uint32_t *p_plane_ids, uint32_t p_num_planes) const {
|
||||
Vector3 size = calculate_size();
|
||||
Vector3 half_extents = size * 0.5;
|
||||
Vector3 ofs = min + half_extents;
|
||||
|
||||
for (unsigned int i = 0; i < p_num_planes; i++) {
|
||||
const Plane &p = p_hull.planes[p_plane_ids[i]];
|
||||
Vector3 point(
|
||||
(p.normal.x > 0) ? -half_extents.x : half_extents.x,
|
||||
(p.normal.y > 0) ? -half_extents.y : half_extents.y,
|
||||
(p.normal.z > 0) ? -half_extents.z : half_extents.z);
|
||||
point += ofs;
|
||||
if (p.is_point_over(point)) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool intersects_convex_partial(const ConvexHull &p_hull) const {
|
||||
Bounds bb;
|
||||
to(bb);
|
||||
return bb.intersects_convex_shape(p_hull.planes, p_hull.num_planes, p_hull.points, p_hull.num_points);
|
||||
}
|
||||
|
||||
IntersectResult intersects_convex(const ConvexHull &p_hull) const {
|
||||
if (intersects_convex_partial(p_hull)) {
|
||||
// fully within? very important for tree checks
|
||||
if (is_within_convex(p_hull)) {
|
||||
return IR_FULL;
|
||||
}
|
||||
|
||||
return IR_PARTIAL;
|
||||
}
|
||||
|
||||
return IR_MISS;
|
||||
}
|
||||
|
||||
bool is_within_convex(const ConvexHull &p_hull) const {
|
||||
// use half extents routine
|
||||
Bounds bb;
|
||||
to(bb);
|
||||
return bb.inside_convex_shape(p_hull.planes, p_hull.num_planes);
|
||||
}
|
||||
|
||||
bool is_point_within_hull(const ConvexHull &p_hull, const Vector3 &p_pt) const {
|
||||
for (int n = 0; n < p_hull.num_planes; n++) {
|
||||
if (p_hull.planes[n].distance_to(p_pt) > 0.0f) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool intersects_segment(const Segment &p_s) const {
|
||||
Bounds bb;
|
||||
to(bb);
|
||||
return bb.intersects_segment(p_s.from, p_s.to);
|
||||
}
|
||||
|
||||
bool intersects_point(const Point &p_pt) const {
|
||||
if (_any_lessthan(-p_pt, neg_max)) {
|
||||
return false;
|
||||
}
|
||||
if (_any_lessthan(p_pt, min)) {
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool intersects(const BVH_ABB &p_o) const {
|
||||
if (_any_morethan(p_o.min, -neg_max)) {
|
||||
return false;
|
||||
}
|
||||
if (_any_morethan(min, -p_o.neg_max)) {
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool is_other_within(const BVH_ABB &p_o) const {
|
||||
if (_any_lessthan(p_o.neg_max, neg_max)) {
|
||||
return false;
|
||||
}
|
||||
if (_any_lessthan(p_o.min, min)) {
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
void grow(const Point &p_change) {
|
||||
neg_max -= p_change;
|
||||
min -= p_change;
|
||||
}
|
||||
|
||||
void expand(real_t p_change) {
|
||||
Point change;
|
||||
change.set_all(p_change);
|
||||
grow(change);
|
||||
}
|
||||
|
||||
// Actually surface area metric.
|
||||
float get_area() const {
|
||||
Point d = calculate_size();
|
||||
return 2.0f * (d.x * d.y + d.y * d.z + d.z * d.x);
|
||||
}
|
||||
|
||||
void set_to_max_opposite_extents() {
|
||||
neg_max.set_all(FLT_MAX);
|
||||
min = neg_max;
|
||||
}
|
||||
|
||||
bool _any_morethan(const Point &p_a, const Point &p_b) const {
|
||||
for (int axis = 0; axis < Point::AXIS_COUNT; ++axis) {
|
||||
if (p_a[axis] > p_b[axis]) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
bool _any_lessthan(const Point &p_a, const Point &p_b) const {
|
||||
for (int axis = 0; axis < Point::AXIS_COUNT; ++axis) {
|
||||
if (p_a[axis] < p_b[axis]) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
};
|
||||
|
||||
#endif // BVH_ABB_H
|
||||
|
|
@ -0,0 +1,534 @@
|
|||
public:
|
||||
// cull parameters is a convenient way of passing a bunch
|
||||
// of arguments through the culling functions without
|
||||
// writing loads of code. Not all members are used for some cull checks
|
||||
struct CullParams {
|
||||
int result_count_overall; // both trees
|
||||
int result_count; // this tree only
|
||||
int result_max;
|
||||
T **result_array;
|
||||
int *subindex_array;
|
||||
|
||||
// nobody truly understands how masks are intended to work.
|
||||
uint32_t mask;
|
||||
uint32_t pairable_type;
|
||||
|
||||
// optional components for different tests
|
||||
Vector3 point;
|
||||
BVHABB_CLASS abb;
|
||||
typename BVHABB_CLASS::ConvexHull hull;
|
||||
typename BVHABB_CLASS::Segment segment;
|
||||
|
||||
// when collision testing, non pairable moving items
|
||||
// only need to be tested against the pairable tree.
|
||||
// collisions with other non pairable items are irrelevant.
|
||||
bool test_pairable_only;
|
||||
};
|
||||
|
||||
private:
|
||||
void _cull_translate_hits(CullParams &p) {
|
||||
int num_hits = _cull_hits.size();
|
||||
int left = p.result_max - p.result_count_overall;
|
||||
|
||||
if (num_hits > left) {
|
||||
num_hits = left;
|
||||
}
|
||||
|
||||
int out_n = p.result_count_overall;
|
||||
|
||||
for (int n = 0; n < num_hits; n++) {
|
||||
uint32_t ref_id = _cull_hits[n];
|
||||
|
||||
const ItemExtra &ex = _extra[ref_id];
|
||||
p.result_array[out_n] = ex.userdata;
|
||||
|
||||
if (p.subindex_array) {
|
||||
p.subindex_array[out_n] = ex.subindex;
|
||||
}
|
||||
|
||||
out_n++;
|
||||
}
|
||||
|
||||
p.result_count = num_hits;
|
||||
p.result_count_overall += num_hits;
|
||||
}
|
||||
|
||||
public:
|
||||
int cull_convex(CullParams &r_params, bool p_translate_hits = true) {
|
||||
_cull_hits.clear();
|
||||
r_params.result_count = 0;
|
||||
|
||||
for (int n = 0; n < NUM_TREES; n++) {
|
||||
if (_root_node_id[n] == BVHCommon::INVALID) {
|
||||
continue;
|
||||
}
|
||||
|
||||
_cull_convex_iterative(_root_node_id[n], r_params);
|
||||
}
|
||||
|
||||
if (p_translate_hits) {
|
||||
_cull_translate_hits(r_params);
|
||||
}
|
||||
|
||||
return r_params.result_count;
|
||||
}
|
||||
|
||||
int cull_segment(CullParams &r_params, bool p_translate_hits = true) {
|
||||
_cull_hits.clear();
|
||||
r_params.result_count = 0;
|
||||
|
||||
for (int n = 0; n < NUM_TREES; n++) {
|
||||
if (_root_node_id[n] == BVHCommon::INVALID) {
|
||||
continue;
|
||||
}
|
||||
|
||||
_cull_segment_iterative(_root_node_id[n], r_params);
|
||||
}
|
||||
|
||||
if (p_translate_hits) {
|
||||
_cull_translate_hits(r_params);
|
||||
}
|
||||
|
||||
return r_params.result_count;
|
||||
}
|
||||
|
||||
int cull_point(CullParams &r_params, bool p_translate_hits = true) {
|
||||
_cull_hits.clear();
|
||||
r_params.result_count = 0;
|
||||
|
||||
for (int n = 0; n < NUM_TREES; n++) {
|
||||
if (_root_node_id[n] == BVHCommon::INVALID) {
|
||||
continue;
|
||||
}
|
||||
|
||||
_cull_point_iterative(_root_node_id[n], r_params);
|
||||
}
|
||||
|
||||
if (p_translate_hits) {
|
||||
_cull_translate_hits(r_params);
|
||||
}
|
||||
|
||||
return r_params.result_count;
|
||||
}
|
||||
|
||||
int cull_aabb(CullParams &r_params, bool p_translate_hits = true) {
|
||||
_cull_hits.clear();
|
||||
r_params.result_count = 0;
|
||||
|
||||
for (int n = 0; n < NUM_TREES; n++) {
|
||||
if (_root_node_id[n] == BVHCommon::INVALID) {
|
||||
continue;
|
||||
}
|
||||
|
||||
if ((n == 0) && r_params.test_pairable_only) {
|
||||
continue;
|
||||
}
|
||||
|
||||
_cull_aabb_iterative(_root_node_id[n], r_params);
|
||||
}
|
||||
|
||||
if (p_translate_hits) {
|
||||
_cull_translate_hits(r_params);
|
||||
}
|
||||
|
||||
return r_params.result_count;
|
||||
}
|
||||
|
||||
bool _cull_hits_full(const CullParams &p) {
|
||||
// instead of checking every hit, we can do a lazy check for this condition.
|
||||
// it isn't a problem if we write too much _cull_hits because they only the
|
||||
// result_max amount will be translated and outputted. But we might as
|
||||
// well stop our cull checks after the maximum has been reached.
|
||||
return (int)_cull_hits.size() >= p.result_max;
|
||||
}
|
||||
|
||||
// write this logic once for use in all routines
|
||||
// double check this as a possible source of bugs in future.
|
||||
bool _cull_pairing_mask_test_hit(uint32_t p_maskA, uint32_t p_typeA, uint32_t p_maskB, uint32_t p_typeB) const {
|
||||
// double check this as a possible source of bugs in future.
|
||||
bool A_match_B = p_maskA & p_typeB;
|
||||
|
||||
if (!A_match_B) {
|
||||
bool B_match_A = p_maskB & p_typeA;
|
||||
if (!B_match_A) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void _cull_hit(uint32_t p_ref_id, CullParams &p) {
|
||||
// take into account masks etc
|
||||
// this would be more efficient to do before plane checks,
|
||||
// but done here for ease to get started
|
||||
if (USE_PAIRS) {
|
||||
const ItemExtra &ex = _extra[p_ref_id];
|
||||
|
||||
if (!_cull_pairing_mask_test_hit(p.mask, p.pairable_type, ex.pairable_mask, ex.pairable_type)) {
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
_cull_hits.push_back(p_ref_id);
|
||||
}
|
||||
|
||||
bool _cull_segment_iterative(uint32_t p_node_id, CullParams &r_params) {
|
||||
// our function parameters to keep on a stack
|
||||
struct CullSegParams {
|
||||
uint32_t node_id;
|
||||
};
|
||||
|
||||
// most of the iterative functionality is contained in this helper class
|
||||
BVH_IterativeInfo<CullSegParams> ii;
|
||||
|
||||
// alloca must allocate the stack from this function, it cannot be allocated in the
|
||||
// helper class
|
||||
ii.stack = (CullSegParams *)alloca(ii.get_alloca_stacksize());
|
||||
|
||||
// seed the stack
|
||||
ii.get_first()->node_id = p_node_id;
|
||||
|
||||
CullSegParams csp;
|
||||
|
||||
// while there are still more nodes on the stack
|
||||
while (ii.pop(csp)) {
|
||||
TNode &tnode = _nodes[csp.node_id];
|
||||
|
||||
if (tnode.is_leaf()) {
|
||||
// lazy check for hits full up condition
|
||||
if (_cull_hits_full(r_params)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
TLeaf &leaf = _node_get_leaf(tnode);
|
||||
|
||||
// test children individually
|
||||
for (int n = 0; n < leaf.num_items; n++) {
|
||||
const BVHABB_CLASS &aabb = leaf.get_aabb(n);
|
||||
|
||||
if (aabb.intersects_segment(r_params.segment)) {
|
||||
uint32_t child_id = leaf.get_item_ref_id(n);
|
||||
|
||||
// register hit
|
||||
_cull_hit(child_id, r_params);
|
||||
}
|
||||
}
|
||||
} else {
|
||||
// test children individually
|
||||
for (int n = 0; n < tnode.num_children; n++) {
|
||||
uint32_t child_id = tnode.children[n];
|
||||
const BVHABB_CLASS &child_abb = _nodes[child_id].aabb;
|
||||
|
||||
if (child_abb.intersects_segment(r_params.segment)) {
|
||||
// add to the stack
|
||||
CullSegParams *child = ii.request();
|
||||
child->node_id = child_id;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} // while more nodes to pop
|
||||
|
||||
// true indicates results are not full
|
||||
return true;
|
||||
}
|
||||
|
||||
bool _cull_point_iterative(uint32_t p_node_id, CullParams &r_params) {
|
||||
// our function parameters to keep on a stack
|
||||
struct CullPointParams {
|
||||
uint32_t node_id;
|
||||
};
|
||||
|
||||
// most of the iterative functionality is contained in this helper class
|
||||
BVH_IterativeInfo<CullPointParams> ii;
|
||||
|
||||
// alloca must allocate the stack from this function, it cannot be allocated in the
|
||||
// helper class
|
||||
ii.stack = (CullPointParams *)alloca(ii.get_alloca_stacksize());
|
||||
|
||||
// seed the stack
|
||||
ii.get_first()->node_id = p_node_id;
|
||||
|
||||
CullPointParams cpp;
|
||||
|
||||
// while there are still more nodes on the stack
|
||||
while (ii.pop(cpp)) {
|
||||
TNode &tnode = _nodes[cpp.node_id];
|
||||
// no hit with this node?
|
||||
if (!tnode.aabb.intersects_point(r_params.point)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
if (tnode.is_leaf()) {
|
||||
// lazy check for hits full up condition
|
||||
if (_cull_hits_full(r_params)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
TLeaf &leaf = _node_get_leaf(tnode);
|
||||
|
||||
// test children individually
|
||||
for (int n = 0; n < leaf.num_items; n++) {
|
||||
if (leaf.get_aabb(n).intersects_point(r_params.point)) {
|
||||
uint32_t child_id = leaf.get_item_ref_id(n);
|
||||
|
||||
// register hit
|
||||
_cull_hit(child_id, r_params);
|
||||
}
|
||||
}
|
||||
} else {
|
||||
// test children individually
|
||||
for (int n = 0; n < tnode.num_children; n++) {
|
||||
uint32_t child_id = tnode.children[n];
|
||||
|
||||
// add to the stack
|
||||
CullPointParams *child = ii.request();
|
||||
child->node_id = child_id;
|
||||
}
|
||||
}
|
||||
|
||||
} // while more nodes to pop
|
||||
|
||||
// true indicates results are not full
|
||||
return true;
|
||||
}
|
||||
|
||||
bool _cull_aabb_iterative(uint32_t p_node_id, CullParams &r_params, bool p_fully_within = false) {
|
||||
// our function parameters to keep on a stack
|
||||
struct CullAABBParams {
|
||||
uint32_t node_id;
|
||||
bool fully_within;
|
||||
};
|
||||
|
||||
// most of the iterative functionality is contained in this helper class
|
||||
BVH_IterativeInfo<CullAABBParams> ii;
|
||||
|
||||
// alloca must allocate the stack from this function, it cannot be allocated in the
|
||||
// helper class
|
||||
ii.stack = (CullAABBParams *)alloca(ii.get_alloca_stacksize());
|
||||
|
||||
// seed the stack
|
||||
ii.get_first()->node_id = p_node_id;
|
||||
ii.get_first()->fully_within = p_fully_within;
|
||||
|
||||
CullAABBParams cap;
|
||||
|
||||
// while there are still more nodes on the stack
|
||||
while (ii.pop(cap)) {
|
||||
TNode &tnode = _nodes[cap.node_id];
|
||||
|
||||
if (tnode.is_leaf()) {
|
||||
// lazy check for hits full up condition
|
||||
if (_cull_hits_full(r_params)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
TLeaf &leaf = _node_get_leaf(tnode);
|
||||
|
||||
// if fully within we can just add all items
|
||||
// as long as they pass mask checks
|
||||
if (cap.fully_within) {
|
||||
for (int n = 0; n < leaf.num_items; n++) {
|
||||
uint32_t child_id = leaf.get_item_ref_id(n);
|
||||
|
||||
// register hit
|
||||
_cull_hit(child_id, r_params);
|
||||
}
|
||||
} else {
|
||||
for (int n = 0; n < leaf.num_items; n++) {
|
||||
const BVHABB_CLASS &aabb = leaf.get_aabb(n);
|
||||
|
||||
if (aabb.intersects(r_params.abb)) {
|
||||
uint32_t child_id = leaf.get_item_ref_id(n);
|
||||
|
||||
// register hit
|
||||
_cull_hit(child_id, r_params);
|
||||
}
|
||||
}
|
||||
} // not fully within
|
||||
} else {
|
||||
if (!cap.fully_within) {
|
||||
// test children individually
|
||||
for (int n = 0; n < tnode.num_children; n++) {
|
||||
uint32_t child_id = tnode.children[n];
|
||||
const BVHABB_CLASS &child_abb = _nodes[child_id].aabb;
|
||||
|
||||
if (child_abb.intersects(r_params.abb)) {
|
||||
// is the node totally within the aabb?
|
||||
bool fully_within = r_params.abb.is_other_within(child_abb);
|
||||
|
||||
// add to the stack
|
||||
CullAABBParams *child = ii.request();
|
||||
|
||||
// should always return valid child
|
||||
child->node_id = child_id;
|
||||
child->fully_within = fully_within;
|
||||
}
|
||||
}
|
||||
} else {
|
||||
for (int n = 0; n < tnode.num_children; n++) {
|
||||
uint32_t child_id = tnode.children[n];
|
||||
|
||||
// add to the stack
|
||||
CullAABBParams *child = ii.request();
|
||||
|
||||
// should always return valid child
|
||||
child->node_id = child_id;
|
||||
child->fully_within = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} // while more nodes to pop
|
||||
|
||||
// true indicates results are not full
|
||||
return true;
|
||||
}
|
||||
|
||||
// returns full up with results
|
||||
bool _cull_convex_iterative(uint32_t p_node_id, CullParams &r_params, bool p_fully_within = false) {
|
||||
// our function parameters to keep on a stack
|
||||
struct CullConvexParams {
|
||||
uint32_t node_id;
|
||||
bool fully_within;
|
||||
};
|
||||
|
||||
// most of the iterative functionality is contained in this helper class
|
||||
BVH_IterativeInfo<CullConvexParams> ii;
|
||||
|
||||
// alloca must allocate the stack from this function, it cannot be allocated in the
|
||||
// helper class
|
||||
ii.stack = (CullConvexParams *)alloca(ii.get_alloca_stacksize());
|
||||
|
||||
// seed the stack
|
||||
ii.get_first()->node_id = p_node_id;
|
||||
ii.get_first()->fully_within = p_fully_within;
|
||||
|
||||
// preallocate these as a once off to be reused
|
||||
uint32_t max_planes = r_params.hull.num_planes;
|
||||
uint32_t *plane_ids = (uint32_t *)alloca(sizeof(uint32_t) * max_planes);
|
||||
|
||||
CullConvexParams ccp;
|
||||
|
||||
// while there are still more nodes on the stack
|
||||
while (ii.pop(ccp)) {
|
||||
const TNode &tnode = _nodes[ccp.node_id];
|
||||
|
||||
if (!ccp.fully_within) {
|
||||
typename BVHABB_CLASS::IntersectResult res = tnode.aabb.intersects_convex(r_params.hull);
|
||||
|
||||
switch (res) {
|
||||
default: {
|
||||
continue; // miss, just move on to the next node in the stack
|
||||
} break;
|
||||
case BVHABB_CLASS::IR_PARTIAL: {
|
||||
} break;
|
||||
case BVHABB_CLASS::IR_FULL: {
|
||||
ccp.fully_within = true;
|
||||
} break;
|
||||
}
|
||||
|
||||
} // if not fully within already
|
||||
|
||||
if (tnode.is_leaf()) {
|
||||
// lazy check for hits full up condition
|
||||
if (_cull_hits_full(r_params)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
const TLeaf &leaf = _node_get_leaf(tnode);
|
||||
|
||||
// if fully within, simply add all items to the result
|
||||
// (taking into account masks)
|
||||
if (ccp.fully_within) {
|
||||
for (int n = 0; n < leaf.num_items; n++) {
|
||||
uint32_t child_id = leaf.get_item_ref_id(n);
|
||||
|
||||
// register hit
|
||||
_cull_hit(child_id, r_params);
|
||||
}
|
||||
|
||||
} else {
|
||||
// we can either use a naive check of all the planes against the AABB,
|
||||
// or an optimized check, which finds in advance which of the planes can possibly
|
||||
// cut the AABB, and only tests those. This can be much faster.
|
||||
#define BVH_CONVEX_CULL_OPTIMIZED
|
||||
#ifdef BVH_CONVEX_CULL_OPTIMIZED
|
||||
// first find which planes cut the aabb
|
||||
uint32_t num_planes = tnode.aabb.find_cutting_planes(r_params.hull, plane_ids);
|
||||
BVH_ASSERT(num_planes <= max_planes);
|
||||
|
||||
//#define BVH_CONVEX_CULL_OPTIMIZED_RIGOR_CHECK
|
||||
#ifdef BVH_CONVEX_CULL_OPTIMIZED_RIGOR_CHECK
|
||||
// rigorous check
|
||||
uint32_t results[MAX_ITEMS];
|
||||
uint32_t num_results = 0;
|
||||
#endif
|
||||
|
||||
// test children individually
|
||||
for (int n = 0; n < leaf.num_items; n++) {
|
||||
//const Item &item = leaf.get_item(n);
|
||||
const BVHABB_CLASS &aabb = leaf.get_aabb(n);
|
||||
|
||||
if (aabb.intersects_convex_optimized(r_params.hull, plane_ids, num_planes)) {
|
||||
uint32_t child_id = leaf.get_item_ref_id(n);
|
||||
|
||||
#ifdef BVH_CONVEX_CULL_OPTIMIZED_RIGOR_CHECK
|
||||
results[num_results++] = child_id;
|
||||
#endif
|
||||
|
||||
// register hit
|
||||
_cull_hit(child_id, r_params);
|
||||
}
|
||||
}
|
||||
|
||||
#ifdef BVH_CONVEX_CULL_OPTIMIZED_RIGOR_CHECK
|
||||
uint32_t test_count = 0;
|
||||
|
||||
for (int n = 0; n < leaf.num_items; n++) {
|
||||
const BVHABB_CLASS &aabb = leaf.get_aabb(n);
|
||||
|
||||
if (aabb.intersects_convex_partial(r_params.hull)) {
|
||||
uint32_t child_id = leaf.get_item_ref_id(n);
|
||||
|
||||
CRASH_COND(child_id != results[test_count++]);
|
||||
CRASH_COND(test_count > num_results);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
#else
|
||||
// not BVH_CONVEX_CULL_OPTIMIZED
|
||||
// test children individually
|
||||
for (int n = 0; n < leaf.num_items; n++) {
|
||||
const BVHABB_CLASS &aabb = leaf.get_aabb(n);
|
||||
|
||||
if (aabb.intersects_convex_partial(r_params.hull)) {
|
||||
uint32_t child_id = leaf.get_item_ref_id(n);
|
||||
|
||||
// full up with results? exit early, no point in further testing
|
||||
if (!_cull_hit(child_id, r_params))
|
||||
return false;
|
||||
}
|
||||
}
|
||||
#endif // BVH_CONVEX_CULL_OPTIMIZED
|
||||
} // if not fully within
|
||||
} else {
|
||||
for (int n = 0; n < tnode.num_children; n++) {
|
||||
uint32_t child_id = tnode.children[n];
|
||||
|
||||
// add to the stack
|
||||
CullConvexParams *child = ii.request();
|
||||
|
||||
// should always return valid child
|
||||
child->node_id = child_id;
|
||||
child->fully_within = ccp.fully_within;
|
||||
}
|
||||
}
|
||||
|
||||
} // while more nodes to pop
|
||||
|
||||
// true indicates results are not full
|
||||
return true;
|
||||
}
|
||||
|
|
@ -0,0 +1,68 @@
|
|||
public:
|
||||
#ifdef BVH_VERBOSE
|
||||
void _debug_recursive_print_tree(int p_tree_id) const {
|
||||
if (_root_node_id[p_tree_id] != BVHCommon::INVALID)
|
||||
_debug_recursive_print_tree_node(_root_node_id[p_tree_id]);
|
||||
}
|
||||
|
||||
String _debug_aabb_to_string(const BVHABB_CLASS &aabb) const {
|
||||
String sz = "(";
|
||||
sz += itos(aabb.min.x);
|
||||
sz += " ~ ";
|
||||
sz += itos(-aabb.neg_max.x);
|
||||
sz += ") (";
|
||||
|
||||
sz += itos(aabb.min.y);
|
||||
sz += " ~ ";
|
||||
sz += itos(-aabb.neg_max.y);
|
||||
sz += ") (";
|
||||
|
||||
sz += itos(aabb.min.z);
|
||||
sz += " ~ ";
|
||||
sz += itos(-aabb.neg_max.z);
|
||||
sz += ") ";
|
||||
|
||||
Vector3 size = aabb.calculate_size();
|
||||
float vol = size.x * size.y * size.z;
|
||||
sz += "vol " + itos(vol);
|
||||
|
||||
return sz;
|
||||
}
|
||||
|
||||
void _debug_recursive_print_tree_node(uint32_t p_node_id, int depth = 0) const {
|
||||
const TNode &tnode = _nodes[p_node_id];
|
||||
|
||||
String sz = "";
|
||||
for (int n = 0; n < depth; n++) {
|
||||
sz += "\t";
|
||||
}
|
||||
sz += itos(p_node_id);
|
||||
|
||||
if (tnode.is_leaf()) {
|
||||
sz += " L";
|
||||
sz += itos(tnode.height) + " ";
|
||||
const TLeaf &leaf = _node_get_leaf(tnode);
|
||||
|
||||
sz += "[";
|
||||
for (int n = 0; n < leaf.num_items; n++) {
|
||||
if (n)
|
||||
sz += ", ";
|
||||
sz += "r";
|
||||
sz += itos(leaf.get_item_ref_id(n));
|
||||
}
|
||||
sz += "] ";
|
||||
} else {
|
||||
sz += " N";
|
||||
sz += itos(tnode.height) + " ";
|
||||
}
|
||||
|
||||
sz += _debug_aabb_to_string(tnode.aabb);
|
||||
print_line(sz);
|
||||
|
||||
if (!tnode.is_leaf()) {
|
||||
for (int n = 0; n < tnode.num_children; n++) {
|
||||
_debug_recursive_print_tree_node(tnode.children[n], depth + 1);
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
|
@ -0,0 +1,42 @@
|
|||
void _integrity_check_all() {
|
||||
#ifdef BVH_INTEGRITY_CHECKS
|
||||
for (int n = 0; n < NUM_TREES; n++) {
|
||||
uint32_t root = _root_node_id[n];
|
||||
if (root != BVHCommon::INVALID) {
|
||||
_integrity_check_down(root);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
void _integrity_check_up(uint32_t p_node_id) {
|
||||
TNode &node = _nodes[p_node_id];
|
||||
|
||||
BVHABB_CLASS abb = node.aabb;
|
||||
node_update_aabb(node);
|
||||
|
||||
BVHABB_CLASS abb2 = node.aabb;
|
||||
abb2.expand(-_node_expansion);
|
||||
|
||||
CRASH_COND(!abb.is_other_within(abb2));
|
||||
}
|
||||
|
||||
void _integrity_check_down(uint32_t p_node_id) {
|
||||
const TNode &node = _nodes[p_node_id];
|
||||
|
||||
if (node.is_leaf()) {
|
||||
_integrity_check_up(p_node_id);
|
||||
} else {
|
||||
CRASH_COND(node.num_children != 2);
|
||||
|
||||
for (int n = 0; n < node.num_children; n++) {
|
||||
uint32_t child_id = node.children[n];
|
||||
|
||||
// check the children parent pointers are correct
|
||||
TNode &child = _nodes[child_id];
|
||||
CRASH_COND(child.parent_id != p_node_id);
|
||||
|
||||
_integrity_check_down(child_id);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,230 @@
|
|||
|
||||
// for slow incremental optimization, we will periodically remove each
|
||||
// item from the tree and reinsert, to give it a chance to find a better position
|
||||
void _logic_item_remove_and_reinsert(uint32_t p_ref_id) {
|
||||
// get the reference
|
||||
ItemRef &ref = _refs[p_ref_id];
|
||||
|
||||
// no need to optimize inactive items
|
||||
if (!ref.is_active()) {
|
||||
return;
|
||||
}
|
||||
|
||||
// special case of debug draw
|
||||
if (ref.item_id == BVHCommon::INVALID) {
|
||||
return;
|
||||
}
|
||||
|
||||
BVH_ASSERT(ref.tnode_id != BVHCommon::INVALID);
|
||||
|
||||
// some overlay elaborate way to find out which tree the node is in!
|
||||
BVHHandle temp_handle;
|
||||
temp_handle.set_id(p_ref_id);
|
||||
_current_tree = _handle_get_tree_id(temp_handle);
|
||||
|
||||
// remove and reinsert
|
||||
BVHABB_CLASS abb;
|
||||
node_remove_item(p_ref_id, &abb);
|
||||
|
||||
// we must choose where to add to tree
|
||||
ref.tnode_id = _logic_choose_item_add_node(_root_node_id[_current_tree], abb);
|
||||
_node_add_item(ref.tnode_id, p_ref_id, abb);
|
||||
|
||||
refit_upward_and_balance(ref.tnode_id);
|
||||
}
|
||||
|
||||
// from randy gaul balance function
|
||||
BVHABB_CLASS _logic_abb_merge(const BVHABB_CLASS &a, const BVHABB_CLASS &b) {
|
||||
BVHABB_CLASS c = a;
|
||||
c.merge(b);
|
||||
return c;
|
||||
}
|
||||
|
||||
//--------------------------------------------------------------------------------------------------
|
||||
/**
|
||||
@file q3DynamicAABBTree.h
|
||||
@author Randy Gaul
|
||||
@date 10/10/2014
|
||||
Copyright (c) 2014 Randy Gaul http://www.randygaul.net
|
||||
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.
|
||||
*/
|
||||
//--------------------------------------------------------------------------------------------------
|
||||
|
||||
// This function is based on the 'Balance' function from Randy Gaul's qu3e
|
||||
// https://github.com/RandyGaul/qu3e
|
||||
// It is MODIFIED from qu3e version.
|
||||
// This is the only function used (and _logic_abb_merge helper function).
|
||||
int32_t _logic_balance(int32_t iA) {
|
||||
// return iA; // uncomment this to bypass balance
|
||||
|
||||
TNode *A = &_nodes[iA];
|
||||
|
||||
if (A->is_leaf() || A->height == 1) {
|
||||
return iA;
|
||||
}
|
||||
|
||||
/* A
|
||||
/ \
|
||||
B C
|
||||
/ \ / \
|
||||
D E F G
|
||||
*/
|
||||
|
||||
CRASH_COND(A->num_children != 2);
|
||||
int32_t iB = A->children[0];
|
||||
int32_t iC = A->children[1];
|
||||
TNode *B = &_nodes[iB];
|
||||
TNode *C = &_nodes[iC];
|
||||
|
||||
int32_t balance = C->height - B->height;
|
||||
|
||||
// C is higher, promote C
|
||||
if (balance > 1) {
|
||||
int32_t iF = C->children[0];
|
||||
int32_t iG = C->children[1];
|
||||
TNode *F = &_nodes[iF];
|
||||
TNode *G = &_nodes[iG];
|
||||
|
||||
// grandParent point to C
|
||||
if (A->parent_id != BVHCommon::INVALID) {
|
||||
if (_nodes[A->parent_id].children[0] == iA) {
|
||||
_nodes[A->parent_id].children[0] = iC;
|
||||
|
||||
} else {
|
||||
_nodes[A->parent_id].children[1] = iC;
|
||||
}
|
||||
} else {
|
||||
// check this .. seems dodgy
|
||||
change_root_node(iC);
|
||||
}
|
||||
|
||||
// Swap A and C
|
||||
C->children[0] = iA;
|
||||
C->parent_id = A->parent_id;
|
||||
A->parent_id = iC;
|
||||
|
||||
// Finish rotation
|
||||
if (F->height > G->height) {
|
||||
C->children[1] = iF;
|
||||
A->children[1] = iG;
|
||||
G->parent_id = iA;
|
||||
A->aabb = _logic_abb_merge(B->aabb, G->aabb);
|
||||
C->aabb = _logic_abb_merge(A->aabb, F->aabb);
|
||||
|
||||
A->height = 1 + MAX(B->height, G->height);
|
||||
C->height = 1 + MAX(A->height, F->height);
|
||||
}
|
||||
|
||||
else {
|
||||
C->children[1] = iG;
|
||||
A->children[1] = iF;
|
||||
F->parent_id = iA;
|
||||
A->aabb = _logic_abb_merge(B->aabb, F->aabb);
|
||||
C->aabb = _logic_abb_merge(A->aabb, G->aabb);
|
||||
|
||||
A->height = 1 + MAX(B->height, F->height);
|
||||
C->height = 1 + MAX(A->height, G->height);
|
||||
}
|
||||
|
||||
return iC;
|
||||
}
|
||||
|
||||
// B is higher, promote B
|
||||
else if (balance < -1) {
|
||||
int32_t iD = B->children[0];
|
||||
int32_t iE = B->children[1];
|
||||
TNode *D = &_nodes[iD];
|
||||
TNode *E = &_nodes[iE];
|
||||
|
||||
// grandParent point to B
|
||||
if (A->parent_id != BVHCommon::INVALID) {
|
||||
if (_nodes[A->parent_id].children[0] == iA) {
|
||||
_nodes[A->parent_id].children[0] = iB;
|
||||
} else {
|
||||
_nodes[A->parent_id].children[1] = iB;
|
||||
}
|
||||
}
|
||||
|
||||
else {
|
||||
// check this .. seems dodgy
|
||||
change_root_node(iB);
|
||||
}
|
||||
|
||||
// Swap A and B
|
||||
B->children[1] = iA;
|
||||
B->parent_id = A->parent_id;
|
||||
A->parent_id = iB;
|
||||
|
||||
// Finish rotation
|
||||
if (D->height > E->height) {
|
||||
B->children[0] = iD;
|
||||
A->children[0] = iE;
|
||||
E->parent_id = iA;
|
||||
A->aabb = _logic_abb_merge(C->aabb, E->aabb);
|
||||
B->aabb = _logic_abb_merge(A->aabb, D->aabb);
|
||||
|
||||
A->height = 1 + MAX(C->height, E->height);
|
||||
B->height = 1 + MAX(A->height, D->height);
|
||||
}
|
||||
|
||||
else {
|
||||
B->children[0] = iE;
|
||||
A->children[0] = iD;
|
||||
D->parent_id = iA;
|
||||
A->aabb = _logic_abb_merge(C->aabb, D->aabb);
|
||||
B->aabb = _logic_abb_merge(A->aabb, E->aabb);
|
||||
|
||||
A->height = 1 + MAX(C->height, D->height);
|
||||
B->height = 1 + MAX(A->height, E->height);
|
||||
}
|
||||
|
||||
return iB;
|
||||
}
|
||||
|
||||
return iA;
|
||||
}
|
||||
|
||||
// either choose an existing node to add item to, or create a new node and return this
|
||||
uint32_t _logic_choose_item_add_node(uint32_t p_node_id, const BVHABB_CLASS &p_aabb) {
|
||||
while (true) {
|
||||
BVH_ASSERT(p_node_id != BVHCommon::INVALID);
|
||||
TNode &tnode = _nodes[p_node_id];
|
||||
|
||||
if (tnode.is_leaf()) {
|
||||
// if a leaf, and non full, use this to add to
|
||||
if (!node_is_leaf_full(tnode)) {
|
||||
return p_node_id;
|
||||
}
|
||||
|
||||
// else split the leaf, and use one of the children to add to
|
||||
return split_leaf(p_node_id, p_aabb);
|
||||
}
|
||||
|
||||
// this should not happen???
|
||||
// is still happening, need to debug and find circumstances. Is not that serious
|
||||
// but would be nice to prevent. I think it only happens with the root node.
|
||||
if (tnode.num_children == 1) {
|
||||
WARN_PRINT_ONCE("BVH::recursive_choose_item_add_node, node with 1 child, recovering");
|
||||
p_node_id = tnode.children[0];
|
||||
} else {
|
||||
BVH_ASSERT(tnode.num_children == 2);
|
||||
TNode &childA = _nodes[tnode.children[0]];
|
||||
TNode &childB = _nodes[tnode.children[1]];
|
||||
int which = p_aabb.select_by_proximity(childA.aabb, childB.aabb);
|
||||
|
||||
p_node_id = tnode.children[which];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,55 @@
|
|||
|
||||
int _handle_get_tree_id(BVHHandle p_handle) const {
|
||||
if (USE_PAIRS) {
|
||||
int tree = 0;
|
||||
if (_extra[p_handle.id()].pairable) {
|
||||
tree = 1;
|
||||
}
|
||||
return tree;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
public:
|
||||
void _handle_sort(BVHHandle &p_ha, BVHHandle &p_hb) const {
|
||||
if (p_ha.id() > p_hb.id()) {
|
||||
BVHHandle temp = p_hb;
|
||||
p_hb = p_ha;
|
||||
p_ha = temp;
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
void create_root_node(int p_tree) {
|
||||
// if there is no root node, create one
|
||||
if (_root_node_id[p_tree] == BVHCommon::INVALID) {
|
||||
uint32_t root_node_id;
|
||||
TNode *node = _nodes.request(root_node_id);
|
||||
node->clear();
|
||||
_root_node_id[p_tree] = root_node_id;
|
||||
|
||||
// make the root node a leaf
|
||||
uint32_t leaf_id;
|
||||
TLeaf *leaf = _leaves.request(leaf_id);
|
||||
leaf->clear();
|
||||
node->neg_leaf_id = -(int)leaf_id;
|
||||
}
|
||||
}
|
||||
|
||||
bool node_is_leaf_full(TNode &tnode) const {
|
||||
const TLeaf &leaf = _node_get_leaf(tnode);
|
||||
return leaf.is_full();
|
||||
}
|
||||
|
||||
public:
|
||||
TLeaf &_node_get_leaf(TNode &tnode) {
|
||||
BVH_ASSERT(tnode.is_leaf());
|
||||
return _leaves[tnode.get_leaf_id()];
|
||||
}
|
||||
|
||||
const TLeaf &_node_get_leaf(const TNode &tnode) const {
|
||||
BVH_ASSERT(tnode.is_leaf());
|
||||
return _leaves[tnode.get_leaf_id()];
|
||||
}
|
||||
|
||||
private:
|
||||
|
|
@ -0,0 +1,62 @@
|
|||
public:
|
||||
// note .. maybe this can be attached to another node structure?
|
||||
// depends which works best for cache.
|
||||
struct ItemPairs {
|
||||
struct Link {
|
||||
void set(BVHHandle h, void *ud) {
|
||||
handle = h;
|
||||
userdata = ud;
|
||||
}
|
||||
BVHHandle handle;
|
||||
void *userdata;
|
||||
};
|
||||
|
||||
void clear() {
|
||||
num_pairs = 0;
|
||||
extended_pairs.reset();
|
||||
expanded_aabb = Bounds();
|
||||
}
|
||||
|
||||
Bounds expanded_aabb;
|
||||
|
||||
// maybe we can just use the number in the vector TODO
|
||||
int32_t num_pairs;
|
||||
LocalVector<Link> extended_pairs;
|
||||
|
||||
void add_pair_to(BVHHandle h, void *p_userdata) {
|
||||
Link temp;
|
||||
temp.set(h, p_userdata);
|
||||
|
||||
extended_pairs.push_back(temp);
|
||||
num_pairs++;
|
||||
}
|
||||
|
||||
uint32_t find_pair_to(BVHHandle h) const {
|
||||
for (int n = 0; n < num_pairs; n++) {
|
||||
if (extended_pairs[n].handle == h) {
|
||||
return n;
|
||||
}
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
bool contains_pair_to(BVHHandle h) const {
|
||||
return find_pair_to(h) != BVHCommon::INVALID;
|
||||
}
|
||||
|
||||
// return success
|
||||
void *remove_pair_to(BVHHandle h) {
|
||||
void *userdata = nullptr;
|
||||
|
||||
for (int n = 0; n < num_pairs; n++) {
|
||||
if (extended_pairs[n].handle == h) {
|
||||
userdata = extended_pairs[n].userdata;
|
||||
extended_pairs.remove_unordered(n);
|
||||
num_pairs--;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
return userdata;
|
||||
}
|
||||
};
|
||||
|
|
@ -0,0 +1,421 @@
|
|||
public:
|
||||
BVHHandle item_add(T *p_userdata, bool p_active, const Bounds &p_aabb, int32_t p_subindex, bool p_pairable, uint32_t p_pairable_type, uint32_t p_pairable_mask, bool p_invisible = false) {
|
||||
#ifdef BVH_VERBOSE_TREE
|
||||
VERBOSE_PRINT("\nitem_add BEFORE");
|
||||
_debug_recursive_print_tree(0);
|
||||
VERBOSE_PRINT("\n");
|
||||
#endif
|
||||
|
||||
BVHABB_CLASS abb;
|
||||
abb.from(p_aabb);
|
||||
|
||||
// handle to be filled with the new item ref
|
||||
BVHHandle handle;
|
||||
|
||||
// ref id easier to pass around than handle
|
||||
uint32_t ref_id;
|
||||
|
||||
// this should never fail
|
||||
ItemRef *ref = _refs.request(ref_id);
|
||||
|
||||
// the extra data should be parallel list to the references
|
||||
uint32_t extra_id;
|
||||
ItemExtra *extra = _extra.request(extra_id);
|
||||
BVH_ASSERT(extra_id == ref_id);
|
||||
|
||||
// pairs info
|
||||
if (USE_PAIRS) {
|
||||
uint32_t pairs_id;
|
||||
ItemPairs *pairs = _pairs.request(pairs_id);
|
||||
pairs->clear();
|
||||
BVH_ASSERT(pairs_id == ref_id);
|
||||
}
|
||||
|
||||
extra->subindex = p_subindex;
|
||||
extra->userdata = p_userdata;
|
||||
extra->last_updated_tick = 0;
|
||||
|
||||
// add an active reference to the list for slow incremental optimize
|
||||
// this list must be kept in sync with the references as they are added or removed.
|
||||
extra->active_ref_id = _active_refs.size();
|
||||
_active_refs.push_back(ref_id);
|
||||
|
||||
if (USE_PAIRS) {
|
||||
extra->pairable_mask = p_pairable_mask;
|
||||
extra->pairable_type = p_pairable_type;
|
||||
extra->pairable = p_pairable;
|
||||
} else {
|
||||
// just for safety, in case this gets queried etc
|
||||
extra->pairable = 0;
|
||||
p_pairable = false;
|
||||
}
|
||||
|
||||
// assign to handle to return
|
||||
handle.set_id(ref_id);
|
||||
|
||||
_current_tree = 0;
|
||||
if (p_pairable) {
|
||||
_current_tree = 1;
|
||||
}
|
||||
|
||||
create_root_node(_current_tree);
|
||||
|
||||
// we must choose where to add to tree
|
||||
if (p_active) {
|
||||
ref->tnode_id = _logic_choose_item_add_node(_root_node_id[_current_tree], abb);
|
||||
|
||||
bool refit = _node_add_item(ref->tnode_id, ref_id, abb);
|
||||
|
||||
if (refit) {
|
||||
// only need to refit from the parent
|
||||
const TNode &add_node = _nodes[ref->tnode_id];
|
||||
if (add_node.parent_id != BVHCommon::INVALID) {
|
||||
refit_upward_and_balance(add_node.parent_id);
|
||||
}
|
||||
}
|
||||
} else {
|
||||
ref->set_inactive();
|
||||
}
|
||||
|
||||
#ifdef BVH_VERBOSE
|
||||
// memory use
|
||||
int mem = _refs.estimate_memory_use();
|
||||
mem += _nodes.estimate_memory_use();
|
||||
|
||||
String sz = _debug_aabb_to_string(abb);
|
||||
VERBOSE_PRINT("\titem_add [" + itos(ref_id) + "] " + itos(_refs.size()) + " refs,\t" + itos(_nodes.size()) + " nodes " + sz);
|
||||
VERBOSE_PRINT("mem use : " + itos(mem) + ", num nodes : " + itos(_nodes.size()));
|
||||
|
||||
#endif
|
||||
|
||||
return handle;
|
||||
}
|
||||
|
||||
void _debug_print_refs() {
|
||||
#ifdef BVH_VERBOSE_TREE
|
||||
print_line("refs.....");
|
||||
for (int n = 0; n < _refs.size(); n++) {
|
||||
const ItemRef &ref = _refs[n];
|
||||
print_line("tnode_id " + itos(ref.tnode_id) + ", item_id " + itos(ref.item_id));
|
||||
}
|
||||
|
||||
#endif
|
||||
}
|
||||
|
||||
// returns false if noop
|
||||
bool item_move(BVHHandle p_handle, const Bounds &p_aabb) {
|
||||
uint32_t ref_id = p_handle.id();
|
||||
|
||||
// get the reference
|
||||
ItemRef &ref = _refs[ref_id];
|
||||
if (!ref.is_active()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
BVHABB_CLASS abb;
|
||||
abb.from(p_aabb);
|
||||
|
||||
BVH_ASSERT(ref.tnode_id != BVHCommon::INVALID);
|
||||
TNode &tnode = _nodes[ref.tnode_id];
|
||||
|
||||
// does it fit within the current aabb?
|
||||
if (tnode.aabb.is_other_within(abb)) {
|
||||
// do nothing .. fast path .. not moved enough to need refit
|
||||
|
||||
// however we WILL update the exact aabb in the leaf, as this will be needed
|
||||
// for accurate collision detection
|
||||
TLeaf &leaf = _node_get_leaf(tnode);
|
||||
|
||||
BVHABB_CLASS &leaf_abb = leaf.get_aabb(ref.item_id);
|
||||
|
||||
// no change?
|
||||
if (leaf_abb == abb) {
|
||||
return false;
|
||||
}
|
||||
|
||||
leaf_abb = abb;
|
||||
_integrity_check_all();
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
_current_tree = _handle_get_tree_id(p_handle);
|
||||
|
||||
// remove and reinsert
|
||||
node_remove_item(ref_id);
|
||||
|
||||
// we must choose where to add to tree
|
||||
ref.tnode_id = _logic_choose_item_add_node(_root_node_id[_current_tree], abb);
|
||||
|
||||
// add to the tree
|
||||
bool needs_refit = _node_add_item(ref.tnode_id, ref_id, abb);
|
||||
|
||||
// only need to refit from the PARENT
|
||||
if (needs_refit) {
|
||||
// only need to refit from the parent
|
||||
const TNode &add_node = _nodes[ref.tnode_id];
|
||||
if (add_node.parent_id != BVHCommon::INVALID) {
|
||||
// not sure we need to rebalance all the time, this can be done less often
|
||||
refit_upward(add_node.parent_id);
|
||||
}
|
||||
//refit_upward_and_balance(add_node.parent_id);
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void item_remove(BVHHandle p_handle) {
|
||||
uint32_t ref_id = p_handle.id();
|
||||
|
||||
_current_tree = _handle_get_tree_id(p_handle);
|
||||
|
||||
VERBOSE_PRINT("item_remove [" + itos(ref_id) + "] ");
|
||||
|
||||
////////////////////////////////////////
|
||||
// remove the active reference from the list for slow incremental optimize
|
||||
// this list must be kept in sync with the references as they are added or removed.
|
||||
uint32_t active_ref_id = _extra[ref_id].active_ref_id;
|
||||
uint32_t ref_id_moved_back = _active_refs[_active_refs.size() - 1];
|
||||
|
||||
// swap back and decrement for fast unordered remove
|
||||
_active_refs[active_ref_id] = ref_id_moved_back;
|
||||
_active_refs.resize(_active_refs.size() - 1);
|
||||
|
||||
// keep the moved active reference up to date
|
||||
_extra[ref_id_moved_back].active_ref_id = active_ref_id;
|
||||
////////////////////////////////////////
|
||||
|
||||
// remove the item from the node (only if active)
|
||||
if (_refs[ref_id].is_active()) {
|
||||
node_remove_item(ref_id);
|
||||
}
|
||||
|
||||
// remove the item reference
|
||||
_refs.free(ref_id);
|
||||
_extra.free(ref_id);
|
||||
if (USE_PAIRS) {
|
||||
_pairs.free(ref_id);
|
||||
}
|
||||
|
||||
// don't think refit_all is necessary?
|
||||
//refit_all(_current_tree);
|
||||
|
||||
#ifdef BVH_VERBOSE_TREE
|
||||
_debug_recursive_print_tree(_current_tree);
|
||||
#endif
|
||||
}
|
||||
|
||||
// returns success
|
||||
bool item_activate(BVHHandle p_handle, const Bounds &p_aabb) {
|
||||
uint32_t ref_id = p_handle.id();
|
||||
ItemRef &ref = _refs[ref_id];
|
||||
if (ref.is_active()) {
|
||||
// noop
|
||||
return false;
|
||||
}
|
||||
|
||||
// add to tree
|
||||
BVHABB_CLASS abb;
|
||||
abb.from(p_aabb);
|
||||
|
||||
_current_tree = _handle_get_tree_id(p_handle);
|
||||
|
||||
// we must choose where to add to tree
|
||||
ref.tnode_id = _logic_choose_item_add_node(_root_node_id[_current_tree], abb);
|
||||
_node_add_item(ref.tnode_id, ref_id, abb);
|
||||
|
||||
refit_upward_and_balance(ref.tnode_id);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
// returns success
|
||||
bool item_deactivate(BVHHandle p_handle) {
|
||||
uint32_t ref_id = p_handle.id();
|
||||
ItemRef &ref = _refs[ref_id];
|
||||
if (!ref.is_active()) {
|
||||
// noop
|
||||
return false;
|
||||
}
|
||||
|
||||
// remove from tree
|
||||
BVHABB_CLASS abb;
|
||||
node_remove_item(ref_id, &abb);
|
||||
|
||||
// mark as inactive
|
||||
ref.set_inactive();
|
||||
return true;
|
||||
}
|
||||
|
||||
bool item_get_active(BVHHandle p_handle) const {
|
||||
uint32_t ref_id = p_handle.id();
|
||||
const ItemRef &ref = _refs[ref_id];
|
||||
return ref.is_active();
|
||||
}
|
||||
|
||||
// during collision testing, we want to set the mask and whether pairable for the item testing from
|
||||
void item_fill_cullparams(BVHHandle p_handle, CullParams &r_params) const {
|
||||
uint32_t ref_id = p_handle.id();
|
||||
const ItemExtra &extra = _extra[ref_id];
|
||||
|
||||
// testing from a non pairable item, we only want to test pairable items
|
||||
r_params.test_pairable_only = extra.pairable == 0;
|
||||
|
||||
// we take into account the mask of the item testing from
|
||||
r_params.mask = extra.pairable_mask;
|
||||
r_params.pairable_type = extra.pairable_type;
|
||||
}
|
||||
|
||||
bool item_is_pairable(const BVHHandle &p_handle) {
|
||||
uint32_t ref_id = p_handle.id();
|
||||
const ItemExtra &extra = _extra[ref_id];
|
||||
return extra.pairable != 0;
|
||||
}
|
||||
|
||||
void item_get_ABB(const BVHHandle &p_handle, BVHABB_CLASS &r_abb) {
|
||||
// change tree?
|
||||
uint32_t ref_id = p_handle.id();
|
||||
const ItemRef &ref = _refs[ref_id];
|
||||
|
||||
TNode &tnode = _nodes[ref.tnode_id];
|
||||
TLeaf &leaf = _node_get_leaf(tnode);
|
||||
|
||||
r_abb = leaf.get_aabb(ref.item_id);
|
||||
}
|
||||
|
||||
bool item_set_pairable(const BVHHandle &p_handle, bool p_pairable, uint32_t p_pairable_type, uint32_t p_pairable_mask) {
|
||||
// change tree?
|
||||
uint32_t ref_id = p_handle.id();
|
||||
|
||||
ItemExtra &ex = _extra[ref_id];
|
||||
ItemRef &ref = _refs[ref_id];
|
||||
|
||||
bool active = ref.is_active();
|
||||
bool pairable_changed = (ex.pairable != 0) != p_pairable;
|
||||
bool state_changed = pairable_changed || (ex.pairable_type != p_pairable_type) || (ex.pairable_mask != p_pairable_mask);
|
||||
|
||||
ex.pairable_type = p_pairable_type;
|
||||
ex.pairable_mask = p_pairable_mask;
|
||||
|
||||
if (active && pairable_changed) {
|
||||
// record abb
|
||||
TNode &tnode = _nodes[ref.tnode_id];
|
||||
TLeaf &leaf = _node_get_leaf(tnode);
|
||||
BVHABB_CLASS abb = leaf.get_aabb(ref.item_id);
|
||||
|
||||
// make sure current tree is correct prior to changing
|
||||
_current_tree = _handle_get_tree_id(p_handle);
|
||||
|
||||
// remove from old tree
|
||||
node_remove_item(ref_id);
|
||||
|
||||
// we must set the pairable AFTER getting the current tree
|
||||
// because the pairable status determines which tree
|
||||
ex.pairable = p_pairable;
|
||||
|
||||
// add to new tree
|
||||
_current_tree = _handle_get_tree_id(p_handle);
|
||||
create_root_node(_current_tree);
|
||||
|
||||
// we must choose where to add to tree
|
||||
ref.tnode_id = _logic_choose_item_add_node(_root_node_id[_current_tree], abb);
|
||||
bool needs_refit = _node_add_item(ref.tnode_id, ref_id, abb);
|
||||
|
||||
// only need to refit from the PARENT
|
||||
if (needs_refit) {
|
||||
// only need to refit from the parent
|
||||
const TNode &add_node = _nodes[ref.tnode_id];
|
||||
if (add_node.parent_id != BVHCommon::INVALID) {
|
||||
refit_upward_and_balance(add_node.parent_id);
|
||||
}
|
||||
}
|
||||
} else {
|
||||
// always keep this up to date
|
||||
ex.pairable = p_pairable;
|
||||
}
|
||||
|
||||
return state_changed;
|
||||
}
|
||||
|
||||
void incremental_optimize() {
|
||||
// first update all aabbs as one off step..
|
||||
// this is cheaper than doing it on each move as each leaf may get touched multiple times
|
||||
// in a frame.
|
||||
for (int n = 0; n < NUM_TREES; n++) {
|
||||
if (_root_node_id[n] != BVHCommon::INVALID) {
|
||||
refit_branch(_root_node_id[n]);
|
||||
}
|
||||
}
|
||||
|
||||
// now do small section reinserting to get things moving
|
||||
// gradually, and keep items in the right leaf
|
||||
if (_current_active_ref >= _active_refs.size()) {
|
||||
_current_active_ref = 0;
|
||||
}
|
||||
|
||||
// special case
|
||||
if (!_active_refs.size()) {
|
||||
return;
|
||||
}
|
||||
|
||||
uint32_t ref_id = _active_refs[_current_active_ref++];
|
||||
|
||||
_logic_item_remove_and_reinsert(ref_id);
|
||||
|
||||
#ifdef BVH_VERBOSE
|
||||
/*
|
||||
// memory use
|
||||
int mem_refs = _refs.estimate_memory_use();
|
||||
int mem_nodes = _nodes.estimate_memory_use();
|
||||
int mem_leaves = _leaves.estimate_memory_use();
|
||||
|
||||
String sz;
|
||||
sz += "mem_refs : " + itos(mem_refs) + " ";
|
||||
sz += "mem_nodes : " + itos(mem_nodes) + " ";
|
||||
sz += "mem_leaves : " + itos(mem_leaves) + " ";
|
||||
sz += ", num nodes : " + itos(_nodes.size());
|
||||
print_line(sz);
|
||||
*/
|
||||
#endif
|
||||
}
|
||||
|
||||
void update() {
|
||||
incremental_optimize();
|
||||
|
||||
// keep the expansion values up to date with the world bound
|
||||
//#define BVH_ALLOW_AUTO_EXPANSION
|
||||
#ifdef BVH_ALLOW_AUTO_EXPANSION
|
||||
if (_auto_node_expansion || _auto_pairing_expansion) {
|
||||
BVHABB_CLASS world_bound;
|
||||
world_bound.set_to_max_opposite_extents();
|
||||
|
||||
bool bound_valid = false;
|
||||
|
||||
for (int n = 0; n < NUM_TREES; n++) {
|
||||
uint32_t node_id = _root_node_id[n];
|
||||
if (node_id != BVHCommon::INVALID) {
|
||||
world_bound.merge(_nodes[node_id].aabb);
|
||||
bound_valid = true;
|
||||
}
|
||||
}
|
||||
|
||||
// if there are no nodes, do nothing, but if there are...
|
||||
if (bound_valid) {
|
||||
Bounds bb;
|
||||
world_bound.to(bb);
|
||||
real_t size = bb.get_longest_axis_size();
|
||||
|
||||
// automatic AI decision for best parameters.
|
||||
// These can be overridden in project settings.
|
||||
|
||||
// these magic numbers are determined by experiment
|
||||
if (_auto_node_expansion) {
|
||||
_node_expansion = size * 0.025;
|
||||
}
|
||||
if (_auto_pairing_expansion) {
|
||||
_pairing_expansion = size * 0.009;
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
|
@ -0,0 +1,141 @@
|
|||
void _debug_node_verify_bound(uint32_t p_node_id) {
|
||||
TNode &node = _nodes[p_node_id];
|
||||
BVHABB_CLASS abb_before = node.aabb;
|
||||
|
||||
node_update_aabb(node);
|
||||
|
||||
BVHABB_CLASS abb_after = node.aabb;
|
||||
CRASH_COND(abb_before != abb_after);
|
||||
}
|
||||
|
||||
void node_update_aabb(TNode &tnode) {
|
||||
tnode.aabb.set_to_max_opposite_extents();
|
||||
tnode.height = 0;
|
||||
|
||||
if (!tnode.is_leaf()) {
|
||||
for (int n = 0; n < tnode.num_children; n++) {
|
||||
uint32_t child_node_id = tnode.children[n];
|
||||
|
||||
// merge with child aabb
|
||||
const TNode &tchild = _nodes[child_node_id];
|
||||
tnode.aabb.merge(tchild.aabb);
|
||||
|
||||
// do heights at the same time
|
||||
if (tchild.height > tnode.height) {
|
||||
tnode.height = tchild.height;
|
||||
}
|
||||
}
|
||||
|
||||
// the height of a non leaf is always 1 bigger than the biggest child
|
||||
tnode.height++;
|
||||
|
||||
#ifdef BVH_CHECKS
|
||||
if (!tnode.num_children) {
|
||||
// the 'blank' aabb will screw up parent aabbs
|
||||
WARN_PRINT("BVH_Tree::TNode no children, AABB is undefined");
|
||||
}
|
||||
#endif
|
||||
} else {
|
||||
// leaf
|
||||
const TLeaf &leaf = _node_get_leaf(tnode);
|
||||
|
||||
for (int n = 0; n < leaf.num_items; n++) {
|
||||
tnode.aabb.merge(leaf.get_aabb(n));
|
||||
}
|
||||
|
||||
// now the leaf items are unexpanded, we expand only in the node AABB
|
||||
tnode.aabb.expand(_node_expansion);
|
||||
#ifdef BVH_CHECKS
|
||||
if (!leaf.num_items) {
|
||||
// the 'blank' aabb will screw up parent aabbs
|
||||
WARN_PRINT("BVH_Tree::TLeaf no items, AABB is undefined");
|
||||
}
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
void refit_all(int p_tree_id) {
|
||||
refit_downward(_root_node_id[p_tree_id]);
|
||||
}
|
||||
|
||||
void refit_upward(uint32_t p_node_id) {
|
||||
while (p_node_id != BVHCommon::INVALID) {
|
||||
TNode &tnode = _nodes[p_node_id];
|
||||
node_update_aabb(tnode);
|
||||
p_node_id = tnode.parent_id;
|
||||
}
|
||||
}
|
||||
|
||||
void refit_upward_and_balance(uint32_t p_node_id) {
|
||||
while (p_node_id != BVHCommon::INVALID) {
|
||||
uint32_t before = p_node_id;
|
||||
p_node_id = _logic_balance(p_node_id);
|
||||
|
||||
if (before != p_node_id) {
|
||||
VERBOSE_PRINT("REBALANCED!");
|
||||
}
|
||||
|
||||
TNode &tnode = _nodes[p_node_id];
|
||||
|
||||
// update overall aabb from the children
|
||||
node_update_aabb(tnode);
|
||||
|
||||
p_node_id = tnode.parent_id;
|
||||
}
|
||||
}
|
||||
|
||||
void refit_downward(uint32_t p_node_id) {
|
||||
TNode &tnode = _nodes[p_node_id];
|
||||
|
||||
// do children first
|
||||
if (!tnode.is_leaf()) {
|
||||
for (int n = 0; n < tnode.num_children; n++) {
|
||||
refit_downward(tnode.children[n]);
|
||||
}
|
||||
}
|
||||
|
||||
node_update_aabb(tnode);
|
||||
}
|
||||
|
||||
// go down to the leaves, then refit upward
|
||||
void refit_branch(uint32_t p_node_id) {
|
||||
// our function parameters to keep on a stack
|
||||
struct RefitParams {
|
||||
uint32_t node_id;
|
||||
};
|
||||
|
||||
// most of the iterative functionality is contained in this helper class
|
||||
BVH_IterativeInfo<RefitParams> ii;
|
||||
|
||||
// alloca must allocate the stack from this function, it cannot be allocated in the
|
||||
// helper class
|
||||
ii.stack = (RefitParams *)alloca(ii.get_alloca_stacksize());
|
||||
|
||||
// seed the stack
|
||||
ii.get_first()->node_id = p_node_id;
|
||||
|
||||
RefitParams rp;
|
||||
|
||||
// while there are still more nodes on the stack
|
||||
while (ii.pop(rp)) {
|
||||
TNode &tnode = _nodes[rp.node_id];
|
||||
|
||||
// do children first
|
||||
if (!tnode.is_leaf()) {
|
||||
for (int n = 0; n < tnode.num_children; n++) {
|
||||
uint32_t child_id = tnode.children[n];
|
||||
|
||||
// add to the stack
|
||||
RefitParams *child = ii.request();
|
||||
child->node_id = child_id;
|
||||
}
|
||||
} else {
|
||||
// leaf .. only refit upward if dirty
|
||||
TLeaf &leaf = _node_get_leaf(tnode);
|
||||
if (leaf.is_dirty()) {
|
||||
leaf.set_dirty(false);
|
||||
refit_upward(p_node_id);
|
||||
}
|
||||
}
|
||||
} // while more nodes to pop
|
||||
}
|
||||
|
|
@ -0,0 +1,294 @@
|
|||
void _split_inform_references(uint32_t p_node_id) {
|
||||
TNode &node = _nodes[p_node_id];
|
||||
TLeaf &leaf = _node_get_leaf(node);
|
||||
|
||||
for (int n = 0; n < leaf.num_items; n++) {
|
||||
uint32_t ref_id = leaf.get_item_ref_id(n);
|
||||
|
||||
ItemRef &ref = _refs[ref_id];
|
||||
ref.tnode_id = p_node_id;
|
||||
ref.item_id = n;
|
||||
}
|
||||
}
|
||||
|
||||
void _split_leaf_sort_groups_simple(int &num_a, int &num_b, uint16_t *group_a, uint16_t *group_b, const BVHABB_CLASS *temp_bounds, const BVHABB_CLASS full_bound) {
|
||||
// special case for low leaf sizes .. should static compile out
|
||||
if (MAX_ITEMS < 4) {
|
||||
uint32_t ind = group_a[0];
|
||||
|
||||
// add to b
|
||||
group_b[num_b++] = ind;
|
||||
|
||||
// remove from a
|
||||
group_a[0] = group_a[num_a - 1];
|
||||
num_a--;
|
||||
return;
|
||||
}
|
||||
|
||||
Point centre = full_bound.calculate_centre();
|
||||
Point size = full_bound.calculate_size();
|
||||
|
||||
int order[3];
|
||||
|
||||
order[0] = size.min_axis();
|
||||
order[2] = size.max_axis();
|
||||
order[1] = 3 - (order[0] + order[2]);
|
||||
|
||||
// simplest case, split on the longest axis
|
||||
int split_axis = order[0];
|
||||
for (int a = 0; a < num_a; a++) {
|
||||
uint32_t ind = group_a[a];
|
||||
|
||||
if (temp_bounds[ind].min.coord[split_axis] > centre.coord[split_axis]) {
|
||||
// add to b
|
||||
group_b[num_b++] = ind;
|
||||
|
||||
// remove from a
|
||||
group_a[a] = group_a[num_a - 1];
|
||||
num_a--;
|
||||
|
||||
// do this one again, as it has been replaced
|
||||
a--;
|
||||
}
|
||||
}
|
||||
|
||||
// detect when split on longest axis failed
|
||||
int min_threshold = MAX_ITEMS / 4;
|
||||
int min_group_size[3];
|
||||
min_group_size[0] = MIN(num_a, num_b);
|
||||
if (min_group_size[0] < min_threshold) {
|
||||
// slow but sure .. first move everything back into a
|
||||
for (int b = 0; b < num_b; b++) {
|
||||
group_a[num_a++] = group_b[b];
|
||||
}
|
||||
num_b = 0;
|
||||
|
||||
// now calculate the best split
|
||||
for (int axis = 1; axis < 3; axis++) {
|
||||
split_axis = order[axis];
|
||||
int count = 0;
|
||||
|
||||
for (int a = 0; a < num_a; a++) {
|
||||
uint32_t ind = group_a[a];
|
||||
|
||||
if (temp_bounds[ind].min.coord[split_axis] > centre.coord[split_axis]) {
|
||||
count++;
|
||||
}
|
||||
}
|
||||
|
||||
min_group_size[axis] = MIN(count, num_a - count);
|
||||
} // for axis
|
||||
|
||||
// best axis
|
||||
int best_axis = 0;
|
||||
int best_min = min_group_size[0];
|
||||
for (int axis = 1; axis < 3; axis++) {
|
||||
if (min_group_size[axis] > best_min) {
|
||||
best_min = min_group_size[axis];
|
||||
best_axis = axis;
|
||||
}
|
||||
}
|
||||
|
||||
// now finally do the split
|
||||
if (best_min > 0) {
|
||||
split_axis = order[best_axis];
|
||||
|
||||
for (int a = 0; a < num_a; a++) {
|
||||
uint32_t ind = group_a[a];
|
||||
|
||||
if (temp_bounds[ind].min.coord[split_axis] > centre.coord[split_axis]) {
|
||||
// add to b
|
||||
group_b[num_b++] = ind;
|
||||
|
||||
// remove from a
|
||||
group_a[a] = group_a[num_a - 1];
|
||||
num_a--;
|
||||
|
||||
// do this one again, as it has been replaced
|
||||
a--;
|
||||
}
|
||||
}
|
||||
} // if there was a split!
|
||||
} // if the longest axis wasn't a good split
|
||||
|
||||
// special case, none crossed threshold
|
||||
if (!num_b) {
|
||||
uint32_t ind = group_a[0];
|
||||
|
||||
// add to b
|
||||
group_b[num_b++] = ind;
|
||||
|
||||
// remove from a
|
||||
group_a[0] = group_a[num_a - 1];
|
||||
num_a--;
|
||||
}
|
||||
// opposite problem! :)
|
||||
if (!num_a) {
|
||||
uint32_t ind = group_b[0];
|
||||
|
||||
// add to a
|
||||
group_a[num_a++] = ind;
|
||||
|
||||
// remove from b
|
||||
group_b[0] = group_b[num_b - 1];
|
||||
num_b--;
|
||||
}
|
||||
}
|
||||
|
||||
void _split_leaf_sort_groups(int &num_a, int &num_b, uint16_t *group_a, uint16_t *group_b, const BVHABB_CLASS *temp_bounds) {
|
||||
BVHABB_CLASS groupb_aabb;
|
||||
groupb_aabb.set_to_max_opposite_extents();
|
||||
for (int n = 0; n < num_b; n++) {
|
||||
int which = group_b[n];
|
||||
groupb_aabb.merge(temp_bounds[which]);
|
||||
}
|
||||
BVHABB_CLASS groupb_aabb_new;
|
||||
|
||||
BVHABB_CLASS rest_aabb;
|
||||
|
||||
float best_size = FLT_MAX;
|
||||
int best_candidate = -1;
|
||||
|
||||
// find most likely from a to move into b
|
||||
for (int check = 0; check < num_a; check++) {
|
||||
rest_aabb.set_to_max_opposite_extents();
|
||||
groupb_aabb_new = groupb_aabb;
|
||||
|
||||
// find aabb of all the rest
|
||||
for (int rest = 0; rest < num_a; rest++) {
|
||||
if (rest == check) {
|
||||
continue;
|
||||
}
|
||||
|
||||
int which = group_a[rest];
|
||||
rest_aabb.merge(temp_bounds[which]);
|
||||
}
|
||||
|
||||
groupb_aabb_new.merge(temp_bounds[group_a[check]]);
|
||||
|
||||
// now compare the sizes
|
||||
float size = groupb_aabb_new.get_area() + rest_aabb.get_area();
|
||||
if (size < best_size) {
|
||||
best_size = size;
|
||||
best_candidate = check;
|
||||
}
|
||||
}
|
||||
|
||||
// we should now have the best, move it from group a to group b
|
||||
group_b[num_b++] = group_a[best_candidate];
|
||||
|
||||
// remove best candidate from group a
|
||||
num_a--;
|
||||
group_a[best_candidate] = group_a[num_a];
|
||||
}
|
||||
|
||||
uint32_t split_leaf(uint32_t p_node_id, const BVHABB_CLASS &p_added_item_aabb) {
|
||||
return split_leaf_complex(p_node_id, p_added_item_aabb);
|
||||
}
|
||||
|
||||
// aabb is the new inserted node
|
||||
uint32_t split_leaf_complex(uint32_t p_node_id, const BVHABB_CLASS &p_added_item_aabb) {
|
||||
VERBOSE_PRINT("split_leaf");
|
||||
|
||||
// note the tnode before and AFTER splitting may be a different address
|
||||
// in memory because the vector could get relocated. So we need to reget
|
||||
// the tnode after the split
|
||||
BVH_ASSERT(_nodes[p_node_id].is_leaf());
|
||||
|
||||
// first create child leaf nodes
|
||||
uint32_t *child_ids = (uint32_t *)alloca(sizeof(uint32_t) * MAX_CHILDREN);
|
||||
|
||||
for (int n = 0; n < MAX_CHILDREN; n++) {
|
||||
// create node children
|
||||
TNode *child_node = _nodes.request(child_ids[n]);
|
||||
|
||||
child_node->clear();
|
||||
|
||||
// back link to parent
|
||||
child_node->parent_id = p_node_id;
|
||||
|
||||
// make each child a leaf node
|
||||
node_make_leaf(child_ids[n]);
|
||||
}
|
||||
|
||||
// don't get any leaves or nodes till AFTER the split
|
||||
TNode &tnode = _nodes[p_node_id];
|
||||
uint32_t orig_leaf_id = tnode.get_leaf_id();
|
||||
const TLeaf &orig_leaf = _node_get_leaf(tnode);
|
||||
|
||||
// store the final child ids
|
||||
for (int n = 0; n < MAX_CHILDREN; n++) {
|
||||
tnode.children[n] = child_ids[n];
|
||||
}
|
||||
|
||||
// mark as no longer a leaf node
|
||||
tnode.num_children = MAX_CHILDREN;
|
||||
|
||||
// 2 groups, A and B, and assign children to each to split equally
|
||||
int max_children = orig_leaf.num_items + 1; // plus 1 for the wildcard .. the item being added
|
||||
//CRASH_COND(max_children > MAX_CHILDREN);
|
||||
|
||||
uint16_t *group_a = (uint16_t *)alloca(sizeof(uint16_t) * max_children);
|
||||
uint16_t *group_b = (uint16_t *)alloca(sizeof(uint16_t) * max_children);
|
||||
|
||||
// we are copying the ABBs. This is ugly, but we need one extra for the inserted item...
|
||||
BVHABB_CLASS *temp_bounds = (BVHABB_CLASS *)alloca(sizeof(BVHABB_CLASS) * max_children);
|
||||
|
||||
int num_a = max_children;
|
||||
int num_b = 0;
|
||||
|
||||
// setup - start with all in group a
|
||||
for (int n = 0; n < orig_leaf.num_items; n++) {
|
||||
group_a[n] = n;
|
||||
temp_bounds[n] = orig_leaf.get_aabb(n);
|
||||
}
|
||||
// wildcard
|
||||
int wildcard = orig_leaf.num_items;
|
||||
|
||||
group_a[wildcard] = wildcard;
|
||||
temp_bounds[wildcard] = p_added_item_aabb;
|
||||
|
||||
// we can choose here either an equal split, or just 1 in the new leaf
|
||||
_split_leaf_sort_groups_simple(num_a, num_b, group_a, group_b, temp_bounds, tnode.aabb);
|
||||
|
||||
uint32_t wildcard_node = BVHCommon::INVALID;
|
||||
|
||||
// now there should be equal numbers in both groups
|
||||
for (int n = 0; n < num_a; n++) {
|
||||
int which = group_a[n];
|
||||
|
||||
if (which != wildcard) {
|
||||
const BVHABB_CLASS &source_item_aabb = orig_leaf.get_aabb(which);
|
||||
uint32_t source_item_ref_id = orig_leaf.get_item_ref_id(which);
|
||||
//const Item &source_item = orig_leaf.get_item(which);
|
||||
_node_add_item(tnode.children[0], source_item_ref_id, source_item_aabb);
|
||||
} else {
|
||||
wildcard_node = tnode.children[0];
|
||||
}
|
||||
}
|
||||
for (int n = 0; n < num_b; n++) {
|
||||
int which = group_b[n];
|
||||
|
||||
if (which != wildcard) {
|
||||
const BVHABB_CLASS &source_item_aabb = orig_leaf.get_aabb(which);
|
||||
uint32_t source_item_ref_id = orig_leaf.get_item_ref_id(which);
|
||||
//const Item &source_item = orig_leaf.get_item(which);
|
||||
_node_add_item(tnode.children[1], source_item_ref_id, source_item_aabb);
|
||||
} else {
|
||||
wildcard_node = tnode.children[1];
|
||||
}
|
||||
}
|
||||
|
||||
// now remove all items from the parent and replace with the child nodes
|
||||
_leaves.free(orig_leaf_id);
|
||||
|
||||
// we should keep the references up to date!
|
||||
for (int n = 0; n < MAX_CHILDREN; n++) {
|
||||
_split_inform_references(tnode.children[n]);
|
||||
}
|
||||
|
||||
refit_upward(p_node_id);
|
||||
|
||||
BVH_ASSERT(wildcard_node != BVHCommon::INVALID);
|
||||
return wildcard_node;
|
||||
}
|
||||
|
|
@ -0,0 +1,181 @@
|
|||
|
||||
public:
|
||||
struct ItemRef {
|
||||
uint32_t tnode_id; // -1 is invalid
|
||||
uint32_t item_id; // in the leaf
|
||||
|
||||
bool is_active() const { return tnode_id != BVHCommon::INACTIVE; }
|
||||
void set_inactive() {
|
||||
tnode_id = BVHCommon::INACTIVE;
|
||||
item_id = BVHCommon::INACTIVE;
|
||||
}
|
||||
};
|
||||
|
||||
// extra info kept in separate parallel list to the references,
|
||||
// as this is less used as keeps cache better
|
||||
struct ItemExtra {
|
||||
uint32_t last_updated_tick;
|
||||
uint32_t pairable;
|
||||
uint32_t pairable_mask;
|
||||
uint32_t pairable_type;
|
||||
|
||||
int32_t subindex;
|
||||
|
||||
// the active reference is a separate list of which references
|
||||
// are active so that we can slowly iterate through it over many frames for
|
||||
// slow optimize.
|
||||
uint32_t active_ref_id;
|
||||
|
||||
T *userdata;
|
||||
};
|
||||
|
||||
// this is an item OR a child node depending on whether a leaf node
|
||||
struct Item {
|
||||
BVHABB_CLASS aabb;
|
||||
uint32_t item_ref_id;
|
||||
};
|
||||
|
||||
// tree leaf
|
||||
struct TLeaf {
|
||||
uint16_t num_items;
|
||||
|
||||
private:
|
||||
uint16_t dirty;
|
||||
// separate data orientated lists for faster SIMD traversal
|
||||
uint32_t item_ref_ids[MAX_ITEMS];
|
||||
BVHABB_CLASS aabbs[MAX_ITEMS];
|
||||
|
||||
public:
|
||||
// accessors
|
||||
BVHABB_CLASS &get_aabb(uint32_t p_id) { return aabbs[p_id]; }
|
||||
const BVHABB_CLASS &get_aabb(uint32_t p_id) const { return aabbs[p_id]; }
|
||||
|
||||
uint32_t &get_item_ref_id(uint32_t p_id) { return item_ref_ids[p_id]; }
|
||||
const uint32_t &get_item_ref_id(uint32_t p_id) const { return item_ref_ids[p_id]; }
|
||||
|
||||
bool is_dirty() const { return dirty; }
|
||||
void set_dirty(bool p) { dirty = p; }
|
||||
|
||||
void clear() {
|
||||
num_items = 0;
|
||||
set_dirty(true);
|
||||
}
|
||||
bool is_full() const { return num_items >= MAX_ITEMS; }
|
||||
|
||||
void remove_item_unordered(uint32_t p_id) {
|
||||
BVH_ASSERT(p_id < num_items);
|
||||
num_items--;
|
||||
aabbs[p_id] = aabbs[num_items];
|
||||
item_ref_ids[p_id] = item_ref_ids[num_items];
|
||||
}
|
||||
|
||||
uint32_t request_item() {
|
||||
if (num_items < MAX_ITEMS) {
|
||||
uint32_t id = num_items;
|
||||
num_items++;
|
||||
return id;
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
};
|
||||
|
||||
// tree node
|
||||
struct TNode {
|
||||
BVHABB_CLASS aabb;
|
||||
// either number of children if positive
|
||||
// or leaf id if negative (leaf id 0 is disallowed)
|
||||
union {
|
||||
int32_t num_children;
|
||||
int32_t neg_leaf_id;
|
||||
};
|
||||
uint32_t parent_id; // or -1
|
||||
uint16_t children[MAX_CHILDREN];
|
||||
|
||||
// height in the tree, where leaves are 0, and all above are 1+
|
||||
// (or the highest where there is a tie off)
|
||||
int32_t height;
|
||||
|
||||
bool is_leaf() const { return num_children < 0; }
|
||||
void set_leaf_id(int id) { neg_leaf_id = -id; }
|
||||
int get_leaf_id() const { return -neg_leaf_id; }
|
||||
|
||||
void clear() {
|
||||
num_children = 0;
|
||||
parent_id = BVHCommon::INVALID;
|
||||
height = 0; // or -1 for testing
|
||||
|
||||
// for safety set to improbable value
|
||||
aabb.set_to_max_opposite_extents();
|
||||
|
||||
// other members are not blanked for speed .. they may be uninitialized
|
||||
}
|
||||
|
||||
bool is_full_of_children() const { return num_children >= MAX_CHILDREN; }
|
||||
|
||||
void remove_child_internal(uint32_t child_num) {
|
||||
children[child_num] = children[num_children - 1];
|
||||
num_children--;
|
||||
}
|
||||
|
||||
int find_child(uint32_t p_child_node_id) {
|
||||
BVH_ASSERT(!is_leaf());
|
||||
|
||||
for (int n = 0; n < num_children; n++) {
|
||||
if (children[n] == p_child_node_id) {
|
||||
return n;
|
||||
}
|
||||
}
|
||||
|
||||
// not found
|
||||
return -1;
|
||||
}
|
||||
};
|
||||
|
||||
// instead of using linked list we maintain
|
||||
// item references (for quick lookup)
|
||||
PooledList<ItemRef, true> _refs;
|
||||
PooledList<ItemExtra, true> _extra;
|
||||
PooledList<ItemPairs> _pairs;
|
||||
|
||||
// these 2 are not in sync .. nodes != leaves!
|
||||
PooledList<TNode, true> _nodes;
|
||||
PooledList<TLeaf, true> _leaves;
|
||||
|
||||
// we can maintain an un-ordered list of which references are active,
|
||||
// in order to do a slow incremental optimize of the tree over each frame.
|
||||
// This will work best if dynamic objects and static objects are in a different tree.
|
||||
LocalVector<uint32_t, uint32_t, true> _active_refs;
|
||||
uint32_t _current_active_ref = 0;
|
||||
|
||||
// instead of translating directly to the userdata output,
|
||||
// we keep an intermediate list of hits as reference IDs, which can be used
|
||||
// for pairing collision detection
|
||||
LocalVector<uint32_t, uint32_t, true> _cull_hits;
|
||||
|
||||
// we now have multiple root nodes, allowing us to store
|
||||
// more than 1 tree. This can be more efficient, while sharing the same
|
||||
// common lists
|
||||
enum { NUM_TREES = 2,
|
||||
};
|
||||
|
||||
// Tree 0 - Non pairable
|
||||
// Tree 1 - Pairable
|
||||
// This is more efficient because in physics we only need check non pairable against the pairable tree.
|
||||
uint32_t _root_node_id[NUM_TREES];
|
||||
int _current_tree = 0;
|
||||
|
||||
// these values may need tweaking according to the project
|
||||
// the bound of the world, and the average velocities of the objects
|
||||
|
||||
// node expansion is important in the rendering tree
|
||||
// larger values give less re-insertion as items move...
|
||||
// but on the other hand over estimates the bounding box of nodes.
|
||||
// we can either use auto mode, where the expansion is based on the root node size, or specify manually
|
||||
real_t _node_expansion = 0.5;
|
||||
bool _auto_node_expansion = true;
|
||||
|
||||
// pairing expansion important for physics pairing
|
||||
// larger values gives more 'sticky' pairing, and is less likely to exhibit tunneling
|
||||
// we can either use auto mode, where the expansion is based on the root node size, or specify manually
|
||||
real_t _pairing_expansion = 0.1;
|
||||
bool _auto_pairing_expansion = true;
|
||||
|
|
@ -0,0 +1,422 @@
|
|||
/*************************************************************************/
|
||||
/* bvh_tree.h */
|
||||
/*************************************************************************/
|
||||
/* 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. */
|
||||
/*************************************************************************/
|
||||
|
||||
#ifndef BVH_TREE_H
|
||||
#define BVH_TREE_H
|
||||
|
||||
// BVH Tree
|
||||
// This is an implementation of a dynamic BVH with templated leaf size.
|
||||
// This differs from most dynamic BVH in that it can handle more than 1 object
|
||||
// in leaf nodes. This can make it far more efficient in certain circumstances.
|
||||
// It also means that the splitting logic etc have to be completely different
|
||||
// to a simpler tree.
|
||||
// Note that MAX_CHILDREN should be fixed at 2 for now.
|
||||
|
||||
#include "core/math/aabb.h"
|
||||
#include "core/math/bvh_abb.h"
|
||||
#include "core/math/geometry_3d.h"
|
||||
#include "core/math/vector3.h"
|
||||
#include "core/string/print_string.h"
|
||||
#include "core/templates/local_vector.h"
|
||||
#include "core/templates/pooled_list.h"
|
||||
#include <limits.h>
|
||||
|
||||
#define BVHABB_CLASS BVH_ABB<Bounds, Point>
|
||||
|
||||
// never do these checks in release
|
||||
#if defined(TOOLS_ENABLED) && defined(DEBUG_ENABLED)
|
||||
//#define BVH_VERBOSE
|
||||
//#define BVH_VERBOSE_TREE
|
||||
|
||||
//#define BVH_VERBOSE_FRAME
|
||||
//#define BVH_CHECKS
|
||||
//#define BVH_INTEGRITY_CHECKS
|
||||
#endif
|
||||
|
||||
// debug only assert
|
||||
#ifdef BVH_CHECKS
|
||||
#define BVH_ASSERT(a) CRASH_COND((a) == false)
|
||||
#else
|
||||
#define BVH_ASSERT(a)
|
||||
#endif
|
||||
|
||||
#ifdef BVH_VERBOSE
|
||||
#define VERBOSE_PRINT print_line
|
||||
#else
|
||||
#define VERBOSE_PRINT(a)
|
||||
#endif
|
||||
|
||||
// really just a namespace
|
||||
struct BVHCommon {
|
||||
// these could possibly also be the same constant,
|
||||
// although this may be useful for debugging.
|
||||
// or use zero for invalid and +1 based indices.
|
||||
static const uint32_t INVALID = (0xffffffff);
|
||||
static const uint32_t INACTIVE = (0xfffffffe);
|
||||
};
|
||||
|
||||
// really a handle, can be anything
|
||||
// note that zero is a valid reference for the BVH .. this may involve using
|
||||
// a plus one based ID for clients that expect 0 to be invalid.
|
||||
struct BVHHandle {
|
||||
// conversion operator
|
||||
operator uint32_t() const { return _data; }
|
||||
void set(uint32_t p_value) { _data = p_value; }
|
||||
|
||||
uint32_t _data;
|
||||
|
||||
void set_invalid() { _data = BVHCommon::INVALID; }
|
||||
bool is_invalid() const { return _data == BVHCommon::INVALID; }
|
||||
uint32_t id() const { return _data; }
|
||||
void set_id(uint32_t p_id) { _data = p_id; }
|
||||
|
||||
bool operator==(const BVHHandle &p_h) const { return _data == p_h._data; }
|
||||
bool operator!=(const BVHHandle &p_h) const { return (*this == p_h) == false; }
|
||||
};
|
||||
|
||||
// helper class to make iterative versions of recursive functions
|
||||
template <class T>
|
||||
class BVH_IterativeInfo {
|
||||
public:
|
||||
enum {
|
||||
ALLOCA_STACK_SIZE = 128
|
||||
};
|
||||
|
||||
int32_t depth = 1;
|
||||
int32_t threshold = ALLOCA_STACK_SIZE - 2;
|
||||
T *stack;
|
||||
//only used in rare occasions when you run out of alloca memory
|
||||
// because tree is too unbalanced.
|
||||
LocalVector<T> aux_stack;
|
||||
int32_t get_alloca_stacksize() const { return ALLOCA_STACK_SIZE * sizeof(T); }
|
||||
|
||||
T *get_first() const {
|
||||
return &stack[0];
|
||||
}
|
||||
|
||||
// pop the last member of the stack, or return false
|
||||
bool pop(T &r_value) {
|
||||
if (!depth) {
|
||||
return false;
|
||||
}
|
||||
|
||||
depth--;
|
||||
r_value = stack[depth];
|
||||
return true;
|
||||
}
|
||||
|
||||
// request new addition to stack
|
||||
T *request() {
|
||||
if (depth > threshold) {
|
||||
if (aux_stack.is_empty()) {
|
||||
aux_stack.resize(ALLOCA_STACK_SIZE * 2);
|
||||
memcpy(aux_stack.ptr(), stack, get_alloca_stacksize());
|
||||
} else {
|
||||
aux_stack.resize(aux_stack.size() * 2);
|
||||
}
|
||||
stack = aux_stack.ptr();
|
||||
threshold = aux_stack.size() - 2;
|
||||
}
|
||||
return &stack[depth++];
|
||||
}
|
||||
};
|
||||
|
||||
template <class T, int MAX_CHILDREN, int MAX_ITEMS, bool USE_PAIRS = false, class Bounds = AABB, class Point = Vector3>
|
||||
class BVH_Tree {
|
||||
friend class BVH;
|
||||
|
||||
#include "bvh_pair.inc"
|
||||
#include "bvh_structs.inc"
|
||||
|
||||
public:
|
||||
BVH_Tree() {
|
||||
for (int n = 0; n < NUM_TREES; n++) {
|
||||
_root_node_id[n] = BVHCommon::INVALID;
|
||||
}
|
||||
|
||||
// disallow zero leaf ids
|
||||
// (as these ids are stored as negative numbers in the node)
|
||||
uint32_t dummy_leaf_id;
|
||||
_leaves.request(dummy_leaf_id);
|
||||
}
|
||||
|
||||
private:
|
||||
bool node_add_child(uint32_t p_node_id, uint32_t p_child_node_id) {
|
||||
TNode &tnode = _nodes[p_node_id];
|
||||
if (tnode.is_full_of_children()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
tnode.children[tnode.num_children] = p_child_node_id;
|
||||
tnode.num_children += 1;
|
||||
|
||||
// back link in the child to the parent
|
||||
TNode &tnode_child = _nodes[p_child_node_id];
|
||||
tnode_child.parent_id = p_node_id;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void node_replace_child(uint32_t p_parent_id, uint32_t p_old_child_id, uint32_t p_new_child_id) {
|
||||
TNode &parent = _nodes[p_parent_id];
|
||||
BVH_ASSERT(!parent.is_leaf());
|
||||
|
||||
int child_num = parent.find_child(p_old_child_id);
|
||||
BVH_ASSERT(child_num != BVHCommon::INVALID);
|
||||
parent.children[child_num] = p_new_child_id;
|
||||
|
||||
TNode &new_child = _nodes[p_new_child_id];
|
||||
new_child.parent_id = p_parent_id;
|
||||
}
|
||||
|
||||
void node_remove_child(uint32_t p_parent_id, uint32_t p_child_id, bool p_prevent_sibling = false) {
|
||||
TNode &parent = _nodes[p_parent_id];
|
||||
BVH_ASSERT(!parent.is_leaf());
|
||||
|
||||
int child_num = parent.find_child(p_child_id);
|
||||
BVH_ASSERT(child_num != BVHCommon::INVALID);
|
||||
|
||||
parent.remove_child_internal(child_num);
|
||||
|
||||
// no need to keep back references for children at the moment
|
||||
|
||||
uint32_t sibling_id; // always a node id, as tnode is never a leaf
|
||||
bool sibling_present = false;
|
||||
|
||||
// if there are more children, or this is the root node, don't try and delete
|
||||
if (parent.num_children > 1) {
|
||||
return;
|
||||
}
|
||||
|
||||
// if there is 1 sibling, it can be moved to be a child of the
|
||||
if (parent.num_children == 1) {
|
||||
// else there is now a redundant node with one child, which can be removed
|
||||
sibling_id = parent.children[0];
|
||||
sibling_present = true;
|
||||
}
|
||||
|
||||
// now there may be no children in this node .. in which case it can be deleted
|
||||
// remove node if empty
|
||||
// remove link from parent
|
||||
uint32_t grandparent_id = parent.parent_id;
|
||||
|
||||
// special case for root node
|
||||
if (grandparent_id == BVHCommon::INVALID) {
|
||||
if (sibling_present) {
|
||||
// change the root node
|
||||
change_root_node(sibling_id);
|
||||
|
||||
// delete the old root node as no longer needed
|
||||
_nodes.free(p_parent_id);
|
||||
}
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
if (sibling_present) {
|
||||
node_replace_child(grandparent_id, p_parent_id, sibling_id);
|
||||
} else {
|
||||
node_remove_child(grandparent_id, p_parent_id, true);
|
||||
}
|
||||
|
||||
// put the node on the free list to recycle
|
||||
_nodes.free(p_parent_id);
|
||||
}
|
||||
|
||||
// this relies on _current_tree being accurate
|
||||
void change_root_node(uint32_t p_new_root_id) {
|
||||
_root_node_id[_current_tree] = p_new_root_id;
|
||||
TNode &root = _nodes[p_new_root_id];
|
||||
|
||||
// mark no parent
|
||||
root.parent_id = BVHCommon::INVALID;
|
||||
}
|
||||
|
||||
void node_make_leaf(uint32_t p_node_id) {
|
||||
uint32_t child_leaf_id;
|
||||
TLeaf *child_leaf = _leaves.request(child_leaf_id);
|
||||
child_leaf->clear();
|
||||
|
||||
// zero is reserved at startup, to prevent this id being used
|
||||
// (as they are stored as negative values in the node, and zero is already taken)
|
||||
BVH_ASSERT(child_leaf_id != 0);
|
||||
|
||||
TNode &node = _nodes[p_node_id];
|
||||
node.neg_leaf_id = -(int)child_leaf_id;
|
||||
}
|
||||
|
||||
void node_remove_item(uint32_t p_ref_id, BVHABB_CLASS *r_old_aabb = nullptr) {
|
||||
// get the reference
|
||||
ItemRef &ref = _refs[p_ref_id];
|
||||
uint32_t owner_node_id = ref.tnode_id;
|
||||
|
||||
// debug draw special
|
||||
// This may not be needed
|
||||
if (owner_node_id == BVHCommon::INVALID) {
|
||||
return;
|
||||
}
|
||||
|
||||
TNode &tnode = _nodes[owner_node_id];
|
||||
CRASH_COND(!tnode.is_leaf());
|
||||
|
||||
TLeaf &leaf = _node_get_leaf(tnode);
|
||||
|
||||
// if the aabb is not determining the corner size, then there is no need to refit!
|
||||
// (optimization, as merging AABBs takes a lot of time)
|
||||
const BVHABB_CLASS &old_aabb = leaf.get_aabb(ref.item_id);
|
||||
|
||||
// shrink a little to prevent using corner aabbs
|
||||
// in order to miss the corners first we shrink by node_expansion
|
||||
// (which is added to the overall bound of the leaf), then we also
|
||||
// shrink by an epsilon, in order to miss out the very corner aabbs
|
||||
// which are important in determining the bound. Any other aabb
|
||||
// within this can be removed and not affect the overall bound.
|
||||
BVHABB_CLASS node_bound = tnode.aabb;
|
||||
node_bound.expand(-_node_expansion - 0.001f);
|
||||
bool refit = true;
|
||||
|
||||
if (node_bound.is_other_within(old_aabb)) {
|
||||
refit = false;
|
||||
}
|
||||
|
||||
// record the old aabb if required (for incremental remove_and_reinsert)
|
||||
if (r_old_aabb) {
|
||||
*r_old_aabb = old_aabb;
|
||||
}
|
||||
|
||||
leaf.remove_item_unordered(ref.item_id);
|
||||
|
||||
if (leaf.num_items) {
|
||||
// the swapped item has to have its reference changed to, to point to the new item id
|
||||
uint32_t swapped_ref_id = leaf.get_item_ref_id(ref.item_id);
|
||||
|
||||
ItemRef &swapped_ref = _refs[swapped_ref_id];
|
||||
|
||||
swapped_ref.item_id = ref.item_id;
|
||||
|
||||
// only have to refit if it is an edge item
|
||||
// This is a VERY EXPENSIVE STEP
|
||||
// we defer the refit updates until the update function is called once per frame
|
||||
if (refit) {
|
||||
leaf.set_dirty(true);
|
||||
}
|
||||
} else {
|
||||
// remove node if empty
|
||||
// remove link from parent
|
||||
if (tnode.parent_id != BVHCommon::INVALID) {
|
||||
// DANGER .. this can potentially end up with root node with 1 child ...
|
||||
// we don't want this and must check for it
|
||||
|
||||
uint32_t parent_id = tnode.parent_id;
|
||||
|
||||
node_remove_child(parent_id, owner_node_id);
|
||||
refit_upward(parent_id);
|
||||
|
||||
// put the node on the free list to recycle
|
||||
_nodes.free(owner_node_id);
|
||||
}
|
||||
|
||||
// else if no parent, it is the root node. Do not delete
|
||||
}
|
||||
|
||||
ref.tnode_id = BVHCommon::INVALID;
|
||||
ref.item_id = BVHCommon::INVALID; // unset
|
||||
}
|
||||
|
||||
// returns true if needs refit of PARENT tree only, the node itself AABB is calculated
|
||||
// within this routine
|
||||
bool _node_add_item(uint32_t p_node_id, uint32_t p_ref_id, const BVHABB_CLASS &p_aabb) {
|
||||
ItemRef &ref = _refs[p_ref_id];
|
||||
ref.tnode_id = p_node_id;
|
||||
|
||||
TNode &node = _nodes[p_node_id];
|
||||
BVH_ASSERT(node.is_leaf());
|
||||
TLeaf &leaf = _node_get_leaf(node);
|
||||
|
||||
// optimization - we only need to do a refit
|
||||
// if the added item is changing the AABB of the node.
|
||||
// in most cases it won't.
|
||||
bool needs_refit = true;
|
||||
|
||||
// expand bound now
|
||||
BVHABB_CLASS expanded = p_aabb;
|
||||
expanded.expand(_node_expansion);
|
||||
|
||||
// the bound will only be valid if there is an item in there already
|
||||
if (leaf.num_items) {
|
||||
if (node.aabb.is_other_within(expanded)) {
|
||||
// no change to node AABBs
|
||||
needs_refit = false;
|
||||
} else {
|
||||
node.aabb.merge(expanded);
|
||||
}
|
||||
} else {
|
||||
// bound of the node = the new aabb
|
||||
node.aabb = expanded;
|
||||
}
|
||||
|
||||
ref.item_id = leaf.request_item();
|
||||
BVH_ASSERT(ref.item_id != BVHCommon::INVALID);
|
||||
|
||||
// set the aabb of the new item
|
||||
leaf.get_aabb(ref.item_id) = p_aabb;
|
||||
|
||||
// back reference on the item back to the item reference
|
||||
leaf.get_item_ref_id(ref.item_id) = p_ref_id;
|
||||
|
||||
return needs_refit;
|
||||
}
|
||||
|
||||
uint32_t _node_create_another_child(uint32_t p_node_id, const BVHABB_CLASS &p_aabb) {
|
||||
uint32_t child_node_id;
|
||||
TNode *child_node = _nodes.request(child_node_id);
|
||||
child_node->clear();
|
||||
|
||||
// may not be necessary
|
||||
child_node->aabb = p_aabb;
|
||||
|
||||
node_add_child(p_node_id, child_node_id);
|
||||
|
||||
return child_node_id;
|
||||
}
|
||||
|
||||
#include "bvh_cull.inc"
|
||||
#include "bvh_debug.inc"
|
||||
#include "bvh_integrity.inc"
|
||||
#include "bvh_logic.inc"
|
||||
#include "bvh_misc.inc"
|
||||
#include "bvh_public.inc"
|
||||
#include "bvh_refit.inc"
|
||||
#include "bvh_split.inc"
|
||||
};
|
||||
|
||||
#undef VERBOSE_PRINT
|
||||
|
||||
#endif // BVH_TREE_H
|
||||
|
|
@ -182,13 +182,17 @@ struct Rect2 {
|
|||
|
||||
inline Rect2 grow(real_t p_amount) const {
|
||||
Rect2 g = *this;
|
||||
g.position.x -= p_amount;
|
||||
g.position.y -= p_amount;
|
||||
g.size.width += p_amount * 2;
|
||||
g.size.height += p_amount * 2;
|
||||
g.grow_by(p_amount);
|
||||
return g;
|
||||
}
|
||||
|
||||
inline void grow_by(real_t p_amount) {
|
||||
position.x -= p_amount;
|
||||
position.y -= p_amount;
|
||||
size.width += p_amount * 2;
|
||||
size.height += p_amount * 2;
|
||||
}
|
||||
|
||||
inline Rect2 grow_side(Side p_side, real_t p_amount) const {
|
||||
Rect2 g = *this;
|
||||
g = g.grow_individual((SIDE_LEFT == p_side) ? p_amount : 0,
|
||||
|
|
|
|||
|
|
@ -37,18 +37,26 @@
|
|||
struct Vector2i;
|
||||
|
||||
struct Vector2 {
|
||||
static const int AXIS_COUNT = 2;
|
||||
|
||||
enum Axis {
|
||||
AXIS_X,
|
||||
AXIS_Y,
|
||||
};
|
||||
|
||||
union {
|
||||
real_t x = 0;
|
||||
real_t width;
|
||||
};
|
||||
union {
|
||||
real_t y = 0;
|
||||
real_t height;
|
||||
struct {
|
||||
union {
|
||||
real_t x;
|
||||
real_t width;
|
||||
};
|
||||
union {
|
||||
real_t y;
|
||||
real_t height;
|
||||
};
|
||||
};
|
||||
|
||||
real_t coord[2] = { 0 };
|
||||
};
|
||||
|
||||
_FORCE_INLINE_ real_t &operator[](int p_idx) {
|
||||
|
|
@ -58,6 +66,18 @@ struct Vector2 {
|
|||
return p_idx ? y : x;
|
||||
}
|
||||
|
||||
_FORCE_INLINE_ void set_all(real_t p_value) {
|
||||
x = y = p_value;
|
||||
}
|
||||
|
||||
_FORCE_INLINE_ int min_axis() const {
|
||||
return x < y ? 0 : 1;
|
||||
}
|
||||
|
||||
_FORCE_INLINE_ int max_axis() const {
|
||||
return x < y ? 1 : 0;
|
||||
}
|
||||
|
||||
void normalize();
|
||||
Vector2 normalized() const;
|
||||
bool is_normalized() const;
|
||||
|
|
|
|||
|
|
@ -52,14 +52,6 @@ real_t Vector3::get_axis(int p_axis) const {
|
|||
return operator[](p_axis);
|
||||
}
|
||||
|
||||
int Vector3::min_axis() const {
|
||||
return x < y ? (x < z ? 0 : 2) : (y < z ? 1 : 2);
|
||||
}
|
||||
|
||||
int Vector3::max_axis() const {
|
||||
return x < y ? (y < z ? 2 : 1) : (x < z ? 2 : 0);
|
||||
}
|
||||
|
||||
void Vector3::snap(Vector3 p_step) {
|
||||
x = Math::snapped(x, p_step.x);
|
||||
y = Math::snapped(y, p_step.y);
|
||||
|
|
|
|||
|
|
@ -38,6 +38,8 @@
|
|||
class Basis;
|
||||
|
||||
struct Vector3 {
|
||||
static const int AXIS_COUNT = 3;
|
||||
|
||||
enum Axis {
|
||||
AXIS_X,
|
||||
AXIS_Y,
|
||||
|
|
@ -65,8 +67,17 @@ struct Vector3 {
|
|||
void set_axis(int p_axis, real_t p_value);
|
||||
real_t get_axis(int p_axis) const;
|
||||
|
||||
int min_axis() const;
|
||||
int max_axis() const;
|
||||
_FORCE_INLINE_ void set_all(real_t p_value) {
|
||||
x = y = z = p_value;
|
||||
}
|
||||
|
||||
_FORCE_INLINE_ int min_axis() const {
|
||||
return x < y ? (x < z ? 0 : 2) : (y < z ? 1 : 2);
|
||||
}
|
||||
|
||||
_FORCE_INLINE_ int max_axis() const {
|
||||
return x < y ? (y < z ? 2 : 1) : (x < z ? 2 : 0);
|
||||
}
|
||||
|
||||
_FORCE_INLINE_ real_t length() const;
|
||||
_FORCE_INLINE_ real_t length_squared() const;
|
||||
|
|
|
|||
|
|
@ -1,5 +1,5 @@
|
|||
/*************************************************************************/
|
||||
/* broad_phase_3d_basic.h */
|
||||
/* pooled_list.h */
|
||||
/*************************************************************************/
|
||||
/* This file is part of: */
|
||||
/* GODOT ENGINE */
|
||||
|
|
@ -28,78 +28,68 @@
|
|||
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
|
||||
/*************************************************************************/
|
||||
|
||||
#ifndef BROAD_PHASE_BASIC_H
|
||||
#define BROAD_PHASE_BASIC_H
|
||||
#pragma once
|
||||
|
||||
#include "broad_phase_3d_sw.h"
|
||||
#include "core/templates/map.h"
|
||||
// Simple template to provide a pool with O(1) allocate and free.
|
||||
// The freelist could alternatively be a linked list placed within the unused elements
|
||||
// to use less memory, however a separate freelist is probably more cache friendly.
|
||||
|
||||
class BroadPhase3DBasic : public BroadPhase3DSW {
|
||||
struct Element {
|
||||
CollisionObject3DSW *owner;
|
||||
bool _static;
|
||||
AABB aabb;
|
||||
int subindex;
|
||||
};
|
||||
// NOTE : Take great care when using this with non POD types. The construction and destruction
|
||||
// is done in the LocalVector, NOT as part of the pool. So requesting a new item does not guarantee
|
||||
// a constructor is run, and free does not guarantee a destructor.
|
||||
// You should generally handle clearing
|
||||
// an item explicitly after a request, as it may contain 'leftovers'.
|
||||
// This is by design for fastest use in the BVH. If you want a more general pool
|
||||
// that does call constructors / destructors on request / free, this should probably be
|
||||
// a separate template.
|
||||
|
||||
Map<ID, Element> element_map;
|
||||
#include "core/templates/local_vector.h"
|
||||
|
||||
ID current;
|
||||
template <class T, bool force_trivial = false>
|
||||
class PooledList {
|
||||
LocalVector<T, uint32_t, force_trivial> list;
|
||||
LocalVector<uint32_t, uint32_t, true> freelist;
|
||||
|
||||
struct PairKey {
|
||||
union {
|
||||
struct {
|
||||
ID a;
|
||||
ID b;
|
||||
};
|
||||
uint64_t key;
|
||||
};
|
||||
|
||||
_FORCE_INLINE_ bool operator<(const PairKey &p_key) const {
|
||||
return key < p_key.key;
|
||||
}
|
||||
|
||||
PairKey() { key = 0; }
|
||||
PairKey(ID p_a, ID p_b) {
|
||||
if (p_a > p_b) {
|
||||
a = p_b;
|
||||
b = p_a;
|
||||
} else {
|
||||
a = p_a;
|
||||
b = p_b;
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
Map<PairKey, void *> pair_map;
|
||||
|
||||
PairCallback pair_callback;
|
||||
void *pair_userdata;
|
||||
UnpairCallback unpair_callback;
|
||||
void *unpair_userdata;
|
||||
// not all list members are necessarily used
|
||||
int _used_size;
|
||||
|
||||
public:
|
||||
// 0 is an invalid ID
|
||||
virtual ID create(CollisionObject3DSW *p_object, int p_subindex = 0);
|
||||
virtual void move(ID p_id, const AABB &p_aabb);
|
||||
virtual void set_static(ID p_id, bool p_static);
|
||||
virtual void remove(ID p_id);
|
||||
PooledList() {
|
||||
_used_size = 0;
|
||||
}
|
||||
|
||||
virtual CollisionObject3DSW *get_object(ID p_id) const;
|
||||
virtual bool is_static(ID p_id) const;
|
||||
virtual int get_subindex(ID p_id) const;
|
||||
int estimate_memory_use() const {
|
||||
return (list.size() * sizeof(T)) + (freelist.size() * sizeof(uint32_t));
|
||||
}
|
||||
|
||||
virtual int cull_point(const Vector3 &p_point, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices = nullptr);
|
||||
virtual int cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices = nullptr);
|
||||
virtual int cull_aabb(const AABB &p_aabb, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices = nullptr);
|
||||
const T &operator[](uint32_t p_index) const {
|
||||
return list[p_index];
|
||||
}
|
||||
T &operator[](uint32_t p_index) {
|
||||
return list[p_index];
|
||||
}
|
||||
|
||||
virtual void set_pair_callback(PairCallback p_pair_callback, void *p_userdata);
|
||||
virtual void set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata);
|
||||
int size() const { return _used_size; }
|
||||
|
||||
virtual void update();
|
||||
T *request(uint32_t &r_id) {
|
||||
_used_size++;
|
||||
|
||||
static BroadPhase3DSW *_create();
|
||||
BroadPhase3DBasic();
|
||||
if (freelist.size()) {
|
||||
// pop from freelist
|
||||
int new_size = freelist.size() - 1;
|
||||
r_id = freelist[new_size];
|
||||
freelist.resize(new_size);
|
||||
return &list[r_id];
|
||||
}
|
||||
|
||||
r_id = list.size();
|
||||
list.resize(r_id + 1);
|
||||
return &list[r_id];
|
||||
}
|
||||
void free(const uint32_t &p_id) {
|
||||
// should not be on free list already
|
||||
CRASH_COND(p_id >= list.size());
|
||||
freelist.push_back(p_id);
|
||||
_used_size--;
|
||||
}
|
||||
};
|
||||
|
||||
#endif // BROAD_PHASE_BASIC_H
|
||||
|
|
@ -1197,12 +1197,6 @@
|
|||
The CA certificates bundle to use for SSL connections. If this is set to a non-empty value, this will [i]override[/i] Godot's default [url=https://github.com/godotengine/godot/blob/master/thirdparty/certs/ca-certificates.crt]Mozilla certificate bundle[/url]. If left empty, the default certificate bundle will be used.
|
||||
If in doubt, leave this setting empty.
|
||||
</member>
|
||||
<member name="physics/2d/bp_hash_table_size" type="int" setter="" getter="" default="4096">
|
||||
Size of the hash table used for the broad-phase 2D hash grid algorithm.
|
||||
</member>
|
||||
<member name="physics/2d/cell_size" type="int" setter="" getter="" default="128">
|
||||
Cell size used for the broad-phase 2D hash grid algorithm (in pixels).
|
||||
</member>
|
||||
<member name="physics/2d/default_angular_damp" type="float" setter="" getter="" default="1.0">
|
||||
The default angular damp in 2D.
|
||||
[b]Note:[/b] Good values are in the range [code]0[/code] to [code]1[/code]. At value [code]0[/code] objects will keep moving with the same velocity. Values greater than [code]1[/code] will aim to reduce the velocity to [code]0[/code] in less than a second e.g. a value of [code]2[/code] will aim to reduce the velocity to [code]0[/code] in half a second. A value equal to or greater than the physics frame rate ([member ProjectSettings.physics/common/physics_fps], [code]60[/code] by default) will bring the object to a stop in one iteration.
|
||||
|
|
@ -1239,9 +1233,6 @@
|
|||
The default linear damp in 2D.
|
||||
[b]Note:[/b] Good values are in the range [code]0[/code] to [code]1[/code]. At value [code]0[/code] objects will keep moving with the same velocity. Values greater than [code]1[/code] will aim to reduce the velocity to [code]0[/code] in less than a second e.g. a value of [code]2[/code] will aim to reduce the velocity to [code]0[/code] in half a second. A value equal to or greater than the physics frame rate ([member ProjectSettings.physics/common/physics_fps], [code]60[/code] by default) will bring the object to a stop in one iteration.
|
||||
</member>
|
||||
<member name="physics/2d/large_object_surface_threshold_in_cells" type="int" setter="" getter="" default="512">
|
||||
Threshold defining the surface size that constitutes a large object with regard to cells in the broad-phase 2D hash grid algorithm.
|
||||
</member>
|
||||
<member name="physics/2d/physics_engine" type="String" setter="" getter="" default=""DEFAULT"">
|
||||
Sets which physics engine to use for 2D physics.
|
||||
"DEFAULT" and "GodotPhysics2D" are the same, as there is currently no alternative 2D physics server implemented.
|
||||
|
|
|
|||
|
|
@ -1,174 +0,0 @@
|
|||
/*************************************************************************/
|
||||
/* broad_phase_2d_basic.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 "broad_phase_2d_basic.h"
|
||||
|
||||
BroadPhase2DBasic::ID BroadPhase2DBasic::create(CollisionObject2DSW *p_object_, int p_subindex) {
|
||||
current++;
|
||||
|
||||
Element e;
|
||||
e.owner = p_object_;
|
||||
e._static = false;
|
||||
e.subindex = p_subindex;
|
||||
|
||||
element_map[current] = e;
|
||||
return current;
|
||||
}
|
||||
|
||||
void BroadPhase2DBasic::move(ID p_id, const Rect2 &p_aabb) {
|
||||
Map<ID, Element>::Element *E = element_map.find(p_id);
|
||||
ERR_FAIL_COND(!E);
|
||||
E->get().aabb = p_aabb;
|
||||
}
|
||||
|
||||
void BroadPhase2DBasic::set_static(ID p_id, bool p_static) {
|
||||
Map<ID, Element>::Element *E = element_map.find(p_id);
|
||||
ERR_FAIL_COND(!E);
|
||||
E->get()._static = p_static;
|
||||
}
|
||||
|
||||
void BroadPhase2DBasic::remove(ID p_id) {
|
||||
Map<ID, Element>::Element *E = element_map.find(p_id);
|
||||
ERR_FAIL_COND(!E);
|
||||
element_map.erase(E);
|
||||
}
|
||||
|
||||
CollisionObject2DSW *BroadPhase2DBasic::get_object(ID p_id) const {
|
||||
const Map<ID, Element>::Element *E = element_map.find(p_id);
|
||||
ERR_FAIL_COND_V(!E, nullptr);
|
||||
return E->get().owner;
|
||||
}
|
||||
|
||||
bool BroadPhase2DBasic::is_static(ID p_id) const {
|
||||
const Map<ID, Element>::Element *E = element_map.find(p_id);
|
||||
ERR_FAIL_COND_V(!E, false);
|
||||
return E->get()._static;
|
||||
}
|
||||
|
||||
int BroadPhase2DBasic::get_subindex(ID p_id) const {
|
||||
const Map<ID, Element>::Element *E = element_map.find(p_id);
|
||||
ERR_FAIL_COND_V(!E, -1);
|
||||
return E->get().subindex;
|
||||
}
|
||||
|
||||
int BroadPhase2DBasic::cull_segment(const Vector2 &p_from, const Vector2 &p_to, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices) {
|
||||
int rc = 0;
|
||||
|
||||
for (Map<ID, Element>::Element *E = element_map.front(); E; E = E->next()) {
|
||||
const Rect2 aabb = E->get().aabb;
|
||||
if (aabb.intersects_segment(p_from, p_to)) {
|
||||
p_results[rc] = E->get().owner;
|
||||
p_result_indices[rc] = E->get().subindex;
|
||||
rc++;
|
||||
if (rc >= p_max_results) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return rc;
|
||||
}
|
||||
|
||||
int BroadPhase2DBasic::cull_aabb(const Rect2 &p_aabb, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices) {
|
||||
int rc = 0;
|
||||
|
||||
for (Map<ID, Element>::Element *E = element_map.front(); E; E = E->next()) {
|
||||
const Rect2 aabb = E->get().aabb;
|
||||
if (aabb.intersects(p_aabb)) {
|
||||
p_results[rc] = E->get().owner;
|
||||
p_result_indices[rc] = E->get().subindex;
|
||||
rc++;
|
||||
if (rc >= p_max_results) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return rc;
|
||||
}
|
||||
|
||||
void BroadPhase2DBasic::set_pair_callback(PairCallback p_pair_callback, void *p_userdata) {
|
||||
pair_userdata = p_userdata;
|
||||
pair_callback = p_pair_callback;
|
||||
}
|
||||
|
||||
void BroadPhase2DBasic::set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) {
|
||||
unpair_userdata = p_userdata;
|
||||
unpair_callback = p_unpair_callback;
|
||||
}
|
||||
|
||||
void BroadPhase2DBasic::update() {
|
||||
// recompute pairs
|
||||
for (Map<ID, Element>::Element *I = element_map.front(); I; I = I->next()) {
|
||||
for (Map<ID, Element>::Element *J = I->next(); J; J = J->next()) {
|
||||
Element *elem_A = &I->get();
|
||||
Element *elem_B = &J->get();
|
||||
|
||||
if (elem_A->owner == elem_B->owner) {
|
||||
continue;
|
||||
}
|
||||
|
||||
bool pair_ok = elem_A->aabb.intersects(elem_B->aabb) && (!elem_A->_static || !elem_B->_static);
|
||||
|
||||
PairKey key(I->key(), J->key());
|
||||
|
||||
Map<PairKey, void *>::Element *E = pair_map.find(key);
|
||||
|
||||
if (!pair_ok && E) {
|
||||
if (unpair_callback) {
|
||||
unpair_callback(elem_A->owner, elem_A->subindex, elem_B->owner, elem_B->subindex, E->get(), unpair_userdata);
|
||||
}
|
||||
pair_map.erase(key);
|
||||
}
|
||||
|
||||
if (pair_ok && !E) {
|
||||
void *data = nullptr;
|
||||
if (pair_callback) {
|
||||
data = pair_callback(elem_A->owner, elem_A->subindex, elem_B->owner, elem_B->subindex, unpair_userdata);
|
||||
if (data) {
|
||||
pair_map.insert(key, data);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
BroadPhase2DSW *BroadPhase2DBasic::_create() {
|
||||
return memnew(BroadPhase2DBasic);
|
||||
}
|
||||
|
||||
BroadPhase2DBasic::BroadPhase2DBasic() {
|
||||
current = 1;
|
||||
unpair_callback = nullptr;
|
||||
unpair_userdata = nullptr;
|
||||
pair_callback = nullptr;
|
||||
pair_userdata = nullptr;
|
||||
}
|
||||
|
|
@ -0,0 +1,116 @@
|
|||
/*************************************************************************/
|
||||
/* broad_phase_2d_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 "broad_phase_2d_bvh.h"
|
||||
#include "collision_object_2d_sw.h"
|
||||
|
||||
BroadPhase2DSW::ID BroadPhase2DBVH::create(CollisionObject2DSW *p_object, int p_subindex, const Rect2 &p_aabb, bool p_static) {
|
||||
ID oid = bvh.create(p_object, true, p_aabb, p_subindex, !p_static, 1 << p_object->get_type(), p_static ? 0 : 0xFFFFF); // Pair everything, don't care?
|
||||
return oid + 1;
|
||||
}
|
||||
|
||||
void BroadPhase2DBVH::move(ID p_id, const Rect2 &p_aabb) {
|
||||
bvh.move(p_id - 1, p_aabb);
|
||||
}
|
||||
|
||||
void BroadPhase2DBVH::set_static(ID p_id, bool p_static) {
|
||||
CollisionObject2DSW *it = bvh.get(p_id - 1);
|
||||
bvh.set_pairable(p_id - 1, !p_static, 1 << it->get_type(), p_static ? 0 : 0xFFFFF, false); // Pair everything, don't care?
|
||||
}
|
||||
|
||||
void BroadPhase2DBVH::remove(ID p_id) {
|
||||
bvh.erase(p_id - 1);
|
||||
}
|
||||
|
||||
CollisionObject2DSW *BroadPhase2DBVH::get_object(ID p_id) const {
|
||||
CollisionObject2DSW *it = bvh.get(p_id - 1);
|
||||
ERR_FAIL_COND_V(!it, nullptr);
|
||||
return it;
|
||||
}
|
||||
|
||||
bool BroadPhase2DBVH::is_static(ID p_id) const {
|
||||
return !bvh.is_pairable(p_id - 1);
|
||||
}
|
||||
|
||||
int BroadPhase2DBVH::get_subindex(ID p_id) const {
|
||||
return bvh.get_subindex(p_id - 1);
|
||||
}
|
||||
|
||||
int BroadPhase2DBVH::cull_segment(const Vector2 &p_from, const Vector2 &p_to, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices) {
|
||||
return bvh.cull_segment(p_from, p_to, p_results, p_max_results, p_result_indices);
|
||||
}
|
||||
|
||||
int BroadPhase2DBVH::cull_aabb(const Rect2 &p_aabb, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices) {
|
||||
return bvh.cull_aabb(p_aabb, p_results, p_max_results, p_result_indices);
|
||||
}
|
||||
|
||||
void *BroadPhase2DBVH::_pair_callback(void *self, uint32_t p_A, CollisionObject2DSW *p_object_A, int subindex_A, uint32_t p_B, CollisionObject2DSW *p_object_B, int subindex_B) {
|
||||
BroadPhase2DBVH *bpo = (BroadPhase2DBVH *)(self);
|
||||
if (!bpo->pair_callback) {
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
return bpo->pair_callback(p_object_A, subindex_A, p_object_B, subindex_B, bpo->pair_userdata);
|
||||
}
|
||||
|
||||
void BroadPhase2DBVH::_unpair_callback(void *self, uint32_t p_A, CollisionObject2DSW *p_object_A, int subindex_A, uint32_t p_B, CollisionObject2DSW *p_object_B, int subindex_B, void *pairdata) {
|
||||
BroadPhase2DBVH *bpo = (BroadPhase2DBVH *)(self);
|
||||
if (!bpo->unpair_callback) {
|
||||
return;
|
||||
}
|
||||
|
||||
bpo->unpair_callback(p_object_A, subindex_A, p_object_B, subindex_B, pairdata, bpo->unpair_userdata);
|
||||
}
|
||||
|
||||
void BroadPhase2DBVH::set_pair_callback(PairCallback p_pair_callback, void *p_userdata) {
|
||||
pair_callback = p_pair_callback;
|
||||
pair_userdata = p_userdata;
|
||||
}
|
||||
|
||||
void BroadPhase2DBVH::set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) {
|
||||
unpair_callback = p_unpair_callback;
|
||||
unpair_userdata = p_userdata;
|
||||
}
|
||||
|
||||
void BroadPhase2DBVH::update() {
|
||||
bvh.update();
|
||||
}
|
||||
|
||||
BroadPhase2DSW *BroadPhase2DBVH::_create() {
|
||||
return memnew(BroadPhase2DBVH);
|
||||
}
|
||||
|
||||
BroadPhase2DBVH::BroadPhase2DBVH() {
|
||||
bvh.set_pair_callback(_pair_callback, this);
|
||||
bvh.set_unpair_callback(_unpair_callback, this);
|
||||
pair_callback = nullptr;
|
||||
pair_userdata = nullptr;
|
||||
unpair_userdata = nullptr;
|
||||
}
|
||||
|
|
@ -1,5 +1,5 @@
|
|||
/*************************************************************************/
|
||||
/* broad_phase_2d_basic.h */
|
||||
/* broad_phase_2d_bvh.h */
|
||||
/*************************************************************************/
|
||||
/* This file is part of: */
|
||||
/* GODOT ENGINE */
|
||||
|
|
@ -28,49 +28,19 @@
|
|||
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
|
||||
/*************************************************************************/
|
||||
|
||||
#ifndef BROAD_PHASE_2D_BASIC_H
|
||||
#define BROAD_PHASE_2D_BASIC_H
|
||||
#ifndef BROAD_PHASE_2D_BVH_H
|
||||
#define BROAD_PHASE_2D_BVH_H
|
||||
|
||||
#include "core/templates/map.h"
|
||||
#include "space_2d_sw.h"
|
||||
class BroadPhase2DBasic : public BroadPhase2DSW {
|
||||
struct Element {
|
||||
CollisionObject2DSW *owner;
|
||||
bool _static;
|
||||
Rect2 aabb;
|
||||
int subindex;
|
||||
};
|
||||
#include "broad_phase_2d_sw.h"
|
||||
#include "core/math/bvh.h"
|
||||
#include "core/math/rect2.h"
|
||||
#include "core/math/vector2.h"
|
||||
|
||||
Map<ID, Element> element_map;
|
||||
class BroadPhase2DBVH : public BroadPhase2DSW {
|
||||
BVH_Manager<CollisionObject2DSW, true, 128, Rect2, Vector2> bvh;
|
||||
|
||||
ID current;
|
||||
|
||||
struct PairKey {
|
||||
union {
|
||||
struct {
|
||||
ID a;
|
||||
ID b;
|
||||
};
|
||||
uint64_t key;
|
||||
};
|
||||
|
||||
_FORCE_INLINE_ bool operator<(const PairKey &p_key) const {
|
||||
return key < p_key.key;
|
||||
}
|
||||
|
||||
PairKey() { key = 0; }
|
||||
PairKey(ID p_a, ID p_b) {
|
||||
if (p_a > p_b) {
|
||||
a = p_b;
|
||||
b = p_a;
|
||||
} else {
|
||||
a = p_a;
|
||||
b = p_b;
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
Map<PairKey, void *> pair_map;
|
||||
static void *_pair_callback(void *, uint32_t, CollisionObject2DSW *, int, uint32_t, CollisionObject2DSW *, int);
|
||||
static void _unpair_callback(void *, uint32_t, CollisionObject2DSW *, int, uint32_t, CollisionObject2DSW *, int, void *);
|
||||
|
||||
PairCallback pair_callback;
|
||||
void *pair_userdata;
|
||||
|
|
@ -79,7 +49,7 @@ class BroadPhase2DBasic : public BroadPhase2DSW {
|
|||
|
||||
public:
|
||||
// 0 is an invalid ID
|
||||
virtual ID create(CollisionObject2DSW *p_object_, int p_subindex = 0);
|
||||
virtual ID create(CollisionObject2DSW *p_object, int p_subindex = 0, const Rect2 &p_aabb = Rect2(), bool p_static = false);
|
||||
virtual void move(ID p_id, const Rect2 &p_aabb);
|
||||
virtual void set_static(ID p_id, bool p_static);
|
||||
virtual void remove(ID p_id);
|
||||
|
|
@ -97,7 +67,7 @@ public:
|
|||
virtual void update();
|
||||
|
||||
static BroadPhase2DSW *_create();
|
||||
BroadPhase2DBasic();
|
||||
BroadPhase2DBVH();
|
||||
};
|
||||
|
||||
#endif // BROAD_PHASE_2D_BASIC_H
|
||||
#endif // BROAD_PHASE_2D_BVH_H
|
||||
|
|
@ -1,738 +0,0 @@
|
|||
/*************************************************************************/
|
||||
/* broad_phase_2d_hash_grid.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 "broad_phase_2d_hash_grid.h"
|
||||
#include "collision_object_2d_sw.h"
|
||||
#include "core/config/project_settings.h"
|
||||
|
||||
#define LARGE_ELEMENT_FI 1.01239812
|
||||
|
||||
void BroadPhase2DHashGrid::_pair_attempt(Element *p_elem, Element *p_with) {
|
||||
if (p_elem->owner == p_with->owner) {
|
||||
return;
|
||||
}
|
||||
if (!_test_collision_mask(p_elem->collision_mask, p_elem->collision_layer, p_with->collision_mask, p_with->collision_layer)) {
|
||||
return;
|
||||
}
|
||||
Map<Element *, PairData *>::Element *E = p_elem->paired.find(p_with);
|
||||
|
||||
ERR_FAIL_COND(p_elem->_static && p_with->_static);
|
||||
|
||||
if (!E) {
|
||||
PairData *pd = memnew(PairData);
|
||||
p_elem->paired[p_with] = pd;
|
||||
p_with->paired[p_elem] = pd;
|
||||
} else {
|
||||
E->get()->rc++;
|
||||
}
|
||||
}
|
||||
|
||||
void BroadPhase2DHashGrid::_unpair_attempt(Element *p_elem, Element *p_with) {
|
||||
if (p_elem->owner == p_with->owner) {
|
||||
return;
|
||||
}
|
||||
if (!_test_collision_mask(p_elem->collision_mask, p_elem->collision_layer, p_with->collision_mask, p_with->collision_layer)) {
|
||||
return;
|
||||
}
|
||||
Map<Element *, PairData *>::Element *E = p_elem->paired.find(p_with);
|
||||
|
||||
ERR_FAIL_COND(!E); //this should really be paired..
|
||||
|
||||
E->get()->rc--;
|
||||
|
||||
if (E->get()->rc == 0) {
|
||||
if (E->get()->colliding) {
|
||||
//uncollide
|
||||
if (unpair_callback) {
|
||||
unpair_callback(p_elem->owner, p_elem->subindex, p_with->owner, p_with->subindex, E->get()->ud, unpair_userdata);
|
||||
}
|
||||
}
|
||||
|
||||
memdelete(E->get());
|
||||
p_elem->paired.erase(E);
|
||||
p_with->paired.erase(p_elem);
|
||||
}
|
||||
}
|
||||
|
||||
void BroadPhase2DHashGrid::_check_motion(Element *p_elem) {
|
||||
for (Map<Element *, PairData *>::Element *E = p_elem->paired.front(); E; E = E->next()) {
|
||||
bool physical_collision = p_elem->aabb.intersects(E->key()->aabb);
|
||||
bool logical_collision = p_elem->owner->test_collision_mask(E->key()->owner);
|
||||
|
||||
if (physical_collision && logical_collision) {
|
||||
if (!E->get()->colliding && pair_callback) {
|
||||
E->get()->ud = pair_callback(p_elem->owner, p_elem->subindex, E->key()->owner, E->key()->subindex, pair_userdata);
|
||||
}
|
||||
E->get()->colliding = true;
|
||||
} else { // No collision
|
||||
if (E->get()->colliding && unpair_callback) {
|
||||
unpair_callback(p_elem->owner, p_elem->subindex, E->key()->owner, E->key()->subindex, E->get()->ud, unpair_userdata);
|
||||
E->get()->ud = nullptr;
|
||||
}
|
||||
E->get()->colliding = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void BroadPhase2DHashGrid::_enter_grid(Element *p_elem, const Rect2 &p_rect, bool p_static, bool p_force_enter) {
|
||||
Vector2 sz = (p_rect.size / cell_size * LARGE_ELEMENT_FI); //use magic number to avoid floating point issues
|
||||
if (sz.width * sz.height > large_object_min_surface) {
|
||||
//large object, do not use grid, must check against all elements
|
||||
for (Map<ID, Element>::Element *E = element_map.front(); E; E = E->next()) {
|
||||
if (E->key() == p_elem->self) {
|
||||
continue; // do not pair against itself
|
||||
}
|
||||
if (E->get()._static && p_static) {
|
||||
continue;
|
||||
}
|
||||
|
||||
_pair_attempt(p_elem, &E->get());
|
||||
}
|
||||
|
||||
large_elements[p_elem].inc();
|
||||
return;
|
||||
}
|
||||
|
||||
Point2i from = (p_rect.position / cell_size).floor();
|
||||
Point2i to = ((p_rect.position + p_rect.size) / cell_size).floor();
|
||||
|
||||
for (int i = from.x; i <= to.x; i++) {
|
||||
for (int j = from.y; j <= to.y; j++) {
|
||||
PosKey pk;
|
||||
pk.x = i;
|
||||
pk.y = j;
|
||||
|
||||
uint32_t idx = pk.hash() % hash_table_size;
|
||||
PosBin *pb = hash_table[idx];
|
||||
|
||||
while (pb) {
|
||||
if (pb->key == pk) {
|
||||
break;
|
||||
}
|
||||
|
||||
pb = pb->next;
|
||||
}
|
||||
|
||||
bool entered = p_force_enter;
|
||||
|
||||
if (!pb) {
|
||||
//does not exist, create!
|
||||
pb = memnew(PosBin);
|
||||
pb->key = pk;
|
||||
pb->next = hash_table[idx];
|
||||
hash_table[idx] = pb;
|
||||
}
|
||||
|
||||
if (p_static) {
|
||||
if (pb->static_object_set[p_elem].inc() == 1) {
|
||||
entered = true;
|
||||
}
|
||||
} else {
|
||||
if (pb->object_set[p_elem].inc() == 1) {
|
||||
entered = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (entered) {
|
||||
for (Map<Element *, RC>::Element *E = pb->object_set.front(); E; E = E->next()) {
|
||||
_pair_attempt(p_elem, E->key());
|
||||
}
|
||||
|
||||
if (!p_static) {
|
||||
for (Map<Element *, RC>::Element *E = pb->static_object_set.front(); E; E = E->next()) {
|
||||
_pair_attempt(p_elem, E->key());
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//pair separatedly with large elements
|
||||
|
||||
for (Map<Element *, RC>::Element *E = large_elements.front(); E; E = E->next()) {
|
||||
if (E->key() == p_elem) {
|
||||
continue; // do not pair against itself
|
||||
}
|
||||
if (E->key()->_static && p_static) {
|
||||
continue;
|
||||
}
|
||||
_pair_attempt(E->key(), p_elem);
|
||||
}
|
||||
}
|
||||
|
||||
void BroadPhase2DHashGrid::_exit_grid(Element *p_elem, const Rect2 &p_rect, bool p_static, bool p_force_exit) {
|
||||
Vector2 sz = (p_rect.size / cell_size * LARGE_ELEMENT_FI);
|
||||
if (sz.width * sz.height > large_object_min_surface) {
|
||||
//unpair all elements, instead of checking all, just check what is already paired, so we at least save from checking static vs static
|
||||
Map<Element *, PairData *>::Element *E = p_elem->paired.front();
|
||||
while (E) {
|
||||
Map<Element *, PairData *>::Element *next = E->next();
|
||||
_unpair_attempt(p_elem, E->key());
|
||||
E = next;
|
||||
}
|
||||
|
||||
if (large_elements[p_elem].dec() == 0) {
|
||||
large_elements.erase(p_elem);
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
Point2i from = (p_rect.position / cell_size).floor();
|
||||
Point2i to = ((p_rect.position + p_rect.size) / cell_size).floor();
|
||||
|
||||
for (int i = from.x; i <= to.x; i++) {
|
||||
for (int j = from.y; j <= to.y; j++) {
|
||||
PosKey pk;
|
||||
pk.x = i;
|
||||
pk.y = j;
|
||||
|
||||
uint32_t idx = pk.hash() % hash_table_size;
|
||||
PosBin *pb = hash_table[idx];
|
||||
|
||||
while (pb) {
|
||||
if (pb->key == pk) {
|
||||
break;
|
||||
}
|
||||
|
||||
pb = pb->next;
|
||||
}
|
||||
|
||||
ERR_CONTINUE(!pb); //should exist!!
|
||||
|
||||
bool exited = p_force_exit;
|
||||
|
||||
if (p_static) {
|
||||
if (pb->static_object_set[p_elem].dec() == 0) {
|
||||
pb->static_object_set.erase(p_elem);
|
||||
exited = true;
|
||||
}
|
||||
} else {
|
||||
if (pb->object_set[p_elem].dec() == 0) {
|
||||
pb->object_set.erase(p_elem);
|
||||
exited = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (exited) {
|
||||
for (Map<Element *, RC>::Element *E = pb->object_set.front(); E; E = E->next()) {
|
||||
_unpair_attempt(p_elem, E->key());
|
||||
}
|
||||
|
||||
if (!p_static) {
|
||||
for (Map<Element *, RC>::Element *E = pb->static_object_set.front(); E; E = E->next()) {
|
||||
_unpair_attempt(p_elem, E->key());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (pb->object_set.is_empty() && pb->static_object_set.is_empty()) {
|
||||
if (hash_table[idx] == pb) {
|
||||
hash_table[idx] = pb->next;
|
||||
} else {
|
||||
PosBin *px = hash_table[idx];
|
||||
|
||||
while (px) {
|
||||
if (px->next == pb) {
|
||||
px->next = pb->next;
|
||||
break;
|
||||
}
|
||||
|
||||
px = px->next;
|
||||
}
|
||||
|
||||
ERR_CONTINUE(!px);
|
||||
}
|
||||
|
||||
memdelete(pb);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (Map<Element *, RC>::Element *E = large_elements.front(); E; E = E->next()) {
|
||||
if (E->key() == p_elem) {
|
||||
continue; // do not pair against itself
|
||||
}
|
||||
if (E->key()->_static && p_static) {
|
||||
continue;
|
||||
}
|
||||
|
||||
//unpair from large elements
|
||||
_unpair_attempt(p_elem, E->key());
|
||||
}
|
||||
}
|
||||
|
||||
BroadPhase2DHashGrid::ID BroadPhase2DHashGrid::create(CollisionObject2DSW *p_object, int p_subindex) {
|
||||
current++;
|
||||
|
||||
Element e;
|
||||
e.owner = p_object;
|
||||
e._static = false;
|
||||
e.collision_mask = p_object->get_collision_mask();
|
||||
e.collision_layer = p_object->get_collision_layer();
|
||||
e.subindex = p_subindex;
|
||||
e.self = current;
|
||||
e.pass = 0;
|
||||
|
||||
element_map[current] = e;
|
||||
return current;
|
||||
}
|
||||
|
||||
void BroadPhase2DHashGrid::move(ID p_id, const Rect2 &p_aabb) {
|
||||
Map<ID, Element>::Element *E = element_map.find(p_id);
|
||||
ERR_FAIL_COND(!E);
|
||||
|
||||
Element &e = E->get();
|
||||
bool layer_changed = e.collision_mask != e.owner->get_collision_mask() || e.collision_layer != e.owner->get_collision_layer();
|
||||
|
||||
if (p_aabb != e.aabb || layer_changed) {
|
||||
uint32_t old_mask = e.collision_mask;
|
||||
uint32_t old_layer = e.collision_layer;
|
||||
if (p_aabb != Rect2()) {
|
||||
e.collision_mask = e.owner->get_collision_mask();
|
||||
e.collision_layer = e.owner->get_collision_layer();
|
||||
|
||||
_enter_grid(&e, p_aabb, e._static, layer_changed);
|
||||
}
|
||||
if (e.aabb != Rect2()) {
|
||||
// Need _exit_grid to remove from cells based on the old layer values.
|
||||
e.collision_mask = old_mask;
|
||||
e.collision_layer = old_layer;
|
||||
|
||||
_exit_grid(&e, e.aabb, e._static, layer_changed);
|
||||
|
||||
e.collision_mask = e.owner->get_collision_mask();
|
||||
e.collision_layer = e.owner->get_collision_layer();
|
||||
}
|
||||
e.aabb = p_aabb;
|
||||
}
|
||||
|
||||
_check_motion(&e);
|
||||
}
|
||||
|
||||
void BroadPhase2DHashGrid::set_static(ID p_id, bool p_static) {
|
||||
Map<ID, Element>::Element *E = element_map.find(p_id);
|
||||
ERR_FAIL_COND(!E);
|
||||
|
||||
Element &e = E->get();
|
||||
|
||||
if (e._static == p_static) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (e.aabb != Rect2()) {
|
||||
_exit_grid(&e, e.aabb, e._static, false);
|
||||
}
|
||||
|
||||
e._static = p_static;
|
||||
|
||||
if (e.aabb != Rect2()) {
|
||||
_enter_grid(&e, e.aabb, e._static, false);
|
||||
_check_motion(&e);
|
||||
}
|
||||
}
|
||||
|
||||
void BroadPhase2DHashGrid::remove(ID p_id) {
|
||||
Map<ID, Element>::Element *E = element_map.find(p_id);
|
||||
ERR_FAIL_COND(!E);
|
||||
|
||||
Element &e = E->get();
|
||||
|
||||
if (e.aabb != Rect2()) {
|
||||
_exit_grid(&e, e.aabb, e._static, false);
|
||||
}
|
||||
|
||||
element_map.erase(p_id);
|
||||
}
|
||||
|
||||
CollisionObject2DSW *BroadPhase2DHashGrid::get_object(ID p_id) const {
|
||||
const Map<ID, Element>::Element *E = element_map.find(p_id);
|
||||
ERR_FAIL_COND_V(!E, nullptr);
|
||||
return E->get().owner;
|
||||
}
|
||||
|
||||
bool BroadPhase2DHashGrid::is_static(ID p_id) const {
|
||||
const Map<ID, Element>::Element *E = element_map.find(p_id);
|
||||
ERR_FAIL_COND_V(!E, false);
|
||||
return E->get()._static;
|
||||
}
|
||||
|
||||
int BroadPhase2DHashGrid::get_subindex(ID p_id) const {
|
||||
const Map<ID, Element>::Element *E = element_map.find(p_id);
|
||||
ERR_FAIL_COND_V(!E, -1);
|
||||
return E->get().subindex;
|
||||
}
|
||||
|
||||
template <bool use_aabb, bool use_segment>
|
||||
void BroadPhase2DHashGrid::_cull(const Point2i p_cell, const Rect2 &p_aabb, const Point2 &p_from, const Point2 &p_to, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices, int &index) {
|
||||
PosKey pk;
|
||||
pk.x = p_cell.x;
|
||||
pk.y = p_cell.y;
|
||||
|
||||
uint32_t idx = pk.hash() % hash_table_size;
|
||||
PosBin *pb = hash_table[idx];
|
||||
|
||||
while (pb) {
|
||||
if (pb->key == pk) {
|
||||
break;
|
||||
}
|
||||
|
||||
pb = pb->next;
|
||||
}
|
||||
|
||||
if (!pb) {
|
||||
return;
|
||||
}
|
||||
|
||||
for (Map<Element *, RC>::Element *E = pb->object_set.front(); E; E = E->next()) {
|
||||
if (index >= p_max_results) {
|
||||
break;
|
||||
}
|
||||
if (E->key()->pass == pass) {
|
||||
continue;
|
||||
}
|
||||
|
||||
E->key()->pass = pass;
|
||||
|
||||
if (use_aabb && !p_aabb.intersects(E->key()->aabb)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
if (use_segment && !E->key()->aabb.intersects_segment(p_from, p_to)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
p_results[index] = E->key()->owner;
|
||||
p_result_indices[index] = E->key()->subindex;
|
||||
index++;
|
||||
}
|
||||
|
||||
for (Map<Element *, RC>::Element *E = pb->static_object_set.front(); E; E = E->next()) {
|
||||
if (index >= p_max_results) {
|
||||
break;
|
||||
}
|
||||
if (E->key()->pass == pass) {
|
||||
continue;
|
||||
}
|
||||
|
||||
if (use_aabb && !p_aabb.intersects(E->key()->aabb)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
if (use_segment && !E->key()->aabb.intersects_segment(p_from, p_to)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
E->key()->pass = pass;
|
||||
p_results[index] = E->key()->owner;
|
||||
p_result_indices[index] = E->key()->subindex;
|
||||
index++;
|
||||
}
|
||||
}
|
||||
|
||||
int BroadPhase2DHashGrid::cull_segment(const Vector2 &p_from, const Vector2 &p_to, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices) {
|
||||
pass++;
|
||||
|
||||
Vector2 dir = (p_to - p_from);
|
||||
if (dir == Vector2()) {
|
||||
return 0;
|
||||
}
|
||||
//avoid divisions by zero
|
||||
dir.normalize();
|
||||
if (dir.x == 0.0) {
|
||||
dir.x = 0.000001;
|
||||
}
|
||||
if (dir.y == 0.0) {
|
||||
dir.y = 0.000001;
|
||||
}
|
||||
Vector2 delta = dir.abs();
|
||||
|
||||
delta.x = cell_size / delta.x;
|
||||
delta.y = cell_size / delta.y;
|
||||
|
||||
Point2i pos = (p_from / cell_size).floor();
|
||||
Point2i end = (p_to / cell_size).floor();
|
||||
|
||||
Point2i step = Vector2(SGN(dir.x), SGN(dir.y));
|
||||
|
||||
Vector2 max;
|
||||
|
||||
if (dir.x < 0) {
|
||||
max.x = (Math::floor((double)pos.x) * cell_size - p_from.x) / dir.x;
|
||||
} else {
|
||||
max.x = (Math::floor((double)pos.x + 1) * cell_size - p_from.x) / dir.x;
|
||||
}
|
||||
|
||||
if (dir.y < 0) {
|
||||
max.y = (Math::floor((double)pos.y) * cell_size - p_from.y) / dir.y;
|
||||
} else {
|
||||
max.y = (Math::floor((double)pos.y + 1) * cell_size - p_from.y) / dir.y;
|
||||
}
|
||||
|
||||
int cullcount = 0;
|
||||
_cull<false, true>(pos, Rect2(), p_from, p_to, p_results, p_max_results, p_result_indices, cullcount);
|
||||
|
||||
bool reached_x = false;
|
||||
bool reached_y = false;
|
||||
|
||||
while (true) {
|
||||
if (max.x < max.y) {
|
||||
max.x += delta.x;
|
||||
pos.x += step.x;
|
||||
} else {
|
||||
max.y += delta.y;
|
||||
pos.y += step.y;
|
||||
}
|
||||
|
||||
if (step.x > 0) {
|
||||
if (pos.x >= end.x) {
|
||||
reached_x = true;
|
||||
}
|
||||
} else if (pos.x <= end.x) {
|
||||
reached_x = true;
|
||||
}
|
||||
|
||||
if (step.y > 0) {
|
||||
if (pos.y >= end.y) {
|
||||
reached_y = true;
|
||||
}
|
||||
} else if (pos.y <= end.y) {
|
||||
reached_y = true;
|
||||
}
|
||||
|
||||
_cull<false, true>(pos, Rect2(), p_from, p_to, p_results, p_max_results, p_result_indices, cullcount);
|
||||
|
||||
if (reached_x && reached_y) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
for (Map<Element *, RC>::Element *E = large_elements.front(); E; E = E->next()) {
|
||||
if (cullcount >= p_max_results) {
|
||||
break;
|
||||
}
|
||||
if (E->key()->pass == pass) {
|
||||
continue;
|
||||
}
|
||||
|
||||
E->key()->pass = pass;
|
||||
|
||||
/*
|
||||
if (use_aabb && !p_aabb.intersects(E->key()->aabb))
|
||||
continue;
|
||||
*/
|
||||
|
||||
if (!E->key()->aabb.intersects_segment(p_from, p_to)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
p_results[cullcount] = E->key()->owner;
|
||||
p_result_indices[cullcount] = E->key()->subindex;
|
||||
cullcount++;
|
||||
}
|
||||
|
||||
return cullcount;
|
||||
}
|
||||
|
||||
int BroadPhase2DHashGrid::cull_aabb(const Rect2 &p_aabb, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices) {
|
||||
pass++;
|
||||
|
||||
Point2i from = (p_aabb.position / cell_size).floor();
|
||||
Point2i to = ((p_aabb.position + p_aabb.size) / cell_size).floor();
|
||||
int cullcount = 0;
|
||||
|
||||
for (int i = from.x; i <= to.x; i++) {
|
||||
for (int j = from.y; j <= to.y; j++) {
|
||||
_cull<true, false>(Point2i(i, j), p_aabb, Point2(), Point2(), p_results, p_max_results, p_result_indices, cullcount);
|
||||
}
|
||||
}
|
||||
|
||||
for (Map<Element *, RC>::Element *E = large_elements.front(); E; E = E->next()) {
|
||||
if (cullcount >= p_max_results) {
|
||||
break;
|
||||
}
|
||||
if (E->key()->pass == pass) {
|
||||
continue;
|
||||
}
|
||||
|
||||
E->key()->pass = pass;
|
||||
|
||||
if (!p_aabb.intersects(E->key()->aabb)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
/*
|
||||
if (!E->key()->aabb.intersects_segment(p_from,p_to))
|
||||
continue;
|
||||
*/
|
||||
|
||||
p_results[cullcount] = E->key()->owner;
|
||||
p_result_indices[cullcount] = E->key()->subindex;
|
||||
cullcount++;
|
||||
}
|
||||
return cullcount;
|
||||
}
|
||||
|
||||
void BroadPhase2DHashGrid::set_pair_callback(PairCallback p_pair_callback, void *p_userdata) {
|
||||
pair_callback = p_pair_callback;
|
||||
pair_userdata = p_userdata;
|
||||
}
|
||||
|
||||
void BroadPhase2DHashGrid::set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) {
|
||||
unpair_callback = p_unpair_callback;
|
||||
unpair_userdata = p_userdata;
|
||||
}
|
||||
|
||||
void BroadPhase2DHashGrid::update() {
|
||||
}
|
||||
|
||||
BroadPhase2DSW *BroadPhase2DHashGrid::_create() {
|
||||
return memnew(BroadPhase2DHashGrid);
|
||||
}
|
||||
|
||||
BroadPhase2DHashGrid::BroadPhase2DHashGrid() {
|
||||
hash_table_size = GLOBAL_DEF("physics/2d/bp_hash_table_size", 4096);
|
||||
ProjectSettings::get_singleton()->set_custom_property_info("physics/2d/bp_hash_table_size", PropertyInfo(Variant::INT, "physics/2d/bp_hash_table_size", PROPERTY_HINT_RANGE, "0,8192,1,or_greater"));
|
||||
hash_table_size = Math::larger_prime(hash_table_size);
|
||||
hash_table = memnew_arr(PosBin *, hash_table_size);
|
||||
|
||||
cell_size = GLOBAL_DEF("physics/2d/cell_size", 128);
|
||||
ProjectSettings::get_singleton()->set_custom_property_info("physics/2d/cell_size", PropertyInfo(Variant::INT, "physics/2d/cell_size", PROPERTY_HINT_RANGE, "0,512,1,or_greater"));
|
||||
|
||||
large_object_min_surface = GLOBAL_DEF("physics/2d/large_object_surface_threshold_in_cells", 512);
|
||||
ProjectSettings::get_singleton()->set_custom_property_info("physics/2d/large_object_surface_threshold_in_cells", PropertyInfo(Variant::INT, "physics/2d/large_object_surface_threshold_in_cells", PROPERTY_HINT_RANGE, "0,1024,1,or_greater"));
|
||||
|
||||
for (uint32_t i = 0; i < hash_table_size; i++) {
|
||||
hash_table[i] = nullptr;
|
||||
}
|
||||
pass = 1;
|
||||
|
||||
current = 0;
|
||||
}
|
||||
|
||||
BroadPhase2DHashGrid::~BroadPhase2DHashGrid() {
|
||||
for (uint32_t i = 0; i < hash_table_size; i++) {
|
||||
while (hash_table[i]) {
|
||||
PosBin *pb = hash_table[i];
|
||||
hash_table[i] = pb->next;
|
||||
memdelete(pb);
|
||||
}
|
||||
}
|
||||
|
||||
memdelete_arr(hash_table);
|
||||
}
|
||||
|
||||
/* 3D version of voxel traversal:
|
||||
|
||||
public IEnumerable<Point3D> GetCellsOnRay(Ray ray, int maxDepth)
|
||||
{
|
||||
// Implementation is based on:
|
||||
// "A Fast Voxel Traversal Algorithm for Ray Tracing"
|
||||
// John Amanatides, Andrew Woo
|
||||
// http://www.cse.yorku.ca/~amana/research/grid.pdf
|
||||
// https://web.archive.org/web/20100616193049/http://www.devmaster.net/articles/raytracing_series/A%20faster%20voxel%20traversal%20algorithm%20for%20ray%20tracing.pdf
|
||||
|
||||
// NOTES:
|
||||
// * This code assumes that the ray's position and direction are in 'cell coordinates', which means
|
||||
// that one unit equals one cell in all directions.
|
||||
// * When the ray doesn't start within the voxel grid, calculate the first position at which the
|
||||
// ray could enter the grid. If it never enters the grid, there is nothing more to do here.
|
||||
// * Also, it is important to test when the ray exits the voxel grid when the grid isn't infinite.
|
||||
// * The Point3D structure is a simple structure having three integer fields (X, Y and Z).
|
||||
|
||||
// The cell in which the ray starts.
|
||||
Point3D start = GetCellAt(ray.Position); // Rounds the position's X, Y and Z down to the nearest integer values.
|
||||
int x = start.X;
|
||||
int y = start.Y;
|
||||
int z = start.Z;
|
||||
|
||||
// Determine which way we go.
|
||||
int stepX = Math.Sign(ray.Direction.X);
|
||||
int stepY = Math.Sign(ray.Direction.Y);
|
||||
int stepZ = Math.Sign(ray.Direction.Z);
|
||||
|
||||
// Calculate cell boundaries. When the step (i.e. direction sign) is positive,
|
||||
// the next boundary is AFTER our current position, meaning that we have to add 1.
|
||||
// Otherwise, it is BEFORE our current position, in which case we add nothing.
|
||||
Point3D cellBoundary = new Point3D(
|
||||
x + (stepX > 0 ? 1 : 0),
|
||||
y + (stepY > 0 ? 1 : 0),
|
||||
z + (stepZ > 0 ? 1 : 0));
|
||||
|
||||
// NOTE: For the following calculations, the result will be Single.PositiveInfinity
|
||||
// when ray.Direction.X, Y or Z equals zero, which is OK. However, when the left-hand
|
||||
// value of the division also equals zero, the result is Single.NaN, which is not OK.
|
||||
|
||||
// Determine how far we can travel along the ray before we hit a voxel boundary.
|
||||
Vector3 tMax = new Vector3(
|
||||
(cellBoundary.X - ray.Position.X) / ray.Direction.X, // Boundary is a plane on the YZ axis.
|
||||
(cellBoundary.Y - ray.Position.Y) / ray.Direction.Y, // Boundary is a plane on the XZ axis.
|
||||
(cellBoundary.Z - ray.Position.Z) / ray.Direction.Z); // Boundary is a plane on the XY axis.
|
||||
if (Single.IsNaN(tMax.X)) tMax.X = Single.PositiveInfinity;
|
||||
if (Single.IsNaN(tMax.Y)) tMax.Y = Single.PositiveInfinity;
|
||||
if (Single.IsNaN(tMax.Z)) tMax.Z = Single.PositiveInfinity;
|
||||
|
||||
// Determine how far we must travel along the ray before we have crossed a gridcell.
|
||||
Vector3 tDelta = new Vector3(
|
||||
stepX / ray.Direction.X, // Crossing the width of a cell.
|
||||
stepY / ray.Direction.Y, // Crossing the height of a cell.
|
||||
stepZ / ray.Direction.Z); // Crossing the depth of a cell.
|
||||
if (Single.IsNaN(tDelta.X)) tDelta.X = Single.PositiveInfinity;
|
||||
if (Single.IsNaN(tDelta.Y)) tDelta.Y = Single.PositiveInfinity;
|
||||
if (Single.IsNaN(tDelta.Z)) tDelta.Z = Single.PositiveInfinity;
|
||||
|
||||
// For each step, determine which distance to the next voxel boundary is lowest (i.e.
|
||||
// which voxel boundary is nearest) and walk that way.
|
||||
for (int i = 0; i < maxDepth; i++)
|
||||
{
|
||||
// Return it.
|
||||
yield return new Point3D(x, y, z);
|
||||
|
||||
// Do the next step.
|
||||
if (tMax.X < tMax.Y && tMax.X < tMax.Z)
|
||||
{
|
||||
// tMax.X is the lowest, an YZ cell boundary plane is nearest.
|
||||
x += stepX;
|
||||
tMax.X += tDelta.X;
|
||||
}
|
||||
else if (tMax.Y < tMax.Z)
|
||||
{
|
||||
// tMax.Y is the lowest, an XZ cell boundary plane is nearest.
|
||||
y += stepY;
|
||||
tMax.Y += tDelta.Y;
|
||||
}
|
||||
else
|
||||
{
|
||||
// tMax.Z is the lowest, an XY cell boundary plane is nearest.
|
||||
z += stepZ;
|
||||
tMax.Z += tDelta.Z;
|
||||
}
|
||||
}
|
||||
|
||||
*/
|
||||
|
|
@ -1,194 +0,0 @@
|
|||
/*************************************************************************/
|
||||
/* broad_phase_2d_hash_grid.h */
|
||||
/*************************************************************************/
|
||||
/* 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. */
|
||||
/*************************************************************************/
|
||||
|
||||
#ifndef BROAD_PHASE_2D_HASH_GRID_H
|
||||
#define BROAD_PHASE_2D_HASH_GRID_H
|
||||
|
||||
#include "broad_phase_2d_sw.h"
|
||||
#include "core/templates/map.h"
|
||||
|
||||
class BroadPhase2DHashGrid : public BroadPhase2DSW {
|
||||
struct PairData {
|
||||
bool colliding;
|
||||
int rc;
|
||||
void *ud;
|
||||
PairData() {
|
||||
colliding = false;
|
||||
rc = 1;
|
||||
ud = nullptr;
|
||||
}
|
||||
};
|
||||
|
||||
struct Element {
|
||||
ID self;
|
||||
CollisionObject2DSW *owner;
|
||||
bool _static;
|
||||
Rect2 aabb;
|
||||
// Owner's collision_mask/layer, used to detect changes in layers.
|
||||
uint32_t collision_mask;
|
||||
uint32_t collision_layer;
|
||||
int subindex;
|
||||
uint64_t pass;
|
||||
Map<Element *, PairData *> paired;
|
||||
};
|
||||
|
||||
struct RC {
|
||||
int ref;
|
||||
|
||||
_FORCE_INLINE_ int inc() {
|
||||
ref++;
|
||||
return ref;
|
||||
}
|
||||
_FORCE_INLINE_ int dec() {
|
||||
ref--;
|
||||
return ref;
|
||||
}
|
||||
|
||||
_FORCE_INLINE_ RC() {
|
||||
ref = 0;
|
||||
}
|
||||
};
|
||||
|
||||
Map<ID, Element> element_map;
|
||||
Map<Element *, RC> large_elements;
|
||||
|
||||
ID current;
|
||||
|
||||
uint64_t pass;
|
||||
|
||||
struct PairKey {
|
||||
union {
|
||||
struct {
|
||||
ID a;
|
||||
ID b;
|
||||
};
|
||||
uint64_t key;
|
||||
};
|
||||
|
||||
_FORCE_INLINE_ bool operator<(const PairKey &p_key) const {
|
||||
return key < p_key.key;
|
||||
}
|
||||
|
||||
PairKey() { key = 0; }
|
||||
PairKey(ID p_a, ID p_b) {
|
||||
if (p_a > p_b) {
|
||||
a = p_b;
|
||||
b = p_a;
|
||||
} else {
|
||||
a = p_a;
|
||||
b = p_b;
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
Map<PairKey, PairData> pair_map;
|
||||
|
||||
int cell_size;
|
||||
int large_object_min_surface;
|
||||
|
||||
PairCallback pair_callback;
|
||||
void *pair_userdata;
|
||||
UnpairCallback unpair_callback;
|
||||
void *unpair_userdata;
|
||||
|
||||
static _FORCE_INLINE_ bool _test_collision_mask(uint32_t p_mask1, uint32_t p_layer1, uint32_t p_mask2, uint32_t p_layer2) {
|
||||
return p_mask1 & p_layer2 || p_mask2 & p_layer1;
|
||||
}
|
||||
|
||||
void _enter_grid(Element *p_elem, const Rect2 &p_rect, bool p_static, bool p_force_enter);
|
||||
void _exit_grid(Element *p_elem, const Rect2 &p_rect, bool p_static, bool p_force_exit);
|
||||
template <bool use_aabb, bool use_segment>
|
||||
_FORCE_INLINE_ void _cull(const Point2i p_cell, const Rect2 &p_aabb, const Point2 &p_from, const Point2 &p_to, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices, int &index);
|
||||
|
||||
struct PosKey {
|
||||
union {
|
||||
struct {
|
||||
int32_t x;
|
||||
int32_t y;
|
||||
};
|
||||
uint64_t key;
|
||||
};
|
||||
|
||||
_FORCE_INLINE_ uint32_t hash() const {
|
||||
uint64_t k = key;
|
||||
k = (~k) + (k << 18); // k = (k << 18) - k - 1;
|
||||
k = k ^ (k >> 31);
|
||||
k = k * 21; // k = (k + (k << 2)) + (k << 4);
|
||||
k = k ^ (k >> 11);
|
||||
k = k + (k << 6);
|
||||
k = k ^ (k >> 22);
|
||||
return k;
|
||||
}
|
||||
|
||||
bool operator==(const PosKey &p_key) const { return key == p_key.key; }
|
||||
_FORCE_INLINE_ bool operator<(const PosKey &p_key) const {
|
||||
return key < p_key.key;
|
||||
}
|
||||
};
|
||||
|
||||
struct PosBin {
|
||||
PosKey key;
|
||||
Map<Element *, RC> object_set;
|
||||
Map<Element *, RC> static_object_set;
|
||||
PosBin *next;
|
||||
};
|
||||
|
||||
uint32_t hash_table_size;
|
||||
PosBin **hash_table;
|
||||
|
||||
void _pair_attempt(Element *p_elem, Element *p_with);
|
||||
void _unpair_attempt(Element *p_elem, Element *p_with);
|
||||
void _check_motion(Element *p_elem);
|
||||
|
||||
public:
|
||||
virtual ID create(CollisionObject2DSW *p_object, int p_subindex = 0);
|
||||
virtual void move(ID p_id, const Rect2 &p_aabb);
|
||||
virtual void set_static(ID p_id, bool p_static);
|
||||
virtual void remove(ID p_id);
|
||||
|
||||
virtual CollisionObject2DSW *get_object(ID p_id) const;
|
||||
virtual bool is_static(ID p_id) const;
|
||||
virtual int get_subindex(ID p_id) const;
|
||||
|
||||
virtual int cull_segment(const Vector2 &p_from, const Vector2 &p_to, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices = nullptr);
|
||||
virtual int cull_aabb(const Rect2 &p_aabb, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices = nullptr);
|
||||
|
||||
virtual void set_pair_callback(PairCallback p_pair_callback, void *p_userdata);
|
||||
virtual void set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata);
|
||||
|
||||
virtual void update();
|
||||
|
||||
static BroadPhase2DSW *_create();
|
||||
|
||||
BroadPhase2DHashGrid();
|
||||
~BroadPhase2DHashGrid();
|
||||
};
|
||||
|
||||
#endif // BROAD_PHASE_2D_HASH_GRID_H
|
||||
|
|
@ -48,7 +48,7 @@ public:
|
|||
typedef void (*UnpairCallback)(CollisionObject2DSW *A, int p_subindex_A, CollisionObject2DSW *B, int p_subindex_B, void *p_data, void *p_userdata);
|
||||
|
||||
// 0 is an invalid ID
|
||||
virtual ID create(CollisionObject2DSW *p_object_, int p_subindex = 0) = 0;
|
||||
virtual ID create(CollisionObject2DSW *p_object_, int p_subindex = 0, const Rect2 &p_aabb = Rect2(), bool p_static = false) = 0;
|
||||
virtual void move(ID p_id, const Rect2 &p_aabb) = 0;
|
||||
virtual void set_static(ID p_id, bool p_static) = 0;
|
||||
virtual void remove(ID p_id) = 0;
|
||||
|
|
|
|||
|
|
@ -182,19 +182,19 @@ void CollisionObject2DSW::_update_shapes() {
|
|||
continue;
|
||||
}
|
||||
|
||||
if (s.bpid == 0) {
|
||||
s.bpid = space->get_broadphase()->create(this, i);
|
||||
space->get_broadphase()->set_static(s.bpid, _static);
|
||||
}
|
||||
|
||||
//not quite correct, should compute the next matrix..
|
||||
Rect2 shape_aabb = s.shape->get_aabb();
|
||||
Transform2D xform = transform * s.xform;
|
||||
shape_aabb = xform.xform(shape_aabb);
|
||||
shape_aabb.grow_by((s.aabb_cache.size.x + s.aabb_cache.size.y) * 0.5 * 0.05);
|
||||
s.aabb_cache = shape_aabb;
|
||||
s.aabb_cache = s.aabb_cache.grow((s.aabb_cache.size.x + s.aabb_cache.size.y) * 0.5 * 0.05);
|
||||
|
||||
space->get_broadphase()->move(s.bpid, s.aabb_cache);
|
||||
if (s.bpid == 0) {
|
||||
s.bpid = space->get_broadphase()->create(this, i, shape_aabb, _static);
|
||||
space->get_broadphase()->set_static(s.bpid, _static);
|
||||
}
|
||||
|
||||
space->get_broadphase()->move(s.bpid, shape_aabb);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -209,11 +209,6 @@ void CollisionObject2DSW::_update_shapes_with_motion(const Vector2 &p_motion) {
|
|||
continue;
|
||||
}
|
||||
|
||||
if (s.bpid == 0) {
|
||||
s.bpid = space->get_broadphase()->create(this, i);
|
||||
space->get_broadphase()->set_static(s.bpid, _static);
|
||||
}
|
||||
|
||||
//not quite correct, should compute the next matrix..
|
||||
Rect2 shape_aabb = s.shape->get_aabb();
|
||||
Transform2D xform = transform * s.xform;
|
||||
|
|
@ -221,6 +216,11 @@ void CollisionObject2DSW::_update_shapes_with_motion(const Vector2 &p_motion) {
|
|||
shape_aabb = shape_aabb.merge(Rect2(shape_aabb.position + p_motion, shape_aabb.size)); //use motion
|
||||
s.aabb_cache = shape_aabb;
|
||||
|
||||
if (s.bpid == 0) {
|
||||
s.bpid = space->get_broadphase()->create(this, i, shape_aabb, _static);
|
||||
space->get_broadphase()->set_static(s.bpid, _static);
|
||||
}
|
||||
|
||||
space->get_broadphase()->move(s.bpid, shape_aabb);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -30,8 +30,7 @@
|
|||
|
||||
#include "physics_server_2d_sw.h"
|
||||
|
||||
#include "broad_phase_2d_basic.h"
|
||||
#include "broad_phase_2d_hash_grid.h"
|
||||
#include "broad_phase_2d_bvh.h"
|
||||
#include "collision_solver_2d_sw.h"
|
||||
#include "core/config/project_settings.h"
|
||||
#include "core/debugger/engine_debugger.h"
|
||||
|
|
@ -1356,8 +1355,7 @@ PhysicsServer2DSW *PhysicsServer2DSW::singletonsw = nullptr;
|
|||
|
||||
PhysicsServer2DSW::PhysicsServer2DSW(bool p_using_threads) {
|
||||
singletonsw = this;
|
||||
BroadPhase2DSW::create_func = BroadPhase2DHashGrid::_create;
|
||||
//BroadPhase2DSW::create_func=BroadPhase2DBasic::_create;
|
||||
BroadPhase2DSW::create_func = BroadPhase2DBVH::_create;
|
||||
|
||||
active = true;
|
||||
island_count = 0;
|
||||
|
|
|
|||
|
|
@ -1,212 +0,0 @@
|
|||
/*************************************************************************/
|
||||
/* broad_phase_3d_basic.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 "broad_phase_3d_basic.h"
|
||||
#include "core/string/print_string.h"
|
||||
#include "core/templates/list.h"
|
||||
|
||||
BroadPhase3DSW::ID BroadPhase3DBasic::create(CollisionObject3DSW *p_object, int p_subindex) {
|
||||
ERR_FAIL_COND_V(p_object == nullptr, 0);
|
||||
|
||||
current++;
|
||||
|
||||
Element e;
|
||||
e.owner = p_object;
|
||||
e._static = false;
|
||||
e.subindex = p_subindex;
|
||||
|
||||
element_map[current] = e;
|
||||
return current;
|
||||
}
|
||||
|
||||
void BroadPhase3DBasic::move(ID p_id, const AABB &p_aabb) {
|
||||
Map<ID, Element>::Element *E = element_map.find(p_id);
|
||||
ERR_FAIL_COND(!E);
|
||||
E->get().aabb = p_aabb;
|
||||
}
|
||||
|
||||
void BroadPhase3DBasic::set_static(ID p_id, bool p_static) {
|
||||
Map<ID, Element>::Element *E = element_map.find(p_id);
|
||||
ERR_FAIL_COND(!E);
|
||||
E->get()._static = p_static;
|
||||
}
|
||||
|
||||
void BroadPhase3DBasic::remove(ID p_id) {
|
||||
Map<ID, Element>::Element *E = element_map.find(p_id);
|
||||
ERR_FAIL_COND(!E);
|
||||
List<PairKey> to_erase;
|
||||
//unpair must be done immediately on removal to avoid potential invalid pointers
|
||||
for (Map<PairKey, void *>::Element *F = pair_map.front(); F; F = F->next()) {
|
||||
if (F->key().a == p_id || F->key().b == p_id) {
|
||||
if (unpair_callback) {
|
||||
Element *elem_A = &element_map[F->key().a];
|
||||
Element *elem_B = &element_map[F->key().b];
|
||||
unpair_callback(elem_A->owner, elem_A->subindex, elem_B->owner, elem_B->subindex, F->get(), unpair_userdata);
|
||||
}
|
||||
to_erase.push_back(F->key());
|
||||
}
|
||||
}
|
||||
while (to_erase.size()) {
|
||||
pair_map.erase(to_erase.front()->get());
|
||||
to_erase.pop_front();
|
||||
}
|
||||
element_map.erase(E);
|
||||
}
|
||||
|
||||
CollisionObject3DSW *BroadPhase3DBasic::get_object(ID p_id) const {
|
||||
const Map<ID, Element>::Element *E = element_map.find(p_id);
|
||||
ERR_FAIL_COND_V(!E, nullptr);
|
||||
return E->get().owner;
|
||||
}
|
||||
|
||||
bool BroadPhase3DBasic::is_static(ID p_id) const {
|
||||
const Map<ID, Element>::Element *E = element_map.find(p_id);
|
||||
ERR_FAIL_COND_V(!E, false);
|
||||
return E->get()._static;
|
||||
}
|
||||
|
||||
int BroadPhase3DBasic::get_subindex(ID p_id) const {
|
||||
const Map<ID, Element>::Element *E = element_map.find(p_id);
|
||||
ERR_FAIL_COND_V(!E, -1);
|
||||
return E->get().subindex;
|
||||
}
|
||||
|
||||
int BroadPhase3DBasic::cull_point(const Vector3 &p_point, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices) {
|
||||
int rc = 0;
|
||||
|
||||
for (Map<ID, Element>::Element *E = element_map.front(); E; E = E->next()) {
|
||||
const AABB aabb = E->get().aabb;
|
||||
if (aabb.has_point(p_point)) {
|
||||
p_results[rc] = E->get().owner;
|
||||
p_result_indices[rc] = E->get().subindex;
|
||||
rc++;
|
||||
if (rc >= p_max_results) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return rc;
|
||||
}
|
||||
|
||||
int BroadPhase3DBasic::cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices) {
|
||||
int rc = 0;
|
||||
|
||||
for (Map<ID, Element>::Element *E = element_map.front(); E; E = E->next()) {
|
||||
const AABB aabb = E->get().aabb;
|
||||
if (aabb.intersects_segment(p_from, p_to)) {
|
||||
p_results[rc] = E->get().owner;
|
||||
p_result_indices[rc] = E->get().subindex;
|
||||
rc++;
|
||||
if (rc >= p_max_results) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return rc;
|
||||
}
|
||||
|
||||
int BroadPhase3DBasic::cull_aabb(const AABB &p_aabb, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices) {
|
||||
int rc = 0;
|
||||
|
||||
for (Map<ID, Element>::Element *E = element_map.front(); E; E = E->next()) {
|
||||
const AABB aabb = E->get().aabb;
|
||||
if (aabb.intersects(p_aabb)) {
|
||||
p_results[rc] = E->get().owner;
|
||||
p_result_indices[rc] = E->get().subindex;
|
||||
rc++;
|
||||
if (rc >= p_max_results) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return rc;
|
||||
}
|
||||
|
||||
void BroadPhase3DBasic::set_pair_callback(PairCallback p_pair_callback, void *p_userdata) {
|
||||
pair_userdata = p_userdata;
|
||||
pair_callback = p_pair_callback;
|
||||
}
|
||||
|
||||
void BroadPhase3DBasic::set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) {
|
||||
unpair_userdata = p_userdata;
|
||||
unpair_callback = p_unpair_callback;
|
||||
}
|
||||
|
||||
void BroadPhase3DBasic::update() {
|
||||
// recompute pairs
|
||||
for (Map<ID, Element>::Element *I = element_map.front(); I; I = I->next()) {
|
||||
for (Map<ID, Element>::Element *J = I->next(); J; J = J->next()) {
|
||||
Element *elem_A = &I->get();
|
||||
Element *elem_B = &J->get();
|
||||
|
||||
if (elem_A->owner == elem_B->owner) {
|
||||
continue;
|
||||
}
|
||||
|
||||
bool pair_ok = elem_A->aabb.intersects(elem_B->aabb) && (!elem_A->_static || !elem_B->_static);
|
||||
|
||||
PairKey key(I->key(), J->key());
|
||||
|
||||
Map<PairKey, void *>::Element *E = pair_map.find(key);
|
||||
|
||||
if (!pair_ok && E) {
|
||||
if (unpair_callback) {
|
||||
unpair_callback(elem_A->owner, elem_A->subindex, elem_B->owner, elem_B->subindex, E->get(), unpair_userdata);
|
||||
}
|
||||
pair_map.erase(key);
|
||||
}
|
||||
|
||||
if (pair_ok && !E) {
|
||||
void *data = nullptr;
|
||||
if (pair_callback) {
|
||||
data = pair_callback(elem_A->owner, elem_A->subindex, elem_B->owner, elem_B->subindex, unpair_userdata);
|
||||
if (data) {
|
||||
pair_map.insert(key, data);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
BroadPhase3DSW *BroadPhase3DBasic::_create() {
|
||||
return memnew(BroadPhase3DBasic);
|
||||
}
|
||||
|
||||
BroadPhase3DBasic::BroadPhase3DBasic() {
|
||||
current = 1;
|
||||
unpair_callback = nullptr;
|
||||
unpair_userdata = nullptr;
|
||||
pair_callback = nullptr;
|
||||
pair_userdata = nullptr;
|
||||
}
|
||||
|
|
@ -1,5 +1,5 @@
|
|||
/*************************************************************************/
|
||||
/* broad_phase_octree.cpp */
|
||||
/* broad_phase_3d_bvh.cpp */
|
||||
/*************************************************************************/
|
||||
/* This file is part of: */
|
||||
/* GODOT ENGINE */
|
||||
|
|
@ -28,55 +28,55 @@
|
|||
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
|
||||
/*************************************************************************/
|
||||
|
||||
#include "broad_phase_octree.h"
|
||||
#include "broad_phase_3d_bvh.h"
|
||||
#include "collision_object_3d_sw.h"
|
||||
|
||||
BroadPhase3DSW::ID BroadPhaseOctree::create(CollisionObject3DSW *p_object, int p_subindex) {
|
||||
ID oid = octree.create(p_object, AABB(), p_subindex, false, 1 << p_object->get_type(), 0);
|
||||
return oid;
|
||||
BroadPhase3DBVH::ID BroadPhase3DBVH::create(CollisionObject3DSW *p_object, int p_subindex, const AABB &p_aabb, bool p_static) {
|
||||
ID oid = bvh.create(p_object, true, p_aabb, p_subindex, !p_static, 1 << p_object->get_type(), p_static ? 0 : 0xFFFFF); // Pair everything, don't care?
|
||||
return oid + 1;
|
||||
}
|
||||
|
||||
void BroadPhaseOctree::move(ID p_id, const AABB &p_aabb) {
|
||||
octree.move(p_id, p_aabb);
|
||||
void BroadPhase3DBVH::move(ID p_id, const AABB &p_aabb) {
|
||||
bvh.move(p_id - 1, p_aabb);
|
||||
}
|
||||
|
||||
void BroadPhaseOctree::set_static(ID p_id, bool p_static) {
|
||||
CollisionObject3DSW *it = octree.get(p_id);
|
||||
octree.set_pairable(p_id, !p_static, 1 << it->get_type(), p_static ? 0 : 0xFFFFF); //pair everything, don't care 1?
|
||||
void BroadPhase3DBVH::set_static(ID p_id, bool p_static) {
|
||||
CollisionObject3DSW *it = bvh.get(p_id - 1);
|
||||
bvh.set_pairable(p_id - 1, !p_static, 1 << it->get_type(), p_static ? 0 : 0xFFFFF, false); // Pair everything, don't care?
|
||||
}
|
||||
|
||||
void BroadPhaseOctree::remove(ID p_id) {
|
||||
octree.erase(p_id);
|
||||
void BroadPhase3DBVH::remove(ID p_id) {
|
||||
bvh.erase(p_id - 1);
|
||||
}
|
||||
|
||||
CollisionObject3DSW *BroadPhaseOctree::get_object(ID p_id) const {
|
||||
CollisionObject3DSW *it = octree.get(p_id);
|
||||
CollisionObject3DSW *BroadPhase3DBVH::get_object(ID p_id) const {
|
||||
CollisionObject3DSW *it = bvh.get(p_id - 1);
|
||||
ERR_FAIL_COND_V(!it, nullptr);
|
||||
return it;
|
||||
}
|
||||
|
||||
bool BroadPhaseOctree::is_static(ID p_id) const {
|
||||
return !octree.is_pairable(p_id);
|
||||
bool BroadPhase3DBVH::is_static(ID p_id) const {
|
||||
return !bvh.is_pairable(p_id - 1);
|
||||
}
|
||||
|
||||
int BroadPhaseOctree::get_subindex(ID p_id) const {
|
||||
return octree.get_subindex(p_id);
|
||||
int BroadPhase3DBVH::get_subindex(ID p_id) const {
|
||||
return bvh.get_subindex(p_id - 1);
|
||||
}
|
||||
|
||||
int BroadPhaseOctree::cull_point(const Vector3 &p_point, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices) {
|
||||
return octree.cull_point(p_point, p_results, p_max_results, p_result_indices);
|
||||
int BroadPhase3DBVH::cull_point(const Vector3 &p_point, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices) {
|
||||
return bvh.cull_point(p_point, p_results, p_max_results, p_result_indices);
|
||||
}
|
||||
|
||||
int BroadPhaseOctree::cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices) {
|
||||
return octree.cull_segment(p_from, p_to, p_results, p_max_results, p_result_indices);
|
||||
int BroadPhase3DBVH::cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices) {
|
||||
return bvh.cull_segment(p_from, p_to, p_results, p_max_results, p_result_indices);
|
||||
}
|
||||
|
||||
int BroadPhaseOctree::cull_aabb(const AABB &p_aabb, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices) {
|
||||
return octree.cull_aabb(p_aabb, p_results, p_max_results, p_result_indices);
|
||||
int BroadPhase3DBVH::cull_aabb(const AABB &p_aabb, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices) {
|
||||
return bvh.cull_aabb(p_aabb, p_results, p_max_results, p_result_indices);
|
||||
}
|
||||
|
||||
void *BroadPhaseOctree::_pair_callback(void *self, OctreeElementID p_A, CollisionObject3DSW *p_object_A, int subindex_A, OctreeElementID p_B, CollisionObject3DSW *p_object_B, int subindex_B) {
|
||||
BroadPhaseOctree *bpo = (BroadPhaseOctree *)(self);
|
||||
void *BroadPhase3DBVH::_pair_callback(void *self, uint32_t p_A, CollisionObject3DSW *p_object_A, int subindex_A, uint32_t p_B, CollisionObject3DSW *p_object_B, int subindex_B) {
|
||||
BroadPhase3DBVH *bpo = (BroadPhase3DBVH *)(self);
|
||||
if (!bpo->pair_callback) {
|
||||
return nullptr;
|
||||
}
|
||||
|
|
@ -84,8 +84,8 @@ void *BroadPhaseOctree::_pair_callback(void *self, OctreeElementID p_A, Collisio
|
|||
return bpo->pair_callback(p_object_A, subindex_A, p_object_B, subindex_B, bpo->pair_userdata);
|
||||
}
|
||||
|
||||
void BroadPhaseOctree::_unpair_callback(void *self, OctreeElementID p_A, CollisionObject3DSW *p_object_A, int subindex_A, OctreeElementID p_B, CollisionObject3DSW *p_object_B, int subindex_B, void *pairdata) {
|
||||
BroadPhaseOctree *bpo = (BroadPhaseOctree *)(self);
|
||||
void BroadPhase3DBVH::_unpair_callback(void *self, uint32_t p_A, CollisionObject3DSW *p_object_A, int subindex_A, uint32_t p_B, CollisionObject3DSW *p_object_B, int subindex_B, void *pairdata) {
|
||||
BroadPhase3DBVH *bpo = (BroadPhase3DBVH *)(self);
|
||||
if (!bpo->unpair_callback) {
|
||||
return;
|
||||
}
|
||||
|
|
@ -93,27 +93,27 @@ void BroadPhaseOctree::_unpair_callback(void *self, OctreeElementID p_A, Collisi
|
|||
bpo->unpair_callback(p_object_A, subindex_A, p_object_B, subindex_B, pairdata, bpo->unpair_userdata);
|
||||
}
|
||||
|
||||
void BroadPhaseOctree::set_pair_callback(PairCallback p_pair_callback, void *p_userdata) {
|
||||
void BroadPhase3DBVH::set_pair_callback(PairCallback p_pair_callback, void *p_userdata) {
|
||||
pair_callback = p_pair_callback;
|
||||
pair_userdata = p_userdata;
|
||||
}
|
||||
|
||||
void BroadPhaseOctree::set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) {
|
||||
void BroadPhase3DBVH::set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) {
|
||||
unpair_callback = p_unpair_callback;
|
||||
unpair_userdata = p_userdata;
|
||||
}
|
||||
|
||||
void BroadPhaseOctree::update() {
|
||||
// does.. not?
|
||||
void BroadPhase3DBVH::update() {
|
||||
bvh.update();
|
||||
}
|
||||
|
||||
BroadPhase3DSW *BroadPhaseOctree::_create() {
|
||||
return memnew(BroadPhaseOctree);
|
||||
BroadPhase3DSW *BroadPhase3DBVH::_create() {
|
||||
return memnew(BroadPhase3DBVH);
|
||||
}
|
||||
|
||||
BroadPhaseOctree::BroadPhaseOctree() {
|
||||
octree.set_pair_callback(_pair_callback, this);
|
||||
octree.set_unpair_callback(_unpair_callback, this);
|
||||
BroadPhase3DBVH::BroadPhase3DBVH() {
|
||||
bvh.set_pair_callback(_pair_callback, this);
|
||||
bvh.set_unpair_callback(_unpair_callback, this);
|
||||
pair_callback = nullptr;
|
||||
pair_userdata = nullptr;
|
||||
unpair_userdata = nullptr;
|
||||
|
|
@ -1,5 +1,5 @@
|
|||
/*************************************************************************/
|
||||
/* broad_phase_octree.h */
|
||||
/* broad_phase_3d_bvh.h */
|
||||
/*************************************************************************/
|
||||
/* This file is part of: */
|
||||
/* GODOT ENGINE */
|
||||
|
|
@ -28,17 +28,17 @@
|
|||
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
|
||||
/*************************************************************************/
|
||||
|
||||
#ifndef BROAD_PHASE_OCTREE_H
|
||||
#define BROAD_PHASE_OCTREE_H
|
||||
#ifndef BROAD_PHASE_3D_BVH_H
|
||||
#define BROAD_PHASE_3D_BVH_H
|
||||
|
||||
#include "broad_phase_3d_sw.h"
|
||||
#include "core/math/octree.h"
|
||||
#include "core/math/bvh.h"
|
||||
|
||||
class BroadPhaseOctree : public BroadPhase3DSW {
|
||||
Octree<CollisionObject3DSW, true> octree;
|
||||
class BroadPhase3DBVH : public BroadPhase3DSW {
|
||||
BVH_Manager<CollisionObject3DSW, true, 128> bvh;
|
||||
|
||||
static void *_pair_callback(void *, OctreeElementID, CollisionObject3DSW *, int, OctreeElementID, CollisionObject3DSW *, int);
|
||||
static void _unpair_callback(void *, OctreeElementID, CollisionObject3DSW *, int, OctreeElementID, CollisionObject3DSW *, int, void *);
|
||||
static void *_pair_callback(void *, uint32_t, CollisionObject3DSW *, int, uint32_t, CollisionObject3DSW *, int);
|
||||
static void _unpair_callback(void *, uint32_t, CollisionObject3DSW *, int, uint32_t, CollisionObject3DSW *, int, void *);
|
||||
|
||||
PairCallback pair_callback;
|
||||
void *pair_userdata;
|
||||
|
|
@ -47,7 +47,7 @@ class BroadPhaseOctree : public BroadPhase3DSW {
|
|||
|
||||
public:
|
||||
// 0 is an invalid ID
|
||||
virtual ID create(CollisionObject3DSW *p_object, int p_subindex = 0);
|
||||
virtual ID create(CollisionObject3DSW *p_object, int p_subindex = 0, const AABB &p_aabb = AABB(), bool p_static = false);
|
||||
virtual void move(ID p_id, const AABB &p_aabb);
|
||||
virtual void set_static(ID p_id, bool p_static);
|
||||
virtual void remove(ID p_id);
|
||||
|
|
@ -66,7 +66,7 @@ public:
|
|||
virtual void update();
|
||||
|
||||
static BroadPhase3DSW *_create();
|
||||
BroadPhaseOctree();
|
||||
BroadPhase3DBVH();
|
||||
};
|
||||
|
||||
#endif // BROAD_PHASE_OCTREE_H
|
||||
#endif // BROAD_PHASE_3D_BVH_H
|
||||
|
|
@ -48,7 +48,7 @@ public:
|
|||
typedef void (*UnpairCallback)(CollisionObject3DSW *A, int p_subindex_A, CollisionObject3DSW *B, int p_subindex_B, void *p_data, void *p_userdata);
|
||||
|
||||
// 0 is an invalid ID
|
||||
virtual ID create(CollisionObject3DSW *p_object_, int p_subindex = 0) = 0;
|
||||
virtual ID create(CollisionObject3DSW *p_object_, int p_subindex = 0, const AABB &p_aabb = AABB(), bool p_static = false) = 0;
|
||||
virtual void move(ID p_id, const AABB &p_aabb) = 0;
|
||||
virtual void set_static(ID p_id, bool p_static) = 0;
|
||||
virtual void remove(ID p_id) = 0;
|
||||
|
|
|
|||
|
|
@ -146,22 +146,23 @@ void CollisionObject3DSW::_update_shapes() {
|
|||
|
||||
for (int i = 0; i < shapes.size(); i++) {
|
||||
Shape &s = shapes.write[i];
|
||||
if (s.bpid == 0) {
|
||||
s.bpid = space->get_broadphase()->create(this, i);
|
||||
space->get_broadphase()->set_static(s.bpid, _static);
|
||||
}
|
||||
|
||||
//not quite correct, should compute the next matrix..
|
||||
AABB shape_aabb = s.shape->get_aabb();
|
||||
Transform xform = transform * s.xform;
|
||||
shape_aabb = xform.xform(shape_aabb);
|
||||
shape_aabb.grow_by((s.aabb_cache.size.x + s.aabb_cache.size.y) * 0.5 * 0.05);
|
||||
s.aabb_cache = shape_aabb;
|
||||
s.aabb_cache = s.aabb_cache.grow((s.aabb_cache.size.x + s.aabb_cache.size.y) * 0.5 * 0.05);
|
||||
|
||||
Vector3 scale = xform.get_basis().get_scale();
|
||||
s.area_cache = s.shape->get_area() * scale.x * scale.y * scale.z;
|
||||
|
||||
space->get_broadphase()->move(s.bpid, s.aabb_cache);
|
||||
if (s.bpid == 0) {
|
||||
s.bpid = space->get_broadphase()->create(this, i, shape_aabb, _static);
|
||||
space->get_broadphase()->set_static(s.bpid, _static);
|
||||
}
|
||||
|
||||
space->get_broadphase()->move(s.bpid, shape_aabb);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -172,18 +173,19 @@ void CollisionObject3DSW::_update_shapes_with_motion(const Vector3 &p_motion) {
|
|||
|
||||
for (int i = 0; i < shapes.size(); i++) {
|
||||
Shape &s = shapes.write[i];
|
||||
if (s.bpid == 0) {
|
||||
s.bpid = space->get_broadphase()->create(this, i);
|
||||
space->get_broadphase()->set_static(s.bpid, _static);
|
||||
}
|
||||
|
||||
//not quite correct, should compute the next matrix..
|
||||
AABB shape_aabb = s.shape->get_aabb();
|
||||
Transform xform = transform * s.xform;
|
||||
shape_aabb = xform.xform(shape_aabb);
|
||||
shape_aabb = shape_aabb.merge(AABB(shape_aabb.position + p_motion, shape_aabb.size)); //use motion
|
||||
shape_aabb.merge_with(AABB(shape_aabb.position + p_motion, shape_aabb.size)); //use motion
|
||||
s.aabb_cache = shape_aabb;
|
||||
|
||||
if (s.bpid == 0) {
|
||||
s.bpid = space->get_broadphase()->create(this, i, shape_aabb, _static);
|
||||
space->get_broadphase()->set_static(s.bpid, _static);
|
||||
}
|
||||
|
||||
space->get_broadphase()->move(s.bpid, shape_aabb);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -30,8 +30,7 @@
|
|||
|
||||
#include "physics_server_3d_sw.h"
|
||||
|
||||
#include "broad_phase_3d_basic.h"
|
||||
#include "broad_phase_octree.h"
|
||||
#include "broad_phase_3d_bvh.h"
|
||||
#include "core/debugger/engine_debugger.h"
|
||||
#include "core/os/os.h"
|
||||
#include "joints/cone_twist_joint_3d_sw.h"
|
||||
|
|
@ -1755,7 +1754,8 @@ void PhysicsServer3DSW::_shape_col_cbk(const Vector3 &p_point_A, int p_index_A,
|
|||
PhysicsServer3DSW *PhysicsServer3DSW::singletonsw = nullptr;
|
||||
PhysicsServer3DSW::PhysicsServer3DSW(bool p_using_threads) {
|
||||
singletonsw = this;
|
||||
BroadPhase3DSW::create_func = BroadPhaseOctree::_create;
|
||||
BroadPhase3DSW::create_func = BroadPhase3DBVH::_create;
|
||||
|
||||
island_count = 0;
|
||||
active_objects = 0;
|
||||
collision_pairs = 0;
|
||||
|
|
|
|||
Loading…
Reference in New Issue