Add a minimum treshold for acquiring rest contacts to avoid numerical precision issues. Fixes #25074

9 files changed, 26 insertions, 0 deletions

diff --git a/scene/3d/physics_body.cpp b/scene/3d/physics_body.cpp
index 980eac7778..4ac2049b91 100644
--- a/scene/3d/physics_body.cpp
+++ b/scene/3d/physics_body.cpp
@@ -1243,6 +1243,7 @@ Vector3 KinematicBody::move_and_slide(const Vector3 &p_linear_velocity, const Ve
 					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);
diff --git a/servers/physics/space_sw.cpp b/servers/physics/space_sw.cpp
index d1e1d1f8a2..250be0cd7a 100644
--- a/servers/physics/space_sw.cpp
+++ b/servers/physics/space_sw.cpp
@@ -387,6 +387,7 @@ struct _RestCallbackData {
 	Vector3 best_contact;
 	Vector3 best_normal;
 	real_t best_len;
+	real_t min_allowed_depth;
 };
 
 static void _rest_cbk_result(const Vector3 &p_point_A, const Vector3 &p_point_B, void *p_userdata) {
@@ -395,6 +396,8 @@ static void _rest_cbk_result(const Vector3 &p_point_A, const Vector3 &p_point_B,
 
 	Vector3 contact_rel = p_point_B - p_point_A;
 	real_t len = contact_rel.length();
+	if (len < rd->min_allowed_depth)
+		return;
 	if (len <= rd->best_len)
 		return;
 
@@ -418,6 +421,7 @@ bool PhysicsDirectSpaceStateSW::rest_info(RID p_shape, const Transform &p_shape_
 	rcd.best_len = 0;
 	rcd.best_object = NULL;
 	rcd.best_shape = 0;
+	rcd.min_allowed_depth = space->test_motion_min_contact_depth;
 
 	for (int i = 0; i < amount; i++) {
 
@@ -929,6 +933,7 @@ bool SpaceSW::test_body_motion(BodySW *p_body, const Transform &p_from, const Ve
 		rcd.best_len = 0;
 		rcd.best_object = NULL;
 		rcd.best_shape = 0;
+		rcd.min_allowed_depth = test_motion_min_contact_depth;
 
 		Transform body_shape_xform = ugt * p_body->get_shape_transform(best_shape);
 		ShapeSW *body_shape = p_body->get_shape(best_shape);
@@ -1153,6 +1158,7 @@ void SpaceSW::set_param(PhysicsServer::SpaceParameter p_param, real_t p_value) {
 		case PhysicsServer::SPACE_PARAM_BODY_TIME_TO_SLEEP: body_time_to_sleep = p_value; break;
 		case PhysicsServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_DAMP_RATIO: body_angular_velocity_damp_ratio = p_value; break;
 		case PhysicsServer::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: constraint_bias = p_value; break;
+		case PhysicsServer::SPACE_PARAM_TEST_MOTION_MIN_CONTACT_DEPTH: test_motion_min_contact_depth = p_value; break;
 	}
 }
 
@@ -1168,6 +1174,7 @@ real_t SpaceSW::get_param(PhysicsServer::SpaceParameter p_param) const {
 		case PhysicsServer::SPACE_PARAM_BODY_TIME_TO_SLEEP: return body_time_to_sleep;
 		case PhysicsServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_DAMP_RATIO: return body_angular_velocity_damp_ratio;
 		case PhysicsServer::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: return constraint_bias;
+		case PhysicsServer::SPACE_PARAM_TEST_MOTION_MIN_CONTACT_DEPTH: return test_motion_min_contact_depth;
 	}
 	return 0;
 }
@@ -1203,6 +1210,7 @@ SpaceSW::SpaceSW() {
 	contact_recycle_radius = 0.01;
 	contact_max_separation = 0.05;
 	contact_max_allowed_penetration = 0.01;
+	test_motion_min_contact_depth = 0.00001;
 
 	constraint_bias = 0.01;
 	body_linear_velocity_sleep_threshold = GLOBAL_DEF("physics/3d/sleep_threshold_linear", 0.1);
diff --git a/servers/physics/space_sw.h b/servers/physics/space_sw.h
index ae6f349e51..3aaa552845 100644
--- a/servers/physics/space_sw.h
+++ b/servers/physics/space_sw.h
@@ -96,6 +96,7 @@ private:
 	real_t contact_max_separation;
 	real_t contact_max_allowed_penetration;
 	real_t constraint_bias;
+	real_t test_motion_min_contact_depth;
 
 	enum {
 
diff --git a/servers/physics_2d/space_2d_sw.cpp b/servers/physics_2d/space_2d_sw.cpp
index bf7adb62a4..4487819585 100644
--- a/servers/physics_2d/space_2d_sw.cpp
+++ b/servers/physics_2d/space_2d_sw.cpp
@@ -375,6 +375,7 @@ struct _RestCallbackData2D {
 	real_t best_len;
 	Vector2 valid_dir;
 	real_t valid_depth;
+	real_t min_allowed_depth;
 };
 
 static void _rest_cbk_result(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_userdata) {
@@ -390,6 +391,10 @@ static void _rest_cbk_result(const Vector2 &p_point_A, const Vector2 &p_point_B,
 
 	Vector2 contact_rel = p_point_B - p_point_A;
 	real_t len = contact_rel.length();
+
+	if (len < rd->min_allowed_depth)
+		return;
+
 	if (len <= rd->best_len)
 		return;
 
@@ -416,6 +421,7 @@ bool Physics2DDirectSpaceStateSW::rest_info(RID p_shape, const Transform2D &p_sh
 	rcd.best_len = 0;
 	rcd.best_object = NULL;
 	rcd.best_shape = 0;
+	rcd.min_allowed_depth = space->test_motion_min_contact_depth;
 
 	for (int i = 0; i < amount; i++) {
 
@@ -1003,6 +1009,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
 		rcd.best_len = 0;
 		rcd.best_object = NULL;
 		rcd.best_shape = 0;
+		rcd.min_allowed_depth = test_motion_min_contact_depth;
 
 		//optimization
 		int from_shape = best_shape != -1 ? best_shape : 0;
@@ -1262,6 +1269,7 @@ void Space2DSW::set_param(Physics2DServer::SpaceParameter p_param, real_t p_valu
 		case Physics2DServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD: body_angular_velocity_sleep_threshold = p_value; break;
 		case Physics2DServer::SPACE_PARAM_BODY_TIME_TO_SLEEP: body_time_to_sleep = p_value; break;
 		case Physics2DServer::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: constraint_bias = p_value; break;
+		case Physics2DServer::SPACE_PARAM_TEST_MOTION_MIN_CONTACT_DEPTH: test_motion_min_contact_depth = p_value; break;
 	}
 }
 
@@ -1276,6 +1284,7 @@ real_t Space2DSW::get_param(Physics2DServer::SpaceParameter p_param) const {
 		case Physics2DServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD: return body_angular_velocity_sleep_threshold;
 		case Physics2DServer::SPACE_PARAM_BODY_TIME_TO_SLEEP: return body_time_to_sleep;
 		case Physics2DServer::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: return constraint_bias;
+		case Physics2DServer::SPACE_PARAM_TEST_MOTION_MIN_CONTACT_DEPTH: return test_motion_min_contact_depth;
 	}
 	return 0;
 }
@@ -1312,6 +1321,7 @@ Space2DSW::Space2DSW() {
 	contact_recycle_radius = 1.0;
 	contact_max_separation = 1.5;
 	contact_max_allowed_penetration = 0.3;
+	test_motion_min_contact_depth = 0.005;
 
 	constraint_bias = 0.2;
 	body_linear_velocity_sleep_threshold = GLOBAL_DEF("physics/2d/sleep_threshold_linear", 2.0);
diff --git a/servers/physics_2d/space_2d_sw.h b/servers/physics_2d/space_2d_sw.h
index 64f8c9e156..14c24959b7 100644
--- a/servers/physics_2d/space_2d_sw.h
+++ b/servers/physics_2d/space_2d_sw.h
@@ -104,6 +104,7 @@ private:
 	real_t contact_max_separation;
 	real_t contact_max_allowed_penetration;
 	real_t constraint_bias;
+	real_t test_motion_min_contact_depth;
 
 	enum {
 
diff --git a/servers/physics_2d_server.cpp b/servers/physics_2d_server.cpp
index ca914b67e0..3a05791f17 100644
--- a/servers/physics_2d_server.cpp
+++ b/servers/physics_2d_server.cpp
@@ -693,6 +693,7 @@ void Physics2DServer::_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_LINE);
 	BIND_ENUM_CONSTANT(SHAPE_RAY);
diff --git a/servers/physics_2d_server.h b/servers/physics_2d_server.h
index dbf50425ff..1de9c7df93 100644
--- a/servers/physics_2d_server.h
+++ b/servers/physics_2d_server.h
@@ -289,6 +289,7 @@ 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;
diff --git a/servers/physics_server.cpp b/servers/physics_server.cpp
index 967e74d322..e07133b5bb 100644
--- a/servers/physics_server.cpp
+++ b/servers/physics_server.cpp
@@ -709,6 +709,8 @@ void PhysicsServer::_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);
 	BIND_ENUM_CONSTANT(BODY_AXIS_LINEAR_Z);
diff --git a/servers/physics_server.h b/servers/physics_server.h
index f2d4c941bf..c71bb01943 100644
--- a/servers/physics_server.h
+++ b/servers/physics_server.h
@@ -274,6 +274,7 @@ 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;