From 2fbb6fff4e42ad66dc064588c515029f8137c7d6 Mon Sep 17 00:00:00 2001 From: PouleyKetchoupp Date: Wed, 30 Jun 2021 18:59:40 -0700 Subject: [PATCH 1/2] 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/Physics2DTestMotionResult.xml | 6 +++ doc/classes/PhysicsTestMotionResult.xml | 6 +++ scene/2d/physics_body_2d.cpp | 58 +++++++++++++++++---- scene/2d/physics_body_2d.h | 2 +- scene/3d/physics_body.cpp | 62 ++++++++++++++++++----- scene/3d/physics_body.h | 2 +- servers/physics/space_sw.cpp | 3 ++ servers/physics_2d/space_2d_sw.cpp | 3 ++ servers/physics_2d_server.cpp | 18 +++++++ servers/physics_2d_server.h | 6 +++ servers/physics_server.cpp | 18 +++++++ servers/physics_server.h | 6 +++ 12 files changed, 166 insertions(+), 24 deletions(-) diff --git a/doc/classes/Physics2DTestMotionResult.xml b/doc/classes/Physics2DTestMotionResult.xml index aca74ee77fc..ea76a4f3e17 100644 --- a/doc/classes/Physics2DTestMotionResult.xml +++ b/doc/classes/Physics2DTestMotionResult.xml @@ -19,10 +19,16 @@ + + + + + + diff --git a/doc/classes/PhysicsTestMotionResult.xml b/doc/classes/PhysicsTestMotionResult.xml index 107778d73c2..9cccdc4b6a5 100644 --- a/doc/classes/PhysicsTestMotionResult.xml +++ b/doc/classes/PhysicsTestMotionResult.xml @@ -19,10 +19,16 @@ + + + + + + diff --git a/scene/2d/physics_body_2d.cpp b/scene/2d/physics_body_2d.cpp index 1b1418b783d..c7889e2bd2e 100644 --- a/scene/2d/physics_body_2d.cpp +++ b/scene/2d/physics_body_2d.cpp @@ -1030,7 +1030,7 @@ 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, const Set &p_exclude) { +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 p_cancel_sliding, 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."); } @@ -1039,6 +1039,39 @@ bool KinematicBody2D::move_and_collide(const Vector2 &p_motion, bool p_infinite_ bool colliding = Physics2DServer::get_singleton()->body_test_motion(get_rid(), gt, p_motion, p_infinite_inertia, margin, &result, p_exclude_raycast_shapes, p_exclude); + // 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 * (result.collision_unsafe_fraction - result.collision_safe_fraction); + + if (result.collision_depth > (real_t)margin + precision) { + p_cancel_sliding = false; + } + } + + if (p_cancel_sliding) { + // Check depth of recovery. + Vector2 motion_normal = p_motion / motion_length; + real_t dot = result.motion.dot(motion_normal); + Vector2 recovery = 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)margin + precision) { + // Apply adjustment to motion. + result.motion = motion_normal * dot; + result.remainder = p_motion - result.motion; + } + } + } + if (colliding) { r_collision.collider_metadata = result.collider_metadata; r_collision.collider_shape = result.collider_shape; @@ -1092,7 +1125,7 @@ Vector2 KinematicBody2D::move_and_slide(const Vector2 &p_linear_velocity, const 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)) { + if (move_and_collide(current_floor_velocity * delta, p_infinite_inertia, floor_collision, true, false, false, exclude)) { colliders.push_back(floor_collision); _set_collision_direction(floor_collision, up_direction, p_floor_max_angle); } @@ -1101,14 +1134,16 @@ Vector2 KinematicBody2D::move_and_slide(const Vector2 &p_linear_velocity, const on_floor_body = RID(); Vector2 motion = body_velocity * delta; - while (p_max_slides) { + // No sliding on first attempt to keep floor motion stable when possible. + bool sliding_enabled = false; + for (int iteration = 0; iteration < p_max_slides; ++iteration) { Collision collision; bool found_collision = false; for (int i = 0; i < 2; ++i) { bool collided; if (i == 0) { //collide - collided = move_and_collide(motion, p_infinite_inertia, collision); + collided = move_and_collide(motion, p_infinite_inertia, collision, true, false, !sliding_enabled); if (!collided) { motion = Vector2(); //clear because no collision happened and motion completed } @@ -1124,12 +1159,11 @@ Vector2 KinematicBody2D::move_and_slide(const Vector2 &p_linear_velocity, const found_collision = true; colliders.push_back(collision); - motion = collision.remainder; _set_collision_direction(collision, up_direction, p_floor_max_angle); if (on_floor && p_stop_on_slope) { - if ((body_velocity_normal + up_direction).length() < 0.01 && collision.travel.length() < 1) { + if ((body_velocity_normal + up_direction).length() < 0.01) { Transform2D gt = get_global_transform(); gt.elements[2] -= collision.travel.slide(up_direction); set_global_transform(gt); @@ -1137,16 +1171,20 @@ Vector2 KinematicBody2D::move_and_slide(const Vector2 &p_linear_velocity, const } } - motion = motion.slide(collision.normal); - body_velocity = body_velocity.slide(collision.normal); + if (sliding_enabled || !on_floor) { + motion = collision.remainder.slide(collision.normal); + body_velocity = body_velocity.slide(collision.normal); + } else { + motion = collision.remainder; + } } + + sliding_enabled = true; } if (!found_collision || motion == Vector2()) { break; } - - --p_max_slides; } if (!on_floor && !on_wall) { diff --git a/scene/2d/physics_body_2d.h b/scene/2d/physics_body_2d.h index 68c123bda66..2c403bb0ace 100644 --- a/scene/2d/physics_body_2d.h +++ b/scene/2d/physics_body_2d.h @@ -311,7 +311,7 @@ protected: 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, const Set &p_exclude = Set()); + 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 p_cancel_sliding = true, const Set &p_exclude = Set()); bool test_move(const Transform2D &p_from, const Vector2 &p_motion, bool p_infinite_inertia = true); diff --git a/scene/3d/physics_body.cpp b/scene/3d/physics_body.cpp index 0feb73e34cf..70c84512f9f 100644 --- a/scene/3d/physics_body.cpp +++ b/scene/3d/physics_body.cpp @@ -973,7 +973,7 @@ Ref KinematicBody::_move(const Vector3 &p_motion, bool p_inf return Ref(); } -bool KinematicBody::move_and_collide(const Vector3 &p_motion, bool p_infinite_inertia, Collision &r_collision, bool p_exclude_raycast_shapes, bool p_test_only) { +bool KinematicBody::move_and_collide(const Vector3 &p_motion, bool p_infinite_inertia, Collision &r_collision, bool p_exclude_raycast_shapes, bool p_test_only, bool p_cancel_sliding) { 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."); } @@ -982,6 +982,39 @@ bool KinematicBody::move_and_collide(const Vector3 &p_motion, bool p_infinite_in PhysicsServer::MotionResult result; bool colliding = PhysicsServer::get_singleton()->body_test_motion(get_rid(), gt, p_motion, p_infinite_inertia, &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 * (result.collision_unsafe_fraction - result.collision_safe_fraction); + + if (result.collision_depth > (real_t)margin + precision) { + p_cancel_sliding = false; + } + } + + if (p_cancel_sliding) { + // Check depth of recovery. + Vector3 motion_normal = p_motion / motion_length; + real_t dot = result.motion.dot(motion_normal); + Vector3 recovery = 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)margin + precision) { + // Apply adjustment to motion. + result.motion = motion_normal * dot; + result.remainder = p_motion - result.motion; + } + } + } + if (colliding) { r_collision.collider_metadata = result.collider_metadata; r_collision.collider_shape = result.collider_shape; @@ -1043,14 +1076,16 @@ Vector3 KinematicBody::move_and_slide(const Vector3 &p_linear_velocity, const Ve floor_normal = Vector3(); floor_velocity = Vector3(); - while (p_max_slides) { + // No sliding on first attempt to keep motion stable when possible. + bool sliding_enabled = false; + for (int iteration = 0; iteration < p_max_slides; ++iteration) { Collision collision; bool found_collision = false; for (int i = 0; i < 2; ++i) { bool collided; if (i == 0) { //collide - collided = move_and_collide(motion, p_infinite_inertia, collision); + collided = move_and_collide(motion, p_infinite_inertia, collision, true, false, !sliding_enabled); if (!collided) { motion = Vector3(); //clear because no collision happened and motion completed } @@ -1066,7 +1101,6 @@ Vector3 KinematicBody::move_and_slide(const Vector3 &p_linear_velocity, const Ve found_collision = true; colliders.push_back(collision); - motion = collision.remainder; if (up_direction == Vector3()) { //all is a wall @@ -1080,7 +1114,7 @@ Vector3 KinematicBody::move_and_slide(const Vector3 &p_linear_velocity, const Ve floor_velocity = collision.collider_vel; if (p_stop_on_slope) { - if ((body_velocity_normal + up_direction).length() < 0.01 && collision.travel.length() < 1) { + if ((body_velocity_normal + up_direction).length() < 0.01) { Transform gt = get_global_transform(); gt.origin -= collision.travel.slide(up_direction); set_global_transform(gt); @@ -1094,22 +1128,26 @@ Vector3 KinematicBody::move_and_slide(const Vector3 &p_linear_velocity, const Ve } } - motion = motion.slide(collision.normal); - body_velocity = body_velocity.slide(collision.normal); + if (sliding_enabled || !on_floor) { + motion = collision.remainder.slide(collision.normal); + body_velocity = body_velocity.slide(collision.normal); - for (int j = 0; j < 3; j++) { - if (locked_axis & (1 << j)) { - body_velocity[j] = 0; + for (int j = 0; j < 3; j++) { + if (locked_axis & (1 << j)) { + body_velocity[j] = 0; + } } + } else { + motion = collision.remainder; } } + + sliding_enabled = true; } if (!found_collision || motion == Vector3()) { break; } - - --p_max_slides; } return body_velocity; diff --git a/scene/3d/physics_body.h b/scene/3d/physics_body.h index d711bdfb5d3..eda958be065 100644 --- a/scene/3d/physics_body.h +++ b/scene/3d/physics_body.h @@ -306,7 +306,7 @@ protected: static void _bind_methods(); public: - bool move_and_collide(const Vector3 &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 Vector3 &p_motion, bool p_infinite_inertia, Collision &r_collision, bool p_exclude_raycast_shapes = true, bool p_test_only = false, bool p_cancel_sliding = true); bool test_move(const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia); bool separate_raycast_shapes(bool p_infinite_inertia, Collision &r_collision); diff --git a/servers/physics/space_sw.cpp b/servers/physics/space_sw.cpp index 64f3330160f..075a5f4d880 100644 --- a/servers/physics/space_sw.cpp +++ b/servers/physics/space_sw.cpp @@ -1008,6 +1008,9 @@ bool SpaceSW::test_body_motion(BodySW *p_body, const Transform &p_from, const Ve 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 BodySW *body = static_cast(rcd.best_object); diff --git a/servers/physics_2d/space_2d_sw.cpp b/servers/physics_2d/space_2d_sw.cpp index 45375652fa0..8efbf41e8fc 100644 --- a/servers/physics_2d/space_2d_sw.cpp +++ b/servers/physics_2d/space_2d_sw.cpp @@ -1128,6 +1128,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_2d_server.cpp b/servers/physics_2d_server.cpp index d3e549eff81..0425923f15e 100644 --- a/servers/physics_2d_server.cpp +++ b/servers/physics_2d_server.cpp @@ -444,6 +444,18 @@ int Physics2DTestMotionResult::get_collider_shape() const { return result.collider_shape; } +real_t Physics2DTestMotionResult::get_collision_depth() const { + return result.collision_depth; +} + +real_t Physics2DTestMotionResult::get_collision_safe_fraction() const { + return result.collision_safe_fraction; +} + +real_t Physics2DTestMotionResult::get_collision_unsafe_fraction() const { + return result.collision_unsafe_fraction; +} + void Physics2DTestMotionResult::_bind_methods() { ClassDB::bind_method(D_METHOD("get_motion"), &Physics2DTestMotionResult::get_motion); ClassDB::bind_method(D_METHOD("get_motion_remainder"), &Physics2DTestMotionResult::get_motion_remainder); @@ -454,6 +466,9 @@ void Physics2DTestMotionResult::_bind_methods() { ClassDB::bind_method(D_METHOD("get_collider_rid"), &Physics2DTestMotionResult::get_collider_rid); ClassDB::bind_method(D_METHOD("get_collider"), &Physics2DTestMotionResult::get_collider); ClassDB::bind_method(D_METHOD("get_collider_shape"), &Physics2DTestMotionResult::get_collider_shape); + ClassDB::bind_method(D_METHOD("get_collision_depth"), &Physics2DTestMotionResult::get_collision_depth); + ClassDB::bind_method(D_METHOD("get_collision_safe_fraction"), &Physics2DTestMotionResult::get_collision_safe_fraction); + ClassDB::bind_method(D_METHOD("get_collision_unsafe_fraction"), &Physics2DTestMotionResult::get_collision_unsafe_fraction); ADD_PROPERTY(PropertyInfo(Variant::VECTOR2, "motion"), "", "get_motion"); ADD_PROPERTY(PropertyInfo(Variant::VECTOR2, "motion_remainder"), "", "get_motion_remainder"); @@ -464,6 +479,9 @@ void Physics2DTestMotionResult::_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::REAL, "collision_depth"), "", "get_collision_depth"); + ADD_PROPERTY(PropertyInfo(Variant::REAL, "collision_safe_fraction"), "", "get_collision_safe_fraction"); + ADD_PROPERTY(PropertyInfo(Variant::REAL, "collision_unsafe_fraction"), "", "get_collision_unsafe_fraction"); } /////////////////////////////////////// diff --git a/servers/physics_2d_server.h b/servers/physics_2d_server.h index 63b5de03af1..bba68981f4a 100644 --- a/servers/physics_2d_server.h +++ b/servers/physics_2d_server.h @@ -469,6 +469,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 = 0; RID collider; @@ -594,6 +597,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 Physics2DServer *(*CreatePhysics2DServerCallback)(); diff --git a/servers/physics_server.cpp b/servers/physics_server.cpp index 57193d15dc1..717c5b01b7d 100644 --- a/servers/physics_server.cpp +++ b/servers/physics_server.cpp @@ -392,6 +392,18 @@ int PhysicsTestMotionResult::get_collider_shape() const { return result.collider_shape; } +real_t PhysicsTestMotionResult::get_collision_depth() const { + return result.collision_depth; +} + +real_t PhysicsTestMotionResult::get_collision_safe_fraction() const { + return result.collision_safe_fraction; +} + +real_t PhysicsTestMotionResult::get_collision_unsafe_fraction() const { + return result.collision_unsafe_fraction; +} + void PhysicsTestMotionResult::_bind_methods() { ClassDB::bind_method(D_METHOD("get_motion"), &PhysicsTestMotionResult::get_motion); ClassDB::bind_method(D_METHOD("get_motion_remainder"), &PhysicsTestMotionResult::get_motion_remainder); @@ -402,6 +414,9 @@ void PhysicsTestMotionResult::_bind_methods() { ClassDB::bind_method(D_METHOD("get_collider_rid"), &PhysicsTestMotionResult::get_collider_rid); ClassDB::bind_method(D_METHOD("get_collider"), &PhysicsTestMotionResult::get_collider); ClassDB::bind_method(D_METHOD("get_collider_shape"), &PhysicsTestMotionResult::get_collider_shape); + ClassDB::bind_method(D_METHOD("get_collision_depth"), &PhysicsTestMotionResult::get_collision_depth); + ClassDB::bind_method(D_METHOD("get_collision_safe_fraction"), &PhysicsTestMotionResult::get_collision_safe_fraction); + ClassDB::bind_method(D_METHOD("get_collision_unsafe_fraction"), &PhysicsTestMotionResult::get_collision_unsafe_fraction); ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "motion"), "", "get_motion"); ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "motion_remainder"), "", "get_motion_remainder"); @@ -412,6 +427,9 @@ void PhysicsTestMotionResult::_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::REAL, "collision_depth"), "", "get_collision_depth"); + ADD_PROPERTY(PropertyInfo(Variant::REAL, "collision_safe_fraction"), "", "get_collision_safe_fraction"); + ADD_PROPERTY(PropertyInfo(Variant::REAL, "collision_unsafe_fraction"), "", "get_collision_unsafe_fraction"); } /////////////////////////////////////// diff --git a/servers/physics_server.h b/servers/physics_server.h index c7641bf38c6..264a16fdd66 100644 --- a/servers/physics_server.h +++ b/servers/physics_server.h @@ -461,6 +461,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 = 0; RID collider; @@ -765,6 +768,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 PhysicsServer *(*CreatePhysicsServerCallback)(); From beeebb4c2fc2b77101d95d2b4501be76536d9dd0 Mon Sep 17 00:00:00 2001 From: PouleyKetchoupp Date: Wed, 30 Jun 2021 19:00:31 -0700 Subject: [PATCH 2/2] More accurate unsafe motion calculation * Safe and unsafe motion are calculated by dichotomy with a limited number of steps. It's good for performance, but on long motions that either collide near the beginning or near the end, the result can be very imprecise. * Now a factor 0.25 or 0.75 is used to converge faster when this case happens, which allows longer motions to get more accurate collision detection. * Makes snap collision more precise, and helps with cases where diagonal collision on the border of a platform can lead to the character being stuck. Additional improvements to move_and_slide: * Handle slide canceling in move_and_collide with 0 velocity instead of not applying it. * Better handling of snap with custom logic to cancel sliding. * Remove small jittering when using stop on slope, by canceling the motion completely when the resulting motion is less than margin instead of always projecting to the up direction (in both body motion and snap). Co-authored-by: fabriceci --- scene/2d/physics_body_2d.cpp | 41 ++++++++++------ scene/3d/physics_body.cpp | 42 ++++++++++------ servers/physics/space_sw.cpp | 77 ++++++++++++++++++++---------- servers/physics_2d/space_2d_sw.cpp | 63 +++++++++++++++++------- 4 files changed, 154 insertions(+), 69 deletions(-) diff --git a/scene/2d/physics_body_2d.cpp b/scene/2d/physics_body_2d.cpp index c7889e2bd2e..e0227c32e23 100644 --- a/scene/2d/physics_body_2d.cpp +++ b/scene/2d/physics_body_2d.cpp @@ -1042,11 +1042,11 @@ bool KinematicBody2D::move_and_collide(const Vector2 &p_motion, bool p_infinite_ // 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) { + if (p_cancel_sliding) { + real_t motion_length = p_motion.length(); real_t precision = 0.001; - if (colliding && p_cancel_sliding) { + if (colliding) { // 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 * (result.collision_unsafe_fraction - result.collision_safe_fraction); @@ -1057,16 +1057,21 @@ bool KinematicBody2D::move_and_collide(const Vector2 &p_motion, bool p_infinite_ } if (p_cancel_sliding) { + // When motion is null, recovery is the resulting motion. + Vector2 motion_normal; + if (motion_length > CMP_EPSILON) { + motion_normal = p_motion / motion_length; + } + // Check depth of recovery. - Vector2 motion_normal = p_motion / motion_length; - real_t dot = result.motion.dot(motion_normal); - Vector2 recovery = result.motion - motion_normal * dot; + real_t projected_length = result.motion.dot(motion_normal); + Vector2 recovery = result.motion - motion_normal * projected_length; 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. + // because we're only taking rest information into account and not general recovery. if (recovery_length < (real_t)margin + precision) { // Apply adjustment to motion. - result.motion = motion_normal * dot; + result.motion = motion_normal * projected_length; result.remainder = p_motion - result.motion; } } @@ -1134,8 +1139,9 @@ Vector2 KinematicBody2D::move_and_slide(const Vector2 &p_linear_velocity, const on_floor_body = RID(); Vector2 motion = body_velocity * delta; - // No sliding on first attempt to keep floor motion stable when possible. - bool sliding_enabled = false; + // No sliding on first attempt to keep floor motion stable when possible, + // when stop on slope is enabled. + bool sliding_enabled = !p_stop_on_slope; for (int iteration = 0; iteration < p_max_slides; ++iteration) { Collision collision; bool found_collision = false; @@ -1165,7 +1171,11 @@ Vector2 KinematicBody2D::move_and_slide(const Vector2 &p_linear_velocity, const if (on_floor && p_stop_on_slope) { if ((body_velocity_normal + up_direction).length() < 0.01) { Transform2D gt = get_global_transform(); - gt.elements[2] -= collision.travel.slide(up_direction); + if (collision.travel.length() > margin) { + gt.elements[2] -= collision.travel.slide(up_direction); + } else { + gt.elements[2] -= collision.travel; + } set_global_transform(gt); return Vector2(); } @@ -1207,7 +1217,7 @@ Vector2 KinematicBody2D::move_and_slide_with_snap(const Vector2 &p_linear_veloci Collision col; Transform2D gt = get_global_transform(); - if (move_and_collide(p_snap, p_infinite_inertia, col, false, true)) { + if (move_and_collide(p_snap, p_infinite_inertia, col, false, true, false)) { bool apply = true; if (up_direction != Vector2()) { if (Math::acos(col.normal.dot(up_direction)) <= p_floor_max_angle + FLOOR_ANGLE_THRESHOLD) { @@ -1218,9 +1228,12 @@ Vector2 KinematicBody2D::move_and_slide_with_snap(const Vector2 &p_linear_veloci if (p_stop_on_slope) { // move and collide may stray the object a bit because of pre un-stucking, // so only ensure that motion happens on floor direction in this case. - col.travel = up_direction * up_direction.dot(col.travel); + if (col.travel.length() > margin) { + col.travel = up_direction * up_direction.dot(col.travel); + } else { + col.travel = Vector2(); + } } - } else { apply = false; } diff --git a/scene/3d/physics_body.cpp b/scene/3d/physics_body.cpp index 70c84512f9f..c6edaac2aaf 100644 --- a/scene/3d/physics_body.cpp +++ b/scene/3d/physics_body.cpp @@ -985,11 +985,11 @@ bool KinematicBody::move_and_collide(const Vector3 &p_motion, bool p_infinite_in // 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 (p_cancel_sliding) { + real_t motion_length = p_motion.length(); + real_t precision = 0.001; - if (colliding && p_cancel_sliding) { + if (colliding) { // 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 * (result.collision_unsafe_fraction - result.collision_safe_fraction); @@ -1000,16 +1000,21 @@ bool KinematicBody::move_and_collide(const Vector3 &p_motion, bool p_infinite_in } if (p_cancel_sliding) { + // When motion is null, recovery is the resulting motion. + Vector3 motion_normal; + if (motion_length > CMP_EPSILON) { + motion_normal = p_motion / motion_length; + } + // Check depth of recovery. - Vector3 motion_normal = p_motion / motion_length; - real_t dot = result.motion.dot(motion_normal); - Vector3 recovery = result.motion - motion_normal * dot; + real_t projected_length = result.motion.dot(motion_normal); + Vector3 recovery = result.motion - motion_normal * projected_length; 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. + // because we're only taking rest information into account and not general recovery. if (recovery_length < (real_t)margin + precision) { // Apply adjustment to motion. - result.motion = motion_normal * dot; + result.motion = motion_normal * projected_length; result.remainder = p_motion - result.motion; } } @@ -1076,8 +1081,9 @@ Vector3 KinematicBody::move_and_slide(const Vector3 &p_linear_velocity, const Ve floor_normal = Vector3(); floor_velocity = Vector3(); - // No sliding on first attempt to keep motion stable when possible. - bool sliding_enabled = false; + // No sliding on first attempt to keep floor motion stable when possible, + // when stop on slope is enabled. + bool sliding_enabled = !p_stop_on_slope; for (int iteration = 0; iteration < p_max_slides; ++iteration) { Collision collision; bool found_collision = false; @@ -1116,7 +1122,11 @@ Vector3 KinematicBody::move_and_slide(const Vector3 &p_linear_velocity, const Ve if (p_stop_on_slope) { if ((body_velocity_normal + up_direction).length() < 0.01) { Transform gt = get_global_transform(); - gt.origin -= collision.travel.slide(up_direction); + if (collision.travel.length() > margin) { + gt.origin -= collision.travel.slide(up_direction); + } else { + gt.origin -= collision.travel; + } set_global_transform(gt); return Vector3(); } @@ -1165,7 +1175,7 @@ Vector3 KinematicBody::move_and_slide_with_snap(const Vector3 &p_linear_velocity Collision col; Transform gt = get_global_transform(); - if (move_and_collide(p_snap, p_infinite_inertia, col, false, true)) { + if (move_and_collide(p_snap, p_infinite_inertia, col, false, true, false)) { bool apply = true; if (up_direction != Vector3()) { if (Math::acos(col.normal.dot(up_direction)) <= p_floor_max_angle + FLOOR_ANGLE_THRESHOLD) { @@ -1176,7 +1186,11 @@ Vector3 KinematicBody::move_and_slide_with_snap(const Vector3 &p_linear_velocity if (p_stop_on_slope) { // move and collide may stray the object a bit because of pre un-stucking, // so only ensure that motion happens on floor direction in this case. - col.travel = col.travel.project(up_direction); + if (col.travel.length() > margin) { + col.travel = col.travel.project(up_direction); + } else { + col.travel = Vector3(); + } } } else { apply = false; //snapped with floor direction, but did not snap to a floor, do not snap. diff --git a/servers/physics/space_sw.cpp b/servers/physics/space_sw.cpp index 075a5f4d880..bc57f342835 100644 --- a/servers/physics/space_sw.cpp +++ b/servers/physics/space_sw.cpp @@ -251,6 +251,8 @@ bool PhysicsDirectSpaceStateSW::cast_motion(const RID &p_shape, const Transform bool best_first = true; + Vector3 motion_normal = p_motion.normalized(); + Vector3 closest_A, closest_B; for (int i = 0; i < amount; i++) { @@ -266,7 +268,7 @@ bool PhysicsDirectSpaceStateSW::cast_motion(const RID &p_shape, const Transform int shape_idx = space->intersection_query_subindex_results[i]; Vector3 point_A, point_B; - Vector3 sep_axis = p_motion.normalized(); + Vector3 sep_axis = motion_normal; Transform col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); //test initial overlap, does it collide if going all the way? @@ -275,35 +277,47 @@ bool PhysicsDirectSpaceStateSW::cast_motion(const RID &p_shape, const Transform } //test initial overlap, ignore objects it's inside of. - sep_axis = p_motion.normalized(); + sep_axis = motion_normal; if (!CollisionSolverSW::solve_distance(shape, p_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, aabb, &sep_axis)) { continue; } //just do kinematic solving - real_t low = 0; - real_t hi = 1; - Vector3 mnormal = p_motion.normalized(); - + real_t low = 0.0; + real_t hi = 1.0; + real_t fraction_coeff = 0.5; for (int j = 0; j < 8; j++) { //steps should be customizable.. + real_t fraction = low + (hi - low) * fraction_coeff; - real_t ofs = (low + hi) * 0.5; - - Vector3 sep = mnormal; //important optimization for this to work fast enough - - mshape.motion = xform_inv.basis.xform(p_motion * ofs); + mshape.motion = xform_inv.basis.xform(p_motion * fraction); Vector3 lA, lB; - + Vector3 sep = motion_normal; //important optimization for this to work fast enough bool collided = !CollisionSolverSW::solve_distance(&mshape, p_xform, col_obj->get_shape(shape_idx), col_obj_xform, lA, lB, aabb, &sep); if (collided) { - hi = ofs; + hi = fraction; + if ((j == 0) || (low > 0.0)) { // Did it not collide before? + // When alternating or first iteration, use dichotomy. + fraction_coeff = 0.5; + } else { + // When colliding again, converge faster towards low fraction + // for more accurate results with long motions that collide near the start. + fraction_coeff = 0.25; + } } else { point_A = lA; point_B = lB; - low = ofs; + low = fraction; + if ((j == 0) || (hi < 1.0)) { // Did it collide before? + // When alternating or first iteration, use dichotomy. + fraction_coeff = 0.5; + } else { + // When not colliding again, converge faster towards high fraction + // for more accurate results with long motions that collide near the end. + fraction_coeff = 0.75; + } } } @@ -894,27 +908,40 @@ bool SpaceSW::test_body_motion(BodySW *p_body, const Transform &p_from, const Ve } //just do kinematic solving - real_t low = 0; - real_t hi = 1; - + real_t low = 0.0; + real_t hi = 1.0; + real_t fraction_coeff = 0.5; for (int k = 0; k < 8; k++) { //steps should be customizable.. + real_t fraction = low + (hi - low) * fraction_coeff; - real_t ofs = (low + hi) * 0.5; - - Vector3 sep = motion_normal; //important optimization for this to work fast enough - - mshape.motion = body_shape_xform_inv.basis.xform(p_motion * ofs); + mshape.motion = body_shape_xform_inv.basis.xform(p_motion * fraction); Vector3 lA, lB; - + Vector3 sep = motion_normal; //important optimization for this to work fast enough bool collided = !CollisionSolverSW::solve_distance(&mshape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj_xform, lA, lB, motion_aabb, &sep); if (collided) { - hi = ofs; + hi = fraction; + if ((k == 0) || (low > 0.0)) { // Did it not collide before? + // When alternating or first iteration, use dichotomy. + fraction_coeff = 0.5; + } else { + // When colliding again, converge faster towards low fraction + // for more accurate results with long motions that collide near the start. + fraction_coeff = 0.25; + } } else { point_A = lA; point_B = lB; - low = ofs; + low = fraction; + if ((k == 0) || (hi < 1.0)) { // Did it collide before? + // When alternating or first iteration, use dichotomy. + fraction_coeff = 0.5; + } else { + // When not colliding again, converge faster towards high fraction + // for more accurate results with long motions that collide near the end. + fraction_coeff = 0.75; + } } } diff --git a/servers/physics_2d/space_2d_sw.cpp b/servers/physics_2d/space_2d_sw.cpp index 8efbf41e8fc..8756477350c 100644 --- a/servers/physics_2d/space_2d_sw.cpp +++ b/servers/physics_2d/space_2d_sw.cpp @@ -283,22 +283,38 @@ bool Physics2DDirectSpaceStateSW::cast_motion(const RID &p_shape, const Transfor continue; } - //just do kinematic solving - real_t low = 0; - real_t hi = 1; Vector2 mnormal = p_motion.normalized(); + //just do kinematic solving + real_t low = 0.0; + real_t hi = 1.0; + real_t fraction_coeff = 0.5; for (int j = 0; j < 8; j++) { //steps should be customizable.. - - real_t ofs = (low + hi) * 0.5; + real_t fraction = low + (hi - low) * fraction_coeff; Vector2 sep = mnormal; //important optimization for this to work fast enough - bool collided = CollisionSolver2DSW::solve(shape, p_xform, p_motion * ofs, col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), nullptr, nullptr, &sep, p_margin); + bool collided = CollisionSolver2DSW::solve(shape, p_xform, p_motion * fraction, col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), nullptr, nullptr, &sep, p_margin); if (collided) { - hi = ofs; + hi = fraction; + if ((j == 0) || (low > 0.0)) { // Did it not collide before? + // When alternating or first iteration, use dichotomy. + fraction_coeff = 0.5; + } else { + // When colliding again, converge faster towards low fraction + // for more accurate results with long motions that collide near the start. + fraction_coeff = 0.25; + } } else { - low = ofs; + low = fraction; + if ((j == 0) || (hi < 1.0)) { // Did it collide before? + // When alternating or first iteration, use dichotomy. + fraction_coeff = 0.5; + } else { + // When not colliding again, converge faster towards high fraction + // for more accurate results with long motions that collide near the end. + fraction_coeff = 0.75; + } } } @@ -962,20 +978,35 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co } //just do kinematic solving - real_t low = 0; - real_t hi = 1; - + real_t low = 0.0; + real_t hi = 1.0; + real_t fraction_coeff = 0.5; for (int k = 0; k < 8; k++) { //steps should be customizable.. - - real_t ofs = (low + hi) * 0.5; + real_t fraction = low + (hi - low) * fraction_coeff; Vector2 sep = motion_normal; //important optimization for this to work fast enough - bool collided = CollisionSolver2DSW::solve(body_shape, body_shape_xform, p_motion * ofs, against_shape, col_obj_shape_xform, Vector2(), nullptr, nullptr, &sep, 0); + bool collided = CollisionSolver2DSW::solve(body_shape, body_shape_xform, p_motion * fraction, against_shape, col_obj_shape_xform, Vector2(), nullptr, nullptr, &sep, 0); if (collided) { - hi = ofs; + hi = fraction; + if ((k == 0) || (low > 0.0)) { // Did it not collide before? + // When alternating or first iteration, use dichotomy. + fraction_coeff = 0.5; + } else { + // When colliding again, converge faster towards low fraction + // for more accurate results with long motions that collide near the start. + fraction_coeff = 0.25; + } } else { - low = ofs; + low = fraction; + if ((k == 0) || (hi < 1.0)) { // Did it collide before? + // When alternating or first iteration, use dichotomy. + fraction_coeff = 0.5; + } else { + // When not colliding again, converge faster towards high fraction + // for more accurate results with long motions that collide near the end. + fraction_coeff = 0.75; + } } }