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