Merge pull request #49901 from nekomatata/move-and-collide-fix-slide

Fix move_and_collide causing sliding on slopes

2 files changed, 51 insertions, 14 deletions

diff --git a/scene/3d/physics_body_3d.cpp b/scene/3d/physics_body_3d.cpp
index 8e10a37afb..7c9245cd3e 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);