Fix move_and_collide causing sliding on slopes

Make sure the direction of the motion is preserved, unless the depth is
higher than the margin, which means the body needs depenetration in any
direction.

Also changed move_and_slide to avoid sliding on the first motion, in
order to avoid issues with unstable position on ground when jumping.

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

2 files changed, 51 insertions, 14 deletions

diff --git a/scene/3d/physics_body_3d.cpp b/scene/3d/physics_body_3d.cpp
index 28a0c72fe3..c9afb5f201 100644
--- a/scene/3d/physics_body_3d.cpp
+++ b/scene/3d/physics_body_3d.cpp
@@ -118,10 +118,43 @@ 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 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) {
 	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);
 
+	// Restore direction of motion to be along original motion,
+	// in order to avoid sliding due to recovery,
+	// but only if collision depth is low enough to avoid tunneling.
+	real_t motion_length = p_motion.length();
+	if (motion_length > CMP_EPSILON) {
+		real_t precision = CMP_EPSILON;
+
+		if (colliding && p_cancel_sliding) {
+			// Can't just use margin as a threshold because collision depth is calculated on unsafe motion,
+			// so even in normal resting cases the depth can be a bit more than the margin.
+			precision += motion_length * (r_result.collision_unsafe_fraction - r_result.collision_safe_fraction);
+
+			if (r_result.collision_depth > (real_t)p_margin + precision) {
+				p_cancel_sliding = false;
+			}
+		}
+
+		if (p_cancel_sliding) {
+			// Check depth of recovery.
+			Vector3 motion_normal = p_motion / motion_length;
+			real_t dot = r_result.motion.dot(motion_normal);
+			Vector3 recovery = r_result.motion - motion_normal * dot;
+			real_t recovery_length = recovery.length();
+			// Fixes cases where canceling slide causes the motion to go too deep into the ground,
+			// Becauses we're only taking rest information into account and not general recovery.
+			if (recovery_length < (real_t)p_margin + precision) {
+				// Apply adjustment to motion.
+				r_result.motion = motion_normal * dot;
+				r_result.remainder = p_motion - r_result.motion;
+			}
+		}
+	}
+
 	for (int i = 0; i < 3; i++) {
 		if (locked_axis & (1 << i)) {
 			r_result.motion[i] = 0;
@@ -978,15 +1011,16 @@ void CharacterBody3D::move_and_slide() {
 	floor_normal = Vector3();
 	floor_velocity = Vector3();
 
-	int slide_count = max_slides;
-	while (slide_count) {
+	// No sliding on first attempt to keep motion stable when possible.
+	bool sliding_enabled = false;
+	for (int iteration = 0; iteration < max_slides; ++iteration) {
 		PhysicsServer3D::MotionResult result;
 		bool found_collision = false;
 
 		for (int i = 0; i < 2; ++i) {
 			bool collided;
 			if (i == 0) { //collide
-				collided = move_and_collide(motion, infinite_inertia, result, margin);
+				collided = move_and_collide(motion, infinite_inertia, result, margin, true, false, !sliding_enabled);
 				if (!collided) {
 					motion = Vector3(); //clear because no collision happened and motion completed
 				}
@@ -1002,7 +1036,6 @@ void CharacterBody3D::move_and_slide() {
 				found_collision = true;
 
 				motion_results.push_back(result);
-				motion = result.remainder;
 
 				if (up_direction == Vector3()) {
 					//all is a wall
@@ -1016,7 +1049,7 @@ void CharacterBody3D::move_and_slide() {
 						floor_velocity = result.collider_velocity;
 
 						if (stop_on_slope) {
-							if ((body_velocity_normal + up_direction).length() < 0.01 && result.motion.length() < 1) {
+							if ((body_velocity_normal + up_direction).length() < 0.01) {
 								Transform3D gt = get_global_transform();
 								gt.origin -= result.motion.slide(up_direction);
 								set_global_transform(gt);
@@ -1031,22 +1064,26 @@ void CharacterBody3D::move_and_slide() {
 					}
 				}
 
-				motion = motion.slide(result.collision_normal);
-				linear_velocity = linear_velocity.slide(result.collision_normal);
+				if (sliding_enabled || !on_floor) {
+					motion = result.remainder.slide(result.collision_normal);
+					linear_velocity = linear_velocity.slide(result.collision_normal);
 
-				for (int j = 0; j < 3; j++) {
-					if (locked_axis & (1 << j)) {
-						linear_velocity[j] = 0.0;
+					for (int j = 0; j < 3; j++) {
+						if (locked_axis & (1 << j)) {
+							linear_velocity[j] = 0.0;
+						}
 					}
+				} else {
+					motion = result.remainder;
 				}
 			}
+
+			sliding_enabled = true;
 		}
 
 		if (!found_collision || motion == Vector3()) {
 			break;
 		}
-
-		--slide_count;
 	}
 
 	if (!was_on_floor || snap == Vector3()) {
diff --git a/scene/3d/physics_body_3d.h b/scene/3d/physics_body_3d.h
index 7d7adf1624..bc4556494d 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 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 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);