Merge pull request #49901 from nekomatata/move-and-collide-fix-slide

Fix move_and_collide causing sliding on slopes

doc/classes/PhysicsTestMotionResult2D.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

@@ -76,13 +76,46 @@ Ref<KinematicCollision2D> PhysicsBody2D::_move(const Vector2 &p_motion, bool p_i
 	return Ref<KinematicCollision2D>();
 }
 
-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;
+	// No sliding on first attempt to keep floor motion stable when possible.
-	while (slide_count) {
+	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);
+				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()) {

scene/2d/physics_body_2d.h

@@ -50,7 +50,7 @@ protected:
 	Ref<KinematicCollision2D> _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<KinematicCollision2D> &r_collision = Ref<KinematicCollision2D>(), real_t p_margin = 0.08);
 
 	TypedArray<PhysicsBody2D> get_collision_exceptions();

scene/3d/physics_body_3d.cpp

@@ -118,10 +118,43 @@ Ref<KinematicCollision3D> PhysicsBody3D::_move(const Vector3 &p_motion, bool p_i
 	return Ref<KinematicCollision3D>();
 }
 
-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;
+	// No sliding on first attempt to keep motion stable when possible.
-	while (slide_count) {
+	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,7 +1064,8 @@ void CharacterBody3D::move_and_slide() {
 					}
 				}
 
-				motion = motion.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++) {
@@ -1039,14 +1073,17 @@ void CharacterBody3D::move_and_slide() {
 							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()) {

scene/3d/physics_body_3d.h

@@ -53,7 +53,7 @@ protected:
 	Ref<KinematicCollision3D> _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<KinematicCollision3D> &r_collision = Ref<KinematicCollision3D>(), real_t p_margin = 0.001);
 
 	void set_axis_lock(PhysicsServer3D::BodyAxis p_axis, bool p_lock);

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

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

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");
 }
 
 ///////////////////////////////////////

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

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;