Improved logic for CharacterBody collision recovery depth

Allows 2D character controller to work without applying gravity when
touching the ground (also more safely in 3D), and collision detection
is more flexible with different safe margin values.

Character body motion changes in 2D and 3D:
-Recovery only for depth > min contact depth to help with collision
detection consistency (rest info could be lost if recovery was too much)
-Adaptive min contact depth (based on margin) instead of space parameter

Extra CharacterBody changes:
-2D: apply changes made in 3D for stop on slope and floor snap that help
fixing some jittering cases
-3D: fix minor inconsistencies in stop on slope and floor snap logic

1 files changed, 9 insertions, 5 deletions

diff --git a/scene/3d/physics_body_3d.cpp b/scene/3d/physics_body_3d.cpp
index 7bd0a89eb3..5cba04eae0 100644
--- a/scene/3d/physics_body_3d.cpp
+++ b/scene/3d/physics_body_3d.cpp
@@ -1145,8 +1145,7 @@ void CharacterBody3D::_move_and_slide_grounded(double p_delta, bool p_was_on_flo
 
 			if (collision_state.floor && floor_stop_on_slope && (linear_velocity.normalized() + up_direction).length() < 0.01) {
 				Transform3D gt = get_global_transform();
-				real_t travel_total = result.travel.length();
-				if (travel_total <= margin + CMP_EPSILON) {
+				if (result.travel.length() <= margin + CMP_EPSILON) {
 					gt.origin -= result.travel;
 				}
 				set_global_transform(gt);
@@ -1185,7 +1184,7 @@ void CharacterBody3D::_move_and_slide_grounded(double p_delta, bool p_was_on_flo
 							Transform3D gt = get_global_transform();
 							real_t travel_total = result.travel.length();
 							real_t cancel_dist_max = MIN(0.1, margin * 20);
-							if (travel_total < margin + CMP_EPSILON) {
+							if (travel_total <= margin + CMP_EPSILON) {
 								gt.origin -= result.travel;
 							} else if (travel_total < cancel_dist_max) { // If the movement is large the body can be prevented from reaching the walls.
 								gt.origin -= result.travel.slide(up_direction);
@@ -1376,7 +1375,9 @@ void CharacterBody3D::_snap_on_floor(bool was_on_floor, bool vel_dir_facing_up)
 		return;
 	}
 
+	// Snap by at least collision margin to keep floor state consistent.
 	real_t length = MAX(floor_snap_length, margin);
+
 	Transform3D gt = get_global_transform();
 	PhysicsServer3D::MotionResult result;
 	if (move_and_collide(-up_direction * length, result, margin, true, 4, false, true)) {
@@ -1402,12 +1403,15 @@ void CharacterBody3D::_snap_on_floor(bool was_on_floor, bool vel_dir_facing_up)
 }
 
 bool CharacterBody3D::_on_floor_if_snapped(bool was_on_floor, bool vel_dir_facing_up) {
-	if (Math::is_zero_approx(floor_snap_length) || up_direction == Vector3() || collision_state.floor || !was_on_floor || vel_dir_facing_up) {
+	if (up_direction == Vector3() || collision_state.floor || !was_on_floor || vel_dir_facing_up) {
 		return false;
 	}
 
+	// Snap by at least collision margin to keep floor state consistent.
+	real_t length = MAX(floor_snap_length, margin);
+
 	PhysicsServer3D::MotionResult result;
-	if (move_and_collide(-up_direction * floor_snap_length, result, margin, true, 4, false, true)) {
+	if (move_and_collide(-up_direction * length, result, margin, true, 4, false, true)) {
 		CollisionState result_state;
 		// Don't apply direction for any type.
 		_set_collision_direction(result, result_state, CollisionState());