diff --git a/servers/physics_2d/space_2d_sw.cpp b/servers/physics_2d/space_2d_sw.cpp index 05136e2501c4..9cbc01d1d372 100644 --- a/servers/physics_2d/space_2d_sw.cpp +++ b/servers/physics_2d/space_2d_sw.cpp @@ -376,6 +376,7 @@ struct _RestCallbackData2D { Vector2 best_normal; real_t best_len; Vector2 valid_dir; + real_t valid_depth; real_t min_allowed_depth; }; @@ -385,22 +386,24 @@ 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 == 0) { + if (len < rd->min_allowed_depth) { return; } - Vector2 normal = contact_rel / len; - - if (rd->valid_dir != Vector2() && rd->valid_dir.dot(normal) > -CMP_EPSILON) { + if (len <= rd->best_len) { return; } - if (len < rd->min_allowed_depth) { - return; - } + Vector2 normal = contact_rel / len; - if (len <= rd->best_len) { - return; + if (rd->valid_dir != Vector2()) { + if (len > rd->valid_depth) { + return; + } + + if (rd->valid_dir.dot(normal) > -CMP_EPSILON) { + return; + } } rd->best_len = len; @@ -739,10 +742,13 @@ 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 separation_margin = MIN(p_margin, MAX(0.0, p_motion.length() - CMP_EPSILON)); //don't separate by more than the intended motion + real_t motion_length = p_motion.length(); + Vector2 motion_normal = p_motion / motion_length; Transform2D body_transform = p_from; + bool recovered = false; + { //STEP 1, FREE BODY IF STUCK @@ -819,7 +825,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co bool did_collide = false; Shape2DSW *against_shape = col_obj->get_shape(shape_idx); - if (CollisionSolver2DSW::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), cbkres, cbkptr, nullptr, separation_margin)) { + if (CollisionSolver2DSW::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), cbkres, cbkptr, nullptr, p_margin)) { did_collide = cbk.passed > current_passed; //more passed, so collision actually existed } @@ -845,11 +851,20 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co } Vector2 recover_motion; - for (int i = 0; i < cbk.amount; i++) { Vector2 a = sr[i * 2 + 0]; Vector2 b = sr[i * 2 + 1]; - recover_motion += (b - a) / cbk.amount; + + // Compute plane on b towards a. + Vector2 n = (a - b).normalized(); + real_t d = n.dot(b); + + // Compute depth on recovered motion. + real_t depth = n.dot(a + recover_motion) - d; + if (depth > 0.0) { + // Only recover if there is penetration. + recover_motion -= n * depth * 0.4; + } } if (recover_motion == Vector2()) { @@ -857,6 +872,8 @@ 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; @@ -929,7 +946,10 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co //test initial overlap if (CollisionSolver2DSW::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), nullptr, nullptr, nullptr, 0)) { if (col_obj->is_shape_set_as_one_way_collision(col_shape_idx)) { - continue; + Vector2 direction = col_obj_shape_xform.get_axis(1).normalized(); + if (motion_normal.dot(direction) < 0) { + continue; + } } stuck = true; @@ -939,13 +959,12 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co //just do kinematic solving real_t low = 0; real_t hi = 1; - Vector2 mnormal = p_motion.normalized(); for (int k = 0; k < 8; k++) { //steps should be customizable.. real_t ofs = (low + hi) * 0.5; - Vector2 sep = mnormal; //important optimization for this to work fast enough + Vector2 sep = motion_normal; //important optimization for this to work fast enough bool collided = CollisionSolver2DSW::solve(body_shape, body_shape_xform, p_motion * ofs, against_shape, col_obj_shape_xform, Vector2(), nullptr, nullptr, &sep, 0); if (collided) { @@ -966,7 +985,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co cbk.valid_depth = 10e20; - Vector2 sep = mnormal; //important optimization for this to work fast enough + Vector2 sep = motion_normal; //important optimization for this to work fast enough bool collided = CollisionSolver2DSW::solve(body_shape, body_shape_xform, p_motion * (hi + contact_max_allowed_penetration), col_obj->get_shape(col_shape_idx), col_obj_shape_xform, Vector2(), PhysicsServer2DSW::_shape_col_cbk, &cbk, &sep, 0); if (!collided || cbk.amount == 0) { continue; @@ -997,11 +1016,12 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co } bool collided = false; - if (safe >= 1) { - best_shape = -1; //no best shape with cast, reset to -1 - } - if (safe < 1) { + if (recovered || (safe < 1)) { + if (safe >= 1) { + best_shape = -1; //no best shape with cast, reset to -1 + } + //it collided, let's get the rest info in unsafe advance Transform2D ugt = body_transform; ugt.elements[2] += p_motion * unsafe; @@ -1010,9 +1030,10 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co rcd.best_len = 0; rcd.best_object = nullptr; rcd.best_shape = 0; - rcd.min_allowed_depth = test_motion_min_contact_depth; - //optimization + // 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); + int from_shape = best_shape != -1 ? best_shape : 0; int to_shape = best_shape != -1 ? best_shape + 1 : p_body->get_shape_count(); @@ -1060,8 +1081,25 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co if (col_obj->is_shape_set_as_one_way_collision(shape_idx)) { rcd.valid_dir = col_obj_shape_xform.get_axis(1).normalized(); + + real_t owc_margin = col_obj->get_shape_one_way_collision_margin(shape_idx); + rcd.valid_depth = MAX(owc_margin, p_margin); //user specified, but never less than actual margin or it won't work + + if (col_obj->get_type() == CollisionObject2DSW::TYPE_BODY) { + const Body2DSW *b = static_cast(col_obj); + if (b->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC || b->get_mode() == PhysicsServer2D::BODY_MODE_RIGID) { + //fix for moving platforms (kinematic and dynamic), margin is increased by how much it moved in the given direction + Vector2 lv = b->get_linear_velocity(); + //compute displacement from linear velocity + Vector2 motion = lv * PhysicsDirectBodyState2DSW::singleton->step; + real_t motion_len = motion.length(); + motion.normalize(); + rcd.valid_depth += motion_len * MAX(motion.dot(-rcd.valid_dir), 0.0); + } + } } else { rcd.valid_dir = Vector2(); + rcd.valid_depth = 0; } rcd.object = col_obj; diff --git a/servers/physics_3d/space_3d_sw.cpp b/servers/physics_3d/space_3d_sw.cpp index 40537c70016d..dd5754b9ac3c 100644 --- a/servers/physics_3d/space_3d_sw.cpp +++ b/servers/physics_3d/space_3d_sw.cpp @@ -384,6 +384,8 @@ bool PhysicsDirectSpaceState3DSW::collide_shape(RID p_shape, const Transform &p_ struct _RestCallbackData { const CollisionObject3DSW *object; const CollisionObject3DSW *best_object; + int local_shape; + int best_local_shape; int shape; int best_shape; Vector3 best_contact; @@ -409,6 +411,7 @@ static void _rest_cbk_result(const Vector3 &p_point_A, const Vector3 &p_point_B, rd->best_normal = contact_rel / len; rd->best_object = rd->object; rd->best_shape = rd->shape; + rd->best_local_shape = rd->local_shape; } bool PhysicsDirectSpaceState3DSW::rest_info(RID p_shape, const Transform &p_shape_xform, real_t p_margin, ShapeRestInfo *r_info, const Set &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { @@ -739,8 +742,13 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons body_aabb = p_from.xform(p_body->get_inv_transform().xform(body_aabb)); body_aabb = body_aabb.grow(p_margin); + real_t motion_length = p_motion.length(); + Vector3 motion_normal = p_motion / motion_length; + Transform body_transform = p_from; + bool recovered = false; + { //STEP 1, FREE BODY IF STUCK @@ -791,7 +799,17 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons for (int i = 0; i < cbk.amount; i++) { Vector3 a = sr[i * 2 + 0]; Vector3 b = sr[i * 2 + 1]; - recover_motion += (b - a) / cbk.amount; + + // Compute plane on b towards a. + Vector3 n = (a - b).normalized(); + real_t d = n.dot(b); + + // Compute depth on recovered motion. + real_t depth = n.dot(a + recover_motion) - d; + if (depth > 0.0) { + // Only recover if there is penetration. + recover_motion -= n * depth * 0.4; + } } if (recover_motion == Vector3()) { @@ -799,6 +817,8 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons break; } + recovered = true; + body_transform.origin += recover_motion; body_aabb.position += recover_motion; @@ -848,14 +868,14 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons //test initial overlap, does it collide if going all the way? Vector3 point_A, point_B; - Vector3 sep_axis = p_motion.normalized(); + Vector3 sep_axis = motion_normal; Transform col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); //test initial overlap, does it collide if going all the way? if (CollisionSolver3DSW::solve_distance(&mshape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, motion_aabb, &sep_axis)) { continue; } - sep_axis = p_motion.normalized(); + sep_axis = motion_normal; if (!CollisionSolver3DSW::solve_distance(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, motion_aabb, &sep_axis)) { stuck = true; @@ -865,13 +885,12 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons //just do kinematic solving real_t low = 0; real_t hi = 1; - Vector3 mnormal = p_motion.normalized(); for (int k = 0; k < 8; k++) { //steps should be customizable.. real_t ofs = (low + hi) * 0.5; - Vector3 sep = mnormal; //important optimization for this to work fast enough + Vector3 sep = motion_normal; //important optimization for this to work fast enough mshape.motion = body_shape_xform_inv.basis.xform(p_motion * ofs); @@ -912,16 +931,11 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons } bool collided = false; - if (safe >= 1) { - //not collided - collided = false; - if (r_result) { - r_result->motion = p_motion; - r_result->remainder = Vector3(); - r_result->motion += (body_transform.get_origin() - p_from.get_origin()); + if (recovered || (safe < 1)) { + if (safe >= 1) { + best_shape = -1; //no best shape with cast, reset to -1 } - } else { //it collided, let's get the rest info in unsafe advance Transform ugt = body_transform; ugt.origin += p_motion * unsafe; @@ -930,25 +944,40 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons rcd.best_len = 0; rcd.best_object = nullptr; 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); - Shape3DSW *body_shape = p_body->get_shape(best_shape); + // 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); - body_aabb.position += p_motion * unsafe; + int from_shape = best_shape != -1 ? best_shape : 0; + int to_shape = best_shape != -1 ? best_shape + 1 : p_body->get_shape_count(); - int amount = _cull_aabb_for_body(p_body, body_aabb); + for (int j = from_shape; j < to_shape; j++) { + if (p_body->is_shape_set_as_disabled(j)) { + continue; + } - for (int i = 0; i < amount; i++) { - const CollisionObject3DSW *col_obj = intersection_query_results[i]; - int shape_idx = intersection_query_subindex_results[i]; + Transform body_shape_xform = ugt * p_body->get_shape_transform(j); + Shape3DSW *body_shape = p_body->get_shape(j); - rcd.object = col_obj; - rcd.shape = shape_idx; - bool sc = CollisionSolver3DSW::solve_static(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), _rest_cbk_result, &rcd, nullptr, p_margin); - if (!sc) { + if (p_exclude_raycast_shapes && body_shape->get_type() == PhysicsServer3D::SHAPE_RAY) { continue; } + + body_aabb.position += p_motion * unsafe; + + int amount = _cull_aabb_for_body(p_body, body_aabb); + + for (int i = 0; i < amount; i++) { + const CollisionObject3DSW *col_obj = intersection_query_results[i]; + int shape_idx = intersection_query_subindex_results[i]; + + rcd.object = col_obj; + rcd.shape = shape_idx; + bool sc = CollisionSolver3DSW::solve_static(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), _rest_cbk_result, &rcd, nullptr, p_margin); + if (!sc) { + continue; + } + } } if (rcd.best_len != 0) { @@ -956,7 +985,7 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons r_result->collider = rcd.best_object->get_self(); r_result->collider_id = rcd.best_object->get_instance_id(); r_result->collider_shape = rcd.best_shape; - r_result->collision_local_shape = best_shape; + r_result->collision_local_shape = rcd.best_local_shape; r_result->collision_normal = rcd.best_normal; r_result->collision_point = rcd.best_contact; //r_result->collider_metadata = rcd.best_object->get_shape_metadata(rcd.best_shape); @@ -972,17 +1001,15 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons } collided = true; - } else { - if (r_result) { - r_result->motion = p_motion; - r_result->remainder = Vector3(); - r_result->motion += (body_transform.get_origin() - p_from.get_origin()); - } - - collided = false; } } + if (!collided && r_result) { + r_result->motion = p_motion; + r_result->remainder = Vector3(); + r_result->motion += (body_transform.get_origin() - p_from.get_origin()); + } + return collided; }