From ee464f56c440ee158fe28f7d3b750280bc63890d Mon Sep 17 00:00:00 2001 From: Juan Linietsky Date: Sat, 16 Feb 2019 13:45:01 -0300 Subject: [PATCH] Add a minimum treshold for acquiring rest contacts to avoid numerical precision issues. Fixes #25074 --- scene/3d/physics_body.cpp | 1 + servers/physics/space_sw.cpp | 8 ++++++++ servers/physics/space_sw.h | 1 + servers/physics_2d/space_2d_sw.cpp | 10 ++++++++++ servers/physics_2d/space_2d_sw.h | 1 + servers/physics_2d_server.cpp | 1 + servers/physics_2d_server.h | 1 + servers/physics_server.cpp | 2 ++ servers/physics_server.h | 1 + 9 files changed, 26 insertions(+) diff --git a/scene/3d/physics_body.cpp b/scene/3d/physics_body.cpp index 980eac77788..4ac2049b911 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 d1e1d1f8a27..250be0cd7a2 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 ae6f349e518..3aaa552845a 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 bf7adb62a44..44878195856 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 64f8c9e1568..14c24959b76 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 ca914b67e05..3a05791f17b 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 dbf50425ff7..1de9c7df939 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 967e74d3221..e07133b5bb5 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 f2d4c941bf1..c71bb019430 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;