diff options
| -rw-r--r-- | servers/physics_3d/godot_collision_solver_3d.cpp | 18 | ||||
| -rw-r--r-- | servers/physics_3d/godot_collision_solver_3d.h | 2 |
2 files changed, 15 insertions, 5 deletions
diff --git a/servers/physics_3d/godot_collision_solver_3d.cpp b/servers/physics_3d/godot_collision_solver_3d.cpp index b2d3e4d876..094d77a582 100644 --- a/servers/physics_3d/godot_collision_solver_3d.cpp +++ b/servers/physics_3d/godot_collision_solver_3d.cpp @@ -38,7 +38,7 @@ #define collision_solver sat_calculate_penetration //#define collision_solver gjk_epa_calculate_penetration -bool GodotCollisionSolver3D::solve_static_world_boundary(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) { +bool GodotCollisionSolver3D::solve_static_world_boundary(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin) { const GodotWorldBoundaryShape3D *world_boundary = static_cast<const GodotWorldBoundaryShape3D *>(p_shape_A); if (p_shape_B->get_type() == PhysicsServer3D::SHAPE_WORLD_BOUNDARY) { return false; @@ -70,6 +70,7 @@ bool GodotCollisionSolver3D::solve_static_world_boundary(const GodotShape3D *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]); if (p.distance_to(supports[i]) >= 0) { continue; @@ -379,9 +380,9 @@ bool GodotCollisionSolver3D::solve_static(const GodotShape3D *p_shape_A, const T } 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_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_result_callback, p_userdata, false, p_margin_B); } } else if (type_A == PhysicsServer3D::SHAPE_SEPARATION_RAY) { @@ -456,8 +457,17 @@ bool GodotCollisionSolver3D::solve_distance_world_boundary(const GodotShape3D *p Vector3 supports[max_supports]; int support_count; GodotShape3D::FeatureType support_type; + Vector3 support_direction = p_transform_B.basis.xform_inv(-p.normal).normalized(); - p_shape_B->get_supports(p_transform_B.basis.xform_inv(-p.normal).normalized(), max_supports, supports, support_count, support_type); + p_shape_B->get_supports(support_direction, max_supports, supports, support_count, support_type); + + if (support_count == 0) { // This is a poor man's way to detect shapes that don't implement get_supports, such as GodotMotionShape3D. + Vector3 support_B = p_transform_B.xform(p_shape_B->get_support(support_direction)); + r_point_A = p.project(support_B); + r_point_B = support_B; + bool collided = p.distance_to(support_B) <= 0; + return collided; + } if (support_type == GodotShape3D::FEATURE_CIRCLE) { ERR_FAIL_COND_V(support_count != 3, false); diff --git a/servers/physics_3d/godot_collision_solver_3d.h b/servers/physics_3d/godot_collision_solver_3d.h index a6a0ebfead..e7d67903e9 100644 --- a/servers/physics_3d/godot_collision_solver_3d.h +++ b/servers/physics_3d/godot_collision_solver_3d.h @@ -42,7 +42,7 @@ private: static void soft_body_contact_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata); static bool soft_body_concave_callback(void *p_userdata, GodotShape3D *p_convex); static bool concave_callback(void *p_userdata, GodotShape3D *p_convex); - static bool solve_static_world_boundary(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result); + static bool solve_static_world_boundary(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin = 0); static bool solve_separation_ray(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin = 0); static bool solve_soft_body(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result); static bool solve_concave(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &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); |