5 files changed, 35 insertions, 4 deletions

diff --git a/servers/physics_2d/body_pair_2d_sw.cpp b/servers/physics_2d/body_pair_2d_sw.cpp
index f51882b5ee..61c0e0063f 100644
--- a/servers/physics_2d/body_pair_2d_sw.cpp
+++ b/servers/physics_2d/body_pair_2d_sw.cpp
@@ -62,6 +62,7 @@ void BodyPair2DSW::_contact_added_callback(const Vector2 &p_point_A, const Vecto
 	contact.local_B = local_B;
 	contact.reused = true;
 	contact.normal = (p_point_A - p_point_B).normalized();
+	contact.mass_normal = 0; // will be computed in setup()
 
 	// attempt to determine if the contact will be reused
 
diff --git a/servers/physics_2d/collision_object_2d_sw.cpp b/servers/physics_2d/collision_object_2d_sw.cpp
index ce06aa9a2b..23084a4241 100644
--- a/servers/physics_2d/collision_object_2d_sw.cpp
+++ b/servers/physics_2d/collision_object_2d_sw.cpp
@@ -73,6 +73,27 @@ void CollisionObject2DSW::set_shape_transform(int p_index, const Transform2D &p_
 	_shapes_changed();
 }
 
+void CollisionObject2DSW::set_shape_as_disabled(int p_idx, bool p_disabled) {
+	ERR_FAIL_INDEX(p_idx, shapes.size());
+
+	CollisionObject2DSW::Shape &shape = shapes[p_idx];
+	if (shape.disabled == p_disabled)
+		return;
+
+	shape.disabled = p_disabled;
+
+	if (!space)
+		return;
+
+	if (p_disabled && shape.bpid != 0) {
+		space->get_broadphase()->remove(shape.bpid);
+		shape.bpid = 0;
+		_update_shapes();
+	} else if (!p_disabled && shape.bpid == 0) {
+		_update_shapes(); // automatically adds shape with bpid == 0
+	}
+}
+
 void CollisionObject2DSW::remove_shape(Shape2DSW *p_shape) {
 
 	//remove a shape, all the times it appears
@@ -139,6 +160,10 @@ void CollisionObject2DSW::_update_shapes() {
 	for (int i = 0; i < shapes.size(); i++) {
 
 		Shape &s = shapes[i];
+
+		if (s.disabled)
+			continue;
+
 		if (s.bpid == 0) {
 			s.bpid = space->get_broadphase()->create(this, i);
 			space->get_broadphase()->set_static(s.bpid, _static);
@@ -163,6 +188,9 @@ void CollisionObject2DSW::_update_shapes_with_motion(const Vector2 &p_motion) {
 	for (int i = 0; i < shapes.size(); i++) {
 
 		Shape &s = shapes[i];
+		if (s.disabled)
+			continue;
+
 		if (s.bpid == 0) {
 			s.bpid = space->get_broadphase()->create(this, i);
 			space->get_broadphase()->set_static(s.bpid, _static);
diff --git a/servers/physics_2d/collision_object_2d_sw.h b/servers/physics_2d/collision_object_2d_sw.h
index 5f25c27158..ab3e219ac0 100644
--- a/servers/physics_2d/collision_object_2d_sw.h
+++ b/servers/physics_2d/collision_object_2d_sw.h
@@ -136,10 +136,7 @@ public:
 	_FORCE_INLINE_ Transform2D get_inv_transform() const { return inv_transform; }
 	_FORCE_INLINE_ Space2DSW *get_space() const { return space; }
 
-	_FORCE_INLINE_ void set_shape_as_disabled(int p_idx, bool p_disabled) {
-		ERR_FAIL_INDEX(p_idx, shapes.size());
-		shapes[p_idx].disabled = p_disabled;
-	}
+	void set_shape_as_disabled(int p_idx, bool p_disabled);
 	_FORCE_INLINE_ bool is_shape_set_as_disabled(int p_idx) const {
 		ERR_FAIL_INDEX_V(p_idx, shapes.size(), false);
 		return shapes[p_idx].disabled;
diff --git a/servers/physics_2d/shape_2d_sw.cpp b/servers/physics_2d/shape_2d_sw.cpp
index 433942708e..2b0eab5999 100644
--- a/servers/physics_2d/shape_2d_sw.cpp
+++ b/servers/physics_2d/shape_2d_sw.cpp
@@ -1011,6 +1011,10 @@ void ConcavePolygonShape2DSW::cull(const Rect2 &p_local_aabb, Callback p_callbac
 		stack[i]=0;
 	*/
 
+	if (segments.size() == 0 || points.size() == 0 || bvh.size() == 0) {
+		return;
+	}
+
 	int level = 0;
 
 	const Segment *segmentptr = &segments[0];
diff --git a/servers/physics_2d/space_2d_sw.cpp b/servers/physics_2d/space_2d_sw.cpp
index c29093d1af..503ca0af4a 100644
--- a/servers/physics_2d/space_2d_sw.cpp
+++ b/servers/physics_2d/space_2d_sw.cpp
@@ -33,6 +33,7 @@
 #include "collision_solver_2d_sw.h"
 #include "pair.h"
 #include "physics_2d_server_sw.h"
+
 _FORCE_INLINE_ static bool _can_collide_with(CollisionObject2DSW *p_object, uint32_t p_collision_mask) {
 
 	return p_object->get_collision_layer() & p_collision_mask;