Merge pull request #52953 from nekomatata/fix-collision-recovery-depth

doc/classes/CharacterBody2D.xml

@@ -133,7 +133,7 @@
 		<member name="floor_max_angle" type="float" setter="set_floor_max_angle" getter="get_floor_max_angle" default="0.785398">
 			Maximum angle (in radians) where a slope is still considered a floor (or a ceiling), rather than a wall, when calling [method move_and_slide]. The default value equals 45 degrees.
 		</member>
-		<member name="floor_snap_length" type="float" setter="set_floor_snap_length" getter="get_floor_snap_length" default="0.0">
+		<member name="floor_snap_length" type="float" setter="set_floor_snap_length" getter="get_floor_snap_length" default="1.0">
 			Sets a snapping distance. When set to a value different from [code]0.0[/code], the body is kept attached to slopes when calling [method move_and_slide]. The snapping vector is determined by the given distance along the opposite direction of the [member up_direction].
 			As long as the snapping vector is in contact with the ground and the body moves against `up_direction`, the body will remain attached to the surface. Snapping is not applied if the body moves along `up_direction`, so it will be able to detach from the ground when jumping.
 		</member>

doc/classes/PhysicsServer2D.xml

@@ -851,8 +851,6 @@
 		<constant name="SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS" value="6" enum="SpaceParameter">
 			Constant to set/get the default solver bias for all physics constraints. A solver bias is a factor controlling how much two objects "rebound", after violating a constraint, to avoid leaving them in that state because of numerical imprecision.
 		</constant>
-		<constant name="SPACE_PARAM_TEST_MOTION_MIN_CONTACT_DEPTH" value="7" enum="SpaceParameter">
-		</constant>
 		<constant name="SHAPE_WORLD_BOUNDARY" value="0" enum="ShapeType">
 			This is the constant for creating world boundary shapes. A world boundary shape is an [i]infinite[/i] line with an origin point, and a normal. Thus, it can be used for front/behind checks.
 		</constant>

doc/classes/PhysicsServer3D.xml

@@ -1361,8 +1361,6 @@
 		<constant name="SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS" value="7" enum="SpaceParameter">
 			Constant to set/get the default solver bias for all physics constraints. A solver bias is a factor controlling how much two objects "rebound", after violating a constraint, to avoid leaving them in that state because of numerical imprecision.
 		</constant>
-		<constant name="SPACE_PARAM_TEST_MOTION_MIN_CONTACT_DEPTH" value="8" enum="SpaceParameter">
-		</constant>
 		<constant name="BODY_AXIS_LINEAR_X" value="1" enum="BodyAxis">
 		</constant>
 		<constant name="BODY_AXIS_LINEAR_Y" value="2" enum="BodyAxis">

scene/2d/physics_body_2d.cpp

@@ -1090,9 +1090,7 @@ void CharacterBody2D::_move_and_slide_grounded(double p_delta, bool p_was_on_flo
 
 			if (on_floor && floor_stop_on_slope && (linear_velocity.normalized() + up_direction).length() < 0.01) {
 				Transform2D gt = get_global_transform();
-				if (result.travel.length() > margin) {
-					gt.elements[2] -= result.travel.slide(up_direction);
-				} else {
+				if (result.travel.length() <= margin + CMP_EPSILON) {
 					gt.elements[2] -= result.travel;
 				}
 				set_global_transform(gt);
@@ -1111,7 +1109,7 @@ void CharacterBody2D::_move_and_slide_grounded(double p_delta, bool p_was_on_flo
 				// Avoid to move forward on a wall if floor_block_on_wall is true.
 				if (p_was_on_floor && !on_floor && !vel_dir_facing_up) {
 					// If the movement is large the body can be prevented from reaching the walls.
-					if (result.travel.length() <= margin) {
+					if (result.travel.length() <= margin + CMP_EPSILON) {
 						// Cancels the motion.
 						Transform2D gt = get_global_transform();
 						gt.elements[2] -= result.travel;
@@ -1240,13 +1238,16 @@ void CharacterBody2D::_move_and_slide_free(double p_delta) {
 }
 
 void CharacterBody2D::_snap_on_floor(bool was_on_floor, bool vel_dir_facing_up) {
-	if (Math::is_equal_approx(floor_snap_length, 0) || on_floor || !was_on_floor || vel_dir_facing_up) {
+	if (on_floor || !was_on_floor || vel_dir_facing_up) {
 		return;
 	}
 
+	// Snap by at least collision margin to keep floor state consistent.
+	real_t length = MAX(floor_snap_length, margin);
+
 	Transform2D gt = get_global_transform();
 	PhysicsServer2D::MotionResult result;
-	if (move_and_collide(up_direction * -floor_snap_length, result, margin, true, false, true)) {
+	if (move_and_collide(-up_direction * length, result, margin, true, false, true)) {
 		bool apply = true;
 		if (result.get_angle(up_direction) <= floor_max_angle + FLOOR_ANGLE_THRESHOLD) {
 			on_floor = true;
@@ -1274,12 +1275,15 @@ void CharacterBody2D::_snap_on_floor(bool was_on_floor, bool vel_dir_facing_up)
 }
 
 bool CharacterBody2D::_on_floor_if_snapped(bool was_on_floor, bool vel_dir_facing_up) {
-	if (Math::is_equal_approx(floor_snap_length, 0) || up_direction == Vector2() || on_floor || !was_on_floor || vel_dir_facing_up) {
+	if (up_direction == Vector2() || on_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);
+
 	PhysicsServer2D::MotionResult result;
-	if (move_and_collide(up_direction * -floor_snap_length, result, margin, true, false, true)) {
+	if (move_and_collide(-up_direction * length, result, margin, true, false, true)) {
 		if (result.get_angle(up_direction) <= floor_max_angle + FLOOR_ANGLE_THRESHOLD) {
 			return true;
 		}
@@ -1568,7 +1572,7 @@ void CharacterBody2D::_bind_methods() {
 	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "floor_constant_speed"), "set_floor_constant_speed_enabled", "is_floor_constant_speed_enabled");
 	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "floor_block_on_wall"), "set_floor_block_on_wall_enabled", "is_floor_block_on_wall_enabled");
 	ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "floor_max_angle", PROPERTY_HINT_RANGE, "0,180,0.1,radians"), "set_floor_max_angle", "get_floor_max_angle");
-	ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "floor_snap_length", PROPERTY_HINT_RANGE, "0,1000,0.1"), "set_floor_snap_length", "get_floor_snap_length");
+	ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "floor_snap_length", PROPERTY_HINT_RANGE, "0,32,0.1,or_greater"), "set_floor_snap_length", "get_floor_snap_length");
 	ADD_GROUP("Moving platform", "moving_platform");
 	ADD_PROPERTY(PropertyInfo(Variant::INT, "moving_platform_floor_layers", PROPERTY_HINT_LAYERS_2D_PHYSICS), "set_moving_platform_floor_layers", "get_moving_platform_floor_layers");
 	ADD_PROPERTY(PropertyInfo(Variant::INT, "moving_platform_wall_layers", PROPERTY_HINT_LAYERS_2D_PHYSICS), "set_moving_platform_wall_layers", "get_moving_platform_wall_layers");

scene/2d/physics_body_2d.h

@@ -334,7 +334,7 @@ private:
 	int max_slides = 4;
 	int platform_layer;
 	real_t floor_max_angle = Math::deg2rad((real_t)45.0);
-	real_t floor_snap_length = 0;
+	real_t floor_snap_length = 1;
 	real_t free_mode_min_slide_angle = Math::deg2rad((real_t)15.0);
 	Vector2 up_direction = Vector2(0.0, -1.0);
 	uint32_t moving_platform_floor_layers = UINT32_MAX;

scene/3d/physics_body_3d.cpp

@@ -1146,8 +1146,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);
@@ -1186,7 +1185,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);
@@ -1377,7 +1376,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)) {
@@ -1403,12 +1404,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());

servers/physics_2d/space_2d_sw.cpp

@@ -34,6 +34,9 @@
 #include "core/os/os.h"
 #include "core/templates/pair.h"
 #include "physics_server_2d_sw.h"
+
+#define TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR 0.05
+
 _FORCE_INLINE_ static bool _can_collide_with(CollisionObject2DSW *p_object, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) {
 	if (!(p_object->get_collision_layer() & p_collision_mask)) {
 		return false;
@@ -435,6 +438,8 @@ bool PhysicsDirectSpaceState2DSW::rest_info(RID p_shape, const Transform2D &p_sh
 	Shape2DSW *shape = PhysicsServer2DSW::singletonsw->shape_owner.getornull(p_shape);
 	ERR_FAIL_COND_V(!shape, 0);
 
+	real_t min_contact_depth = p_margin * TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR;
+
 	Rect2 aabb = p_shape_xform.xform(shape->get_aabb());
 	aabb = aabb.merge(Rect2(aabb.position + p_motion, aabb.size)); //motion
 	aabb = aabb.grow(p_margin);
@@ -445,7 +450,7 @@ bool PhysicsDirectSpaceState2DSW::rest_info(RID p_shape, const Transform2D &p_sh
 	rcd.best_len = 0;
 	rcd.best_object = nullptr;
 	rcd.best_shape = 0;
-	rcd.min_allowed_depth = space->test_motion_min_contact_depth;
+	rcd.min_allowed_depth = min_contact_depth;
 
 	for (int i = 0; i < amount; i++) {
 		if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) {
@@ -569,6 +574,8 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
 	ExcludedShapeSW excluded_shape_pairs[max_excluded_shape_pairs];
 	int excluded_shape_pair_count = 0;
 
+	real_t min_contact_depth = p_margin * TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR;
+
 	real_t motion_length = p_motion.length();
 	Vector2 motion_normal = p_motion / motion_length;
 
@@ -671,6 +678,8 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
 				break;
 			}
 
+			recovered = true;
+
 			Vector2 recover_motion;
 			for (int i = 0; i < cbk.amount; i++) {
 				Vector2 a = sr[i * 2 + 0];
@@ -682,9 +691,9 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
 
 				// Compute depth on recovered motion.
 				real_t depth = n.dot(a + recover_motion) - d;
-				if (depth > 0.0) {
+				if (depth > min_contact_depth + CMP_EPSILON) {
 					// Only recover if there is penetration.
-					recover_motion -= n * depth * 0.4;
+					recover_motion -= n * (depth - min_contact_depth) * 0.4;
 				}
 			}
 
@@ -693,8 +702,6 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
 				break;
 			}
 
-			recovered = true;
-
 			body_transform.elements[2] += recover_motion;
 			body_aabb.position += recover_motion;
 
@@ -870,7 +877,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
 		rcd.best_shape = 0;
 
 		// Allowed depth can't be lower than motion length, in order to handle contacts at low speed.
-		rcd.min_allowed_depth = MIN(motion_length, test_motion_min_contact_depth);
+		rcd.min_allowed_depth = MIN(motion_length, min_contact_depth);
 
 		int from_shape = best_shape != -1 ? best_shape : 0;
 		int to_shape = best_shape != -1 ? best_shape + 1 : p_body->get_shape_count();
@@ -1141,9 +1148,6 @@ void Space2DSW::set_param(PhysicsServer2D::SpaceParameter p_param, real_t p_valu
 		case PhysicsServer2D::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS:
 			constraint_bias = p_value;
 			break;
-		case PhysicsServer2D::SPACE_PARAM_TEST_MOTION_MIN_CONTACT_DEPTH:
-			test_motion_min_contact_depth = p_value;
-			break;
 	}
 }
 
@@ -1163,8 +1167,6 @@ real_t Space2DSW::get_param(PhysicsServer2D::SpaceParameter p_param) const {
 			return body_time_to_sleep;
 		case PhysicsServer2D::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS:
 			return constraint_bias;
-		case PhysicsServer2D::SPACE_PARAM_TEST_MOTION_MIN_CONTACT_DEPTH:
-			return test_motion_min_contact_depth;
 	}
 	return 0;
 }

servers/physics_2d/space_2d_sw.h

@@ -102,7 +102,6 @@ private:
 	real_t contact_max_separation = 1.5;
 	real_t contact_max_allowed_penetration = 0.3;
 	real_t constraint_bias = 0.2;
-	real_t test_motion_min_contact_depth = 0.005;
 
 	enum {
 		INTERSECTION_QUERY_MAX = 2048

servers/physics_3d/space_3d_sw.cpp

@@ -34,6 +34,8 @@
 #include "core/config/project_settings.h"
 #include "physics_server_3d_sw.h"
 
+#define TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR 0.05
+
 _FORCE_INLINE_ static bool _can_collide_with(CollisionObject3DSW *p_object, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) {
 	if (!(p_object->get_collision_layer() & p_collision_mask)) {
 		return false;
@@ -488,13 +490,15 @@ bool PhysicsDirectSpaceState3DSW::rest_info(RID p_shape, const Transform3D &p_sh
 	Shape3DSW *shape = PhysicsServer3DSW::singletonsw->shape_owner.getornull(p_shape);
 	ERR_FAIL_COND_V(!shape, 0);
 
+	real_t min_contact_depth = p_margin * TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR;
+
 	AABB aabb = p_shape_xform.xform(shape->get_aabb());
 	aabb = aabb.grow(p_margin);
 
 	int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, Space3DSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
 
 	_RestCallbackData rcd;
-	rcd.min_allowed_depth = space->test_motion_min_contact_depth;
+	rcd.min_allowed_depth = min_contact_depth;
 
 	for (int i = 0; i < amount; i++) {
 		if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) {
@@ -658,6 +662,8 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, co
 	body_aabb = p_from.xform(p_body->get_inv_transform().xform(body_aabb));
 	body_aabb = body_aabb.grow(p_margin);
 
+	real_t min_contact_depth = p_margin * TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR;
+
 	real_t motion_length = p_motion.length();
 	Vector3 motion_normal = p_motion / motion_length;
 
@@ -711,8 +717,9 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, co
 				break;
 			}
 
-			Vector3 recover_motion;
+			recovered = true;
 
+			Vector3 recover_motion;
 			for (int i = 0; i < cbk.amount; i++) {
 				Vector3 a = sr[i * 2 + 0];
 				Vector3 b = sr[i * 2 + 1];
@@ -723,9 +730,9 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, co
 
 				// Compute depth on recovered motion.
 				real_t depth = n.dot(a + recover_motion) - d;
-				if (depth > 0.0) {
+				if (depth > min_contact_depth + CMP_EPSILON) {
 					// Only recover if there is penetration.
-					recover_motion -= n * depth * 0.4;
+					recover_motion -= n * (depth - min_contact_depth) * 0.4;
 				}
 			}
 
@@ -734,8 +741,6 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, co
 				break;
 			}
 
-			recovered = true;
-
 			body_transform.origin += recover_motion;
 			body_aabb.position += recover_motion;
 
@@ -889,7 +894,7 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, co
 		}
 
 		// Allowed depth can't be lower than motion length, in order to handle contacts at low speed.
-		rcd.min_allowed_depth = MIN(motion_length, test_motion_min_contact_depth);
+		rcd.min_allowed_depth = MIN(motion_length, min_contact_depth);
 
 		int from_shape = best_shape != -1 ? best_shape : 0;
 		int to_shape = best_shape != -1 ? best_shape + 1 : p_body->get_shape_count();
@@ -1158,9 +1163,6 @@ void Space3DSW::set_param(PhysicsServer3D::SpaceParameter p_param, real_t p_valu
 		case PhysicsServer3D::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS:
 			constraint_bias = p_value;
 			break;
-		case PhysicsServer3D::SPACE_PARAM_TEST_MOTION_MIN_CONTACT_DEPTH:
-			test_motion_min_contact_depth = p_value;
-			break;
 	}
 }
 
@@ -1182,8 +1184,6 @@ real_t Space3DSW::get_param(PhysicsServer3D::SpaceParameter p_param) const {
 			return body_angular_velocity_damp_ratio;
 		case PhysicsServer3D::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS:
 			return constraint_bias;
-		case PhysicsServer3D::SPACE_PARAM_TEST_MOTION_MIN_CONTACT_DEPTH:
-			return test_motion_min_contact_depth;
 	}
 	return 0;
 }

servers/physics_3d/space_3d_sw.h

@@ -96,7 +96,6 @@ private:
 	real_t contact_max_separation = 0.05;
 	real_t contact_max_allowed_penetration = 0.01;
 	real_t constraint_bias = 0.01;
-	real_t test_motion_min_contact_depth = 0.00001;
 
 	enum {
 		INTERSECTION_QUERY_MAX = 2048

servers/physics_server_2d.cpp

@@ -670,7 +670,6 @@ void PhysicsServer2D::_bind_methods() {
 	BIND_ENUM_CONSTANT(SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD);
 	BIND_ENUM_CONSTANT(SPACE_PARAM_BODY_TIME_TO_SLEEP);
 	BIND_ENUM_CONSTANT(SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS);
-	BIND_ENUM_CONSTANT(SPACE_PARAM_TEST_MOTION_MIN_CONTACT_DEPTH);
 
 	BIND_ENUM_CONSTANT(SHAPE_WORLD_BOUNDARY);
 	BIND_ENUM_CONSTANT(SHAPE_SEPARATION_RAY);

servers/physics_server_2d.h

@@ -262,7 +262,6 @@ public:
 		SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD,
 		SPACE_PARAM_BODY_TIME_TO_SLEEP,
 		SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS,
-		SPACE_PARAM_TEST_MOTION_MIN_CONTACT_DEPTH,
 	};
 
 	virtual void space_set_param(RID p_space, SpaceParameter p_param, real_t p_value) = 0;

servers/physics_server_3d.cpp

@@ -867,7 +867,6 @@ void PhysicsServer3D::_bind_methods() {
 	BIND_ENUM_CONSTANT(SPACE_PARAM_BODY_TIME_TO_SLEEP);
 	BIND_ENUM_CONSTANT(SPACE_PARAM_BODY_ANGULAR_VELOCITY_DAMP_RATIO);
 	BIND_ENUM_CONSTANT(SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS);
-	BIND_ENUM_CONSTANT(SPACE_PARAM_TEST_MOTION_MIN_CONTACT_DEPTH);
 
 	BIND_ENUM_CONSTANT(BODY_AXIS_LINEAR_X);
 	BIND_ENUM_CONSTANT(BODY_AXIS_LINEAR_Y);

servers/physics_server_3d.h

@@ -271,7 +271,6 @@ public:
 		SPACE_PARAM_BODY_TIME_TO_SLEEP,
 		SPACE_PARAM_BODY_ANGULAR_VELOCITY_DAMP_RATIO,
 		SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS,
-		SPACE_PARAM_TEST_MOTION_MIN_CONTACT_DEPTH
 	};
 
 	virtual void space_set_param(RID p_space, SpaceParameter p_param, real_t p_value) = 0;