From 3e5e8b6c9e9bb3ead3f127ff2d656a18c974a2a3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pedro=20J=2E=20Est=C3=A9banez?= Date: Tue, 30 May 2017 18:56:43 +0200 Subject: [PATCH] Fix multiple issues with 2D & 3D physics - Use `NOTIFICATION_ENTER`/`EXIT_WORLD` for `Area` (intead of `*_TREE`). - Now both bodies' and areas' constraints are cleaned up. - And now also that happens as soon as the space is set to null (i.e., when exiting the tree) instead of only at freeing time. - When clearing constraints, the loop goes on to the next if the current is already released, instead of breaking. - When one has been already released, no error is shown from now on, as it's something expected, since a pair (our kind of constraint of interest) can be freed by any of its involved collision objects and the other will try again. - Implement index shifting (or marking as -1) for shapes indices in collision pairs shapes are removed. - Standarize behavior of bodies and areas so that anything that invalidates a given pair gives the same result (collision mask, actual collision, etc); for instance, triggering area enter/exit signals. - Add missing member initializations. - Extend the new-space-equals-area/body-current-space test to every case. - Fix 3D ray-casts early accepting Areas (skipping the mask check). - Fix unpairing of large elements (2D's `BroadPhase2DHashGrid`). Some of these prevent random crashes caused by constraints with dangling pointers to involved objects. Fixes #8856. Fixes #7589. Fixes #6676. And maybe others. --- scene/3d/area.cpp | 5 +- servers/physics/area_pair_sw.cpp | 51 ++++++++++++++----- servers/physics/area_pair_sw.h | 4 ++ servers/physics/area_sw.cpp | 7 +++ servers/physics/area_sw.h | 4 ++ servers/physics/body_pair_sw.cpp | 21 ++++++++ servers/physics/body_pair_sw.h | 2 + servers/physics/body_sw.cpp | 9 ++++ servers/physics/body_sw.h | 4 ++ servers/physics/collision_object_sw.cpp | 1 + servers/physics/collision_object_sw.h | 1 + servers/physics/constraint_sw.h | 2 + servers/physics/physics_server_sw.cpp | 33 ++++++++---- servers/physics/space_sw.cpp | 6 +-- servers/physics_2d/area_2d_sw.cpp | 7 +++ servers/physics_2d/area_2d_sw.h | 4 ++ servers/physics_2d/area_pair_2d_sw.cpp | 38 +++++++++++++- servers/physics_2d/area_pair_2d_sw.h | 4 ++ servers/physics_2d/body_2d_sw.cpp | 7 +++ servers/physics_2d/body_2d_sw.h | 4 ++ servers/physics_2d/body_pair_2d_sw.cpp | 21 ++++++++ servers/physics_2d/body_pair_2d_sw.h | 2 + .../physics_2d/broad_phase_2d_hash_grid.cpp | 6 ++- servers/physics_2d/collision_object_2d_sw.cpp | 1 + servers/physics_2d/collision_object_2d_sw.h | 1 + servers/physics_2d/constraint_2d_sw.h | 2 + servers/physics_2d/physics_2d_server_sw.cpp | 37 ++++++++++---- 27 files changed, 243 insertions(+), 41 deletions(-) diff --git a/scene/3d/area.cpp b/scene/3d/area.cpp index 8792f2d027..00b9d0191b 100644 --- a/scene/3d/area.cpp +++ b/scene/3d/area.cpp @@ -275,13 +275,13 @@ void Area::_notification(int p_what) { switch (p_what) { - case NOTIFICATION_EXIT_TREE: { + case NOTIFICATION_EXIT_WORLD: { monitoring_stored = monitoring; set_enable_monitoring(false); _clear_monitoring(); } break; - case NOTIFICATION_ENTER_TREE: { + case NOTIFICATION_ENTER_WORLD: { if (monitoring_stored) { set_enable_monitoring(true); @@ -646,6 +646,7 @@ Area::Area() angular_damp = 1; priority = 0; monitoring = false; + monitorable = false; collision_mask = 1; layer_mask = 1; monitoring_stored = false; diff --git a/servers/physics/area_pair_sw.cpp b/servers/physics/area_pair_sw.cpp index 54cb63b240..9d57ad432e 100644 --- a/servers/physics/area_pair_sw.cpp +++ b/servers/physics/area_pair_sw.cpp @@ -32,12 +32,10 @@ bool AreaPairSW::setup(float p_step) { - if (!area->test_collision_mask(body)) { - colliding = false; - return false; - } - - bool result = CollisionSolverSW::solve_static(body->get_shape(body_shape), body->get_transform() * body->get_shape_transform(body_shape), area->get_shape(area_shape), area->get_transform() * area->get_shape_transform(area_shape), NULL, this); + bool result = + area_shape != -1 && body_shape != -1 && + area->test_collision_mask(body) && + CollisionSolverSW::solve_static(body->get_shape(body_shape), body->get_transform() * body->get_shape_transform(body_shape), area->get_shape(area_shape), area->get_transform() * area->get_shape_transform(area_shape), NULL, this); if (result != colliding) { @@ -65,6 +63,21 @@ bool AreaPairSW::setup(float p_step) { void AreaPairSW::solve(float p_step) { } +void AreaPairSW::shift_shape_indices(const CollisionObjectSW *p_object, int p_removed_index) { + + if (p_object == body) { + if (body_shape == p_removed_index) + body_shape = -1; + else if (body_shape > p_removed_index) + body_shape--; + } else if (p_object == area) { + if (area_shape == p_removed_index) + area_shape = -1; + else if (area_shape > p_removed_index) + area_shape--; + } +} + AreaPairSW::AreaPairSW(BodySW *p_body, int p_body_shape, AreaSW *p_area, int p_area_shape) { body = p_body; @@ -95,13 +108,10 @@ AreaPairSW::~AreaPairSW() { bool Area2PairSW::setup(float p_step) { - if (!area_a->test_collision_mask(area_b)) { - colliding = false; - return false; - } - - // bool result = area_a->test_collision_mask(area_b) && CollisionSolverSW::solve(area_a->get_shape(shape_a),area_a->get_transform() * area_a->get_shape_transform(shape_a),Vector2(),area_b->get_shape(shape_b),area_b->get_transform() * area_b->get_shape_transform(shape_b),Vector2(),NULL,this); - bool result = CollisionSolverSW::solve_static(area_a->get_shape(shape_a), area_a->get_transform() * area_a->get_shape_transform(shape_a), area_b->get_shape(shape_b), area_b->get_transform() * area_b->get_shape_transform(shape_b), NULL, this); + bool result = + shape_a != -1 && shape_b != -1 && + area_a->test_collision_mask(area_b) && + CollisionSolverSW::solve_static(area_a->get_shape(shape_a), area_a->get_transform() * area_a->get_shape_transform(shape_a), area_b->get_shape(shape_b), area_b->get_transform() * area_b->get_shape_transform(shape_b), NULL, this); if (result != colliding) { @@ -131,6 +141,21 @@ bool Area2PairSW::setup(float p_step) { void Area2PairSW::solve(float p_step) { } +void Area2PairSW::shift_shape_indices(const CollisionObjectSW *p_object, int p_removed_index) { + + if (p_object == area_a) { + if (shape_a == p_removed_index) + shape_a = -1; + else if (shape_a > p_removed_index) + shape_a--; + } else if (p_object == area_b) { + if (shape_b == p_removed_index) + shape_b = -1; + else if (shape_b > p_removed_index) + shape_b--; + } +} + Area2PairSW::Area2PairSW(AreaSW *p_area_a, int p_shape_a, AreaSW *p_area_b, int p_shape_b) { area_a = p_area_a; diff --git a/servers/physics/area_pair_sw.h b/servers/physics/area_pair_sw.h index 201f544a5c..e6614b7f53 100644 --- a/servers/physics/area_pair_sw.h +++ b/servers/physics/area_pair_sw.h @@ -46,6 +46,8 @@ public: bool setup(float p_step); void solve(float p_step); + virtual void shift_shape_indices(const CollisionObjectSW *p_object, int p_removed_index); + AreaPairSW(BodySW *p_body, int p_body_shape, AreaSW *p_area, int p_area_shape); ~AreaPairSW(); }; @@ -62,6 +64,8 @@ public: bool setup(float p_step); void solve(float p_step); + virtual void shift_shape_indices(const CollisionObjectSW *p_object, int p_removed_index); + Area2PairSW(AreaSW *p_area_a, int p_shape_a, AreaSW *p_area_b, int p_shape_b); ~Area2PairSW(); }; diff --git a/servers/physics/area_sw.cpp b/servers/physics/area_sw.cpp index af34955077..64505122c0 100644 --- a/servers/physics/area_sw.cpp +++ b/servers/physics/area_sw.cpp @@ -47,6 +47,13 @@ AreaSW::BodyKey::BodyKey(AreaSW *p_body, uint32_t p_body_shape, uint32_t p_area_ void AreaSW::_shapes_changed() { } +void AreaSW::_shape_index_removed(int p_index) { + + for (Set::Element *E = constraints.front(); E; E = E->next()) { + E->get()->shift_shape_indices(this, p_index); + } +} + void AreaSW::set_transform(const Transform &p_transform) { if (!moved_list.in_list() && get_space()) diff --git a/servers/physics/area_sw.h b/servers/physics/area_sw.h index acdc1a1776..9e7ebb484e 100644 --- a/servers/physics/area_sw.h +++ b/servers/physics/area_sw.h @@ -105,6 +105,9 @@ class AreaSW : public CollisionObjectSW { virtual void _shapes_changed(); void _queue_monitor_update(); +protected: + virtual void _shape_index_removed(int p_index); + public: //_FORCE_INLINE_ const Transform& get_inverse_transform() const { return inverse_transform; } //_FORCE_INLINE_ SpaceSW* get_owner() { return owner; } @@ -154,6 +157,7 @@ public: _FORCE_INLINE_ void add_constraint(ConstraintSW *p_constraint) { constraints.insert(p_constraint); } _FORCE_INLINE_ void remove_constraint(ConstraintSW *p_constraint) { constraints.erase(p_constraint); } _FORCE_INLINE_ const Set &get_constraints() const { return constraints; } + _FORCE_INLINE_ void clear_constraints() { constraints.clear(); } void set_monitorable(bool p_monitorable); _FORCE_INLINE_ bool is_monitorable() const { return monitorable; } diff --git a/servers/physics/body_pair_sw.cpp b/servers/physics/body_pair_sw.cpp index 11d300d90d..5cfff47dac 100644 --- a/servers/physics/body_pair_sw.cpp +++ b/servers/physics/body_pair_sw.cpp @@ -208,6 +208,12 @@ bool BodyPairSW::_test_ccd(float p_step, BodySW *p_A, int p_shape_A, const Trans bool BodyPairSW::setup(float p_step) { + //one or both shapes have been removed + if (shape_A == -1 || shape_B == -1) { + collided = false; + return false; + } + //cannot collide if (!A->test_collision_mask(B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self()) || (A->get_mode() <= PhysicsServer::BODY_MODE_KINEMATIC && B->get_mode() <= PhysicsServer::BODY_MODE_KINEMATIC && A->get_max_contacts_reported() == 0 && B->get_max_contacts_reported() == 0)) { collided = false; @@ -467,6 +473,21 @@ void BodyPairSW::solve(float p_step) { } } +void BodyPairSW::shift_shape_indices(const CollisionObjectSW *p_object, int p_removed_index) { + + if (p_object == A) { + if (shape_A == p_removed_index) + shape_A = -1; + else if (shape_A > p_removed_index) + shape_A--; + } else if (p_object == B) { + if (shape_B == p_removed_index) + shape_B = -1; + else if (shape_B > p_removed_index) + shape_B--; + } +} + BodyPairSW::BodyPairSW(BodySW *p_A, int p_shape_A, BodySW *p_B, int p_shape_B) : ConstraintSW(_arr, 2) { diff --git a/servers/physics/body_pair_sw.h b/servers/physics/body_pair_sw.h index ec0e8878eb..e7d8b315af 100644 --- a/servers/physics/body_pair_sw.h +++ b/servers/physics/body_pair_sw.h @@ -89,6 +89,8 @@ public: bool setup(float p_step); void solve(float p_step); + virtual void shift_shape_indices(const CollisionObjectSW *p_object, int p_removed_index); + BodyPairSW(BodySW *p_A, int p_shape_A, BodySW *p_B, int p_shape_B); ~BodyPairSW(); }; diff --git a/servers/physics/body_sw.cpp b/servers/physics/body_sw.cpp index c5a1834783..0943b14293 100644 --- a/servers/physics/body_sw.cpp +++ b/servers/physics/body_sw.cpp @@ -246,6 +246,13 @@ void BodySW::_shapes_changed() { _update_inertia(); } +void BodySW::_shape_index_removed(int p_index) { + + for (Map::Element *E = constraint_map.front(); E; E = E->next()) { + E->key()->shift_shape_indices(this, p_index); + } +} + void BodySW::set_state(PhysicsServer::BodyState p_state, const Variant &p_variant) { switch (p_state) { @@ -715,6 +722,8 @@ BodySW::BodySW() contact_count = 0; gravity_scale = 1.0; + linear_damp = -1; + angular_damp = -1; area_angular_damp = 0; area_linear_damp = 0; diff --git a/servers/physics/body_sw.h b/servers/physics/body_sw.h index 625a3681dc..efe0ab0d2a 100644 --- a/servers/physics/body_sw.h +++ b/servers/physics/body_sw.h @@ -138,6 +138,9 @@ class BodySW : public CollisionObjectSW { friend class PhysicsDirectBodyStateSW; // i give up, too many functions to expose +protected: + virtual void _shape_index_removed(int p_index); + public: void set_force_integration_callback(ObjectID p_id, const StringName &p_method, const Variant &p_udata = Variant()); @@ -186,6 +189,7 @@ public: _FORCE_INLINE_ void add_constraint(ConstraintSW *p_constraint, int p_pos) { constraint_map[p_constraint] = p_pos; } _FORCE_INLINE_ void remove_constraint(ConstraintSW *p_constraint) { constraint_map.erase(p_constraint); } const Map &get_constraint_map() const { return constraint_map; } + _FORCE_INLINE_ void clear_constraint_map() { constraint_map.clear(); } _FORCE_INLINE_ void set_omit_force_integration(bool p_omit_force_integration) { omit_force_integration = p_omit_force_integration; } _FORCE_INLINE_ bool get_omit_force_integration() const { return omit_force_integration; } diff --git a/servers/physics/collision_object_sw.cpp b/servers/physics/collision_object_sw.cpp index 0ee61fb6d4..fd2145c0fc 100644 --- a/servers/physics/collision_object_sw.cpp +++ b/servers/physics/collision_object_sw.cpp @@ -87,6 +87,7 @@ void CollisionObjectSW::remove_shape(int p_index) { space->get_broadphase()->remove(shapes[i].bpid); shapes[i].bpid = 0; } + _shape_index_removed(p_index); shapes[p_index].shape->remove_owner(this); shapes.remove(p_index); diff --git a/servers/physics/collision_object_sw.h b/servers/physics/collision_object_sw.h index 8b7d984a66..ff8a4e2159 100644 --- a/servers/physics/collision_object_sw.h +++ b/servers/physics/collision_object_sw.h @@ -97,6 +97,7 @@ protected: void _set_static(bool p_static); virtual void _shapes_changed() = 0; + virtual void _shape_index_removed(int p_index) = 0; void _set_space(SpaceSW *space); bool ray_pickable; diff --git a/servers/physics/constraint_sw.h b/servers/physics/constraint_sw.h index d010d5a105..74aeafbb24 100644 --- a/servers/physics/constraint_sw.h +++ b/servers/physics/constraint_sw.h @@ -73,6 +73,8 @@ public: virtual bool setup(float p_step) = 0; virtual void solve(float p_step) = 0; + virtual void shift_shape_indices(const CollisionObjectSW *p_object, int p_removed_index) {} + virtual ~ConstraintSW() {} }; diff --git a/servers/physics/physics_server_sw.cpp b/servers/physics/physics_server_sw.cpp index 200d719ffe..9eb2586117 100644 --- a/servers/physics/physics_server_sw.cpp +++ b/servers/physics/physics_server_sw.cpp @@ -222,13 +222,25 @@ void PhysicsServerSW::area_set_space(RID p_area, RID p_space) { AreaSW *area = area_owner.get(p_area); ERR_FAIL_COND(!area); + SpaceSW *space = NULL; if (p_space.is_valid()) { space = space_owner.get(p_space); ERR_FAIL_COND(!space); } + if (area->get_space() == space) + return; //pointless + area->set_space(space); + + for (Set::Element *E = area->get_constraints().front(); E; E = E->next()) { + RID self = E->get()->get_self(); + if (!self.is_valid()) + continue; + free(self); + } + area->clear_constraints(); }; RID PhysicsServerSW::area_get_space(RID p_area) const { @@ -463,6 +475,7 @@ void PhysicsServerSW::body_set_space(RID p_body, RID p_space) { BodySW *body = body_owner.get(p_body); ERR_FAIL_COND(!body); + SpaceSW *space = NULL; if (p_space.is_valid()) { @@ -471,9 +484,17 @@ void PhysicsServerSW::body_set_space(RID p_body, RID p_space) { } if (body->get_space() == space) - return; //pointles + return; //pointless body->set_space(space); + + for (Map::Element *E = body->get_constraint_map().front(); E; E = E->next()) { + RID self = E->key()->get_self(); + if (!self.is_valid()) + continue; + free(self); + } + body->clear_constraint_map(); }; RID PhysicsServerSW::body_get_space(RID p_body) const { @@ -1302,19 +1323,13 @@ void PhysicsServerSW::free(RID p_rid) { // if (body->get_direct_state_query()) // _clear_query(body->get_direct_state_query()); - body->set_space(NULL); + body_set_space(p_rid, RID()); while (body->get_shape_count()) { body->remove_shape(0); } - while (body->get_constraint_map().size()) { - RID self = body->get_constraint_map().front()->key()->get_self(); - ERR_FAIL_COND(!self.is_valid()); - free(self); - } - body_owner.free(p_rid); memdelete(body); @@ -1325,7 +1340,7 @@ void PhysicsServerSW::free(RID p_rid) { // if (area->get_monitor_query()) // _clear_query(area->get_monitor_query()); - area->set_space(NULL); + area_set_space(p_rid, RID()); while (area->get_shape_count()) { diff --git a/servers/physics/space_sw.cpp b/servers/physics/space_sw.cpp index 685710f776..846333d133 100644 --- a/servers/physics/space_sw.cpp +++ b/servers/physics/space_sw.cpp @@ -34,12 +34,12 @@ _FORCE_INLINE_ static bool _match_object_type_query(CollisionObjectSW *p_object, uint32_t p_layer_mask, uint32_t p_type_mask) { - if (p_object->get_type() == CollisionObjectSW::TYPE_AREA) - return p_type_mask & PhysicsDirectSpaceState::TYPE_MASK_AREA; - if ((p_object->get_layer_mask() & p_layer_mask) == 0) return false; + if (p_object->get_type() == CollisionObjectSW::TYPE_AREA) + return p_type_mask & PhysicsDirectSpaceState::TYPE_MASK_AREA; + BodySW *body = static_cast(p_object); return (1 << body->get_mode()) & p_type_mask; diff --git a/servers/physics_2d/area_2d_sw.cpp b/servers/physics_2d/area_2d_sw.cpp index 7591dc06b0..e9d682ce8b 100644 --- a/servers/physics_2d/area_2d_sw.cpp +++ b/servers/physics_2d/area_2d_sw.cpp @@ -47,6 +47,13 @@ Area2DSW::BodyKey::BodyKey(Area2DSW *p_body, uint32_t p_body_shape, uint32_t p_a void Area2DSW::_shapes_changed() { } +void Area2DSW::_shape_index_removed(int p_index) { + + for (Set::Element *E = constraints.front(); E; E = E->next()) { + E->get()->shift_shape_indices(this, p_index); + } +} + void Area2DSW::set_transform(const Matrix32 &p_transform) { if (!moved_list.in_list() && get_space()) diff --git a/servers/physics_2d/area_2d_sw.h b/servers/physics_2d/area_2d_sw.h index e45c7421b9..59f4f1eb4a 100644 --- a/servers/physics_2d/area_2d_sw.h +++ b/servers/physics_2d/area_2d_sw.h @@ -104,6 +104,9 @@ class Area2DSW : public CollisionObject2DSW { virtual void _shapes_changed(); void _queue_monitor_update(); +protected: + virtual void _shape_index_removed(int p_index); + public: //_FORCE_INLINE_ const Matrix32& get_inverse_transform() const { return inverse_transform; } //_FORCE_INLINE_ SpaceSW* get_owner() { return owner; } @@ -153,6 +156,7 @@ public: _FORCE_INLINE_ void add_constraint(Constraint2DSW *p_constraint) { constraints.insert(p_constraint); } _FORCE_INLINE_ void remove_constraint(Constraint2DSW *p_constraint) { constraints.erase(p_constraint); } _FORCE_INLINE_ const Set &get_constraints() const { return constraints; } + _FORCE_INLINE_ void clear_constraints() { constraints.clear(); } void set_monitorable(bool p_monitorable); _FORCE_INLINE_ bool is_monitorable() const { return monitorable; } diff --git a/servers/physics_2d/area_pair_2d_sw.cpp b/servers/physics_2d/area_pair_2d_sw.cpp index 7447da22c1..87bb169007 100644 --- a/servers/physics_2d/area_pair_2d_sw.cpp +++ b/servers/physics_2d/area_pair_2d_sw.cpp @@ -32,7 +32,9 @@ bool AreaPair2DSW::setup(float p_step) { - bool result = area->test_collision_mask(body) && CollisionSolver2DSW::solve(body->get_shape(body_shape), body->get_transform() * body->get_shape_transform(body_shape), Vector2(), area->get_shape(area_shape), area->get_transform() * area->get_shape_transform(area_shape), Vector2(), NULL, this); + bool result = + area_shape != -1 && body_shape != -1 && + area->test_collision_mask(body) && CollisionSolver2DSW::solve(body->get_shape(body_shape), body->get_transform() * body->get_shape_transform(body_shape), Vector2(), area->get_shape(area_shape), area->get_transform() * area->get_shape_transform(area_shape), Vector2(), NULL, this); if (result != colliding) { @@ -60,6 +62,21 @@ bool AreaPair2DSW::setup(float p_step) { void AreaPair2DSW::solve(float p_step) { } +void AreaPair2DSW::shift_shape_indices(const CollisionObject2DSW *p_object, int p_removed_index) { + + if (p_object == body) { + if (body_shape == p_removed_index) + body_shape = -1; + else if (body_shape > p_removed_index) + body_shape--; + } else if (p_object == area) { + if (area_shape == p_removed_index) + area_shape = -1; + else if (area_shape > p_removed_index) + area_shape--; + } +} + AreaPair2DSW::AreaPair2DSW(Body2DSW *p_body, int p_body_shape, Area2DSW *p_area, int p_area_shape) { body = p_body; @@ -90,7 +107,9 @@ AreaPair2DSW::~AreaPair2DSW() { bool Area2Pair2DSW::setup(float p_step) { - bool result = area_a->test_collision_mask(area_b) && CollisionSolver2DSW::solve(area_a->get_shape(shape_a), area_a->get_transform() * area_a->get_shape_transform(shape_a), Vector2(), area_b->get_shape(shape_b), area_b->get_transform() * area_b->get_shape_transform(shape_b), Vector2(), NULL, this); + bool result = + shape_a != -1 && shape_b != -1 && + area_a->test_collision_mask(area_b) && CollisionSolver2DSW::solve(area_a->get_shape(shape_a), area_a->get_transform() * area_a->get_shape_transform(shape_a), Vector2(), area_b->get_shape(shape_b), area_b->get_transform() * area_b->get_shape_transform(shape_b), Vector2(), NULL, this); if (result != colliding) { @@ -120,6 +139,21 @@ bool Area2Pair2DSW::setup(float p_step) { void Area2Pair2DSW::solve(float p_step) { } +void Area2Pair2DSW::shift_shape_indices(const CollisionObject2DSW *p_object, int p_removed_index) { + + if (p_object == area_a) { + if (shape_a == p_removed_index) + shape_a = -1; + else if (shape_a > p_removed_index) + shape_a--; + } else if (p_object == area_b) { + if (shape_b == p_removed_index) + shape_b = -1; + else if (shape_b > p_removed_index) + shape_b--; + } +} + Area2Pair2DSW::Area2Pair2DSW(Area2DSW *p_area_a, int p_shape_a, Area2DSW *p_area_b, int p_shape_b) { area_a = p_area_a; diff --git a/servers/physics_2d/area_pair_2d_sw.h b/servers/physics_2d/area_pair_2d_sw.h index efb6a735dd..548a80a3ad 100644 --- a/servers/physics_2d/area_pair_2d_sw.h +++ b/servers/physics_2d/area_pair_2d_sw.h @@ -46,6 +46,8 @@ public: bool setup(float p_step); void solve(float p_step); + virtual void shift_shape_indices(const CollisionObject2DSW *p_object, int p_removed_index); + AreaPair2DSW(Body2DSW *p_body, int p_body_shape, Area2DSW *p_area, int p_area_shape); ~AreaPair2DSW(); }; @@ -62,6 +64,8 @@ public: bool setup(float p_step); void solve(float p_step); + virtual void shift_shape_indices(const CollisionObject2DSW *p_object, int p_removed_index); + Area2Pair2DSW(Area2DSW *p_area_a, int p_shape_a, Area2DSW *p_area_b, int p_shape_b); ~Area2Pair2DSW(); }; diff --git a/servers/physics_2d/body_2d_sw.cpp b/servers/physics_2d/body_2d_sw.cpp index dea6f042bf..caa609e5ad 100644 --- a/servers/physics_2d/body_2d_sw.cpp +++ b/servers/physics_2d/body_2d_sw.cpp @@ -255,6 +255,13 @@ void Body2DSW::_shapes_changed() { wakeup_neighbours(); } +void Body2DSW::_shape_index_removed(int p_index) { + + for (Map::Element *E = constraint_map.front(); E; E = E->next()) { + E->key()->shift_shape_indices(this, p_index); + } +} + void Body2DSW::set_state(Physics2DServer::BodyState p_state, const Variant &p_variant) { switch (p_state) { diff --git a/servers/physics_2d/body_2d_sw.h b/servers/physics_2d/body_2d_sw.h index d1ce02a74a..d101650841 100644 --- a/servers/physics_2d/body_2d_sw.h +++ b/servers/physics_2d/body_2d_sw.h @@ -136,6 +136,9 @@ class Body2DSW : public CollisionObject2DSW { friend class Physics2DDirectBodyStateSW; // i give up, too many functions to expose +protected: + virtual void _shape_index_removed(int p_index); + public: void set_force_integration_callback(ObjectID p_id, const StringName &p_method, const Variant &p_udata = Variant()); @@ -185,6 +188,7 @@ public: _FORCE_INLINE_ void add_constraint(Constraint2DSW *p_constraint, int p_pos) { constraint_map[p_constraint] = p_pos; } _FORCE_INLINE_ void remove_constraint(Constraint2DSW *p_constraint) { constraint_map.erase(p_constraint); } const Map &get_constraint_map() const { return constraint_map; } + _FORCE_INLINE_ void clear_constraint_map() { constraint_map.clear(); } _FORCE_INLINE_ void set_omit_force_integration(bool p_omit_force_integration) { omit_force_integration = p_omit_force_integration; } _FORCE_INLINE_ bool get_omit_force_integration() const { return omit_force_integration; } diff --git a/servers/physics_2d/body_pair_2d_sw.cpp b/servers/physics_2d/body_pair_2d_sw.cpp index a6cce8bcde..408a17f39b 100644 --- a/servers/physics_2d/body_pair_2d_sw.cpp +++ b/servers/physics_2d/body_pair_2d_sw.cpp @@ -219,6 +219,12 @@ bool BodyPair2DSW::_test_ccd(float p_step, Body2DSW *p_A, int p_shape_A, const M bool BodyPair2DSW::setup(float p_step) { + //one or both shapes have been removed + if (shape_A == -1 || shape_B == -1) { + collided = false; + return false; + } + //cannot collide if (!A->test_collision_mask(B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self()) || (A->get_mode() <= Physics2DServer::BODY_MODE_KINEMATIC && B->get_mode() <= Physics2DServer::BODY_MODE_KINEMATIC && A->get_max_contacts_reported() == 0 && B->get_max_contacts_reported() == 0)) { collided = false; @@ -495,6 +501,21 @@ void BodyPair2DSW::solve(float p_step) { } } +void BodyPair2DSW::shift_shape_indices(const CollisionObject2DSW *p_object, int p_removed_index) { + + if (p_object == A) { + if (shape_A == p_removed_index) + shape_A = -1; + else if (shape_A > p_removed_index) + shape_A--; + } else if (p_object == B) { + if (shape_B == p_removed_index) + shape_B = -1; + else if (shape_B > p_removed_index) + shape_B--; + } +} + BodyPair2DSW::BodyPair2DSW(Body2DSW *p_A, int p_shape_A, Body2DSW *p_B, int p_shape_B) : Constraint2DSW(_arr, 2) { diff --git a/servers/physics_2d/body_pair_2d_sw.h b/servers/physics_2d/body_pair_2d_sw.h index 5773902bc1..10d971465d 100644 --- a/servers/physics_2d/body_pair_2d_sw.h +++ b/servers/physics_2d/body_pair_2d_sw.h @@ -88,6 +88,8 @@ public: bool setup(float p_step); void solve(float p_step); + virtual void shift_shape_indices(const CollisionObject2DSW *p_object, int p_removed_index); + BodyPair2DSW(Body2DSW *p_A, int p_shape_A, Body2DSW *p_B, int p_shape_B); ~BodyPair2DSW(); }; diff --git a/servers/physics_2d/broad_phase_2d_hash_grid.cpp b/servers/physics_2d/broad_phase_2d_hash_grid.cpp index 332dbb11e6..9a9e0aeeb1 100644 --- a/servers/physics_2d/broad_phase_2d_hash_grid.cpp +++ b/servers/physics_2d/broad_phase_2d_hash_grid.cpp @@ -203,9 +203,11 @@ void BroadPhase2DHashGrid::_exit_grid(Element *p_elem, const Rect2 &p_rect, bool 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 - for (Map::Element *E = p_elem->paired.front(); E; E = E->next()) { - + Map::Element *E = p_elem->paired.front(); + while (E) { + Map::Element *next = E->next(); _unpair_attempt(p_elem, E->key()); + E = next; } if (large_elements[p_elem].dec() == 0) { diff --git a/servers/physics_2d/collision_object_2d_sw.cpp b/servers/physics_2d/collision_object_2d_sw.cpp index 91cdc5821e..547755f1a7 100644 --- a/servers/physics_2d/collision_object_2d_sw.cpp +++ b/servers/physics_2d/collision_object_2d_sw.cpp @@ -95,6 +95,7 @@ void CollisionObject2DSW::remove_shape(int p_index) { space->get_broadphase()->remove(shapes[i].bpid); shapes[i].bpid = 0; } + _shape_index_removed(p_index); shapes[p_index].shape->remove_owner(this); shapes.remove(p_index); diff --git a/servers/physics_2d/collision_object_2d_sw.h b/servers/physics_2d/collision_object_2d_sw.h index 13f69fb6f9..8da62d959c 100644 --- a/servers/physics_2d/collision_object_2d_sw.h +++ b/servers/physics_2d/collision_object_2d_sw.h @@ -86,6 +86,7 @@ protected: void _set_static(bool p_static); virtual void _shapes_changed() = 0; + virtual void _shape_index_removed(int p_index) = 0; void _set_space(Space2DSW *space); CollisionObject2DSW(Type p_type); diff --git a/servers/physics_2d/constraint_2d_sw.h b/servers/physics_2d/constraint_2d_sw.h index 85ab4d81d7..b04561ff0f 100644 --- a/servers/physics_2d/constraint_2d_sw.h +++ b/servers/physics_2d/constraint_2d_sw.h @@ -68,6 +68,8 @@ public: virtual bool setup(float p_step) = 0; virtual void solve(float p_step) = 0; + virtual void shift_shape_indices(const CollisionObject2DSW *p_object, int p_removed_index) {} + virtual ~Constraint2DSW() {} }; diff --git a/servers/physics_2d/physics_2d_server_sw.cpp b/servers/physics_2d/physics_2d_server_sw.cpp index 353dd0046f..27fa11268c 100644 --- a/servers/physics_2d/physics_2d_server_sw.cpp +++ b/servers/physics_2d/physics_2d_server_sw.cpp @@ -286,13 +286,25 @@ void Physics2DServerSW::area_set_space(RID p_area, RID p_space) { Area2DSW *area = area_owner.get(p_area); ERR_FAIL_COND(!area); + Space2DSW *space = NULL; if (p_space.is_valid()) { space = space_owner.get(p_space); ERR_FAIL_COND(!space); } + if (area->get_space() == space) + return; //pointless + area->set_space(space); + + for (Set::Element *E = area->get_constraints().front(); E; E = E->next()) { + RID self = E->get()->get_self(); + if (!self.is_valid()) + continue; + free(self); + } + area->clear_constraints(); }; RID Physics2DServerSW::area_get_space(RID p_area) const { @@ -518,13 +530,25 @@ void Physics2DServerSW::body_set_space(RID p_body, RID p_space) { Body2DSW *body = body_owner.get(p_body); ERR_FAIL_COND(!body); + Space2DSW *space = NULL; if (p_space.is_valid()) { space = space_owner.get(p_space); ERR_FAIL_COND(!space); } + if (body->get_space() == space) + return; //pointless + body->set_space(space); + + for (Map::Element *E = body->get_constraint_map().front(); E; E = E->next()) { + RID self = E->key()->get_self(); + if (!self.is_valid()) + continue; + free(self); + } + body->clear_constraint_map(); }; RID Physics2DServerSW::body_get_space(RID p_body) const { @@ -1103,22 +1127,15 @@ void Physics2DServerSW::free(RID p_rid) { // if (body->get_direct_state_query()) // _clear_query(body->get_direct_state_query()); - body->set_space(NULL); + body_set_space(p_rid, RID()); while (body->get_shape_count()) { body->remove_shape(0); } - while (body->get_constraint_map().size()) { - RID self = body->get_constraint_map().front()->key()->get_self(); - ERR_FAIL_COND(!self.is_valid()); - free(self); - } - body_owner.free(p_rid); memdelete(body); - } else if (area_owner.owns(p_rid)) { Area2DSW *area = area_owner.get(p_rid); @@ -1126,7 +1143,7 @@ void Physics2DServerSW::free(RID p_rid) { // if (area->get_monitor_query()) // _clear_query(area->get_monitor_query()); - area->set_space(NULL); + area_set_space(p_rid, RID()); while (area->get_shape_count()) { @@ -1286,7 +1303,7 @@ Physics2DServerSW::Physics2DServerSW() { singletonsw = this; BroadPhase2DSW::create_func = BroadPhase2DHashGrid::_create; - // BroadPhase2DSW::create_func=BroadPhase2DBasic::_create; + //BroadPhase2DSW::create_func = BroadPhase2DBasic::_create; active = true; island_count = 0;