3 files changed, 18 insertions, 6 deletions

diff --git a/servers/physics/area_sw.h b/servers/physics/area_sw.h
index 06e58e3d5a..3dae1db13f 100644
--- a/servers/physics/area_sw.h
+++ b/servers/physics/area_sw.h
@@ -154,6 +154,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_sw.h b/servers/physics/body_sw.h
index c3e051c2d0..512b868570 100644
--- a/servers/physics/body_sw.h
+++ b/servers/physics/body_sw.h
@@ -194,6 +194,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/physics_server_sw.cpp b/servers/physics/physics_server_sw.cpp
index 51d376cfa5..833c77216e 100644
--- a/servers/physics/physics_server_sw.cpp
+++ b/servers/physics/physics_server_sw.cpp
@@ -232,6 +232,14 @@ void PhysicsServerSW::area_set_space(RID p_area, RID p_space) {
 	if (area->get_space() == space)
 		return; //pointless
 
+	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();
+
 	area->set_space(space);
 };
 
@@ -485,6 +493,14 @@ void PhysicsServerSW::body_set_space(RID p_body, RID p_space) {
 	if (body->get_space() == space)
 		return; //pointless
 
+	while (body->get_constraint_map().size()) {
+		RID self = body->get_constraint_map().front()->key()->get_self();
+		if (!self.is_valid())
+			continue;
+		free(self);
+	}
+	body->clear_constraint_map();
+
 	body->set_space(space);
 };
 
@@ -1333,12 +1349,6 @@ void PhysicsServerSW::free(RID p_rid) {
 			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);