From 219a582a151e1a248c5e17b012bc2844636633bb Mon Sep 17 00:00:00 2001 From: Ricardo Buring Date: Mon, 29 Aug 2022 17:34:06 +0200 Subject: [PATCH] Fix collision solving between world boundary and shapes in motion (2D) --- servers/physics_2d/godot_collision_solver_2d.cpp | 8 +++++--- servers/physics_2d/godot_collision_solver_2d.h | 2 +- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/servers/physics_2d/godot_collision_solver_2d.cpp b/servers/physics_2d/godot_collision_solver_2d.cpp index 0d7b42b80da..761a1c33e2b 100644 --- a/servers/physics_2d/godot_collision_solver_2d.cpp +++ b/servers/physics_2d/godot_collision_solver_2d.cpp @@ -34,7 +34,7 @@ #define collision_solver sat_2d_calculate_penetration //#define collision_solver gjk_epa_calculate_penetration -bool GodotCollisionSolver2D::solve_static_world_boundary(const GodotShape2D *p_shape_A, const Transform2D &p_transform_A, const GodotShape2D *p_shape_B, const Transform2D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) { +bool GodotCollisionSolver2D::solve_static_world_boundary(const GodotShape2D *p_shape_A, const Transform2D &p_transform_A, const GodotShape2D *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin) { const GodotWorldBoundaryShape2D *world_boundary = static_cast(p_shape_A); if (p_shape_B->get_type() == PhysicsServer2D::SHAPE_WORLD_BOUNDARY) { return false; @@ -52,7 +52,9 @@ bool GodotCollisionSolver2D::solve_static_world_boundary(const GodotShape2D *p_s bool found = false; for (int i = 0; i < support_count; i++) { + supports[i] += p_margin * supports[i].normalized(); supports[i] = p_transform_B.xform(supports[i]); + supports[i] += p_motion_B; real_t pd = n.dot(supports[i]); if (pd >= d) { continue; @@ -231,9 +233,9 @@ bool GodotCollisionSolver2D::solve(const GodotShape2D *p_shape_A, const Transfor } if (swap) { - return solve_static_world_boundary(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true); + return solve_static_world_boundary(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_motion_A, p_result_callback, p_userdata, true, p_margin_A); } else { - return solve_static_world_boundary(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false); + return solve_static_world_boundary(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_motion_B, p_result_callback, p_userdata, false, p_margin_B); } } else if (type_A == PhysicsServer2D::SHAPE_SEPARATION_RAY) { diff --git a/servers/physics_2d/godot_collision_solver_2d.h b/servers/physics_2d/godot_collision_solver_2d.h index bd90641f040..3aac2751e03 100644 --- a/servers/physics_2d/godot_collision_solver_2d.h +++ b/servers/physics_2d/godot_collision_solver_2d.h @@ -38,7 +38,7 @@ public: typedef void (*CallbackResult)(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_userdata); private: - static bool solve_static_world_boundary(const GodotShape2D *p_shape_A, const Transform2D &p_transform_A, const GodotShape2D *p_shape_B, const Transform2D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result); + static bool solve_static_world_boundary(const GodotShape2D *p_shape_A, const Transform2D &p_transform_A, const GodotShape2D *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin = 0); static bool concave_callback(void *p_userdata, GodotShape2D *p_convex); static bool solve_concave(const GodotShape2D *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const GodotShape2D *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 *r_sep_axis = nullptr, real_t p_margin_A = 0, real_t p_margin_B = 0); static bool solve_separation_ray(const GodotShape2D *p_shape_A, const Vector2 &p_motion_A, const Transform2D &p_transform_A, const GodotShape2D *p_shape_B, const Transform2D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *r_sep_axis = nullptr, real_t p_margin = 0);