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

@@ -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;

servers/physics/area_pair_sw.cpp

@@ -32,12 +32,10 @@
 
 bool AreaPairSW::setup(float p_step) {
 
-	if (!area->test_collision_mask(body)) {
+	bool result =
-		colliding = false;
+			area_shape != -1 && body_shape != -1 &&
-		return false;
+			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);
-
-	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);
 
 	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)) {
+	bool result =
-		colliding = false;
+			shape_a != -1 && shape_b != -1 &&
-		return false;
+			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);
-
-	//	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);
 
 	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;

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();
 };

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<ConstraintSW *>::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())

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<ConstraintSW *> &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; }

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) {
 

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();
 };

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<ConstraintSW *, int>::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;
 

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<ConstraintSW *, int> &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; }

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);
 

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;

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() {}
 };
 

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<ConstraintSW *>::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<ConstraintSW *, int>::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()) {
 

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<BodySW *>(p_object);
 
 	return (1 << body->get_mode()) & p_type_mask;

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<Constraint2DSW *>::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())

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<Constraint2DSW *> &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; }

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;

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();
 };

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<Constraint2DSW *, int>::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) {

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<Constraint2DSW *, int> &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; }

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) {
 

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();
 };

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 *, PairData *>::Element *E = p_elem->paired.front(); E; E = E->next()) {
+		Map<Element *, PairData *>::Element *E = p_elem->paired.front();
-
+		while (E) {
+			Map<Element *, PairData *>::Element *next = E->next();
 			_unpair_attempt(p_elem, E->key());
+			E = next;
 		}
 
 		if (large_elements[p_elem].dec() == 0) {

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);
 

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);

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() {}
 };
 

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<Constraint2DSW *>::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<Constraint2DSW *, int>::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;