From 255febefb2cc49480624ebe7f3fce9834b34fea2 Mon Sep 17 00:00:00 2001 From: PouleyKetchoupp Date: Fri, 8 Jan 2021 13:07:50 -0700 Subject: [PATCH] Fix Rayshape recovery in test_body_ray_separation 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 depth takes into account the current recovery vector (just like test_body_motion) to fix jittering issues with multiple ray shapes due to applying too much recovery. --- servers/physics/collision_solver_sw.cpp | 8 +++--- servers/physics/collision_solver_sw.h | 2 +- servers/physics/space_sw.cpp | 28 ++++++++++--------- 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(+), 35 deletions(-) diff --git a/servers/physics/collision_solver_sw.cpp b/servers/physics/collision_solver_sw.cpp index 2f42e9a52b3..77c995299a5 100644 --- a/servers/physics/collision_solver_sw.cpp +++ b/servers/physics/collision_solver_sw.cpp @@ -89,11 +89,11 @@ 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(); @@ -232,9 +232,9 @@ bool CollisionSolverSW::solve_static(const ShapeSW *p_shape_A, const Transform & } 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 690e2f3616c..6b55a3112d9 100644 --- a/servers/physics/collision_solver_sw.h +++ b/servers/physics/collision_solver_sw.h @@ -40,7 +40,7 @@ public: private: static bool 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 bool 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 7ecc153a94a..c0f9273a737 100644 --- a/servers/physics/space_sw.cpp +++ b/servers/physics/space_sw.cpp @@ -589,7 +589,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; @@ -664,18 +664,28 @@ 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) / cbk.amount; + // Compute plane on b towards a. + Vector3 n = (a - b).normalized(); + float d = n.dot(b); + + // Compute depth on recovered motion. + float depth = n.dot(a + recover_motion) - d; + + // Apply recovery without margin. + float separation_depth = depth - p_margin; + if (separation_depth > 0.0) { + // Only recover if there is penetration. + recover_motion -= n * separation_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 = -n; result.collision_local_shape = j; result.collider = col_obj->get_self(); result.collider_id = col_obj->get_instance_id(); result.collider_shape = shape_idx; - //result.collider_metadata = col_obj->get_shape_metadata(shape_idx); if (col_obj->get_type() == CollisionObjectSW::TYPE_BODY) { BodySW *body = (BodySW *)col_obj; @@ -700,14 +710,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 df3e5f66650..9ee9048b709 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() == 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(); @@ -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 96a94329528..3f0f2d8a0a7 100644 --- a/servers/physics_2d/collision_solver_2d_sw.h +++ b/servers/physics_2d/collision_solver_2d_sw.h @@ -41,7 +41,7 @@ private: 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 bool 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 8756477350c..0d7d6708fcb 100644 --- a/servers/physics_2d/space_2d_sw.cpp +++ b/servers/physics_2d/space_2d_sw.cpp @@ -560,7 +560,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; @@ -660,13 +660,24 @@ 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) / cbk.amount; + // Compute plane on b towards a. + Vector2 n = (a - b).normalized(); + float d = n.dot(b); + + // Compute depth on recovered motion. + float depth = n.dot(a + recover_motion) - d; + + // Apply recovery without margin. + float separation_depth = depth - p_margin; + if (separation_depth > 0.0) { + // Only recover if there is penetration. + recover_motion -= n * separation_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 = -n; result.collision_local_shape = j; result.collider_shape = shape_idx; result.collider = col_obj->get_self(); @@ -696,14 +707,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; }