From fba4c9d5520f2e96054b74ba457e05067f23ccc2 Mon Sep 17 00:00:00 2001 From: fabriceci Date: Sun, 4 Jul 2021 20:05:22 +0200 Subject: [PATCH] Fixing 2D moving platform logic MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Fixing by applying the movement in two steps, first the platform movement, and then the body movement. Plus, add the platform movement when we are on_wall. --- doc/classes/Physics2DServer.xml | 4 + scene/2d/physics_body_2d.cpp | 87 ++++++++++++------- scene/2d/physics_body_2d.h | 3 +- servers/physics_2d/physics_2d_server_sw.cpp | 4 +- servers/physics_2d/physics_2d_server_sw.h | 2 +- .../physics_2d/physics_2d_server_wrap_mt.h | 4 +- servers/physics_2d/space_2d_sw.cpp | 11 ++- servers/physics_2d/space_2d_sw.h | 2 +- servers/physics_2d_server.cpp | 10 ++- servers/physics_2d_server.h | 4 +- 10 files changed, 88 insertions(+), 43 deletions(-) diff --git a/doc/classes/Physics2DServer.xml b/doc/classes/Physics2DServer.xml index 70a1cbe8b49..0f8d01bad73 100644 --- a/doc/classes/Physics2DServer.xml +++ b/doc/classes/Physics2DServer.xml @@ -822,6 +822,10 @@ + + + + Returns [code]true[/code] if a collision would result from moving in the given direction from a given point in space. Margin increases the size of the shapes involved in the collision detection. [Physics2DTestMotionResult] can be passed to return additional information in. diff --git a/scene/2d/physics_body_2d.cpp b/scene/2d/physics_body_2d.cpp index 5092492f532..1b1418b783d 100644 --- a/scene/2d/physics_body_2d.cpp +++ b/scene/2d/physics_body_2d.cpp @@ -1030,13 +1030,14 @@ bool KinematicBody2D::separate_raycast_shapes(bool p_infinite_inertia, Collision } } -bool KinematicBody2D::move_and_collide(const Vector2 &p_motion, bool p_infinite_inertia, Collision &r_collision, bool p_exclude_raycast_shapes, bool p_test_only) { +bool KinematicBody2D::move_and_collide(const Vector2 &p_motion, bool p_infinite_inertia, Collision &r_collision, bool p_exclude_raycast_shapes, bool p_test_only, const Set &p_exclude) { if (sync_to_physics) { ERR_PRINT("Functions move_and_slide and move_and_collide do not work together with 'sync to physics' option. Please read the documentation."); } Transform2D gt = get_global_transform(); Physics2DServer::MotionResult result; - bool colliding = Physics2DServer::get_singleton()->body_test_motion(get_rid(), gt, p_motion, p_infinite_inertia, margin, &result, p_exclude_raycast_shapes); + + bool colliding = Physics2DServer::get_singleton()->body_test_motion(get_rid(), gt, p_motion, p_infinite_inertia, margin, &result, p_exclude_raycast_shapes, p_exclude); if (colliding) { r_collision.collider_metadata = result.collider_metadata; @@ -1067,8 +1068,12 @@ Vector2 KinematicBody2D::move_and_slide(const Vector2 &p_linear_velocity, const Vector2 body_velocity_normal = body_velocity.normalized(); Vector2 up_direction = p_up_direction.normalized(); + // Hack in order to work with calling from _process as well as from _physics_process; calling from thread is risky + float delta = Engine::get_singleton()->is_in_physics_frame() ? get_physics_process_delta_time() : get_process_delta_time(); + Vector2 current_floor_velocity = floor_velocity; - if (on_floor && on_floor_body.is_valid()) { + + if ((on_floor || on_wall) && on_floor_body.is_valid()) { //this approach makes sure there is less delay between the actual body velocity and the one we saved Physics2DDirectBodyState *bs = Physics2DServer::get_singleton()->body_get_direct_state(on_floor_body); if (bs) { @@ -1076,17 +1081,26 @@ Vector2 KinematicBody2D::move_and_slide(const Vector2 &p_linear_velocity, const } } - // Hack in order to work with calling from _process as well as from _physics_process; calling from thread is risky - Vector2 motion = (current_floor_velocity + body_velocity) * (Engine::get_singleton()->is_in_physics_frame() ? get_physics_process_delta_time() : get_process_delta_time()); - + colliders.clear(); on_floor = false; - on_floor_body = RID(); on_ceiling = false; on_wall = false; - colliders.clear(); floor_normal = Vector2(); floor_velocity = Vector2(); + if (current_floor_velocity != Vector2()) { + Collision floor_collision; + Set exclude; + exclude.insert(on_floor_body); + if (move_and_collide(current_floor_velocity * delta, p_infinite_inertia, floor_collision, true, false, exclude)) { + colliders.push_back(floor_collision); + _set_collision_direction(floor_collision, up_direction, p_floor_max_angle); + } + } + + on_floor_body = RID(); + Vector2 motion = body_velocity * delta; + while (p_max_slides) { Collision collision; bool found_collision = false; @@ -1112,29 +1126,14 @@ Vector2 KinematicBody2D::move_and_slide(const Vector2 &p_linear_velocity, const colliders.push_back(collision); motion = collision.remainder; - if (up_direction == Vector2()) { - //all is a wall - on_wall = true; - } else { - if (Math::acos(collision.normal.dot(up_direction)) <= p_floor_max_angle + FLOOR_ANGLE_THRESHOLD) { //floor + _set_collision_direction(collision, up_direction, p_floor_max_angle); - on_floor = true; - floor_normal = collision.normal; - on_floor_body = collision.collider_rid; - floor_velocity = collision.collider_vel; - - if (p_stop_on_slope) { - if ((body_velocity_normal + up_direction).length() < 0.01 && collision.travel.length() < 1) { - Transform2D gt = get_global_transform(); - gt.elements[2] -= collision.travel.slide(up_direction); - set_global_transform(gt); - return Vector2(); - } - } - } else if (Math::acos(collision.normal.dot(-up_direction)) <= p_floor_max_angle + FLOOR_ANGLE_THRESHOLD) { //ceiling - on_ceiling = true; - } else { - on_wall = true; + if (on_floor && p_stop_on_slope) { + if ((body_velocity_normal + up_direction).length() < 0.01 && collision.travel.length() < 1) { + Transform2D gt = get_global_transform(); + gt.elements[2] -= collision.travel.slide(up_direction); + set_global_transform(gt); + return Vector2(); } } @@ -1150,6 +1149,11 @@ Vector2 KinematicBody2D::move_and_slide(const Vector2 &p_linear_velocity, const --p_max_slides; } + if (!on_floor && !on_wall) { + // Add last platform velocity when just left a moving platform. + return body_velocity + current_floor_velocity; + } + return body_velocity; } @@ -1193,6 +1197,29 @@ Vector2 KinematicBody2D::move_and_slide_with_snap(const Vector2 &p_linear_veloci return ret; } +void KinematicBody2D::_set_collision_direction(const Collision &p_collision, const Vector2 &p_up_direction, float p_floor_max_angle) { + on_floor = false; + on_ceiling = false; + on_wall = false; + if (p_up_direction == Vector2()) { + //all is a wall + on_wall = true; + } else { + if (Math::acos(p_collision.normal.dot(p_up_direction)) <= p_floor_max_angle + FLOOR_ANGLE_THRESHOLD) { //floor + on_floor = true; + floor_normal = p_collision.normal; + on_floor_body = p_collision.collider_rid; + floor_velocity = p_collision.collider_vel; + } else if (Math::acos(p_collision.normal.dot(-p_up_direction)) <= p_floor_max_angle + FLOOR_ANGLE_THRESHOLD) { //ceiling + on_ceiling = true; + } else { + on_wall = true; + on_floor_body = p_collision.collider_rid; + floor_velocity = p_collision.collider_vel; + } + } +} + bool KinematicBody2D::is_on_floor() const { return on_floor; } diff --git a/scene/2d/physics_body_2d.h b/scene/2d/physics_body_2d.h index b14dd57e43e..68c123bda66 100644 --- a/scene/2d/physics_body_2d.h +++ b/scene/2d/physics_body_2d.h @@ -304,13 +304,14 @@ private: Transform2D last_valid_transform; void _direct_state_changed(Object *p_state); + void _set_collision_direction(const Collision &p_collision, const Vector2 &p_up_direction, float p_floor_max_angle); protected: void _notification(int p_what); static void _bind_methods(); public: - bool move_and_collide(const Vector2 &p_motion, bool p_infinite_inertia, Collision &r_collision, bool p_exclude_raycast_shapes = true, bool p_test_only = false); + bool move_and_collide(const Vector2 &p_motion, bool p_infinite_inertia, Collision &r_collision, bool p_exclude_raycast_shapes = true, bool p_test_only = false, const Set &p_exclude = Set()); bool test_move(const Transform2D &p_from, const Vector2 &p_motion, bool p_infinite_inertia = true); diff --git a/servers/physics_2d/physics_2d_server_sw.cpp b/servers/physics_2d/physics_2d_server_sw.cpp index 12641172cae..eb526fe2ea4 100644 --- a/servers/physics_2d/physics_2d_server_sw.cpp +++ b/servers/physics_2d/physics_2d_server_sw.cpp @@ -933,7 +933,7 @@ void Physics2DServerSW::body_set_pickable(RID p_body, bool p_pickable) { body->set_pickable(p_pickable); } -bool Physics2DServerSW::body_test_motion(RID p_body, const Transform2D &p_from, const Vector2 &p_motion, bool p_infinite_inertia, real_t p_margin, MotionResult *r_result, bool p_exclude_raycast_shapes) { +bool Physics2DServerSW::body_test_motion(RID p_body, const Transform2D &p_from, const Vector2 &p_motion, bool p_infinite_inertia, real_t p_margin, MotionResult *r_result, bool p_exclude_raycast_shapes, const Set &p_exclude) { Body2DSW *body = body_owner.get(p_body); ERR_FAIL_COND_V(!body, false); ERR_FAIL_COND_V(!body->get_space(), false); @@ -941,7 +941,7 @@ bool Physics2DServerSW::body_test_motion(RID p_body, const Transform2D &p_from, _update_shapes(); - return body->get_space()->test_body_motion(body, p_from, p_motion, p_infinite_inertia, p_margin, r_result, p_exclude_raycast_shapes); + return body->get_space()->test_body_motion(body, p_from, p_motion, p_infinite_inertia, p_margin, r_result, p_exclude_raycast_shapes, p_exclude); } int Physics2DServerSW::body_test_ray_separation(RID p_body, const Transform2D &p_transform, bool p_infinite_inertia, Vector2 &r_recover_motion, SeparationResult *r_results, int p_result_max, float p_margin) { diff --git a/servers/physics_2d/physics_2d_server_sw.h b/servers/physics_2d/physics_2d_server_sw.h index 6ab6459f5b8..7d5ab25e842 100644 --- a/servers/physics_2d/physics_2d_server_sw.h +++ b/servers/physics_2d/physics_2d_server_sw.h @@ -246,7 +246,7 @@ public: virtual void body_set_pickable(RID p_body, bool p_pickable); - virtual bool body_test_motion(RID p_body, const Transform2D &p_from, const Vector2 &p_motion, bool p_infinite_inertia, real_t p_margin = 0.08, MotionResult *r_result = nullptr, bool p_exclude_raycast_shapes = true); + virtual bool body_test_motion(RID p_body, const Transform2D &p_from, const Vector2 &p_motion, bool p_infinite_inertia, real_t p_margin = 0.08, MotionResult *r_result = nullptr, bool p_exclude_raycast_shapes = true, const Set &p_exclude = Set()); virtual int body_test_ray_separation(RID p_body, const Transform2D &p_transform, bool p_infinite_inertia, Vector2 &r_recover_motion, SeparationResult *r_results, int p_result_max, float p_margin = 0.08); // this function only works on physics process, errors and returns null otherwise diff --git a/servers/physics_2d/physics_2d_server_wrap_mt.h b/servers/physics_2d/physics_2d_server_wrap_mt.h index f11ed99db29..a4ee5ef4311 100644 --- a/servers/physics_2d/physics_2d_server_wrap_mt.h +++ b/servers/physics_2d/physics_2d_server_wrap_mt.h @@ -251,9 +251,9 @@ public: FUNC2(body_set_pickable, RID, bool); - bool body_test_motion(RID p_body, const Transform2D &p_from, const Vector2 &p_motion, bool p_infinite_inertia, real_t p_margin = 0.08, MotionResult *r_result = nullptr, bool p_exclude_raycast_shapes = true) { + bool body_test_motion(RID p_body, const Transform2D &p_from, const Vector2 &p_motion, bool p_infinite_inertia, real_t p_margin = 0.08, MotionResult *r_result = nullptr, bool p_exclude_raycast_shapes = true, const Set &p_exclude = Set()) { ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), false); - return physics_2d_server->body_test_motion(p_body, p_from, p_motion, p_infinite_inertia, p_margin, r_result, p_exclude_raycast_shapes); + return physics_2d_server->body_test_motion(p_body, p_from, p_motion, p_infinite_inertia, p_margin, r_result, p_exclude_raycast_shapes, p_exclude); } int body_test_ray_separation(RID p_body, const Transform2D &p_transform, bool p_infinite_inertia, Vector2 &r_recover_motion, SeparationResult *r_results, int p_result_max, float p_margin = 0.08) { diff --git a/servers/physics_2d/space_2d_sw.cpp b/servers/physics_2d/space_2d_sw.cpp index 02b49f3e444..45375652fa0 100644 --- a/servers/physics_2d/space_2d_sw.cpp +++ b/servers/physics_2d/space_2d_sw.cpp @@ -692,7 +692,7 @@ int Space2DSW::test_body_ray_separation(Body2DSW *p_body, const Transform2D &p_t return rays_found; } -bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, const Vector2 &p_motion, bool p_infinite_inertia, real_t p_margin, Physics2DServer::MotionResult *r_result, bool p_exclude_raycast_shapes) { +bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, const Vector2 &p_motion, bool p_infinite_inertia, real_t p_margin, Physics2DServer::MotionResult *r_result, bool p_exclude_raycast_shapes, const Set &p_exclude) { //give me back regular physics engine logic //this is madness //and most people using this function will think @@ -784,6 +784,9 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co Transform2D body_shape_xform = body_transform * p_body->get_shape_transform(j); for (int i = 0; i < amount; i++) { const CollisionObject2DSW *col_obj = intersection_query_results[i]; + if (p_exclude.has(col_obj->get_self())) { + continue; + } int shape_idx = intersection_query_subindex_results[i]; if (CollisionObject2DSW::TYPE_BODY == col_obj->get_type()) { @@ -913,6 +916,9 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co for (int i = 0; i < amount; i++) { const CollisionObject2DSW *col_obj = intersection_query_results[i]; + if (p_exclude.has(col_obj->get_self())) { + continue; + } int col_shape_idx = intersection_query_subindex_results[i]; Shape2DSW *against_shape = col_obj->get_shape(col_shape_idx); @@ -1054,6 +1060,9 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co for (int i = 0; i < amount; i++) { const CollisionObject2DSW *col_obj = intersection_query_results[i]; + if (p_exclude.has(col_obj->get_self())) { + continue; + } int shape_idx = intersection_query_subindex_results[i]; if (CollisionObject2DSW::TYPE_BODY == col_obj->get_type()) { diff --git a/servers/physics_2d/space_2d_sw.h b/servers/physics_2d/space_2d_sw.h index 432592fcb20..c8a5ea7d7d7 100644 --- a/servers/physics_2d/space_2d_sw.h +++ b/servers/physics_2d/space_2d_sw.h @@ -184,7 +184,7 @@ public: int get_collision_pairs() const { return collision_pairs; } - bool test_body_motion(Body2DSW *p_body, const Transform2D &p_from, const Vector2 &p_motion, bool p_infinite_inertia, real_t p_margin, Physics2DServer::MotionResult *r_result, bool p_exclude_raycast_shapes = true); + bool test_body_motion(Body2DSW *p_body, const Transform2D &p_from, const Vector2 &p_motion, bool p_infinite_inertia, real_t p_margin, Physics2DServer::MotionResult *r_result, bool p_exclude_raycast_shapes = true, const Set &p_exclude = Set()); int test_body_ray_separation(Body2DSW *p_body, const Transform2D &p_transform, bool p_infinite_inertia, Vector2 &r_recover_motion, Physics2DServer::SeparationResult *r_results, int p_result_max, real_t p_margin); void set_debug_contacts(int p_amount) { contact_debug.resize(p_amount); } diff --git a/servers/physics_2d_server.cpp b/servers/physics_2d_server.cpp index 12f11a54362..d3e549eff81 100644 --- a/servers/physics_2d_server.cpp +++ b/servers/physics_2d_server.cpp @@ -468,12 +468,16 @@ void Physics2DTestMotionResult::_bind_methods() { /////////////////////////////////////// -bool Physics2DServer::_body_test_motion(RID p_body, const Transform2D &p_from, const Vector2 &p_motion, bool p_infinite_inertia, float p_margin, const Ref &p_result) { +bool Physics2DServer::_body_test_motion(RID p_body, const Transform2D &p_from, const Vector2 &p_motion, bool p_infinite_inertia, float p_margin, const Ref &p_result, bool p_exclude_raycast_shapes, const Vector &p_exclude) { MotionResult *r = nullptr; if (p_result.is_valid()) { r = p_result->get_result_ptr(); } - return body_test_motion(p_body, p_from, p_motion, p_infinite_inertia, p_margin, r); + Set exclude; + for (int i = 0; i < p_exclude.size(); i++) { + exclude.insert(p_exclude[i]); + } + return body_test_motion(p_body, p_from, p_motion, p_infinite_inertia, p_margin, r, p_exclude_raycast_shapes, exclude); } void Physics2DServer::_bind_methods() { @@ -600,7 +604,7 @@ void Physics2DServer::_bind_methods() { ClassDB::bind_method(D_METHOD("body_set_force_integration_callback", "body", "receiver", "method", "userdata"), &Physics2DServer::body_set_force_integration_callback, DEFVAL(Variant())); - ClassDB::bind_method(D_METHOD("body_test_motion", "body", "from", "motion", "infinite_inertia", "margin", "result"), &Physics2DServer::_body_test_motion, DEFVAL(0.08), DEFVAL(Variant())); + ClassDB::bind_method(D_METHOD("body_test_motion", "body", "from", "motion", "infinite_inertia", "margin", "result", "exclude_raycast_shapes", "exclude"), &Physics2DServer::_body_test_motion, DEFVAL(0.08), DEFVAL(Variant()), DEFVAL(true), DEFVAL(Array())); ClassDB::bind_method(D_METHOD("body_get_direct_state", "body"), &Physics2DServer::body_get_direct_state); diff --git a/servers/physics_2d_server.h b/servers/physics_2d_server.h index 6794b432a74..63b5de03af1 100644 --- a/servers/physics_2d_server.h +++ b/servers/physics_2d_server.h @@ -205,7 +205,7 @@ class Physics2DServer : public Object { static Physics2DServer *singleton; - virtual bool _body_test_motion(RID p_body, const Transform2D &p_from, const Vector2 &p_motion, bool p_infinite_inertia, float p_margin = 0.08, const Ref &p_result = Ref()); + virtual bool _body_test_motion(RID p_body, const Transform2D &p_from, const Vector2 &p_motion, bool p_infinite_inertia, float p_margin = 0.08, const Ref &p_result = Ref(), bool p_exclude_raycast_shapes = true, const Vector &p_exclude = Vector()); protected: static void _bind_methods(); @@ -476,7 +476,7 @@ public: Variant collider_metadata; }; - virtual bool body_test_motion(RID p_body, const Transform2D &p_from, const Vector2 &p_motion, bool p_infinite_inertia, float p_margin = 0.08, MotionResult *r_result = nullptr, bool p_exclude_raycast_shapes = true) = 0; + virtual bool body_test_motion(RID p_body, const Transform2D &p_from, const Vector2 &p_motion, bool p_infinite_inertia, float p_margin = 0.08, MotionResult *r_result = nullptr, bool p_exclude_raycast_shapes = true, const Set &p_exclude = Set()) = 0; struct SeparationResult { float collision_depth;