Remove any constraints connected to a Bullet body when removing it

3 files changed, 12 insertions, 13 deletions

diff --git a/modules/bullet/rigid_body_bullet.cpp b/modules/bullet/rigid_body_bullet.cpp
index eb599df74c..648acdf70e 100644
--- a/modules/bullet/rigid_body_bullet.cpp
+++ b/modules/bullet/rigid_body_bullet.cpp
@@ -323,9 +323,6 @@ void RigidBodyBullet::set_space(SpaceBullet *p_space) {
 		can_integrate_forces = false;
 		isScratchedSpaceOverrideModificator = false;
 
-		// Remove all eventual constraints
-		assert_no_constraints();
-
 		// Remove this object form the physics world
 		space->remove_rigid_body(this);
 	}
@@ -443,12 +440,6 @@ bool RigidBodyBullet::was_colliding(RigidBodyBullet *p_other_object) {
 	return false;
 }
 
-void RigidBodyBullet::assert_no_constraints() {
-	if (btBody->getNumConstraintRefs()) {
-		WARN_PRINT("A body with a joints is destroyed. Please check the implementation in order to destroy the joint before the body.");
-	}
-}
-
 void RigidBodyBullet::set_activation_state(bool p_active) {
 	if (p_active) {
 		btBody->activate();
diff --git a/modules/bullet/rigid_body_bullet.h b/modules/bullet/rigid_body_bullet.h
index c643611397..8ff96577b6 100644
--- a/modules/bullet/rigid_body_bullet.h
+++ b/modules/bullet/rigid_body_bullet.h
@@ -267,8 +267,6 @@ public:
 	bool add_collision_object(RigidBodyBullet *p_otherObject, const Vector3 &p_hitWorldLocation, const Vector3 &p_hitLocalLocation, const Vector3 &p_hitNormal, const float &p_appliedImpulse, int p_other_shape_index, int p_local_shape_index);
 	bool was_colliding(RigidBodyBullet *p_other_object);
 
-	void assert_no_constraints();
-
 	void set_activation_state(bool p_active);
 	bool is_active() const;
 
diff --git a/modules/bullet/space_bullet.cpp b/modules/bullet/space_bullet.cpp
index abad1beacb..3bfcd83606 100644
--- a/modules/bullet/space_bullet.cpp
+++ b/modules/bullet/space_bullet.cpp
@@ -478,10 +478,20 @@ void SpaceBullet::add_rigid_body(RigidBodyBullet *p_body) {
 }
 
 void SpaceBullet::remove_rigid_body(RigidBodyBullet *p_body) {
+	btRigidBody *btBody = p_body->get_bt_rigid_body();
+
+	int constraints = btBody->getNumConstraintRefs();
+	if (constraints > 0) {
+		WARN_PRINT("A body connected to joints was removed. Ensure bodies are disconnected from joints before removing them.");
+		for (int i = 0; i < constraints; i++) {
+			dynamicsWorld->removeConstraint(btBody->getConstraintRef(i));
+		}
+	}
+
 	if (p_body->is_static()) {
-		dynamicsWorld->removeCollisionObject(p_body->get_bt_rigid_body());
+		dynamicsWorld->removeCollisionObject(btBody);
 	} else {
-		dynamicsWorld->removeRigidBody(p_body->get_bt_rigid_body());
+		dynamicsWorld->removeRigidBody(btBody);
 	}
 }