Fix 3D moving platform logic

Same thing that was already done in 2D, applies moving platform motion
by using a call to move_and_collide that excludes the platform itself,
instead of making it part of the body motion.

Helps with handling walls and slopes correctly when the character walks
on the moving platform.

Also made some minor adjustments to the 2D version and documentation.

Co-authored-by: fabriceci <fabricecipolla@gmail.com>

4 files changed, 67 insertions, 39 deletions

diff --git a/scene/2d/physics_body_2d.cpp b/scene/2d/physics_body_2d.cpp
index 00eeb60192..9235a2290e 100644
--- a/scene/2d/physics_body_2d.cpp
+++ b/scene/2d/physics_body_2d.cpp
@@ -1075,7 +1075,7 @@ void CharacterBody2D::move_and_slide() {
 	floor_normal = Vector2();
 	floor_velocity = Vector2();
 
-	if (current_floor_velocity != Vector2()) {
+	if (current_floor_velocity != Vector2() && on_floor_body.is_valid()) {
 		PhysicsServer2D::MotionResult floor_result;
 		Set<RID> exclude;
 		exclude.insert(on_floor_body);
diff --git a/scene/2d/physics_body_2d.h b/scene/2d/physics_body_2d.h
index 7a319aabc9..c20372fe75 100644
--- a/scene/2d/physics_body_2d.h
+++ b/scene/2d/physics_body_2d.h
@@ -292,6 +292,8 @@ private:
 
 	Ref<KinematicCollision2D> _get_slide_collision(int p_bounce);
 
+	void _set_collision_direction(const PhysicsServer2D::MotionResult &p_result);
+
 	bool separate_raycast_shapes(PhysicsServer2D::MotionResult &r_result);
 
 	void set_safe_margin(real_t p_margin);
@@ -314,7 +316,6 @@ private:
 
 	const Vector2 &get_up_direction() const;
 	void set_up_direction(const Vector2 &p_up_direction);
-	void _set_collision_direction(const PhysicsServer2D::MotionResult &p_result);
 
 protected:
 	void _notification(int p_what);
diff --git a/scene/3d/physics_body_3d.cpp b/scene/3d/physics_body_3d.cpp
index 38104a8365..53ec07f1ce 100644
--- a/scene/3d/physics_body_3d.cpp
+++ b/scene/3d/physics_body_3d.cpp
@@ -117,9 +117,9 @@ Ref<KinematicCollision3D> PhysicsBody3D::_move(const Vector3 &p_motion, bool p_i
 	return Ref<KinematicCollision3D>();
 }
 
-bool PhysicsBody3D::move_and_collide(const Vector3 &p_motion, bool p_infinite_inertia, PhysicsServer3D::MotionResult &r_result, real_t p_margin, bool p_exclude_raycast_shapes, bool p_test_only, bool p_cancel_sliding) {
+bool PhysicsBody3D::move_and_collide(const Vector3 &p_motion, bool p_infinite_inertia, PhysicsServer3D::MotionResult &r_result, real_t p_margin, bool p_exclude_raycast_shapes, bool p_test_only, bool p_cancel_sliding, const Set<RID> &p_exclude) {
 	Transform3D gt = get_global_transform();
-	bool colliding = PhysicsServer3D::get_singleton()->body_test_motion(get_rid(), gt, p_motion, p_infinite_inertia, p_margin, &r_result, p_exclude_raycast_shapes);
+	bool colliding = PhysicsServer3D::get_singleton()->body_test_motion(get_rid(), gt, p_motion, p_infinite_inertia, p_margin, &r_result, p_exclude_raycast_shapes, p_exclude);
 
 	// Restore direction of motion to be along original motion,
 	// in order to avoid sliding due to recovery,
@@ -1090,6 +1090,9 @@ void CharacterBody3D::move_and_slide() {
 
 	bool was_on_floor = on_floor;
 
+	// Hack in order to work with calling from _process as well as from _physics_process; calling from thread is risky
+	float delta = Engine::get_singleton()->is_in_physics_frame() ? get_physics_process_delta_time() : get_process_delta_time();
+
 	for (int i = 0; i < 3; i++) {
 		if (locked_axis & (1 << i)) {
 			linear_velocity[i] = 0.0;
@@ -1097,7 +1100,7 @@ void CharacterBody3D::move_and_slide() {
 	}
 
 	Vector3 current_floor_velocity = floor_velocity;
-	if (on_floor && on_floor_body.is_valid()) {
+	if ((on_floor || on_wall) && on_floor_body.is_valid()) {
 		//this approach makes sure there is less delay between the actual body velocity and the one we saved
 		PhysicsDirectBodyState3D *bs = PhysicsServer3D::get_singleton()->body_get_direct_state(on_floor_body);
 		if (bs) {
@@ -1107,20 +1110,30 @@ void CharacterBody3D::move_and_slide() {
 		}
 	}
 
-	// Hack in order to work with calling from _process as well as from _physics_process; calling from thread is risky
-	Vector3 motion = (floor_velocity + linear_velocity) * (Engine::get_singleton()->is_in_physics_frame() ? get_physics_process_delta_time() : get_process_delta_time());
-
+	motion_results.clear();
 	on_floor = false;
-	on_floor_body = RID();
 	on_ceiling = false;
 	on_wall = false;
-	motion_results.clear();
 	floor_normal = Vector3();
 	floor_velocity = Vector3();
 
+	if (current_floor_velocity != Vector3() && on_floor_body.is_valid()) {
+		PhysicsServer3D::MotionResult floor_result;
+		Set<RID> exclude;
+		exclude.insert(on_floor_body);
+		if (move_and_collide(current_floor_velocity * delta, infinite_inertia, floor_result, true, false, false, false, exclude)) {
+			motion_results.push_back(floor_result);
+			_set_collision_direction(floor_result);
+		}
+	}
+
+	on_floor_body = RID();
+	Vector3 motion = linear_velocity * delta;
+
 	// No sliding on first attempt to keep floor motion stable when possible,
 	// when stop on slope is enabled.
 	bool sliding_enabled = !stop_on_slope;
+
 	for (int iteration = 0; iteration < max_slides; ++iteration) {
 		PhysicsServer3D::MotionResult result;
 		bool found_collision = false;
@@ -1144,35 +1157,19 @@ void CharacterBody3D::move_and_slide() {
 				found_collision = true;
 
 				motion_results.push_back(result);
-
-				if (up_direction == Vector3()) {
-					//all is a wall
-					on_wall = true;
-				} else {
-					if (Math::acos(result.collision_normal.dot(up_direction)) <= floor_max_angle + FLOOR_ANGLE_THRESHOLD) { //floor
-
-						on_floor = true;
-						floor_normal = result.collision_normal;
-						on_floor_body = result.collider;
-						floor_velocity = result.collider_velocity;
-
-						if (stop_on_slope) {
-							if ((body_velocity_normal + up_direction).length() < 0.01) {
-								Transform3D gt = get_global_transform();
-								if (result.motion.length() > margin) {
-									gt.origin -= result.motion.slide(up_direction);
-								} else {
-									gt.origin -= result.motion;
-								}
-								set_global_transform(gt);
-								linear_velocity = Vector3();
-								return;
-							}
+				_set_collision_direction(result);
+
+				if (on_floor && stop_on_slope) {
+					if ((body_velocity_normal + up_direction).length() < 0.01) {
+						Transform3D gt = get_global_transform();
+						if (result.motion.length() > margin) {
+							gt.origin -= result.motion.slide(up_direction);
+						} else {
+							gt.origin -= result.motion;
 						}
-					} else if (Math::acos(result.collision_normal.dot(-up_direction)) <= floor_max_angle + FLOOR_ANGLE_THRESHOLD) { //ceiling
-						on_ceiling = true;
-					} else {
-						on_wall = true;
+						set_global_transform(gt);
+						linear_velocity = Vector3();
+						return;
 					}
 				}
 
@@ -1198,6 +1195,11 @@ void CharacterBody3D::move_and_slide() {
 		}
 	}
 
+	if (!on_floor && !on_wall) {
+		// Add last platform velocity when just left a moving platform.
+		linear_velocity += current_floor_velocity;
+	}
+
 	if (!was_on_floor || snap == Vector3()) {
 		return;
 	}
@@ -1233,6 +1235,29 @@ void CharacterBody3D::move_and_slide() {
 	}
 }
 
+void CharacterBody3D::_set_collision_direction(const PhysicsServer3D::MotionResult &p_result) {
+	on_floor = false;
+	on_ceiling = false;
+	on_wall = false;
+	if (up_direction == Vector3()) {
+		//all is a wall
+		on_wall = true;
+	} else {
+		if (Math::acos(p_result.collision_normal.dot(up_direction)) <= floor_max_angle + FLOOR_ANGLE_THRESHOLD) { //floor
+			on_floor = true;
+			floor_normal = p_result.collision_normal;
+			on_floor_body = p_result.collider;
+			floor_velocity = p_result.collider_velocity;
+		} else if (Math::acos(p_result.collision_normal.dot(-up_direction)) <= floor_max_angle + FLOOR_ANGLE_THRESHOLD) { //ceiling
+			on_ceiling = true;
+		} else {
+			on_wall = true;
+			on_floor_body = p_result.collider;
+			floor_velocity = p_result.collider_velocity;
+		}
+	}
+}
+
 bool CharacterBody3D::separate_raycast_shapes(PhysicsServer3D::MotionResult &r_result) {
 	PhysicsServer3D::SeparationResult sep_res[8]; //max 8 rays
 
diff --git a/scene/3d/physics_body_3d.h b/scene/3d/physics_body_3d.h
index 0ef9c78f3b..227e6d43b6 100644
--- a/scene/3d/physics_body_3d.h
+++ b/scene/3d/physics_body_3d.h
@@ -53,7 +53,7 @@ protected:
 	Ref<KinematicCollision3D> _move(const Vector3 &p_motion, bool p_infinite_inertia = true, bool p_exclude_raycast_shapes = true, bool p_test_only = false, real_t p_margin = 0.001);
 
 public:
-	bool move_and_collide(const Vector3 &p_motion, bool p_infinite_inertia, PhysicsServer3D::MotionResult &r_result, real_t p_margin, bool p_exclude_raycast_shapes = true, bool p_test_only = false, bool p_cancel_sliding = true);
+	bool move_and_collide(const Vector3 &p_motion, bool p_infinite_inertia, PhysicsServer3D::MotionResult &r_result, real_t p_margin, bool p_exclude_raycast_shapes = true, bool p_test_only = false, bool p_cancel_sliding = true, const Set<RID> &p_exclude = Set<RID>());
 	bool test_move(const Transform3D &p_from, const Vector3 &p_motion, bool p_infinite_inertia = true, bool p_exclude_raycast_shapes = true, const Ref<KinematicCollision3D> &r_collision = Ref<KinematicCollision3D>(), real_t p_margin = 0.001);
 
 	void set_axis_lock(PhysicsServer3D::BodyAxis p_axis, bool p_lock);
@@ -298,6 +298,8 @@ private:
 
 	Ref<KinematicCollision3D> _get_slide_collision(int p_bounce);
 
+	void _set_collision_direction(const PhysicsServer3D::MotionResult &p_result);
+
 	bool separate_raycast_shapes(PhysicsServer3D::MotionResult &r_result);
 
 	void set_safe_margin(real_t p_margin);