Fix errors in KinematicBody when floor is destroyed or removed

In all physics servers, body_get_direct_state() now silently returns
nullptr when the body has been already freed or is removed from space,
so the client code can detect this state and invalidate the body rid.

In 2D, there is no change in behavior (just no more errors).

In 3D, the Bullet server returned a valid direct body state when the
body was removed from the physics space, but in this case it didn't
make sense to use the information from the body state.

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;