Fix errors in CharacterBody 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.

7 files changed, 35 insertions, 4 deletions

diff --git a/doc/classes/PhysicsServer2D.xml b/doc/classes/PhysicsServer2D.xml
index 7368fe06ab..6f88707259 100644
--- a/doc/classes/PhysicsServer2D.xml
+++ b/doc/classes/PhysicsServer2D.xml
@@ -346,7 +346,7 @@
 			<return type="PhysicsDirectBodyState2D" />
 			<argument index="0" name="body" type="RID" />
 			<description>
-				Returns the [PhysicsDirectBodyState2D] of the body.
+				Returns the [PhysicsDirectBodyState2D] 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">
diff --git a/doc/classes/PhysicsServer3D.xml b/doc/classes/PhysicsServer3D.xml
index 0f02cdf92f..b144c2c299 100644
--- a/doc/classes/PhysicsServer3D.xml
+++ b/doc/classes/PhysicsServer3D.xml
@@ -320,7 +320,7 @@
 			<return type="PhysicsDirectBodyState3D" />
 			<argument index="0" name="body" type="RID" />
 			<description>
-				Returns the [PhysicsDirectBodyState3D] of the body.
+				Returns the [PhysicsDirectBodyState3D] 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">
diff --git a/modules/bullet/bullet_physics_server.cpp b/modules/bullet/bullet_physics_server.cpp
index 610d2145cd..684a20cf4d 100644
--- a/modules/bullet/bullet_physics_server.cpp
+++ b/modules/bullet/bullet_physics_server.cpp
@@ -837,8 +837,17 @@ void BulletPhysicsServer3D::body_set_ray_pickable(RID p_body, bool p_enable) {
 }
 
 PhysicsDirectBodyState3D *BulletPhysicsServer3D::body_get_direct_state(RID p_body) {
+	if (!rigid_body_owner.owns(p_body)) {
+		return nullptr;
+	}
+
 	RigidBodyBullet *body = rigid_body_owner.get_or_null(p_body);
 	ERR_FAIL_COND_V(!body, nullptr);
+
+	if (!body->get_space()) {
+		return nullptr;
+	}
+
 	return BulletPhysicsDirectBodyState3D::get_singleton(body);
 }
 
diff --git a/scene/2d/physics_body_2d.cpp b/scene/2d/physics_body_2d.cpp
index a43d498a62..9d140f744f 100644
--- a/scene/2d/physics_body_2d.cpp
+++ b/scene/2d/physics_body_2d.cpp
@@ -1101,6 +1101,10 @@ bool CharacterBody2D::move_and_slide() {
 			if (bs) {
 				Vector2 local_position = gt.elements[2] - bs->get_transform().elements[2];
 				current_platform_velocity = bs->get_velocity_at_local_position(local_position);
+			} else {
+				// Body is removed or destroyed, invalidate floor.
+				current_platform_velocity = Vector2();
+				platform_rid = RID();
 			}
 		} else {
 			current_platform_velocity = Vector2();
diff --git a/scene/3d/physics_body_3d.cpp b/scene/3d/physics_body_3d.cpp
index d0506227b4..ea881b6d54 100644
--- a/scene/3d/physics_body_3d.cpp
+++ b/scene/3d/physics_body_3d.cpp
@@ -1145,6 +1145,10 @@ bool CharacterBody3D::move_and_slide() {
 			if (bs) {
 				Vector3 local_position = gt.origin - bs->get_transform().origin;
 				current_platform_velocity = bs->get_velocity_at_local_position(local_position);
+			} else {
+				// Body is removed or destroyed, invalidate floor.
+				current_platform_velocity = Vector3();
+				platform_rid = RID();
 			}
 		} else {
 			current_platform_velocity = Vector3();
diff --git a/servers/physics_2d/godot_physics_server_2d.cpp b/servers/physics_2d/godot_physics_server_2d.cpp
index cf66b80076..617fa6470a 100644
--- a/servers/physics_2d/godot_physics_server_2d.cpp
+++ b/servers/physics_2d/godot_physics_server_2d.cpp
@@ -963,10 +963,17 @@ bool GodotPhysicsServer2D::body_test_motion(RID p_body, const MotionParameters &
 PhysicsDirectBodyState2D *GodotPhysicsServer2D::body_get_direct_state(RID p_body) {
 	ERR_FAIL_COND_V_MSG((using_threads && !doing_sync), nullptr, "Body state is inaccessible right now, wait for iteration or physics process notification.");
 
+	if (!body_owner.owns(p_body)) {
+		return nullptr;
+	}
+
 	GodotBody2D *body = body_owner.get_or_null(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.");
 
 	return body->get_direct_state();
diff --git a/servers/physics_3d/godot_physics_server_3d.cpp b/servers/physics_3d/godot_physics_server_3d.cpp
index 73654939ca..3e7f9faa27 100644
--- a/servers/physics_3d/godot_physics_server_3d.cpp
+++ b/servers/physics_3d/godot_physics_server_3d.cpp
@@ -881,10 +881,17 @@ bool GodotPhysicsServer3D::body_test_motion(RID p_body, const MotionParameters &
 PhysicsDirectBodyState3D *GodotPhysicsServer3D::body_get_direct_state(RID p_body) {
 	ERR_FAIL_COND_V_MSG((using_threads && !doing_sync), nullptr, "Body state is inaccessible right now, wait for iteration or physics process notification.");
 
+	if (!body_owner.owns(p_body)) {
+		return nullptr;
+	}
+
 	GodotBody3D *body = body_owner.get_or_null(p_body);
 	ERR_FAIL_NULL_V(body, nullptr);
 
-	ERR_FAIL_NULL_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.");
 
 	return body->get_direct_state();