From 1061bb364ee9e7152a835b71fc13257abebf3a03 Mon Sep 17 00:00:00 2001 From: Marcel Admiraal Date: Mon, 5 Oct 2020 14:32:50 +0100 Subject: [PATCH] Fix multiple issues with one-way collisions For RigidBodies, uses the collision normal determined by relative motion to determine whether or not a one-way collision has occurred. For KinematicBodies, performs additional checks to ensure a one-way collision has occurred, and averages the recovery step over all collision shapes. Co-authored-by: Sergej Gureev --- servers/physics/space_sw.cpp | 4 +-- servers/physics_2d/body_pair_2d_sw.cpp | 39 +++++++++------------ servers/physics_2d/physics_2d_server_sw.cpp | 32 +++++++---------- servers/physics_2d/space_2d_sw.cpp | 28 +++++++-------- 4 files changed, 43 insertions(+), 60 deletions(-) diff --git a/servers/physics/space_sw.cpp b/servers/physics/space_sw.cpp index ba1070a3cb8b..0c00f8a5ae4a 100644 --- a/servers/physics/space_sw.cpp +++ b/servers/physics/space_sw.cpp @@ -645,7 +645,7 @@ int SpaceSW::test_body_ray_separation(BodySW *p_body, const Transform &p_transfo Vector3 a = sr[k * 2 + 0]; Vector3 b = sr[k * 2 + 1]; - recover_motion += (b - a) * 0.4; + recover_motion += (b - a) / cbk.amount; float depth = a.distance_to(b); if (depth > result.collision_depth) { @@ -791,7 +791,7 @@ bool SpaceSW::test_body_motion(BodySW *p_body, const Transform &p_from, const Ve Vector3 a = sr[i * 2 + 0]; Vector3 b = sr[i * 2 + 1]; - recover_motion += (b - a) * 0.4; + recover_motion += (b - a) / cbk.amount; } if (recover_motion == Vector3()) { diff --git a/servers/physics_2d/body_pair_2d_sw.cpp b/servers/physics_2d/body_pair_2d_sw.cpp index 1523d40d1eac..dd90096f3d4a 100644 --- a/servers/physics_2d/body_pair_2d_sw.cpp +++ b/servers/physics_2d/body_pair_2d_sw.cpp @@ -295,19 +295,15 @@ bool BodyPair2DSW::setup(real_t p_step) { if (A->is_shape_set_as_one_way_collision(shape_A)) { Vector2 direction = xform_A.get_axis(1).normalized(); bool valid = false; - if (B->get_linear_velocity().dot(direction) >= 0) { - for (int i = 0; i < contact_count; i++) { - Contact &c = contacts[i]; - if (!c.reused) - continue; - if (c.normal.dot(direction) > 0) //greater (normal inverted) - continue; - - valid = true; - break; - } + for (int i = 0; i < contact_count; i++) { + Contact &c = contacts[i]; + if (!c.reused) + continue; + if (c.normal.dot(direction) > -CMP_EPSILON) //greater (normal inverted) + continue; + valid = true; + break; } - if (!valid) { collided = false; oneway_disabled = true; @@ -318,17 +314,14 @@ bool BodyPair2DSW::setup(real_t p_step) { if (B->is_shape_set_as_one_way_collision(shape_B)) { Vector2 direction = xform_B.get_axis(1).normalized(); bool valid = false; - if (A->get_linear_velocity().dot(direction) >= 0) { - for (int i = 0; i < contact_count; i++) { - Contact &c = contacts[i]; - if (!c.reused) - continue; - if (c.normal.dot(direction) < 0) //less (normal ok) - continue; - - valid = true; - break; - } + for (int i = 0; i < contact_count; i++) { + Contact &c = contacts[i]; + if (!c.reused) + continue; + if (c.normal.dot(direction) < CMP_EPSILON) //less (normal ok) + continue; + valid = true; + break; } if (!valid) { collided = false; diff --git a/servers/physics_2d/physics_2d_server_sw.cpp b/servers/physics_2d/physics_2d_server_sw.cpp index 938cf4071f90..852bfb7596ad 100644 --- a/servers/physics_2d/physics_2d_server_sw.cpp +++ b/servers/physics_2d/physics_2d_server_sw.cpp @@ -167,24 +167,19 @@ void Physics2DServerSW::_shape_col_cbk(const Vector2 &p_point_A, const Vector2 & if (cbk->max == 0) return; + Vector2 rel_dir = (p_point_A - p_point_B); + real_t rel_length2 = rel_dir.length_squared(); if (cbk->valid_dir != Vector2()) { - if (p_point_A.distance_squared_to(p_point_B) > cbk->valid_depth * cbk->valid_depth) { - cbk->invalid_by_dir++; - return; - } - Vector2 rel_dir = (p_point_A - p_point_B).normalized(); - - if (cbk->valid_dir.dot(rel_dir) < Math_SQRT12) { //sqrt(2)/2.0 - 45 degrees - cbk->invalid_by_dir++; - - /* - print_line("A: "+p_point_A); - print_line("B: "+p_point_B); - print_line("discard too angled "+rtos(cbk->valid_dir.dot((p_point_A-p_point_B)))); - print_line("resnorm: "+(p_point_A-p_point_B).normalized()); - print_line("distance: "+rtos(p_point_A.distance_to(p_point_B))); - */ - return; + if (cbk->valid_depth < 10e20) { + if (rel_length2 > cbk->valid_depth * cbk->valid_depth || + (rel_length2 > CMP_EPSILON && cbk->valid_dir.dot(rel_dir.normalized()) < CMP_EPSILON)) { + cbk->invalid_by_dir++; + return; + } + } else { + if (rel_length2 > 0 && cbk->valid_dir.dot(rel_dir.normalized()) < CMP_EPSILON) { + return; + } } } @@ -201,8 +196,7 @@ void Physics2DServerSW::_shape_col_cbk(const Vector2 &p_point_A, const Vector2 & } } - real_t d = p_point_A.distance_squared_to(p_point_B); - if (d < min_depth) + if (rel_length2 < min_depth) return; cbk->ptr[min_depth_idx * 2 + 0] = p_point_A; cbk->ptr[min_depth_idx * 2 + 1] = p_point_B; diff --git a/servers/physics_2d/space_2d_sw.cpp b/servers/physics_2d/space_2d_sw.cpp index 4748094fe26a..a23bae517f11 100644 --- a/servers/physics_2d/space_2d_sw.cpp +++ b/servers/physics_2d/space_2d_sw.cpp @@ -370,7 +370,6 @@ struct _RestCallbackData2D { Vector2 best_normal; real_t best_len; Vector2 valid_dir; - real_t valid_depth; real_t min_allowed_depth; }; @@ -378,16 +377,17 @@ static void _rest_cbk_result(const Vector2 &p_point_A, const Vector2 &p_point_B, _RestCallbackData2D *rd = (_RestCallbackData2D *)p_userdata; - if (rd->valid_dir != Vector2()) { - if (p_point_A.distance_squared_to(p_point_B) > rd->valid_depth * rd->valid_depth) - return; - if (rd->valid_dir.dot((p_point_A - p_point_B).normalized()) < Math_PI * 0.25) - return; - } - Vector2 contact_rel = p_point_B - p_point_A; real_t len = contact_rel.length(); + if (len == 0) + return; + + Vector2 normal = contact_rel / len; + + if (rd->valid_dir != Vector2() && rd->valid_dir.dot(normal) > -CMP_EPSILON) + return; + if (len < rd->min_allowed_depth) return; @@ -396,7 +396,7 @@ static void _rest_cbk_result(const Vector2 &p_point_A, const Vector2 &p_point_B, rd->best_len = len; rd->best_contact = p_point_B; - rd->best_normal = contact_rel / len; + rd->best_normal = normal; rd->best_object = rd->object; rd->best_shape = rd->shape; rd->best_local_shape = rd->local_shape; @@ -431,7 +431,6 @@ bool Physics2DDirectSpaceStateSW::rest_info(RID p_shape, const Transform2D &p_sh continue; rcd.valid_dir = Vector2(); - rcd.valid_depth = 0; rcd.object = col_obj; rcd.shape = shape_idx; rcd.local_shape = 0; @@ -638,7 +637,7 @@ int Space2DSW::test_body_ray_separation(Body2DSW *p_body, const Transform2D &p_t Vector2 a = sr[k * 2 + 0]; Vector2 b = sr[k * 2 + 1]; - recover_motion += (b - a) * 0.4; + recover_motion += (b - a) / cbk.amount; float depth = a.distance_to(b); if (depth > result.collision_depth) { @@ -849,7 +848,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co Vector2 a = sr[i * 2 + 0]; Vector2 b = sr[i * 2 + 1]; - recover_motion += (b - a) * 0.4; + recover_motion += (b - a) / cbk.amount; } if (recover_motion == Vector2()) { @@ -1010,7 +1009,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co best_shape = -1; //no best shape with cast, reset to -1 } - { + if (safe < 1) { //it collided, let's get the rest info in unsafe advance Transform2D ugt = body_transform; @@ -1070,12 +1069,9 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co Transform2D col_obj_shape_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); if (col_obj->is_shape_set_as_one_way_collision(shape_idx)) { - rcd.valid_dir = col_obj_shape_xform.get_axis(1).normalized(); - rcd.valid_depth = 10e20; } else { rcd.valid_dir = Vector2(); - rcd.valid_depth = 0; } rcd.object = col_obj;