godotengine
/
godot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fix physics BVH broadphase update when changing collision layer/mask 

The BVH implementation is not checking collision layers on existing
pairs on move like other physics broadphases do.

This is solved by adding a new call to trigger pair callbacks again so
the physics engine can check layers again (specific to the BVH version,
other broadphase implementations just trigger a move like before).
This commit is contained in:
PouleyKetchoupp 2021-10-21 12:06:09 -07:00
parent 2c548d5005
commit 48144ed40e
24 changed files with 263 additions and 62 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
core/math/bvh.h
View File

@ -59,6 +59,7 @@ public:
// 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 *);
typedef void *(*CheckPairCallback)(void *, uint32_t, T *, int, uint32_t, T *, int, void *);
// allow locally toggling thread safety if the template has been compiled with BVH_THREAD_SAFE
void params_set_thread_safe(bool p_enable) {
@ -97,6 +98,11 @@ public:
unpair_callback = p_callback;
unpair_callback_userdata = p_userdata;
}
void set_check_pair_callback(CheckPairCallback p_callback, void *p_userdata) {
BVH_LOCKED_FUNCTION
check_pair_callback = p_callback;
check_pair_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) {
BVH_LOCKED_FUNCTION
@ -142,6 +148,12 @@ public:
move(h, p_aabb);
}
void recheck_pairs(uint32_t p_handle) {
BVHHandle h;
h.set(p_handle);
recheck_pairs(h);
}
void erase(uint32_t p_handle) {
BVHHandle h;
h.set(p_handle);
@ -200,6 +212,13 @@ public:
}
}
void recheck_pairs(BVHHandle p_handle) {
BVH_LOCKED_FUNCTION
if (USE_PAIRS) {
_recheck_pairs(p_handle);
}
}
void erase(BVHHandle p_handle) {
BVH_LOCKED_FUNCTION
// call unpair and remove all references to the item
@ -517,6 +536,23 @@ private:
}
}
void _recheck_pair(BVHHandle p_from, BVHHandle p_to, void *p_pair_data) {
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;
}
// callback
if (check_pair_callback) {
check_pair_callback(check_pair_callback_userdata, p_from, exa.userdata, exa.subindex, p_to, exb.userdata, exb.subindex, p_pair_data);
}
}
// 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;
@ -620,6 +656,18 @@ private:
}
}
// Send pair callbacks again for all existing pairs for the given handle.
void _recheck_pairs(BVHHandle p_handle) {
typename BVHTREE_CLASS::ItemPairs &p_from = tree._pairs[p_handle.id()];
// checking pair for every partner.
for (unsigned int n = 0; n < p_from.extended_pairs.size(); n++) {
const typename BVHTREE_CLASS::ItemPairs::Link &pair = p_from.extended_pairs[n];
BVHHandle h_to = pair.handle;
_recheck_pair(p_handle, h_to, pair.userdata);
}
}
private:
const typename BVHTREE_CLASS::ItemExtra &_get_extra(BVHHandle p_handle) const {
return tree._extra[p_handle.id()];
@ -696,8 +744,10 @@ private:
PairCallback pair_callback;
UnpairCallback unpair_callback;
CheckPairCallback check_pair_callback;
void *pair_callback_userdata;
void *unpair_callback_userdata;
void *check_pair_callback_userdata;
BVHTREE_CLASS tree;

7
core/math/octree_definition.inc
View File

@ -445,6 +445,7 @@ public:
bool is_pairable(OctreeElementID p_id) const;
T *get(OctreeElementID p_id) const;
int get_subindex(OctreeElementID p_id) const;
AABB get_aabb(OctreeElementID p_id) const;
int cull_convex(const Vector<Plane> &p_convex, T **p_result_array, int p_result_max, uint32_t p_mask = 0xFFFFFFFF);
int cull_aabb(const AABB &p_aabb, T **p_result_array, int p_result_max, int *p_subindex_array = nullptr, uint32_t p_mask = 0xFFFFFFFF);
@ -498,6 +499,12 @@ OCTREE_FUNC(int)::get_subindex(OctreeElementID p_id) const {
return E->get().subindex;
}
OCTREE_FUNC(AABB)::get_aabb(OctreeElementID p_id) const {
const typename ElementMap::Element *E = element_map.find(p_id);
ERR_FAIL_COND_V(!E, AABB());
return E->get().aabb;
}
#define OCTREE_DIVISOR 4
OCTREE_FUNC(void)::_insert_element(Element *p_element, Octant *p_octant) {

8
servers/physics/broad_phase_basic.cpp
View File

@ -51,11 +51,17 @@ void BroadPhaseBasic::move(ID p_id, const AABB &p_aabb) {
ERR_FAIL_COND(!E);
E->get().aabb = p_aabb;
}
void BroadPhaseBasic::recheck_pairs(ID p_id) {
// Not supported.
}
void BroadPhaseBasic::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 BroadPhaseBasic::remove(ID p_id) {
Map<ID, Element>::Element *E = element_map.find(p_id);
ERR_FAIL_COND(!E);
@ -183,7 +189,7 @@ void BroadPhaseBasic::update() {
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);
data = pair_callback(elem_A->owner, elem_A->subindex, elem_B->owner, elem_B->subindex, nullptr, unpair_userdata);
if (data) {
pair_map.insert(key, data);
}

1
servers/physics/broad_phase_basic.h
View File

@ -82,6 +82,7 @@ public:
// 0 is an invalid ID
virtual ID create(CollisionObjectSW *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 recheck_pairs(ID p_id);
virtual void set_static(ID p_id, bool p_static);
virtual void remove(ID p_id);

30
servers/physics/broad_phase_bvh.cpp
View File

@ -41,10 +41,15 @@ void BroadPhaseBVH::move(ID p_id, const AABB &p_aabb) {
bvh.move(p_id - 1, p_aabb);
}
void BroadPhaseBVH::recheck_pairs(ID p_id) {
bvh.recheck_pairs(p_id - 1);
}
void BroadPhaseBVH::set_static(ID p_id, bool p_static) {
CollisionObjectSW *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 BroadPhaseBVH::remove(ID p_id) {
bvh.erase(p_id - 1);
}
@ -54,9 +59,11 @@ CollisionObjectSW *BroadPhaseBVH::get_object(ID p_id) const {
ERR_FAIL_COND_V(!it, nullptr);
return it;
}
bool BroadPhaseBVH::is_static(ID p_id) const {
return !bvh.is_pairable(p_id - 1);
}
int BroadPhaseBVH::get_subindex(ID p_id) const {
return bvh.get_subindex(p_id - 1);
}
@ -73,28 +80,38 @@ int BroadPhaseBVH::cull_aabb(const AABB &p_aabb, CollisionObjectSW **p_results,
return bvh.cull_aabb(p_aabb, p_results, p_max_results, p_result_indices);
}
void *BroadPhaseBVH::_pair_callback(void *self, uint32_t p_A, CollisionObjectSW *p_object_A, int subindex_A, uint32_t p_B, CollisionObjectSW *p_object_B, int subindex_B) {
BroadPhaseBVH *bpo = (BroadPhaseBVH *)(self);
void *BroadPhaseBVH::_pair_callback(void *p_self, uint32_t p_id_A, CollisionObjectSW *p_object_A, int p_subindex_A, uint32_t p_id_B, CollisionObjectSW *p_object_B, int p_subindex_B) {
BroadPhaseBVH *bpo = (BroadPhaseBVH *)(p_self);
if (!bpo->pair_callback) {
return nullptr;
}
return bpo->pair_callback(p_object_A, subindex_A, p_object_B, subindex_B, bpo->pair_userdata);
return bpo->pair_callback(p_object_A, p_subindex_A, p_object_B, p_subindex_B, nullptr, bpo->pair_userdata);
}
void BroadPhaseBVH::_unpair_callback(void *self, uint32_t p_A, CollisionObjectSW *p_object_A, int subindex_A, uint32_t p_B, CollisionObjectSW *p_object_B, int subindex_B, void *pairdata) {
BroadPhaseBVH *bpo = (BroadPhaseBVH *)(self);
void BroadPhaseBVH::_unpair_callback(void *p_self, uint32_t p_id_A, CollisionObjectSW *p_object_A, int p_subindex_A, uint32_t p_id_B, CollisionObjectSW *p_object_B, int p_subindex_B, void *p_pair_data) {
BroadPhaseBVH *bpo = (BroadPhaseBVH *)(p_self);
if (!bpo->unpair_callback) {
return;
}
bpo->unpair_callback(p_object_A, subindex_A, p_object_B, subindex_B, pairdata, bpo->unpair_userdata);
bpo->unpair_callback(p_object_A, p_subindex_A, p_object_B, p_subindex_B, p_pair_data, bpo->unpair_userdata);
}
void *BroadPhaseBVH::_check_pair_callback(void *p_self, uint32_t p_id_A, CollisionObjectSW *p_object_A, int p_subindex_A, uint32_t p_id_B, CollisionObjectSW *p_object_B, int p_subindex_B, void *p_pair_data) {
BroadPhaseBVH *bpo = (BroadPhaseBVH *)(p_self);
if (!bpo->pair_callback) {
return nullptr;
}
return bpo->pair_callback(p_object_A, p_subindex_A, p_object_B, p_subindex_B, p_pair_data, bpo->pair_userdata);
}
void BroadPhaseBVH::set_pair_callback(PairCallback p_pair_callback, void *p_userdata) {
pair_callback = p_pair_callback;
pair_userdata = p_userdata;
}
void BroadPhaseBVH::set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) {
unpair_callback = p_unpair_callback;
unpair_userdata = p_userdata;
@ -112,6 +129,7 @@ BroadPhaseBVH::BroadPhaseBVH() {
bvh.params_set_thread_safe(GLOBAL_GET("rendering/threads/thread_safe_bvh"));
bvh.set_pair_callback(_pair_callback, this);
bvh.set_unpair_callback(_unpair_callback, this);
bvh.set_check_pair_callback(_check_pair_callback, this);
pair_callback = nullptr;
pair_userdata = nullptr;
unpair_userdata = nullptr;

6
servers/physics/broad_phase_bvh.h
View File

@ -37,8 +37,9 @@
class BroadPhaseBVH : public BroadPhaseSW {
BVH_Manager<CollisionObjectSW, true, 128> bvh;
static void *_pair_callback(void *, uint32_t, CollisionObjectSW *, int, uint32_t, CollisionObjectSW *, int);
static void _unpair_callback(void *, uint32_t, CollisionObjectSW *, int, uint32_t, CollisionObjectSW *, int, void *);
static void *_pair_callback(void *p_self, uint32_t p_id_A, CollisionObjectSW *p_object_A, int p_subindex_A, uint32_t p_id_B, CollisionObjectSW *p_object_B, int p_subindex_B);
static void _unpair_callback(void *p_self, uint32_t p_id_A, CollisionObjectSW *p_object_A, int p_subindex_A, uint32_t p_id_B, CollisionObjectSW *p_object_B, int p_subindex_B, void *p_pair_data);
static void *_check_pair_callback(void *p_self, uint32_t p_id_A, CollisionObjectSW *p_object_A, int p_subindex_A, uint32_t p_id_B, CollisionObjectSW *p_object_B, int p_subindex_B, void *p_pair_data);
PairCallback pair_callback;
void *pair_userdata;
@ -49,6 +50,7 @@ public:
// 0 is an invalid ID
virtual ID create(CollisionObjectSW *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 recheck_pairs(ID p_id);
virtual void set_static(ID p_id, bool p_static);
virtual void remove(ID p_id);

8
servers/physics/broad_phase_octree.cpp
View File

@ -40,10 +40,16 @@ void BroadPhaseOctree::move(ID p_id, const AABB &p_aabb) {
octree.move(p_id, p_aabb);
}
void BroadPhaseOctree::recheck_pairs(ID p_id) {
AABB aabb = octree.get_aabb(p_id);
octree.move(p_id, aabb);
}
void BroadPhaseOctree::set_static(ID p_id, bool p_static) {
CollisionObjectSW *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 BroadPhaseOctree::remove(ID p_id) {
octree.erase(p_id);
}
@ -78,7 +84,7 @@ void *BroadPhaseOctree::_pair_callback(void *self, OctreeElementID p_A, Collisio
return nullptr;
}
return bpo->pair_callback(p_object_A, subindex_A, p_object_B, subindex_B, bpo->pair_userdata);
return bpo->pair_callback(p_object_A, subindex_A, p_object_B, subindex_B, nullptr, bpo->pair_userdata);
}
void BroadPhaseOctree::_unpair_callback(void *self, OctreeElementID p_A, CollisionObjectSW *p_object_A, int subindex_A, OctreeElementID p_B, CollisionObjectSW *p_object_B, int subindex_B, void *pairdata) {

1
servers/physics/broad_phase_octree.h
View File

@ -49,6 +49,7 @@ public:
// 0 is an invalid ID
virtual ID create(CollisionObjectSW *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 recheck_pairs(ID p_id);
virtual void set_static(ID p_id, bool p_static);
virtual void remove(ID p_id);

5
servers/physics/broad_phase_sw.h
View File

@ -44,12 +44,13 @@ public:
typedef uint32_t ID;
typedef void *(*PairCallback)(CollisionObjectSW *A, int p_subindex_A, CollisionObjectSW *B, int p_subindex_B, void *p_userdata);
typedef void (*UnpairCallback)(CollisionObjectSW *A, int p_subindex_A, CollisionObjectSW *B, int p_subindex_B, void *p_data, void *p_userdata);
typedef void *(*PairCallback)(CollisionObjectSW *p_object_A, int p_subindex_A, CollisionObjectSW *p_object_B, int p_subindex_B, void *p_pair_data, void *p_user_data);
typedef void (*UnpairCallback)(CollisionObjectSW *p_object_A, int p_subindex_A, CollisionObjectSW *p_object_B, int p_subindex_B, void *p_pair_data, void *p_user_data);
// 0 is an invalid ID
virtual ID create(CollisionObjectSW *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 recheck_pairs(ID p_id) = 0;
virtual void set_static(ID p_id, bool p_static) = 0;
virtual void remove(ID p_id) = 0;

17
servers/physics/collision_object_sw.cpp
View File

@ -180,6 +180,23 @@ void CollisionObjectSW::_update_shapes() {
}
}
void CollisionObjectSW::_recheck_shapes() {
if (!space) {
return;
}
for (int i = 0; i < shapes.size(); i++) {
Shape &s = shapes.write[i];
if (s.disabled) {
continue;
}
if (s.bpid != 0) {
space->get_broadphase()->recheck_pairs(s.bpid);
}
}
}
void CollisionObjectSW::_update_shapes_with_motion(const Vector3 &p_motion) {
if (!space) {
return;

7
servers/physics/collision_object_sw.h
View File

@ -79,6 +79,7 @@ private:
SelfList<CollisionObjectSW> pending_shape_update_list;
void _update_shapes();
void _recheck_shapes();
protected:
void _update_shapes_with_motion(const Vector3 &p_motion);
@ -155,13 +156,15 @@ public:
_FORCE_INLINE_ void set_collision_layer(uint32_t p_layer) {
collision_layer = p_layer;
_shape_changed();
_recheck_shapes();
_shapes_changed();
}
_FORCE_INLINE_ uint32_t get_collision_layer() const { return collision_layer; }
_FORCE_INLINE_ void set_collision_mask(uint32_t p_mask) {
collision_mask = p_mask;
_shape_changed();
_recheck_shapes();
_shapes_changed();
}
_FORCE_INLINE_ uint32_t get_collision_mask() const { return collision_mask; }

46
servers/physics/space_sw.cpp
View File

@ -1079,15 +1079,29 @@ bool SpaceSW::test_body_motion(BodySW *p_body, const Transform &p_from, const Ve
return collided;
}
void *SpaceSW::_broadphase_pair(CollisionObjectSW *A, int p_subindex_A, CollisionObjectSW *B, int p_subindex_B, void *p_self) {
if (!A->test_collision_mask(B)) {
void *SpaceSW::_broadphase_pair(CollisionObjectSW *p_object_A, int p_subindex_A, CollisionObjectSW *p_object_B, int p_subindex_B, void *p_pair_data, void *p_self) {
bool valid_collision_pair = p_object_A->test_collision_mask(p_object_B);
if (p_pair_data) {
// Checking an existing pair.
if (valid_collision_pair) {
// Nothing to do, pair is still valid.
return p_pair_data;
} else {
// Logical collision not valid anymore, unpair.
_broadphase_unpair(p_object_A, p_subindex_A, p_object_B, p_subindex_B, p_pair_data, p_self);
return nullptr;
}
}
if (!valid_collision_pair) {
return nullptr;
}
CollisionObjectSW::Type type_A = A->get_type();
CollisionObjectSW::Type type_B = B->get_type();
CollisionObjectSW::Type type_A = p_object_A->get_type();
CollisionObjectSW::Type type_B = p_object_B->get_type();
if (type_A > type_B) {
SWAP(A, B);
SWAP(p_object_A, p_object_B);
SWAP(p_subindex_A, p_subindex_B);
SWAP(type_A, type_B);
}
@ -1097,32 +1111,34 @@ void *SpaceSW::_broadphase_pair(CollisionObjectSW *A, int p_subindex_A, Collisio
self->collision_pairs++;
if (type_A == CollisionObjectSW::TYPE_AREA) {
AreaSW *area = static_cast<AreaSW *>(A);
AreaSW *area_a = static_cast<AreaSW *>(p_object_A);
if (type_B == CollisionObjectSW::TYPE_AREA) {
AreaSW *area_b = static_cast<AreaSW *>(B);
Area2PairSW *area2_pair = memnew(Area2PairSW(area_b, p_subindex_B, area, p_subindex_A));
AreaSW *area_b = static_cast<AreaSW *>(p_object_B);
Area2PairSW *area2_pair = memnew(Area2PairSW(area_b, p_subindex_B, area_a, p_subindex_A));
return area2_pair;
} else {
BodySW *body = static_cast<BodySW *>(B);
AreaPairSW *area_pair = memnew(AreaPairSW(body, p_subindex_B, area, p_subindex_A));
BodySW *body_b = static_cast<BodySW *>(p_object_B);
AreaPairSW *area_pair = memnew(AreaPairSW(body_b, p_subindex_B, area_a, p_subindex_A));
return area_pair;
}
} else {
BodyPairSW *b = memnew(BodyPairSW((BodySW *)A, p_subindex_A, (BodySW *)B, p_subindex_B));
return b;
BodySW *body_a = static_cast<BodySW *>(p_object_A);
BodySW *body_b = static_cast<BodySW *>(p_object_B);
BodyPairSW *body_pair = memnew(BodyPairSW(body_a, p_subindex_A, body_b, p_subindex_B));
return body_pair;
}
return nullptr;
}
void SpaceSW::_broadphase_unpair(CollisionObjectSW *A, int p_subindex_A, CollisionObjectSW *B, int p_subindex_B, void *p_data, void *p_self) {
if (!p_data) {
void SpaceSW::_broadphase_unpair(CollisionObjectSW *p_object_A, int p_subindex_A, CollisionObjectSW *p_object_B, int p_subindex_B, void *p_pair_data, void *p_self) {
if (!p_pair_data) {
return;
}
SpaceSW *self = (SpaceSW *)p_self;
self->collision_pairs--;
ConstraintSW *c = (ConstraintSW *)p_data;
ConstraintSW *c = (ConstraintSW *)p_pair_data;
memdelete(c);
}

4
servers/physics/space_sw.h
View File

@ -83,8 +83,8 @@ private:
SelfList<AreaSW>::List monitor_query_list;
SelfList<AreaSW>::List area_moved_list;
static void *_broadphase_pair(CollisionObjectSW *A, int p_subindex_A, CollisionObjectSW *B, int p_subindex_B, void *p_self);
static void _broadphase_unpair(CollisionObjectSW *A, int p_subindex_A, CollisionObjectSW *B, int p_subindex_B, void *p_data, void *p_self);
static void *_broadphase_pair(CollisionObjectSW *p_object_A, int p_subindex_A, CollisionObjectSW *p_object_B, int p_subindex_B, void *p_pair_data, void *p_self);
static void _broadphase_unpair(CollisionObjectSW *p_object_A, int p_subindex_A, CollisionObjectSW *p_object_B, int p_subindex_B, void *p_pair_data, void *p_self);
Set<CollisionObjectSW *> objects;

8
servers/physics_2d/broad_phase_2d_basic.cpp
View File

@ -47,11 +47,17 @@ void BroadPhase2DBasic::move(ID p_id, const Rect2 &p_aabb) {
ERR_FAIL_COND(!E);
E->get().aabb = p_aabb;
}
void BroadPhase2DBasic::recheck_pairs(ID p_id) {
// Not supported.
}
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);
@ -145,7 +151,7 @@ void BroadPhase2DBasic::update() {
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);
data = pair_callback(elem_A->owner, elem_A->subindex, elem_B->owner, elem_B->subindex, nullptr, unpair_userdata);
if (data) {
pair_map.insert(key, data);
}

1
servers/physics_2d/broad_phase_2d_basic.h
View File

@ -81,6 +81,7 @@ public:
// 0 is an invalid ID
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 recheck_pairs(ID p_id);
virtual void set_static(ID p_id, bool p_static);
virtual void remove(ID p_id);

29
servers/physics_2d/broad_phase_2d_bvh.cpp
View File

@ -41,10 +41,15 @@ void BroadPhase2DBVH::move(ID p_id, const Rect2 &p_aabb) {
bvh.move(p_id - 1, p_aabb);
}
void BroadPhase2DBVH::recheck_pairs(ID p_id) {
bvh.recheck_pairs(p_id - 1);
}
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);
}
@ -54,9 +59,11 @@ CollisionObject2DSW *BroadPhase2DBVH::get_object(ID p_id) const {
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);
}
@ -69,22 +76,31 @@ int BroadPhase2DBVH::cull_aabb(const Rect2 &p_aabb, CollisionObject2DSW **p_resu
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);
void *BroadPhase2DBVH::_pair_callback(void *p_self, uint32_t p_id_A, CollisionObject2DSW *p_object_A, int p_subindex_A, uint32_t p_id_B, CollisionObject2DSW *p_object_B, int p_subindex_B) {
BroadPhase2DBVH *bpo = (BroadPhase2DBVH *)(p_self);
if (!bpo->pair_callback) {
return nullptr;
}
return bpo->pair_callback(p_object_A, subindex_A, p_object_B, subindex_B, bpo->pair_userdata);
return bpo->pair_callback(p_object_A, p_subindex_A, p_object_B, p_subindex_B, nullptr, 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);
void BroadPhase2DBVH::_unpair_callback(void *p_self, uint32_t p_id_A, CollisionObject2DSW *p_object_A, int p_subindex_A, uint32_t p_id_B, CollisionObject2DSW *p_object_B, int p_subindex_B, void *p_pair_data) {
BroadPhase2DBVH *bpo = (BroadPhase2DBVH *)(p_self);
if (!bpo->unpair_callback) {
return;
}
bpo->unpair_callback(p_object_A, subindex_A, p_object_B, subindex_B, pairdata, bpo->unpair_userdata);
bpo->unpair_callback(p_object_A, p_subindex_A, p_object_B, p_subindex_B, p_pair_data, bpo->unpair_userdata);
}
void *BroadPhase2DBVH::_check_pair_callback(void *p_self, uint32_t p_id_A, CollisionObject2DSW *p_object_A, int p_subindex_A, uint32_t p_id_B, CollisionObject2DSW *p_object_B, int p_subindex_B, void *p_pair_data) {
BroadPhase2DBVH *bpo = (BroadPhase2DBVH *)(p_self);
if (!bpo->pair_callback) {
return nullptr;
}
return bpo->pair_callback(p_object_A, p_subindex_A, p_object_B, p_subindex_B, p_pair_data, bpo->pair_userdata);
}
void BroadPhase2DBVH::set_pair_callback(PairCallback p_pair_callback, void *p_userdata) {
@ -109,6 +125,7 @@ BroadPhase2DBVH::BroadPhase2DBVH() {
bvh.params_set_thread_safe(GLOBAL_GET("rendering/threads/thread_safe_bvh"));
bvh.set_pair_callback(_pair_callback, this);
bvh.set_unpair_callback(_unpair_callback, this);
bvh.set_check_pair_callback(_check_pair_callback, this);
pair_callback = nullptr;
pair_userdata = nullptr;
unpair_userdata = nullptr;

6
servers/physics_2d/broad_phase_2d_bvh.h
View File

@ -39,8 +39,9 @@
class BroadPhase2DBVH : public BroadPhase2DSW {
BVH_Manager<CollisionObject2DSW, true, 128, Rect2, Vector2> bvh;
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 *);
static void *_pair_callback(void *p_self, uint32_t p_id_A, CollisionObject2DSW *p_object_A, int p_subindex_A, uint32_t p_id_B, CollisionObject2DSW *p_object_B, int p_subindex_B);
static void _unpair_callback(void *p_self, uint32_t p_id_A, CollisionObject2DSW *p_object_A, int p_subindex_A, uint32_t p_id_B, CollisionObject2DSW *p_object_B, int p_subindex_B, void *p_pair_data);
static void *_check_pair_callback(void *p_self, uint32_t p_id_A, CollisionObject2DSW *p_object_A, int p_subindex_A, uint32_t p_id_B, CollisionObject2DSW *p_object_B, int p_subindex_B, void *p_pair_data);
PairCallback pair_callback;
void *pair_userdata;
@ -51,6 +52,7 @@ public:
// 0 is an invalid ID
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 recheck_pairs(ID p_id);
virtual void set_static(ID p_id, bool p_static);
virtual void remove(ID p_id);

10
servers/physics_2d/broad_phase_2d_hash_grid.cpp
View File

@ -88,7 +88,7 @@ void BroadPhase2DHashGrid::_check_motion(Element *p_elem) {
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()->ud = pair_callback(p_elem->owner, p_elem->subindex, E->key()->owner, E->key()->subindex, nullptr, pair_userdata);
}
E->get()->colliding = true;
} else { // No collision
@ -336,6 +336,14 @@ void BroadPhase2DHashGrid::move(ID p_id, const Rect2 &p_aabb) {
_check_motion(&e);
}
void BroadPhase2DHashGrid::recheck_pairs(ID p_id) {
Map<ID, Element>::Element *E = element_map.find(p_id);
ERR_FAIL_COND(!E);
Element &e = E->get();
move(p_id, e.aabb);
}
void BroadPhase2DHashGrid::set_static(ID p_id, bool p_static) {
Map<ID, Element>::Element *E = element_map.find(p_id);
ERR_FAIL_COND(!E);

2
servers/physics_2d/broad_phase_2d_hash_grid.h
View File

@ -165,11 +165,13 @@ class BroadPhase2DHashGrid : public BroadPhase2DSW {
void _pair_attempt(Element *p_elem, Element *p_with);
void _unpair_attempt(Element *p_elem, Element *p_with);
void _move_internal(Element *p_elem, const Rect2 &p_aabb);
void _check_motion(Element *p_elem);
public:
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 recheck_pairs(ID p_id);
virtual void set_static(ID p_id, bool p_static);
virtual void remove(ID p_id);

5
servers/physics_2d/broad_phase_2d_sw.h
View File

@ -44,12 +44,13 @@ public:
typedef uint32_t ID;
typedef void *(*PairCallback)(CollisionObject2DSW *A, int p_subindex_A, CollisionObject2DSW *B, int p_subindex_B, void *p_userdata);
typedef void (*UnpairCallback)(CollisionObject2DSW *A, int p_subindex_A, CollisionObject2DSW *B, int p_subindex_B, void *p_data, void *p_userdata);
typedef void *(*PairCallback)(CollisionObject2DSW *p_object_A, int p_subindex_A, CollisionObject2DSW *p_object_B, int p_subindex_B, void *p_pair_data, void *p_user_data);
typedef void (*UnpairCallback)(CollisionObject2DSW *p_object_A, int p_subindex_A, CollisionObject2DSW *p_object_B, int p_subindex_B, void *p_pair_data, void *p_user_data);
// 0 is an invalid ID
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 recheck_pairs(ID p_id) = 0;
virtual void set_static(ID p_id, bool p_static) = 0;
virtual void remove(ID p_id) = 0;

17
servers/physics_2d/collision_object_2d_sw.cpp
View File

@ -187,6 +187,23 @@ void CollisionObject2DSW::_update_shapes() {
}
}
void CollisionObject2DSW::_recheck_shapes() {
if (!space) {
return;
}
for (int i = 0; i < shapes.size(); i++) {
Shape &s = shapes.write[i];
if (s.disabled) {
continue;
}
if (s.bpid != 0) {
space->get_broadphase()->recheck_pairs(s.bpid);
}
}
}
void CollisionObject2DSW::_update_shapes_with_motion(const Vector2 &p_motion) {
if (!space) {
return;

7
servers/physics_2d/collision_object_2d_sw.h
View File

@ -80,6 +80,7 @@ private:
SelfList<CollisionObject2DSW> pending_shape_update_list;
void _update_shapes();
void _recheck_shapes();
protected:
void _update_shapes_with_motion(const Vector2 &p_motion);
@ -166,13 +167,15 @@ public:
void set_collision_mask(uint32_t p_mask) {
collision_mask = p_mask;
_shape_changed();
_recheck_shapes();
_shapes_changed();
}
_FORCE_INLINE_ uint32_t get_collision_mask() const { return collision_mask; }
void set_collision_layer(uint32_t p_layer) {
collision_layer = p_layer;
_shape_changed();
_recheck_shapes();
_shapes_changed();
}
_FORCE_INLINE_ uint32_t get_collision_layer() const { return collision_layer; }

46
servers/physics_2d/space_2d_sw.cpp
View File

@ -1196,15 +1196,29 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
return collided;
}
void *Space2DSW::_broadphase_pair(CollisionObject2DSW *A, int p_subindex_A, CollisionObject2DSW *B, int p_subindex_B, void *p_self) {
if (!A->test_collision_mask(B)) {
void *Space2DSW::_broadphase_pair(CollisionObject2DSW *p_object_A, int p_subindex_A, CollisionObject2DSW *p_object_B, int p_subindex_B, void *p_pair_data, void *p_self) {
bool valid_collision_pair = p_object_A->test_collision_mask(p_object_B);
if (p_pair_data) {
// Checking an existing pair.
if (valid_collision_pair) {
// Nothing to do, pair is still valid.
return p_pair_data;
} else {
// Logical collision not valid anymore, unpair.
_broadphase_unpair(p_object_A, p_subindex_A, p_object_B, p_subindex_B, p_pair_data, p_self);
return nullptr;
}
}
if (!valid_collision_pair) {
return nullptr;
}
CollisionObject2DSW::Type type_A = A->get_type();
CollisionObject2DSW::Type type_B = B->get_type();
CollisionObject2DSW::Type type_A = p_object_A->get_type();
CollisionObject2DSW::Type type_B = p_object_B->get_type();
if (type_A > type_B) {
SWAP(A, B);
SWAP(p_object_A, p_object_B);
SWAP(p_subindex_A, p_subindex_B);
SWAP(type_A, type_B);
}
@ -1213,33 +1227,35 @@ void *Space2DSW::_broadphase_pair(CollisionObject2DSW *A, int p_subindex_A, Coll
self->collision_pairs++;
if (type_A == CollisionObject2DSW::TYPE_AREA) {
Area2DSW *area = static_cast<Area2DSW *>(A);
Area2DSW *area_a = static_cast<Area2DSW *>(p_object_A);
if (type_B == CollisionObject2DSW::TYPE_AREA) {
Area2DSW *area_b = static_cast<Area2DSW *>(B);
Area2Pair2DSW *area2_pair = memnew(Area2Pair2DSW(area_b, p_subindex_B, area, p_subindex_A));
Area2DSW *area_b = static_cast<Area2DSW *>(p_object_B);
Area2Pair2DSW *area2_pair = memnew(Area2Pair2DSW(area_b, p_subindex_B, area_a, p_subindex_A));
return area2_pair;
} else {
Body2DSW *body = static_cast<Body2DSW *>(B);
AreaPair2DSW *area_pair = memnew(AreaPair2DSW(body, p_subindex_B, area, p_subindex_A));
Body2DSW *body_b = static_cast<Body2DSW *>(p_object_B);
AreaPair2DSW *area_pair = memnew(AreaPair2DSW(body_b, p_subindex_B, area_a, p_subindex_A));
return area_pair;
}
} else {
BodyPair2DSW *b = memnew(BodyPair2DSW((Body2DSW *)A, p_subindex_A, (Body2DSW *)B, p_subindex_B));
return b;
Body2DSW *body_a = static_cast<Body2DSW *>(p_object_A);
Body2DSW *body_b = static_cast<Body2DSW *>(p_object_B);
BodyPair2DSW *body_pair = memnew(BodyPair2DSW(body_a, p_subindex_A, body_b, p_subindex_B));
return body_pair;
}
return nullptr;
}
void Space2DSW::_broadphase_unpair(CollisionObject2DSW *A, int p_subindex_A, CollisionObject2DSW *B, int p_subindex_B, void *p_data, void *p_self) {
if (!p_data) {
void Space2DSW::_broadphase_unpair(CollisionObject2DSW *p_object_A, int p_subindex_A, CollisionObject2DSW *p_object_B, int p_subindex_B, void *p_pair_data, void *p_self) {
if (!p_pair_data) {
return;
}
Space2DSW *self = (Space2DSW *)p_self;
self->collision_pairs--;
Constraint2DSW *c = (Constraint2DSW *)p_data;
Constraint2DSW *c = (Constraint2DSW *)p_pair_data;
memdelete(c);
}

4
servers/physics_2d/space_2d_sw.h
View File

@ -91,8 +91,8 @@ private:
SelfList<Area2DSW>::List monitor_query_list;
SelfList<Area2DSW>::List area_moved_list;
static void *_broadphase_pair(CollisionObject2DSW *A, int p_subindex_A, CollisionObject2DSW *B, int p_subindex_B, void *p_self);
static void _broadphase_unpair(CollisionObject2DSW *A, int p_subindex_A, CollisionObject2DSW *B, int p_subindex_B, void *p_data, void *p_self);
static void *_broadphase_pair(CollisionObject2DSW *p_object_A, int p_subindex_A, CollisionObject2DSW *p_object_B, int p_subindex_B, void *p_pair_data, void *p_self);
static void _broadphase_unpair(CollisionObject2DSW *p_object_A, int p_subindex_A, CollisionObject2DSW *p_object_B, int p_subindex_B, void *p_pair_data, void *p_self);
Set<CollisionObject2DSW *> objects;