3 files changed, 33 insertions, 24 deletions

diff --git a/servers/physics_2d/joints_2d_sw.cpp b/servers/physics_2d/joints_2d_sw.cpp
index f503868ba5..c7b556deba 100644
--- a/servers/physics_2d/joints_2d_sw.cpp
+++ b/servers/physics_2d/joints_2d_sw.cpp
@@ -205,15 +205,6 @@ PinJoint2DSW::PinJoint2DSW(const Vector2 &p_pos, Body2DSW *p_body_a, Body2DSW *p
 	}
 }
 
-PinJoint2DSW::~PinJoint2DSW() {
-	if (A) {
-		A->remove_constraint(this, 0);
-	}
-	if (B) {
-		B->remove_constraint(this, 1);
-	}
-}
-
 //////////////////////////////////////////////
 //////////////////////////////////////////////
 //////////////////////////////////////////////
@@ -346,11 +337,6 @@ GrooveJoint2DSW::GrooveJoint2DSW(const Vector2 &p_a_groove1, const Vector2 &p_a_
 	B->add_constraint(this, 1);
 }
 
-GrooveJoint2DSW::~GrooveJoint2DSW() {
-	A->remove_constraint(this, 0);
-	B->remove_constraint(this, 1);
-}
-
 //////////////////////////////////////////////
 //////////////////////////////////////////////
 //////////////////////////////////////////////
@@ -442,8 +428,3 @@ DampedSpringJoint2DSW::DampedSpringJoint2DSW(const Vector2 &p_anchor_a, const Ve
 	A->add_constraint(this, 0);
 	B->add_constraint(this, 1);
 }
-
-DampedSpringJoint2DSW::~DampedSpringJoint2DSW() {
-	A->remove_constraint(this, 0);
-	B->remove_constraint(this, 1);
-}
diff --git a/servers/physics_2d/joints_2d_sw.h b/servers/physics_2d/joints_2d_sw.h
index 6050dc2775..628de972ae 100644
--- a/servers/physics_2d/joints_2d_sw.h
+++ b/servers/physics_2d/joints_2d_sw.h
@@ -60,6 +60,15 @@ public:
 		bias = 0;
 		max_force = max_bias = 3.40282e+38;
 	};
+
+	virtual ~Joint2DSW() {
+		for (int i = 0; i < get_body_count(); i++) {
+			Body2DSW *body = get_body_ptr()[i];
+			if (body) {
+				body->remove_constraint(this, i);
+			}
+		}
+	};
 };
 
 class PinJoint2DSW : public Joint2DSW {
@@ -90,7 +99,6 @@ public:
 	real_t get_param(PhysicsServer2D::PinJointParam p_param) const;
 
 	PinJoint2DSW(const Vector2 &p_pos, Body2DSW *p_body_a, Body2DSW *p_body_b = nullptr);
-	~PinJoint2DSW();
 };
 
 class GrooveJoint2DSW : public Joint2DSW {
@@ -124,7 +132,6 @@ public:
 	virtual void solve(real_t p_step);
 
 	GrooveJoint2DSW(const Vector2 &p_a_groove1, const Vector2 &p_a_groove2, const Vector2 &p_b_anchor, Body2DSW *p_body_a, Body2DSW *p_body_b);
-	~GrooveJoint2DSW();
 };
 
 class DampedSpringJoint2DSW : public Joint2DSW {
@@ -160,7 +167,6 @@ public:
 	real_t get_param(PhysicsServer2D::DampedSpringParam p_param) const;
 
 	DampedSpringJoint2DSW(const Vector2 &p_anchor_a, const Vector2 &p_anchor_b, Body2DSW *p_body_a, Body2DSW *p_body_b);
-	~DampedSpringJoint2DSW();
 };
 
 #endif // JOINTS_2D_SW_H
diff --git a/servers/physics_2d/space_2d_sw.cpp b/servers/physics_2d/space_2d_sw.cpp
index 9cbc01d1d3..4f12248c3e 100644
--- a/servers/physics_2d/space_2d_sw.cpp
+++ b/servers/physics_2d/space_2d_sw.cpp
@@ -84,6 +84,10 @@ int PhysicsDirectSpaceState2DSW::_intersect_point_impl(const Vector2 &p_point, S
 
 		int shape_idx = space->intersection_query_subindex_results[i];
 
+		if (col_obj->is_shape_set_as_disabled(shape_idx)) {
+			continue;
+		}
+
 		Shape2DSW *shape = col_obj->get_shape(shape_idx);
 
 		Vector2 local_point = (col_obj->get_transform() * col_obj->get_shape_transform(shape_idx)).affine_inverse().xform(p_point);
@@ -229,6 +233,10 @@ int PhysicsDirectSpaceState2DSW::intersect_shape(const RID &p_shape, const Trans
 		const CollisionObject2DSW *col_obj = space->intersection_query_results[i];
 		int shape_idx = space->intersection_query_subindex_results[i];
 
+		if (col_obj->is_shape_set_as_disabled(shape_idx)) {
+			continue;
+		}
+
 		if (!CollisionSolver2DSW::solve(shape, p_xform, p_motion, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), nullptr, nullptr, nullptr, p_margin)) {
 			continue;
 		}
@@ -272,6 +280,10 @@ bool PhysicsDirectSpaceState2DSW::cast_motion(const RID &p_shape, const Transfor
 		const CollisionObject2DSW *col_obj = space->intersection_query_results[i];
 		int shape_idx = space->intersection_query_subindex_results[i];
 
+		if (col_obj->is_shape_set_as_disabled(shape_idx)) {
+			continue;
+		}
+
 		Transform2D col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
 		//test initial overlap, does it collide if going all the way?
 		if (!CollisionSolver2DSW::solve(shape, p_xform, p_motion, col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), nullptr, nullptr, nullptr, p_margin)) {
@@ -346,12 +358,17 @@ bool PhysicsDirectSpaceState2DSW::collide_shape(RID p_shape, const Transform2D &
 		}
 
 		const CollisionObject2DSW *col_obj = space->intersection_query_results[i];
-		int shape_idx = space->intersection_query_subindex_results[i];
 
 		if (p_exclude.has(col_obj->get_self())) {
 			continue;
 		}
 
+		int shape_idx = space->intersection_query_subindex_results[i];
+
+		if (col_obj->is_shape_set_as_disabled(shape_idx)) {
+			continue;
+		}
+
 		cbk.valid_dir = Vector2();
 		cbk.valid_depth = 0;
 
@@ -436,12 +453,17 @@ bool PhysicsDirectSpaceState2DSW::rest_info(RID p_shape, const Transform2D &p_sh
 		}
 
 		const CollisionObject2DSW *col_obj = space->intersection_query_results[i];
-		int shape_idx = space->intersection_query_subindex_results[i];
 
 		if (p_exclude.has(col_obj->get_self())) {
 			continue;
 		}
 
+		int shape_idx = space->intersection_query_subindex_results[i];
+
+		if (col_obj->is_shape_set_as_disabled(shape_idx)) {
+			continue;
+		}
+
 		rcd.valid_dir = Vector2();
 		rcd.object = col_obj;
 		rcd.shape = shape_idx;