3 files changed, 11 insertions, 3 deletions
diff --git a/modules/bullet/rigid_body_bullet.cpp b/modules/bullet/rigid_body_bullet.cpp
index 7a53f91b33..4763098584 100644
--- a/modules/bullet/rigid_body_bullet.cpp
+++ b/modules/bullet/rigid_body_bullet.cpp
@@ -322,7 +322,8 @@ void RigidBodyBullet::set_space(SpaceBullet *p_space) {
 	if (space) {
 		can_integrate_forces = false;
 		isScratchedSpaceOverrideModificator = false;
-
+		// Remove any constraints
+		space->remove_rigid_body_constraints(this);
 		// Remove this object form the physics world
 		space->remove_rigid_body(this);
 	}
diff --git a/modules/bullet/space_bullet.cpp b/modules/bullet/space_bullet.cpp
index a8d55b59b3..d7dd11d2a5 100644
--- a/modules/bullet/space_bullet.cpp
+++ b/modules/bullet/space_bullet.cpp
@@ -177,8 +177,10 @@ bool BulletPhysicsDirectSpaceState::cast_motion(const RID &p_shape, const Transf
 	bt_xform_to.getOrigin() += bt_motion;
 
 	if ((bt_xform_to.getOrigin() - bt_xform_from.getOrigin()).fuzzyZero()) {
+		r_closest_safe = 1.0f;
+		r_closest_unsafe = 1.0f;
 		bulletdelete(btShape);
-		return false;
+		return true;
 	}
 
 	GodotClosestConvexResultCallback btResult(bt_xform_from.getOrigin(), bt_xform_to.getOrigin(), &p_exclude, p_collide_with_bodies, p_collide_with_areas);
@@ -477,7 +479,7 @@ void SpaceBullet::add_rigid_body(RigidBodyBullet *p_body) {
 	}
 }
 
-void SpaceBullet::remove_rigid_body(RigidBodyBullet *p_body) {
+void SpaceBullet::remove_rigid_body_constraints(RigidBodyBullet *p_body) {
 	btRigidBody *btBody = p_body->get_bt_rigid_body();
 
 	int constraints = btBody->getNumConstraintRefs();
@@ -487,6 +489,10 @@ void SpaceBullet::remove_rigid_body(RigidBodyBullet *p_body) {
 			dynamicsWorld->removeConstraint(btBody->getConstraintRef(i));
 		}
 	}
+}
+
+void SpaceBullet::remove_rigid_body(RigidBodyBullet *p_body) {
+	btRigidBody *btBody = p_body->get_bt_rigid_body();
 
 	if (p_body->is_static()) {
 		dynamicsWorld->removeCollisionObject(btBody);
diff --git a/modules/bullet/space_bullet.h b/modules/bullet/space_bullet.h
index 42f982d5f0..0f2482e551 100644
--- a/modules/bullet/space_bullet.h
+++ b/modules/bullet/space_bullet.h
@@ -151,6 +151,7 @@ public:
 	void reload_collision_filters(AreaBullet *p_area);
 
 	void add_rigid_body(RigidBodyBullet *p_body);
+	void remove_rigid_body_constraints(RigidBodyBullet *p_body);
 	void remove_rigid_body(RigidBodyBullet *p_body);
 	void reload_collision_filters(RigidBodyBullet *p_body);