From 9758a75221854b5ed533bc826ebba1beb7f8cf3f Mon Sep 17 00:00:00 2001 From: PouleyKetchoupp Date: Thu, 24 Jun 2021 19:25:26 -0700 Subject: [PATCH] Fix move_and_collide causing sliding on slopes Make sure the direction of the motion is preserved, unless the depth is higher than the margin, which means the body needs depenetration in any direction. Also changed move_and_slide to avoid sliding on the first motion, in order to avoid issues with unstable position on ground when jumping. Co-authored-by: fabriceci --- doc/classes/PhysicsTestMotionResult2D.xml | 6 +++ scene/2d/physics_body_2d.cpp | 57 ++++++++++++++++---- scene/2d/physics_body_2d.h | 2 +- scene/3d/physics_body_3d.cpp | 63 ++++++++++++++++++----- scene/3d/physics_body_3d.h | 2 +- servers/physics_2d/space_2d_sw.cpp | 3 ++ servers/physics_3d/space_3d_sw.cpp | 3 ++ servers/physics_server_2d.cpp | 18 +++++++ servers/physics_server_2d.h | 6 +++ servers/physics_server_3d.h | 3 ++ 10 files changed, 138 insertions(+), 25 deletions(-) diff --git a/doc/classes/PhysicsTestMotionResult2D.xml b/doc/classes/PhysicsTestMotionResult2D.xml index da04ffa86a1..bf3497386e8 100644 --- a/doc/classes/PhysicsTestMotionResult2D.xml +++ b/doc/classes/PhysicsTestMotionResult2D.xml @@ -19,10 +19,16 @@ + + + + + + diff --git a/scene/2d/physics_body_2d.cpp b/scene/2d/physics_body_2d.cpp index ce63b25ce07..baf092c530c 100644 --- a/scene/2d/physics_body_2d.cpp +++ b/scene/2d/physics_body_2d.cpp @@ -76,13 +76,46 @@ Ref PhysicsBody2D::_move(const Vector2 &p_motion, bool p_i return Ref(); } -bool PhysicsBody2D::move_and_collide(const Vector2 &p_motion, bool p_infinite_inertia, PhysicsServer2D::MotionResult &r_result, real_t p_margin, bool p_exclude_raycast_shapes, bool p_test_only) { +bool PhysicsBody2D::move_and_collide(const Vector2 &p_motion, bool p_infinite_inertia, PhysicsServer2D::MotionResult &r_result, real_t p_margin, bool p_exclude_raycast_shapes, bool p_test_only, bool p_cancel_sliding) { if (is_only_update_transform_changes_enabled()) { ERR_PRINT("Move functions do not work together with 'sync to physics' option. Please read the documentation."); } Transform2D gt = get_global_transform(); bool colliding = PhysicsServer2D::get_singleton()->body_test_motion(get_rid(), gt, p_motion, p_infinite_inertia, p_margin, &r_result, p_exclude_raycast_shapes); + // Restore direction of motion to be along original motion, + // in order to avoid sliding due to recovery, + // but only if collision depth is low enough to avoid tunneling. + real_t motion_length = p_motion.length(); + if (motion_length > CMP_EPSILON) { + real_t precision = 0.001; + + if (colliding && p_cancel_sliding) { + // Can't just use margin as a threshold because collision depth is calculated on unsafe motion, + // so even in normal resting cases the depth can be a bit more than the margin. + precision += motion_length * (r_result.collision_unsafe_fraction - r_result.collision_safe_fraction); + + if (r_result.collision_depth > (real_t)p_margin + precision) { + p_cancel_sliding = false; + } + } + + if (p_cancel_sliding) { + // Check depth of recovery. + Vector2 motion_normal = p_motion / motion_length; + real_t dot = r_result.motion.dot(motion_normal); + Vector2 recovery = r_result.motion - motion_normal * dot; + real_t recovery_length = recovery.length(); + // Fixes cases where canceling slide causes the motion to go too deep into the ground, + // Becauses we're only taking rest information into account and not general recovery. + if (recovery_length < (real_t)p_margin + precision) { + // Apply adjustment to motion. + r_result.motion = motion_normal * dot; + r_result.remainder = p_motion - r_result.motion; + } + } + } + if (!p_test_only) { gt.elements[2] += r_result.motion; set_global_transform(gt); @@ -942,15 +975,16 @@ void CharacterBody2D::move_and_slide() { floor_normal = Vector2(); floor_velocity = Vector2(); - int slide_count = max_slides; - while (slide_count) { + // No sliding on first attempt to keep floor motion stable when possible. + bool sliding_enabled = false; + for (int iteration = 0; iteration < max_slides; ++iteration) { PhysicsServer2D::MotionResult result; bool found_collision = false; for (int i = 0; i < 2; ++i) { bool collided; if (i == 0) { //collide - collided = move_and_collide(motion, infinite_inertia, result, margin); + collided = move_and_collide(motion, infinite_inertia, result, margin, true, false, !sliding_enabled); if (!collided) { motion = Vector2(); //clear because no collision happened and motion completed } @@ -966,7 +1000,6 @@ void CharacterBody2D::move_and_slide() { found_collision = true; motion_results.push_back(result); - motion = result.remainder; if (up_direction == Vector2()) { //all is a wall @@ -980,7 +1013,7 @@ void CharacterBody2D::move_and_slide() { floor_velocity = result.collider_velocity; if (stop_on_slope) { - if ((body_velocity_normal + up_direction).length() < 0.01 && result.motion.length() < 1) { + if ((body_velocity_normal + up_direction).length() < 0.01) { Transform2D gt = get_global_transform(); gt.elements[2] -= result.motion.slide(up_direction); set_global_transform(gt); @@ -995,16 +1028,20 @@ void CharacterBody2D::move_and_slide() { } } - motion = motion.slide(result.collision_normal); - linear_velocity = linear_velocity.slide(result.collision_normal); + if (sliding_enabled || !on_floor) { + motion = result.remainder.slide(result.collision_normal); + linear_velocity = linear_velocity.slide(result.collision_normal); + } else { + motion = result.remainder; + } } + + sliding_enabled = true; } if (!found_collision || motion == Vector2()) { break; } - - --slide_count; } if (!was_on_floor || snap == Vector2()) { diff --git a/scene/2d/physics_body_2d.h b/scene/2d/physics_body_2d.h index f084a247aa9..b1d48a2cc9c 100644 --- a/scene/2d/physics_body_2d.h +++ b/scene/2d/physics_body_2d.h @@ -50,7 +50,7 @@ protected: Ref _move(const Vector2 &p_motion, bool p_infinite_inertia = true, bool p_exclude_raycast_shapes = true, bool p_test_only = false, real_t p_margin = 0.08); public: - bool move_and_collide(const Vector2 &p_motion, bool p_infinite_inertia, PhysicsServer2D::MotionResult &r_result, real_t p_margin, bool p_exclude_raycast_shapes = true, bool p_test_only = false); + bool move_and_collide(const Vector2 &p_motion, bool p_infinite_inertia, PhysicsServer2D::MotionResult &r_result, real_t p_margin, bool p_exclude_raycast_shapes = true, bool p_test_only = false, bool p_cancel_sliding = true); bool test_move(const Transform2D &p_from, const Vector2 &p_motion, bool p_infinite_inertia = true, bool p_exclude_raycast_shapes = true, const Ref &r_collision = Ref(), real_t p_margin = 0.08); TypedArray get_collision_exceptions(); diff --git a/scene/3d/physics_body_3d.cpp b/scene/3d/physics_body_3d.cpp index 28a0c72fe36..c9afb5f201e 100644 --- a/scene/3d/physics_body_3d.cpp +++ b/scene/3d/physics_body_3d.cpp @@ -118,10 +118,43 @@ Ref PhysicsBody3D::_move(const Vector3 &p_motion, bool p_i return Ref(); } -bool PhysicsBody3D::move_and_collide(const Vector3 &p_motion, bool p_infinite_inertia, PhysicsServer3D::MotionResult &r_result, real_t p_margin, bool p_exclude_raycast_shapes, bool p_test_only) { +bool PhysicsBody3D::move_and_collide(const Vector3 &p_motion, bool p_infinite_inertia, PhysicsServer3D::MotionResult &r_result, real_t p_margin, bool p_exclude_raycast_shapes, bool p_test_only, bool p_cancel_sliding) { Transform3D gt = get_global_transform(); bool colliding = PhysicsServer3D::get_singleton()->body_test_motion(get_rid(), gt, p_motion, p_infinite_inertia, p_margin, &r_result, p_exclude_raycast_shapes); + // Restore direction of motion to be along original motion, + // in order to avoid sliding due to recovery, + // but only if collision depth is low enough to avoid tunneling. + real_t motion_length = p_motion.length(); + if (motion_length > CMP_EPSILON) { + real_t precision = CMP_EPSILON; + + if (colliding && p_cancel_sliding) { + // Can't just use margin as a threshold because collision depth is calculated on unsafe motion, + // so even in normal resting cases the depth can be a bit more than the margin. + precision += motion_length * (r_result.collision_unsafe_fraction - r_result.collision_safe_fraction); + + if (r_result.collision_depth > (real_t)p_margin + precision) { + p_cancel_sliding = false; + } + } + + if (p_cancel_sliding) { + // Check depth of recovery. + Vector3 motion_normal = p_motion / motion_length; + real_t dot = r_result.motion.dot(motion_normal); + Vector3 recovery = r_result.motion - motion_normal * dot; + real_t recovery_length = recovery.length(); + // Fixes cases where canceling slide causes the motion to go too deep into the ground, + // Becauses we're only taking rest information into account and not general recovery. + if (recovery_length < (real_t)p_margin + precision) { + // Apply adjustment to motion. + r_result.motion = motion_normal * dot; + r_result.remainder = p_motion - r_result.motion; + } + } + } + for (int i = 0; i < 3; i++) { if (locked_axis & (1 << i)) { r_result.motion[i] = 0; @@ -978,15 +1011,16 @@ void CharacterBody3D::move_and_slide() { floor_normal = Vector3(); floor_velocity = Vector3(); - int slide_count = max_slides; - while (slide_count) { + // No sliding on first attempt to keep motion stable when possible. + bool sliding_enabled = false; + for (int iteration = 0; iteration < max_slides; ++iteration) { PhysicsServer3D::MotionResult result; bool found_collision = false; for (int i = 0; i < 2; ++i) { bool collided; if (i == 0) { //collide - collided = move_and_collide(motion, infinite_inertia, result, margin); + collided = move_and_collide(motion, infinite_inertia, result, margin, true, false, !sliding_enabled); if (!collided) { motion = Vector3(); //clear because no collision happened and motion completed } @@ -1002,7 +1036,6 @@ void CharacterBody3D::move_and_slide() { found_collision = true; motion_results.push_back(result); - motion = result.remainder; if (up_direction == Vector3()) { //all is a wall @@ -1016,7 +1049,7 @@ void CharacterBody3D::move_and_slide() { floor_velocity = result.collider_velocity; if (stop_on_slope) { - if ((body_velocity_normal + up_direction).length() < 0.01 && result.motion.length() < 1) { + if ((body_velocity_normal + up_direction).length() < 0.01) { Transform3D gt = get_global_transform(); gt.origin -= result.motion.slide(up_direction); set_global_transform(gt); @@ -1031,22 +1064,26 @@ void CharacterBody3D::move_and_slide() { } } - motion = motion.slide(result.collision_normal); - linear_velocity = linear_velocity.slide(result.collision_normal); + if (sliding_enabled || !on_floor) { + motion = result.remainder.slide(result.collision_normal); + linear_velocity = linear_velocity.slide(result.collision_normal); - for (int j = 0; j < 3; j++) { - if (locked_axis & (1 << j)) { - linear_velocity[j] = 0.0; + for (int j = 0; j < 3; j++) { + if (locked_axis & (1 << j)) { + linear_velocity[j] = 0.0; + } } + } else { + motion = result.remainder; } } + + sliding_enabled = true; } if (!found_collision || motion == Vector3()) { break; } - - --slide_count; } if (!was_on_floor || snap == Vector3()) { diff --git a/scene/3d/physics_body_3d.h b/scene/3d/physics_body_3d.h index 7d7adf16245..bc4556494d9 100644 --- a/scene/3d/physics_body_3d.h +++ b/scene/3d/physics_body_3d.h @@ -53,7 +53,7 @@ protected: Ref _move(const Vector3 &p_motion, bool p_infinite_inertia = true, bool p_exclude_raycast_shapes = true, bool p_test_only = false, real_t p_margin = 0.001); public: - bool move_and_collide(const Vector3 &p_motion, bool p_infinite_inertia, PhysicsServer3D::MotionResult &r_result, real_t p_margin, bool p_exclude_raycast_shapes = true, bool p_test_only = false); + bool move_and_collide(const Vector3 &p_motion, bool p_infinite_inertia, PhysicsServer3D::MotionResult &r_result, real_t p_margin, bool p_exclude_raycast_shapes = true, bool p_test_only = false, bool p_cancel_sliding = true); bool test_move(const Transform3D &p_from, const Vector3 &p_motion, bool p_infinite_inertia = true, bool p_exclude_raycast_shapes = true, const Ref &r_collision = Ref(), real_t p_margin = 0.001); void set_axis_lock(PhysicsServer3D::BodyAxis p_axis, bool p_lock); diff --git a/servers/physics_2d/space_2d_sw.cpp b/servers/physics_2d/space_2d_sw.cpp index 1380e57b570..905f08011b6 100644 --- a/servers/physics_2d/space_2d_sw.cpp +++ b/servers/physics_2d/space_2d_sw.cpp @@ -1142,6 +1142,9 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co r_result->collision_local_shape = rcd.best_local_shape; r_result->collision_normal = rcd.best_normal; r_result->collision_point = rcd.best_contact; + r_result->collision_depth = rcd.best_len; + r_result->collision_safe_fraction = safe; + r_result->collision_unsafe_fraction = unsafe; r_result->collider_metadata = rcd.best_object->get_shape_metadata(rcd.best_shape); const Body2DSW *body = static_cast(rcd.best_object); diff --git a/servers/physics_3d/space_3d_sw.cpp b/servers/physics_3d/space_3d_sw.cpp index c1e09c9a229..e411ff8266d 100644 --- a/servers/physics_3d/space_3d_sw.cpp +++ b/servers/physics_3d/space_3d_sw.cpp @@ -1032,6 +1032,9 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, co r_result->collision_local_shape = rcd.best_local_shape; r_result->collision_normal = rcd.best_normal; r_result->collision_point = rcd.best_contact; + r_result->collision_depth = rcd.best_len; + r_result->collision_safe_fraction = safe; + r_result->collision_unsafe_fraction = unsafe; //r_result->collider_metadata = rcd.best_object->get_shape_metadata(rcd.best_shape); const Body3DSW *body = static_cast(rcd.best_object); diff --git a/servers/physics_server_2d.cpp b/servers/physics_server_2d.cpp index faab4f43b8b..12200989fd2 100644 --- a/servers/physics_server_2d.cpp +++ b/servers/physics_server_2d.cpp @@ -487,6 +487,18 @@ int PhysicsTestMotionResult2D::get_collider_shape() const { return result.collider_shape; } +real_t PhysicsTestMotionResult2D::get_collision_depth() const { + return result.collision_depth; +} + +real_t PhysicsTestMotionResult2D::get_collision_safe_fraction() const { + return result.collision_safe_fraction; +} + +real_t PhysicsTestMotionResult2D::get_collision_unsafe_fraction() const { + return result.collision_unsafe_fraction; +} + void PhysicsTestMotionResult2D::_bind_methods() { ClassDB::bind_method(D_METHOD("get_motion"), &PhysicsTestMotionResult2D::get_motion); ClassDB::bind_method(D_METHOD("get_motion_remainder"), &PhysicsTestMotionResult2D::get_motion_remainder); @@ -497,6 +509,9 @@ void PhysicsTestMotionResult2D::_bind_methods() { ClassDB::bind_method(D_METHOD("get_collider_rid"), &PhysicsTestMotionResult2D::get_collider_rid); ClassDB::bind_method(D_METHOD("get_collider"), &PhysicsTestMotionResult2D::get_collider); ClassDB::bind_method(D_METHOD("get_collider_shape"), &PhysicsTestMotionResult2D::get_collider_shape); + ClassDB::bind_method(D_METHOD("get_collision_depth"), &PhysicsTestMotionResult2D::get_collision_depth); + ClassDB::bind_method(D_METHOD("get_collision_safe_fraction"), &PhysicsTestMotionResult2D::get_collision_safe_fraction); + ClassDB::bind_method(D_METHOD("get_collision_unsafe_fraction"), &PhysicsTestMotionResult2D::get_collision_unsafe_fraction); ADD_PROPERTY(PropertyInfo(Variant::VECTOR2, "motion"), "", "get_motion"); ADD_PROPERTY(PropertyInfo(Variant::VECTOR2, "motion_remainder"), "", "get_motion_remainder"); @@ -507,6 +522,9 @@ void PhysicsTestMotionResult2D::_bind_methods() { ADD_PROPERTY(PropertyInfo(Variant::RID, "collider_rid"), "", "get_collider_rid"); ADD_PROPERTY(PropertyInfo(Variant::OBJECT, "collider"), "", "get_collider"); ADD_PROPERTY(PropertyInfo(Variant::INT, "collider_shape"), "", "get_collider_shape"); + ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "collision_depth"), "", "get_collision_depth"); + ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "collision_safe_fraction"), "", "get_collision_safe_fraction"); + ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "collision_unsafe_fraction"), "", "get_collision_unsafe_fraction"); } /////////////////////////////////////// diff --git a/servers/physics_server_2d.h b/servers/physics_server_2d.h index df39d6ae505..90f50810f9e 100644 --- a/servers/physics_server_2d.h +++ b/servers/physics_server_2d.h @@ -493,6 +493,9 @@ public: Vector2 collision_point; Vector2 collision_normal; Vector2 collider_velocity; + real_t collision_depth = 0.0; + real_t collision_safe_fraction = 0.0; + real_t collision_unsafe_fraction = 0.0; int collision_local_shape = 0; ObjectID collider_id; RID collider; @@ -619,6 +622,9 @@ public: RID get_collider_rid() const; Object *get_collider() const; int get_collider_shape() const; + real_t get_collision_depth() const; + real_t get_collision_safe_fraction() const; + real_t get_collision_unsafe_fraction() const; }; typedef PhysicsServer2D *(*CreatePhysicsServer2DCallback)(); diff --git a/servers/physics_server_3d.h b/servers/physics_server_3d.h index d9b805de87c..fcdd207843d 100644 --- a/servers/physics_server_3d.h +++ b/servers/physics_server_3d.h @@ -497,6 +497,9 @@ public: Vector3 collision_point; Vector3 collision_normal; Vector3 collider_velocity; + real_t collision_depth = 0.0; + real_t collision_safe_fraction = 0.0; + real_t collision_unsafe_fraction = 0.0; int collision_local_shape = 0; ObjectID collider_id; RID collider;