3 files changed, 36 insertions, 6 deletions

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/joints_2d_sw.cpp b/servers/physics_2d/joints_2d_sw.cpp
index 7fba8acebd..d49c1b8376 100644
--- a/servers/physics_2d/joints_2d_sw.cpp
+++ b/servers/physics_2d/joints_2d_sw.cpp
@@ -146,14 +146,19 @@ bool PinJoint2DSW::setup(real_t p_step) {
 	return true;
 }
 
+inline Vector2 custom_cross(const Vector2 &p_vec, real_t p_other) {
+
+	return Vector2(p_other * p_vec.y, -p_other * p_vec.x);
+}
+
 void PinJoint2DSW::solve(real_t p_step) {
 
 	// compute relative velocity
-	Vector2 vA = A->get_linear_velocity() - rA.cross(A->get_angular_velocity());
+	Vector2 vA = A->get_linear_velocity() - custom_cross(rA, A->get_angular_velocity());
 
 	Vector2 rel_vel;
 	if (B)
-		rel_vel = B->get_linear_velocity() - rB.cross(B->get_angular_velocity()) - vA;
+		rel_vel = B->get_linear_velocity() - custom_cross(rB, B->get_angular_velocity()) - vA;
 	else
 		rel_vel = -vA;