[3.x] Fix errors in KinematicBody when floor is destroyed or removed

doc/classes/Physics2DServer.xml

@@ -348,7 +348,7 @@
 			<return type="Physics2DDirectBodyState" />
 			<argument index="0" name="body" type="RID" />
 			<description>
-				Returns the [Physics2DDirectBodyState] of the body.
+				Returns the [Physics2DDirectBodyState] of the body. Returns [code]null[/code] if the body is destroyed or removed from the physics space.
 			</description>
 		</method>
 		<method name="body_get_max_contacts_reported" qualifiers="const">

doc/classes/PhysicsServer.xml

@@ -332,7 +332,7 @@
 			<return type="PhysicsDirectBodyState" />
 			<argument index="0" name="body" type="RID" />
 			<description>
-				Returns the [PhysicsDirectBodyState] of the body.
+				Returns the [PhysicsDirectBodyState] of the body. Returns [code]null[/code] if the body is destroyed or removed from the physics space.
 			</description>
 		</method>
 		<method name="body_get_kinematic_safe_margin" qualifiers="const">

modules/bullet/bullet_physics_server.cpp

@@ -851,8 +851,17 @@ bool BulletPhysicsServer::body_is_ray_pickable(RID p_body) const {
 }
 
 PhysicsDirectBodyState *BulletPhysicsServer::body_get_direct_state(RID p_body) {
+	if (!rigid_body_owner.owns(p_body)) {
+		return nullptr;
+	}
+
 	RigidBodyBullet *body = rigid_body_owner.get(p_body);
 	ERR_FAIL_COND_V(!body, nullptr);
+
+	if (!body->get_space()) {
+		return nullptr;
+	}
+
 	return BulletPhysicsDirectBodyState::get_singleton(body);
 }
 

scene/2d/physics_body_2d.cpp

@@ -1120,6 +1120,10 @@ Vector2 KinematicBody2D::_move_and_slide_internal(const Vector2 &p_linear_veloci
 			Transform2D gt = get_global_transform();
 			Vector2 local_position = gt.elements[2] - bs->get_transform().elements[2];
 			current_floor_velocity = bs->get_velocity_at_local_position(local_position);
+		} else {
+			// Body is removed or destroyed, invalidate floor.
+			current_floor_velocity = Vector2();
+			on_floor_body = RID();
 		}
 	}
 

scene/3d/physics_body.cpp

@@ -1074,6 +1074,10 @@ Vector3 KinematicBody::_move_and_slide_internal(const Vector3 &p_linear_velocity
 			Transform gt = get_global_transform();
 			Vector3 local_position = gt.origin - bs->get_transform().origin;
 			current_floor_velocity = bs->get_velocity_at_local_position(local_position);
+		} else {
+			// Body is removed or destroyed, invalidate floor.
+			current_floor_velocity = Vector3();
+			on_floor_body = RID();
 		}
 	}
 

servers/physics/physics_server_sw.cpp

@@ -881,8 +881,17 @@ int PhysicsServerSW::body_test_ray_separation(RID p_body, const Transform &p_tra
 }
 
 PhysicsDirectBodyState *PhysicsServerSW::body_get_direct_state(RID p_body) {
+	if (!body_owner.owns(p_body)) {
+		return nullptr;
+	}
+
 	BodySW *body = body_owner.get(p_body);
 	ERR_FAIL_COND_V(!body, nullptr);
+
+	if (!body->get_space()) {
+		return nullptr;
+	}
+
 	ERR_FAIL_COND_V_MSG(body->get_space()->is_locked(), nullptr, "Body state is inaccessible right now, wait for iteration or physics process notification.");
 
 	direct_state->body = body;

servers/physics_2d/physics_2d_server_sw.cpp

@@ -962,7 +962,11 @@ Physics2DDirectBodyState *Physics2DServerSW::body_get_direct_state(RID p_body) {
 
 	Body2DSW *body = body_owner.get(p_body);
 	ERR_FAIL_COND_V(!body, nullptr);
-	ERR_FAIL_COND_V(!body->get_space(), nullptr);
+
+	if (!body->get_space()) {
+		return nullptr;
+	}
+
 	ERR_FAIL_COND_V_MSG(body->get_space()->is_locked(), nullptr, "Body state is inaccessible right now, wait for iteration or physics process notification.");
 
 	direct_state->body = body;