Remove any constraints connected to a Bullet body when removing it

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();

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;
 

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);
 	}
 }