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.

26 files changed, 240 insertions, 39 deletions

diff --git a/servers/physics/area_pair_sw.cpp b/servers/physics/area_pair_sw.cpp
index 54cb63b24..9d57ad432 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 201f544a5..e6614b7f5 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 af3495507..64505122c 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<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())
diff --git a/servers/physics/area_sw.h b/servers/physics/area_sw.h
index acdc1a177..9e7ebb484 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<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; }
diff --git a/servers/physics/body_pair_sw.cpp b/servers/physics/body_pair_sw.cpp
index 11d300d90..5cfff47da 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 ec0e8878e..e7d8b315a 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 c5a183478..0943b1429 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<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;
 
diff --git a/servers/physics/body_sw.h b/servers/physics/body_sw.h
index 625a3681d..efe0ab0d2 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<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; }
diff --git a/servers/physics/collision_object_sw.cpp b/servers/physics/collision_object_sw.cpp
index 0ee61fb6d..fd2145c0f 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 8b7d984a6..ff8a4e215 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 d010d5a10..74aeafbb2 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 200d719ff..9eb258611 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<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()) {
 
diff --git a/servers/physics/space_sw.cpp b/servers/physics/space_sw.cpp
index 685710f77..846333d13 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<BodySW *>(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 7591dc06b..e9d682ce8 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<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())
diff --git a/servers/physics_2d/area_2d_sw.h b/servers/physics_2d/area_2d_sw.h
index e45c7421b..59f4f1eb4 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<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; }
diff --git a/servers/physics_2d/area_pair_2d_sw.cpp b/servers/physics_2d/area_pair_2d_sw.cpp
index 7447da22c..87bb16900 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 efb6a735d..548a80a3a 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 dea6f042b..caa609e5a 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<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) {
diff --git a/servers/physics_2d/body_2d_sw.h b/servers/physics_2d/body_2d_sw.h
index d1ce02a74..d10165084 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<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; }
diff --git a/servers/physics_2d/body_pair_2d_sw.cpp b/servers/physics_2d/body_pair_2d_sw.cpp
index a6cce8bcd..408a17f39 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 5773902bc..10d971465 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 332dbb11e..9a9e0aeeb 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 *, 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) {
diff --git a/servers/physics_2d/collision_object_2d_sw.cpp b/servers/physics_2d/collision_object_2d_sw.cpp
index 91cdc5821..547755f1a 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 13f69fb6f..8da62d959 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 85ab4d81d..b04561ff0 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 353dd0046..27fa11268 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<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;