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)();