Merge pull request #9832 from RandomShaper/port-physics-fixes

Port physics fixes

9 files changed, 59 insertions, 21 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.cpp b/servers/physics/body_sw.cpp
index 1f32c059a8..e065fae2be 100644
--- a/servers/physics/body_sw.cpp
+++ b/servers/physics/body_sw.cpp
@@ -757,7 +757,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 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 101bd4b185..833c77216e 100644
--- a/servers/physics/physics_server_sw.cpp
+++ b/servers/physics/physics_server_sw.cpp
@@ -222,12 +222,24 @@ 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
+
+	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);
 };
 
@@ -471,15 +483,23 @@ 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;
 
+	SpaceSW *space = NULL;
 	if (p_space.is_valid()) {
 		space = space_owner.get(p_space);
 		ERR_FAIL_COND(!space);
 	}
 
 	if (body->get_space() == space)
-		return; //pointles
+		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);
 };
@@ -1329,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);
 
diff --git a/servers/physics/space_sw.cpp b/servers/physics/space_sw.cpp
index 5679fc8f60..094cfa4656 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_collision_layer, 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_collision_layer() & p_collision_layer) == 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.h b/servers/physics_2d/area_2d_sw.h
index 68b3c61e44..6d74a4b0f6 100644
--- a/servers/physics_2d/area_2d_sw.h
+++ b/servers/physics_2d/area_2d_sw.h
@@ -153,6 +153,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/body_2d_sw.h b/servers/physics_2d/body_2d_sw.h
index 9e5deef3f2..412f2f51cd 100644
--- a/servers/physics_2d/body_2d_sw.h
+++ b/servers/physics_2d/body_2d_sw.h
@@ -181,6 +181,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/broad_phase_2d_hash_grid.cpp b/servers/physics_2d/broad_phase_2d_hash_grid.cpp
index 5b6c7e2f38..0330bfa9f3 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/physics_2d_server_sw.cpp b/servers/physics_2d/physics_2d_server_sw.cpp
index c20d0d14a2..add376bfb2 100644
--- a/servers/physics_2d/physics_2d_server_sw.cpp
+++ b/servers/physics_2d/physics_2d_server_sw.cpp
@@ -286,12 +286,24 @@ 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
+
+	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();
+
 	area->set_space(space);
 };
 
@@ -533,6 +545,17 @@ void Physics2DServerSW::body_set_space(RID p_body, RID p_space) {
 		ERR_FAIL_COND(!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);
 };
 
@@ -1073,19 +1096,13 @@ void Physics2DServerSW::free(RID p_rid) {
 			_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);