diff --git a/core/math/bvh.h b/core/math/bvh.h index c64dbc1a641e..0f1e0bf355ef 100644 --- a/core/math/bvh.h +++ b/core/math/bvh.h @@ -59,6 +59,7 @@ class BVH_Manager { // 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 @@ class BVH_Manager { 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 @@ class BVH_Manager { 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 @@ class BVH_Manager { } } + 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 @@ class BVH_Manager { } } + 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 @@ class BVH_Manager { } } + // 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 @@ class BVH_Manager { 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; diff --git a/core/math/octree_definition.inc b/core/math/octree_definition.inc index 8946f76b74fd..3aafa6dce4e9 100644 --- a/core/math/octree_definition.inc +++ b/core/math/octree_definition.inc @@ -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 &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) { diff --git a/servers/physics/broad_phase_basic.cpp b/servers/physics/broad_phase_basic.cpp index 6632796fc434..d43a859aeed7 100644 --- a/servers/physics/broad_phase_basic.cpp +++ b/servers/physics/broad_phase_basic.cpp @@ -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::Element *E = element_map.find(p_id); ERR_FAIL_COND(!E); E->get()._static = p_static; } + void BroadPhaseBasic::remove(ID p_id) { Map::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); } diff --git a/servers/physics/broad_phase_basic.h b/servers/physics/broad_phase_basic.h index 085a48cfbee0..bd683e7454f5 100644 --- a/servers/physics/broad_phase_basic.h +++ b/servers/physics/broad_phase_basic.h @@ -82,6 +82,7 @@ class BroadPhaseBasic : public BroadPhaseSW { // 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); diff --git a/servers/physics/broad_phase_bvh.cpp b/servers/physics/broad_phase_bvh.cpp index 46677b0d37f7..0154a931c573 100644 --- a/servers/physics/broad_phase_bvh.cpp +++ b/servers/physics/broad_phase_bvh.cpp @@ -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; diff --git a/servers/physics/broad_phase_bvh.h b/servers/physics/broad_phase_bvh.h index 4e8890829d94..132b99dfbc89 100644 --- a/servers/physics/broad_phase_bvh.h +++ b/servers/physics/broad_phase_bvh.h @@ -37,8 +37,9 @@ class BroadPhaseBVH : public BroadPhaseSW { BVH_Manager 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 @@ class BroadPhaseBVH : public BroadPhaseSW { // 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); diff --git a/servers/physics/broad_phase_octree.cpp b/servers/physics/broad_phase_octree.cpp index 639ac73e8c61..af1e5857cbe6 100644 --- a/servers/physics/broad_phase_octree.cpp +++ b/servers/physics/broad_phase_octree.cpp @@ -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) { diff --git a/servers/physics/broad_phase_octree.h b/servers/physics/broad_phase_octree.h index e1529e11159b..c06a1cd4f100 100644 --- a/servers/physics/broad_phase_octree.h +++ b/servers/physics/broad_phase_octree.h @@ -49,6 +49,7 @@ class BroadPhaseOctree : public BroadPhaseSW { // 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); diff --git a/servers/physics/broad_phase_sw.h b/servers/physics/broad_phase_sw.h index 9e2606b2418c..bb4f0f3ac8cb 100644 --- a/servers/physics/broad_phase_sw.h +++ b/servers/physics/broad_phase_sw.h @@ -44,12 +44,13 @@ class BroadPhaseSW { 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; diff --git a/servers/physics/collision_object_sw.cpp b/servers/physics/collision_object_sw.cpp index f8cf5a9357f9..7779da9de51c 100644 --- a/servers/physics/collision_object_sw.cpp +++ b/servers/physics/collision_object_sw.cpp @@ -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; diff --git a/servers/physics/collision_object_sw.h b/servers/physics/collision_object_sw.h index 405e7897c9cc..3425f64b0cb2 100644 --- a/servers/physics/collision_object_sw.h +++ b/servers/physics/collision_object_sw.h @@ -79,6 +79,7 @@ class CollisionObjectSW : public ShapeOwnerSW { SelfList pending_shape_update_list; void _update_shapes(); + void _recheck_shapes(); protected: void _update_shapes_with_motion(const Vector3 &p_motion); @@ -155,13 +156,15 @@ class CollisionObjectSW : public ShapeOwnerSW { _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; } diff --git a/servers/physics/space_sw.cpp b/servers/physics/space_sw.cpp index ed33e6430402..607a92db1d32 100644 --- a/servers/physics/space_sw.cpp +++ b/servers/physics/space_sw.cpp @@ -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(A); + AreaSW *area_a = static_cast(p_object_A); if (type_B == CollisionObjectSW::TYPE_AREA) { - AreaSW *area_b = static_cast(B); - Area2PairSW *area2_pair = memnew(Area2PairSW(area_b, p_subindex_B, area, p_subindex_A)); + AreaSW *area_b = static_cast(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(B); - AreaPairSW *area_pair = memnew(AreaPairSW(body, p_subindex_B, area, p_subindex_A)); + BodySW *body_b = static_cast(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(p_object_A); + BodySW *body_b = static_cast(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); } diff --git a/servers/physics/space_sw.h b/servers/physics/space_sw.h index 6a0f7ed55234..fe959f4dc204 100644 --- a/servers/physics/space_sw.h +++ b/servers/physics/space_sw.h @@ -83,8 +83,8 @@ class SpaceSW : public RID_Data { SelfList::List monitor_query_list; SelfList::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 objects; diff --git a/servers/physics_2d/broad_phase_2d_basic.cpp b/servers/physics_2d/broad_phase_2d_basic.cpp index 6733c12319f6..ea9277d30349 100644 --- a/servers/physics_2d/broad_phase_2d_basic.cpp +++ b/servers/physics_2d/broad_phase_2d_basic.cpp @@ -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::Element *E = element_map.find(p_id); ERR_FAIL_COND(!E); E->get()._static = p_static; } + void BroadPhase2DBasic::remove(ID p_id) { Map::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); } diff --git a/servers/physics_2d/broad_phase_2d_basic.h b/servers/physics_2d/broad_phase_2d_basic.h index 6af7df963dd2..c951cc8037f5 100644 --- a/servers/physics_2d/broad_phase_2d_basic.h +++ b/servers/physics_2d/broad_phase_2d_basic.h @@ -81,6 +81,7 @@ class BroadPhase2DBasic : public BroadPhase2DSW { // 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); diff --git a/servers/physics_2d/broad_phase_2d_bvh.cpp b/servers/physics_2d/broad_phase_2d_bvh.cpp index af5948737476..6b2fe8a812ef 100644 --- a/servers/physics_2d/broad_phase_2d_bvh.cpp +++ b/servers/physics_2d/broad_phase_2d_bvh.cpp @@ -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; diff --git a/servers/physics_2d/broad_phase_2d_bvh.h b/servers/physics_2d/broad_phase_2d_bvh.h index 6c11d2561b6e..58f37850b2b8 100644 --- a/servers/physics_2d/broad_phase_2d_bvh.h +++ b/servers/physics_2d/broad_phase_2d_bvh.h @@ -39,8 +39,9 @@ class BroadPhase2DBVH : public BroadPhase2DSW { BVH_Manager 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 @@ class BroadPhase2DBVH : public BroadPhase2DSW { // 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); diff --git a/servers/physics_2d/broad_phase_2d_hash_grid.cpp b/servers/physics_2d/broad_phase_2d_hash_grid.cpp index 529d3883d9e1..94696a445e5b 100644 --- a/servers/physics_2d/broad_phase_2d_hash_grid.cpp +++ b/servers/physics_2d/broad_phase_2d_hash_grid.cpp @@ -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::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::Element *E = element_map.find(p_id); ERR_FAIL_COND(!E); diff --git a/servers/physics_2d/broad_phase_2d_hash_grid.h b/servers/physics_2d/broad_phase_2d_hash_grid.h index 0483e043a632..5958b0ef08d2 100644 --- a/servers/physics_2d/broad_phase_2d_hash_grid.h +++ b/servers/physics_2d/broad_phase_2d_hash_grid.h @@ -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); diff --git a/servers/physics_2d/broad_phase_2d_sw.h b/servers/physics_2d/broad_phase_2d_sw.h index 0f82f06b9c94..2b2fc6539bad 100644 --- a/servers/physics_2d/broad_phase_2d_sw.h +++ b/servers/physics_2d/broad_phase_2d_sw.h @@ -44,12 +44,13 @@ class BroadPhase2DSW { 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; diff --git a/servers/physics_2d/collision_object_2d_sw.cpp b/servers/physics_2d/collision_object_2d_sw.cpp index d49608c7b668..9cdd9692b8e8 100644 --- a/servers/physics_2d/collision_object_2d_sw.cpp +++ b/servers/physics_2d/collision_object_2d_sw.cpp @@ -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; diff --git a/servers/physics_2d/collision_object_2d_sw.h b/servers/physics_2d/collision_object_2d_sw.h index 585bc535bdee..e2532164e721 100644 --- a/servers/physics_2d/collision_object_2d_sw.h +++ b/servers/physics_2d/collision_object_2d_sw.h @@ -80,6 +80,7 @@ class CollisionObject2DSW : public ShapeOwner2DSW { SelfList pending_shape_update_list; void _update_shapes(); + void _recheck_shapes(); protected: void _update_shapes_with_motion(const Vector2 &p_motion); @@ -166,13 +167,15 @@ class CollisionObject2DSW : public ShapeOwner2DSW { 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; } diff --git a/servers/physics_2d/space_2d_sw.cpp b/servers/physics_2d/space_2d_sw.cpp index 8f20956c8582..9913547a23eb 100644 --- a/servers/physics_2d/space_2d_sw.cpp +++ b/servers/physics_2d/space_2d_sw.cpp @@ -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(A); + Area2DSW *area_a = static_cast(p_object_A); if (type_B == CollisionObject2DSW::TYPE_AREA) { - Area2DSW *area_b = static_cast(B); - Area2Pair2DSW *area2_pair = memnew(Area2Pair2DSW(area_b, p_subindex_B, area, p_subindex_A)); + Area2DSW *area_b = static_cast(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(B); - AreaPair2DSW *area_pair = memnew(AreaPair2DSW(body, p_subindex_B, area, p_subindex_A)); + Body2DSW *body_b = static_cast(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(p_object_A); + Body2DSW *body_b = static_cast(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); } diff --git a/servers/physics_2d/space_2d_sw.h b/servers/physics_2d/space_2d_sw.h index d8f845575097..1c348411ad22 100644 --- a/servers/physics_2d/space_2d_sw.h +++ b/servers/physics_2d/space_2d_sw.h @@ -91,8 +91,8 @@ class Space2DSW : public RID_Data { SelfList::List monitor_query_list; SelfList::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 objects;