Ensure move_and_slide() is consistent between the 2D and 3D versions.

In the 3D version:
- Partially revert #20908 that was reverted in the 2D version as part
  of #21653. This ensures that the Vector returned is always perpendicular
  to the surface collided with; and not the floor_normal Vector passed to
  the function when on a floor.
- Include an update of the floor velocity before multiplying by the time
  delta, which was added to the 2D version as part of commit 13a8014.

In the 2D version:
- Use the Vector2.slide() function instead of Vector2.tangent() to adjust
  the amount of motion the stop_on_slope undoes to ensure that it is in the
  right direction. This is a implementation of the 3D approach from #30588.
- Combine the !found_collision and motion == Vector2() checks for break.
- Other minor formating changes to make the functions look identical.

Also renamed some variables to align with their use.

2 files changed, 34 insertions, 39 deletions

diff --git a/scene/2d/physics_body_2d.cpp b/scene/2d/physics_body_2d.cpp
index 3a4f397fe0..a57bfd4cbe 100644
--- a/scene/2d/physics_body_2d.cpp
+++ b/scene/2d/physics_body_2d.cpp
@@ -1214,18 +1214,20 @@ bool KinematicBody2D::move_and_collide(const Vector2 &p_motion, bool p_infinite_
 
 Vector2 KinematicBody2D::move_and_slide(const Vector2 &p_linear_velocity, const Vector2 &p_floor_direction, bool p_stop_on_slope, int p_max_slides, float p_floor_max_angle, bool p_infinite_inertia) {
 
-	Vector2 floor_motion = floor_velocity;
+	Vector2 body_velocity = p_linear_velocity;
+	Vector2 body_velocity_normal = body_velocity.normalized();
+
+	Vector2 current_floor_velocity = floor_velocity;
 	if (on_floor && on_floor_body.is_valid()) {
 		//this approach makes sure there is less delay between the actual body velocity and the one we saved
 		Physics2DDirectBodyState *bs = Physics2DServer::get_singleton()->body_get_direct_state(on_floor_body);
 		if (bs) {
-			floor_motion = bs->get_linear_velocity();
+			current_floor_velocity = bs->get_linear_velocity();
 		}
 	}
 
 	// Hack in order to work with calling from _process as well as from _physics_process; calling from thread is risky
-	Vector2 motion = (floor_motion + p_linear_velocity) * (Engine::get_singleton()->is_in_physics_frame() ? get_physics_process_delta_time() : get_process_delta_time());
-	Vector2 lv = p_linear_velocity;
+	Vector2 motion = (current_floor_velocity + body_velocity) * (Engine::get_singleton()->is_in_physics_frame() ? get_physics_process_delta_time() : get_process_delta_time());
 
 	on_floor = false;
 	on_floor_body = RID();
@@ -1234,14 +1236,12 @@ Vector2 KinematicBody2D::move_and_slide(const Vector2 &p_linear_velocity, const
 	colliders.clear();
 	floor_velocity = Vector2();
 
-	Vector2 lv_n = p_linear_velocity.normalized();
-
 	while (p_max_slides) {
 
 		Collision collision;
 		bool found_collision = false;
 
-		for (int i = 0; i < 2; i++) {
+		for (int i = 0; i < 2; ++i) {
 			bool collided;
 			if (i == 0) { //collide
 				collided = move_and_collide(motion, p_infinite_inertia, collision);
@@ -1273,14 +1273,13 @@ Vector2 KinematicBody2D::move_and_slide(const Vector2 &p_linear_velocity, const
 						floor_velocity = collision.collider_vel;
 
 						if (p_stop_on_slope) {
-							if ((lv_n + p_floor_direction).length() < 0.01 && collision.travel.length() < 1) {
+							if ((body_velocity_normal + p_floor_direction).length() < 0.01 && collision.travel.length() < 1) {
 								Transform2D gt = get_global_transform();
-								gt.elements[2] -= collision.travel.project(p_floor_direction.tangent());
+								gt.elements[2] -= collision.travel.slide(p_floor_direction);
 								set_global_transform(gt);
 								return Vector2();
 							}
 						}
-
 					} else if (Math::acos(collision.normal.dot(-p_floor_direction)) <= p_floor_max_angle + FLOOR_ANGLE_THRESHOLD) { //ceiling
 						on_ceiling = true;
 					} else {
@@ -1288,21 +1287,18 @@ Vector2 KinematicBody2D::move_and_slide(const Vector2 &p_linear_velocity, const
 					}
 				}
 
-				Vector2 n = collision.normal;
-				motion = motion.slide(n);
-				lv = lv.slide(n);
+				motion = motion.slide(collision.normal);
+				body_velocity = body_velocity.slide(collision.normal);
 			}
 		}
 
-		if (!found_collision) {
-			break;
-		}
-		p_max_slides--;
-		if (motion == Vector2())
+		if (!found_collision || motion == Vector2())
 			break;
+
+		--p_max_slides;
 	}
 
-	return lv;
+	return body_velocity;
 }
 
 Vector2 KinematicBody2D::move_and_slide_with_snap(const Vector2 &p_linear_velocity, const Vector2 &p_snap, const Vector2 &p_floor_direction, bool p_stop_on_slope, int p_max_slides, float p_floor_max_angle, bool p_infinite_inertia) {
diff --git a/scene/3d/physics_body.cpp b/scene/3d/physics_body.cpp
index 2c0e6ee744..6049b6cdb4 100644
--- a/scene/3d/physics_body.cpp
+++ b/scene/3d/physics_body.cpp
@@ -1142,25 +1142,34 @@ bool KinematicBody::move_and_collide(const Vector3 &p_motion, bool p_infinite_in
 
 Vector3 KinematicBody::move_and_slide(const Vector3 &p_linear_velocity, const Vector3 &p_floor_direction, bool p_stop_on_slope, int p_max_slides, float p_floor_max_angle, bool p_infinite_inertia) {
 
-	Vector3 lv = p_linear_velocity;
+	Vector3 body_velocity = p_linear_velocity;
+	Vector3 body_velocity_normal = body_velocity.normalized();
 
 	for (int i = 0; i < 3; i++) {
 		if (locked_axis & (1 << i)) {
-			lv[i] = 0;
+			body_velocity[i] = 0;
+		}
+	}
+
+	Vector3 current_floor_velocity = floor_velocity;
+	if (on_floor && on_floor_body.is_valid()) {
+		//this approach makes sure there is less delay between the actual body velocity and the one we saved
+		PhysicsDirectBodyState *bs = PhysicsServer::get_singleton()->body_get_direct_state(on_floor_body);
+		if (bs) {
+			current_floor_velocity = bs->get_linear_velocity();
 		}
 	}
 
 	// Hack in order to work with calling from _process as well as from _physics_process; calling from thread is risky
-	Vector3 motion = (floor_velocity + lv) * (Engine::get_singleton()->is_in_physics_frame() ? get_physics_process_delta_time() : get_process_delta_time());
+	Vector3 motion = (current_floor_velocity + body_velocity) * (Engine::get_singleton()->is_in_physics_frame() ? get_physics_process_delta_time() : get_process_delta_time());
 
 	on_floor = false;
+	on_floor_body = RID();
 	on_ceiling = false;
 	on_wall = false;
 	colliders.clear();
 	floor_velocity = Vector3();
 
-	Vector3 lv_n = p_linear_velocity.normalized();
-
 	while (p_max_slides) {
 
 		Collision collision;
@@ -1187,7 +1196,6 @@ Vector3 KinematicBody::move_and_slide(const Vector3 &p_linear_velocity, const Ve
 				colliders.push_back(collision);
 				motion = collision.remainder;
 
-				bool is_on_slope = false;
 				if (p_floor_direction == Vector3()) {
 					//all is a wall
 					on_wall = true;
@@ -1199,16 +1207,13 @@ Vector3 KinematicBody::move_and_slide(const Vector3 &p_linear_velocity, const Ve
 						floor_velocity = collision.collider_vel;
 
 						if (p_stop_on_slope) {
-							if ((lv_n + p_floor_direction).length() < 0.01 && collision.travel.length() < 1) {
+							if ((body_velocity_normal + p_floor_direction).length() < 0.01 && collision.travel.length() < 1) {
 								Transform gt = get_global_transform();
 								gt.origin -= collision.travel.slide(p_floor_direction);
 								set_global_transform(gt);
 								return Vector3();
 							}
 						}
-
-						is_on_slope = true;
-
 					} else if (Math::acos(collision.normal.dot(-p_floor_direction)) <= p_floor_max_angle + FLOOR_ANGLE_THRESHOLD) { //ceiling
 						on_ceiling = true;
 					} else {
@@ -1216,18 +1221,12 @@ Vector3 KinematicBody::move_and_slide(const Vector3 &p_linear_velocity, const Ve
 					}
 				}
 
-				if (p_stop_on_slope && is_on_slope) {
-					motion = motion.slide(p_floor_direction);
-					lv = lv.slide(p_floor_direction);
-				} else {
-					Vector3 n = collision.normal;
-					motion = motion.slide(n);
-					lv = lv.slide(n);
-				}
+				motion = motion.slide(collision.normal);
+				body_velocity = body_velocity.slide(collision.normal);
 
 				for (int j = 0; j < 3; j++) {
 					if (locked_axis & (1 << j)) {
-						lv[j] = 0;
+						body_velocity[j] = 0;
 					}
 				}
 			}
@@ -1239,7 +1238,7 @@ Vector3 KinematicBody::move_and_slide(const Vector3 &p_linear_velocity, const Ve
 		--p_max_slides;
 	}
 
-	return lv;
+	return body_velocity;
 }
 
 Vector3 KinematicBody::move_and_slide_with_snap(const Vector3 &p_linear_velocity, const Vector3 &p_snap, const Vector3 &p_floor_direction, bool p_stop_on_slope, int p_max_slides, float p_floor_max_angle, bool p_infinite_inertia) {