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; }