From a3d8043e000033b7679b10036529b8017b59bfc1 Mon Sep 17 00:00:00 2001 From: PouleyKetchoupp Date: Thu, 7 Jan 2021 20:50:54 -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_2d/collision_solver_2d_sw.cpp | 8 +++--- servers/physics_2d/collision_solver_2d_sw.h | 2 +- servers/physics_2d/space_2d_sw.cpp | 27 ++++++++++--------- servers/physics_3d/collision_solver_3d_sw.cpp | 8 +++--- servers/physics_3d/collision_solver_3d_sw.h | 2 +- servers/physics_3d/space_3d_sw.cpp | 27 ++++++++++--------- 6 files changed, 40 insertions(+), 34 deletions(-) diff --git a/servers/physics_2d/collision_solver_2d_sw.cpp b/servers/physics_2d/collision_solver_2d_sw.cpp index 5bd4d498c62a..ec19bb5d14c8 100644 --- a/servers/physics_2d/collision_solver_2d_sw.cpp +++ b/servers/physics_2d/collision_solver_2d_sw.cpp @@ -73,14 +73,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() == PhysicsServer2D::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(); @@ -226,9 +226,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 4f12ca9e882c..1e4b4ce80140 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 = nullptr, 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 = nullptr); + 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 = nullptr, 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 = nullptr, 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 ad5dca37e411..75a3349aef07 100644 --- a/servers/physics_2d/space_2d_sw.cpp +++ b/servers/physics_2d/space_2d_sw.cpp @@ -543,7 +543,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; @@ -561,6 +561,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; @@ -579,6 +580,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]; int shape_idx = intersection_query_subindex_results[i]; @@ -638,17 +641,23 @@ int Space2DSW::test_body_ray_separation(Body2DSW *p_body, const Transform2D &p_t if (ray_index != -1) { PhysicsServer2D::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(); @@ -671,6 +680,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; @@ -678,14 +689,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; } diff --git a/servers/physics_3d/collision_solver_3d_sw.cpp b/servers/physics_3d/collision_solver_3d_sw.cpp index 1150696b8443..a0777995969c 100644 --- a/servers/physics_3d/collision_solver_3d_sw.cpp +++ b/servers/physics_3d/collision_solver_3d_sw.cpp @@ -72,11 +72,11 @@ bool CollisionSolver3DSW::solve_static_plane(const Shape3DSW *p_shape_A, const T return found; } -bool CollisionSolver3DSW::solve_ray(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) { +bool CollisionSolver3DSW::solve_ray(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin) { const RayShape3DSW *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(); @@ -212,9 +212,9 @@ bool CollisionSolver3DSW::solve_static(const Shape3DSW *p_shape_A, const Transfo } 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_3d/collision_solver_3d_sw.h b/servers/physics_3d/collision_solver_3d_sw.h index 81d87e9773d5..7ab1047e7a61 100644 --- a/servers/physics_3d/collision_solver_3d_sw.h +++ b/servers/physics_3d/collision_solver_3d_sw.h @@ -40,7 +40,7 @@ class CollisionSolver3DSW { private: static void concave_callback(void *p_userdata, Shape3DSW *p_convex); static bool solve_static_plane(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result); - static bool solve_ray(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result); + static bool solve_ray(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *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 Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *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, Shape3DSW *p_convex); static bool solve_distance_plane(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B); diff --git a/servers/physics_3d/space_3d_sw.cpp b/servers/physics_3d/space_3d_sw.cpp index 0395a3339c7b..a646cab76b25 100644 --- a/servers/physics_3d/space_3d_sw.cpp +++ b/servers/physics_3d/space_3d_sw.cpp @@ -574,7 +574,7 @@ int Space3DSW::test_body_ray_separation(Body3DSW *p_body, const Transform &p_tra 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; @@ -592,6 +592,7 @@ int Space3DSW::test_body_ray_separation(Body3DSW *p_body, const Transform &p_tra do { Vector3 recover_motion; + int recover_count = 0; bool collided = false; @@ -610,6 +611,8 @@ int Space3DSW::test_body_ray_separation(Body3DSW *p_body, const Transform &p_tra 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 CollisionObject3DSW *col_obj = intersection_query_results[i]; int shape_idx = intersection_query_subindex_results[i]; @@ -645,17 +648,23 @@ int Space3DSW::test_body_ray_separation(Body3DSW *p_body, const Transform &p_tra if (ray_index != -1) { PhysicsServer3D::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(); @@ -679,6 +688,8 @@ int Space3DSW::test_body_ray_separation(Body3DSW *p_body, const Transform &p_tra break; } + recover_motion /= recover_count; + body_transform.origin += recover_motion; body_aabb.position += recover_motion; @@ -686,14 +697,6 @@ int Space3DSW::test_body_ray_separation(Body3DSW *p_body, const Transform &p_tra } 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; }