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 <[email protected]>

doc/classes/Physics2DTestMotionResult.xml

@@ -19,10 +19,16 @@
 		</member>
 		<member name="collider_velocity" type="Vector2" setter="" getter="get_collider_velocity" default="Vector2( 0, 0 )">
 		</member>
+		<member name="collision_depth" type="float" setter="" getter="get_collision_depth" default="0.0">
+		</member>
 		<member name="collision_normal" type="Vector2" setter="" getter="get_collision_normal" default="Vector2( 0, 0 )">
 		</member>
 		<member name="collision_point" type="Vector2" setter="" getter="get_collision_point" default="Vector2( 0, 0 )">
 		</member>
+		<member name="collision_safe_fraction" type="float" setter="" getter="get_collision_safe_fraction" default="0.0">
+		</member>
+		<member name="collision_unsafe_fraction" type="float" setter="" getter="get_collision_unsafe_fraction" default="0.0">
+		</member>
 		<member name="motion" type="Vector2" setter="" getter="get_motion" default="Vector2( 0, 0 )">
 		</member>
 		<member name="motion_remainder" type="Vector2" setter="" getter="get_motion_remainder" default="Vector2( 0, 0 )">

scene/2d/physics_body_2d.cpp

@@ -1030,14 +1030,47 @@ 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, 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.");
 	}
 	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);
 
+	// 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;
@@ -1087,14 +1120,16 @@ Vector2 KinematicBody2D::move_and_slide(const Vector2 &p_linear_velocity, const
 	floor_normal = Vector2();
 	floor_velocity = Vector2();
 
-	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
 				}
@@ -1110,7 +1145,6 @@ Vector2 KinematicBody2D::move_and_slide(const Vector2 &p_linear_velocity, const
 				found_collision = true;
 
 				colliders.push_back(collision);
-				motion = collision.remainder;
 
 				if (up_direction == Vector2()) {
 					//all is a wall
@@ -1124,7 +1158,7 @@ Vector2 KinematicBody2D::move_and_slide(const Vector2 &p_linear_velocity, const
 						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) {
 								Transform2D gt = get_global_transform();
 								gt.elements[2] -= collision.travel.slide(up_direction);
 								set_global_transform(gt);
@@ -1138,16 +1172,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;
 	}
 
 	return body_velocity;

scene/2d/physics_body_2d.h

@@ -310,7 +310,7 @@ class KinematicBody2D : public PhysicsBody2D {
 	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, bool p_cancel_sliding = true);
 
 	bool test_move(const Transform2D &p_from, const Vector2 &p_motion, bool p_infinite_inertia = true);
 

scene/3d/physics_body.cpp

@@ -973,11 +973,44 @@ Ref<KinematicCollision> KinematicBody::_move(const Vector3 &p_motion, bool p_inf
 	return Ref<KinematicCollision>();
 }
 
-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) {
 	Transform gt = get_global_transform();
 	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;
@@ -1030,14 +1063,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
 				}
@@ -1053,7 +1088,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
@@ -1067,7 +1101,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);
@@ -1081,22 +1115,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;

scene/3d/physics_body.h

@@ -302,7 +302,7 @@ class KinematicBody : public PhysicsBody {
 	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);

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<const BodySW *>(rcd.best_object);

servers/physics_2d/space_2d_sw.cpp

@@ -1119,6 +1119,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<const Body2DSW *>(rcd.best_object);

servers/physics_2d_server.cpp

@@ -466,6 +466,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);
@@ -476,6 +488,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");
@@ -486,6 +501,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");
 }
 
 Physics2DTestMotionResult::Physics2DTestMotionResult() {

servers/physics_2d_server.h

@@ -491,6 +491,9 @@ class Physics2DServer : public Object {
 		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;
 		ObjectID collider_id;
 		RID collider;
@@ -622,6 +625,9 @@ class Physics2DTestMotionResult : public Reference {
 	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;
 
 	Physics2DTestMotionResult();
 };

servers/physics_server.h

@@ -479,6 +479,9 @@ class PhysicsServer : public Object {
 		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;
 		ObjectID collider_id;
 		RID collider;