From ed52bfd63ba3e6ba912b3ea9831eb9e9a0a95b88 Mon Sep 17 00:00:00 2001 From: PouleyKetchoupp Date: Fri, 8 Jan 2021 13:07:50 -0700 Subject: [PATCH] Fix Rayshape not working properly in move_and_slide These changes improve Rayshape behavior for Godot Physics 2D and 3D when using move_and_slide with and without snapping. Kinematic margin is now applied to ray shapes when handling snapping collision tests and separation raycasts to help getting consistent results in slopes and flat surfaces. Recovery is calculated without the margin and a depth of 0 is still considered a collision to stabilize results when on flat surface. Recovery is split based on the amount of shapes to fix cases where multiple rayshapes would cause the body to bounce. Fixes #34098 Fixes #34663 --- servers/physics/collision_solver_sw.cpp | 8 +++--- servers/physics/collision_solver_sw.h | 2 +- servers/physics/space_sw.cpp | 27 ++++++++++--------- servers/physics_2d/collision_solver_2d_sw.cpp | 8 +++--- servers/physics_2d/collision_solver_2d_sw.h | 2 +- servers/physics_2d/space_2d_sw.cpp | 27 ++++++++++--------- 6 files changed, 40 insertions(+), 34 deletions(-) diff --git a/servers/physics/collision_solver_sw.cpp b/servers/physics/collision_solver_sw.cpp index ce24ba6bca71..4c5b598ee064 100644 --- a/servers/physics/collision_solver_sw.cpp +++ b/servers/physics/collision_solver_sw.cpp @@ -71,12 +71,12 @@ bool CollisionSolverSW::solve_static_plane(const ShapeSW *p_shape_A, const Trans return found; } -bool CollisionSolverSW::solve_ray(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) { +bool CollisionSolverSW::solve_ray(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin) { const RayShapeSW *ray = static_cast(p_shape_A); Vector3 from = p_transform_A.origin; - Vector3 to = from + p_transform_A.basis.get_axis(2) * ray->get_length(); + Vector3 to = from + p_transform_A.basis.get_axis(2) * (ray->get_length() + p_margin); Vector3 support_A = to; Transform ai = p_transform_B.affine_inverse(); @@ -214,9 +214,9 @@ bool CollisionSolverSW::solve_static(const ShapeSW *p_shape_A, const Transform & return false; if (swap) { - return solve_ray(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true); + return solve_ray(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true, p_margin_B); } else { - return solve_ray(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false); + return solve_ray(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false, p_margin_A); } } else if (concave_B) { diff --git a/servers/physics/collision_solver_sw.h b/servers/physics/collision_solver_sw.h index d4dc1ac2a207..02d973659aa2 100644 --- a/servers/physics/collision_solver_sw.h +++ b/servers/physics/collision_solver_sw.h @@ -40,7 +40,7 @@ class CollisionSolverSW { private: static void concave_callback(void *p_userdata, ShapeSW *p_convex); static bool solve_static_plane(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result); - static bool solve_ray(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result); + static bool solve_ray(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin = 0); static bool solve_concave(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin_A = 0, real_t p_margin_B = 0); static void concave_distance_callback(void *p_userdata, ShapeSW *p_convex); static bool solve_distance_plane(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B); diff --git a/servers/physics/space_sw.cpp b/servers/physics/space_sw.cpp index 0c00f8a5ae4a..8f884c97e0e8 100644 --- a/servers/physics/space_sw.cpp +++ b/servers/physics/space_sw.cpp @@ -570,7 +570,7 @@ int SpaceSW::test_body_ray_separation(BodySW *p_body, const Transform &p_transfo for (int i = 0; i < p_result_max; i++) { //reset results - r_results[i].collision_depth = 0; + r_results[i].collision_depth = -1.0; } int rays_found = 0; @@ -589,6 +589,7 @@ int SpaceSW::test_body_ray_separation(BodySW *p_body, const Transform &p_transfo do { Vector3 recover_motion; + int recover_count = 0; bool collided = false; @@ -605,6 +606,8 @@ int SpaceSW::test_body_ray_separation(BodySW *p_body, const Transform &p_transfo Transform body_shape_xform = body_transform * p_body->get_shape_transform(j); + Vector3 ray_normal = -body_shape_xform.basis.get_axis(2); + for (int i = 0; i < amount; i++) { const CollisionObjectSW *col_obj = intersection_query_results[i]; @@ -641,18 +644,24 @@ int SpaceSW::test_body_ray_separation(BodySW *p_body, const Transform &p_transfo if (ray_index != -1) { PhysicsServer::SeparationResult &result = r_results[ray_index]; + recover_count += cbk.amount; for (int k = 0; k < cbk.amount; k++) { Vector3 a = sr[k * 2 + 0]; Vector3 b = sr[k * 2 + 1]; + Vector3 separation = (b - a); - recover_motion += (b - a) / cbk.amount; + // Apply recovery without margin. + float depth = separation.length(); + float separation_depth = depth - p_margin; + if (separation_depth > 0.0) { + recover_motion += separation * (separation_depth / depth); + } - float depth = a.distance_to(b); if (depth > result.collision_depth) { result.collision_depth = depth; result.collision_point = b; - result.collision_normal = (b - a).normalized(); + result.collision_normal = ray_normal; result.collision_local_shape = j; result.collider = col_obj->get_self(); result.collider_id = col_obj->get_instance_id(); @@ -676,6 +685,8 @@ int SpaceSW::test_body_ray_separation(BodySW *p_body, const Transform &p_transfo break; } + recover_motion /= recover_count; + body_transform.origin += recover_motion; body_aabb.position += recover_motion; @@ -683,14 +694,6 @@ int SpaceSW::test_body_ray_separation(BodySW *p_body, const Transform &p_transfo } while (recover_attempts); } - //optimize results (remove non colliding) - for (int i = 0; i < rays_found; i++) { - if (r_results[i].collision_depth == 0) { - rays_found--; - SWAP(r_results[i], r_results[rays_found]); - } - } - r_recover_motion = body_transform.origin - p_transform.origin; return rays_found; } diff --git a/servers/physics_2d/collision_solver_2d_sw.cpp b/servers/physics_2d/collision_solver_2d_sw.cpp index a83b68088810..c412c44d3ad3 100644 --- a/servers/physics_2d/collision_solver_2d_sw.cpp +++ b/servers/physics_2d/collision_solver_2d_sw.cpp @@ -72,14 +72,14 @@ bool CollisionSolver2DSW::solve_static_line(const Shape2DSW *p_shape_A, const Tr return found; } -bool CollisionSolver2DSW::solve_raycast(const Shape2DSW *p_shape_A, const Vector2 &p_motion_A, const Transform2D &p_transform_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *sep_axis) { +bool CollisionSolver2DSW::solve_raycast(const Shape2DSW *p_shape_A, const Vector2 &p_motion_A, const Transform2D &p_transform_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *sep_axis, real_t p_margin) { const RayShape2DSW *ray = static_cast(p_shape_A); if (p_shape_B->get_type() == Physics2DServer::SHAPE_RAY) return false; Vector2 from = p_transform_A.get_origin(); - Vector2 to = from + p_transform_A[1] * ray->get_length(); + Vector2 to = from + p_transform_A[1] * (ray->get_length() + p_margin); if (p_motion_A != Vector2()) { //not the best but should be enough Vector2 normal = (to - from).normalized(); @@ -230,9 +230,9 @@ bool CollisionSolver2DSW::solve(const Shape2DSW *p_shape_A, const Transform2D &p } if (swap) { - return solve_raycast(p_shape_B, p_motion_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true, sep_axis); + return solve_raycast(p_shape_B, p_motion_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true, sep_axis, p_margin_B); } else { - return solve_raycast(p_shape_A, p_motion_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false, sep_axis); + return solve_raycast(p_shape_A, p_motion_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false, sep_axis, p_margin_A); } } else if (concave_B) { diff --git a/servers/physics_2d/collision_solver_2d_sw.h b/servers/physics_2d/collision_solver_2d_sw.h index e73ee8fd7ebf..255d7b30e71b 100644 --- a/servers/physics_2d/collision_solver_2d_sw.h +++ b/servers/physics_2d/collision_solver_2d_sw.h @@ -41,7 +41,7 @@ class CollisionSolver2DSW { static bool solve_static_line(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result); static void concave_callback(void *p_userdata, Shape2DSW *p_convex); static bool solve_concave(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *sep_axis = NULL, real_t p_margin_A = 0, real_t p_margin_B = 0); - static bool solve_raycast(const Shape2DSW *p_shape_A, const Vector2 &p_motion_A, const Transform2D &p_transform_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *sep_axis = NULL); + static bool solve_raycast(const Shape2DSW *p_shape_A, const Vector2 &p_motion_A, const Transform2D &p_transform_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *sep_axis = NULL, real_t p_margin = 0); public: static bool solve(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, Vector2 *sep_axis = NULL, real_t p_margin_A = 0, real_t p_margin_B = 0); diff --git a/servers/physics_2d/space_2d_sw.cpp b/servers/physics_2d/space_2d_sw.cpp index a23bae517f11..45c496e670db 100644 --- a/servers/physics_2d/space_2d_sw.cpp +++ b/servers/physics_2d/space_2d_sw.cpp @@ -536,7 +536,7 @@ int Space2DSW::test_body_ray_separation(Body2DSW *p_body, const Transform2D &p_t for (int i = 0; i < p_result_max; i++) { //reset results - r_results[i].collision_depth = 0; + r_results[i].collision_depth = -1.0; } int rays_found = 0; @@ -555,6 +555,7 @@ int Space2DSW::test_body_ray_separation(Body2DSW *p_body, const Transform2D &p_t do { Vector2 recover_motion; + int recover_count = 0; bool collided = false; @@ -571,6 +572,8 @@ int Space2DSW::test_body_ray_separation(Body2DSW *p_body, const Transform2D &p_t Transform2D body_shape_xform = body_transform * p_body->get_shape_transform(j); + Vector2 ray_normal = -body_shape_xform[1]; + for (int i = 0; i < amount; i++) { const CollisionObject2DSW *col_obj = intersection_query_results[i]; @@ -633,18 +636,24 @@ int Space2DSW::test_body_ray_separation(Body2DSW *p_body, const Transform2D &p_t Physics2DServer::SeparationResult &result = r_results[ray_index]; + recover_count += cbk.amount; for (int k = 0; k < cbk.amount; k++) { Vector2 a = sr[k * 2 + 0]; Vector2 b = sr[k * 2 + 1]; + Vector2 separation = (b - a); - recover_motion += (b - a) / cbk.amount; + // Apply recovery without margin. + float depth = separation.length(); + float separation_depth = depth - p_margin; + if (separation_depth > 0.0) { + recover_motion += separation * (separation_depth / depth); + } - float depth = a.distance_to(b); if (depth > result.collision_depth) { result.collision_depth = depth; result.collision_point = b; - result.collision_normal = (b - a).normalized(); + result.collision_normal = ray_normal; result.collision_local_shape = j; result.collider_shape = shape_idx; result.collider = col_obj->get_self(); @@ -667,6 +676,8 @@ int Space2DSW::test_body_ray_separation(Body2DSW *p_body, const Transform2D &p_t break; } + recover_motion /= recover_count; + body_transform.elements[2] += recover_motion; body_aabb.position += recover_motion; @@ -674,14 +685,6 @@ int Space2DSW::test_body_ray_separation(Body2DSW *p_body, const Transform2D &p_t } while (recover_attempts); } - //optimize results (remove non colliding) - for (int i = 0; i < rays_found; i++) { - if (r_results[i].collision_depth == 0) { - rays_found--; - SWAP(r_results[i], r_results[rays_found]); - } - } - r_recover_motion = body_transform.elements[2] - p_transform.elements[2]; return rays_found; }