Fix 3D character snap on moving platforms

Applying the platform velocity when leaving the platform floor should be
done after snapping to keep things consistent.

Now it's done in both 2D and 3D, as it's already done in 2D on master.

scene/2d/physics_body_2d.cpp

@@ -1101,10 +1101,11 @@ bool KinematicBody2D::move_and_collide(const Vector2 &p_motion, bool p_infinite_
 //so, if you pass 45 as limit, avoid numerical precision errors when angle is 45.
 #define FLOOR_ANGLE_THRESHOLD 0.01
 
-Vector2 KinematicBody2D::move_and_slide(const Vector2 &p_linear_velocity, const Vector2 &p_up_direction, bool p_stop_on_slope, int p_max_slides, float p_floor_max_angle, bool p_infinite_inertia) {
+Vector2 KinematicBody2D::_move_and_slide_internal(const Vector2 &p_linear_velocity, const Vector2 &p_snap, const Vector2 &p_up_direction, bool p_stop_on_slope, int p_max_slides, float p_floor_max_angle, bool p_infinite_inertia) {
 	Vector2 body_velocity = p_linear_velocity;
 	Vector2 body_velocity_normal = body_velocity.normalized();
 	Vector2 up_direction = p_up_direction.normalized();
+	bool was_on_floor = on_floor;
 
 	// Hack in order to work with calling from _process as well as from _physics_process; calling from thread is risky
 	float delta = Engine::get_singleton()->is_in_physics_frame() ? get_physics_process_delta_time() : get_process_delta_time();
@@ -1199,6 +1200,40 @@ Vector2 KinematicBody2D::move_and_slide(const Vector2 &p_linear_velocity, const
 		}
 	}
 
+	if (was_on_floor && p_snap != Vector2() && !on_floor) {
+		// Apply snap.
+		Collision col;
+		Transform2D gt = get_global_transform();
+
+		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) {
+					on_floor = true;
+					floor_normal = col.normal;
+					on_floor_body = col.collider_rid;
+					floor_velocity = col.collider_vel;
+					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.
+						if (col.travel.length() > margin) {
+							col.travel = up_direction * up_direction.dot(col.travel);
+						} else {
+							col.travel = Vector2();
+						}
+					}
+				} else {
+					apply = false;
+				}
+			}
+
+			if (apply) {
+				gt.elements[2] += col.travel;
+				set_global_transform(gt);
+			}
+		}
+	}
+
 	if (!on_floor && !on_wall) {
 		// Add last platform velocity when just left a moving platform.
 		return body_velocity + current_floor_velocity;
@@ -1207,47 +1242,12 @@ Vector2 KinematicBody2D::move_and_slide(const Vector2 &p_linear_velocity, const
 	return body_velocity;
 }
 
-Vector2 KinematicBody2D::move_and_slide_with_snap(const Vector2 &p_linear_velocity, const Vector2 &p_snap, const Vector2 &p_up_direction, bool p_stop_on_slope, int p_max_slides, float p_floor_max_angle, bool p_infinite_inertia) {
-	Vector2 up_direction = p_up_direction.normalized();
-	bool was_on_floor = on_floor;
-
-	Vector2 ret = move_and_slide(p_linear_velocity, up_direction, p_stop_on_slope, p_max_slides, p_floor_max_angle, p_infinite_inertia);
-	if (!was_on_floor || p_snap == Vector2() || on_floor) {
-		return ret;
-	}
-
-	Collision col;
-	Transform2D gt = get_global_transform();
-
-	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) {
-				on_floor = true;
-				floor_normal = col.normal;
-				on_floor_body = col.collider_rid;
-				floor_velocity = col.collider_vel;
-				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.
-					if (col.travel.length() > margin) {
-						col.travel = up_direction * up_direction.dot(col.travel);
-					} else {
-						col.travel = Vector2();
-					}
-				}
-			} else {
-				apply = false;
-			}
-		}
-
-		if (apply) {
-			gt.elements[2] += col.travel;
-			set_global_transform(gt);
-		}
-	}
+Vector2 KinematicBody2D::move_and_slide(const Vector2 &p_linear_velocity, const Vector2 &p_up_direction, bool p_stop_on_slope, int p_max_slides, float p_floor_max_angle, bool p_infinite_inertia) {
+	return _move_and_slide_internal(p_linear_velocity, Vector2(), p_up_direction, p_stop_on_slope, p_max_slides, p_floor_max_angle, p_infinite_inertia);
+}
 
-	return ret;
+Vector2 KinematicBody2D::move_and_slide_with_snap(const Vector2 &p_linear_velocity, const Vector2 &p_snap, const Vector2 &p_up_direction, bool p_stop_on_slope, int p_max_slides, float p_floor_max_angle, bool p_infinite_inertia) {
+	return _move_and_slide_internal(p_linear_velocity, p_snap, p_up_direction, p_stop_on_slope, p_max_slides, p_floor_max_angle, p_infinite_inertia);
 }
 
 void KinematicBody2D::_set_collision_direction(const Collision &p_collision, const Vector2 &p_up_direction, float p_floor_max_angle) {

scene/2d/physics_body_2d.h

@@ -297,13 +297,12 @@ private:
 	Vector<Ref<KinematicCollision2D>> slide_colliders;
 	Ref<KinematicCollision2D> motion_cache;
 
-	_FORCE_INLINE_ bool _ignores_mode(Physics2DServer::BodyMode) const;
-
 	Ref<KinematicCollision2D> _move(const Vector2 &p_motion, bool p_infinite_inertia = true, bool p_exclude_raycast_shapes = true, bool p_test_only = false);
 	Ref<KinematicCollision2D> _get_slide_collision(int p_bounce);
 
 	Transform2D last_valid_transform;
 	void _direct_state_changed(Object *p_state);
+	Vector2 _move_and_slide_internal(const Vector2 &p_linear_velocity, const Vector2 &p_snap, const Vector2 &p_up_direction = Vector2(0, 0), bool p_stop_on_slope = false, int p_max_slides = 4, float p_floor_max_angle = Math::deg2rad((float)45), bool p_infinite_inertia = true);
 	void _set_collision_direction(const Collision &p_collision, const Vector2 &p_up_direction, float p_floor_max_angle);
 
 protected:

scene/3d/physics_body.cpp

@@ -1050,10 +1050,11 @@ bool KinematicBody::move_and_collide(const Vector3 &p_motion, bool p_infinite_in
 //so, if you pass 45 as limit, avoid numerical precision errors when angle is 45.
 #define FLOOR_ANGLE_THRESHOLD 0.01
 
-Vector3 KinematicBody::move_and_slide(const Vector3 &p_linear_velocity, const Vector3 &p_up_direction, bool p_stop_on_slope, int p_max_slides, float p_floor_max_angle, bool p_infinite_inertia) {
+Vector3 KinematicBody::_move_and_slide_internal(const Vector3 &p_linear_velocity, const Vector3 &p_snap, const Vector3 &p_up_direction, bool p_stop_on_slope, int p_max_slides, float p_floor_max_angle, bool p_infinite_inertia) {
 	Vector3 body_velocity = p_linear_velocity;
 	Vector3 body_velocity_normal = body_velocity.normalized();
 	Vector3 up_direction = p_up_direction.normalized();
+	bool was_on_floor = on_floor;
 
 	for (int i = 0; i < 3; i++) {
 		if (locked_axis & (1 << i)) {
@@ -1159,6 +1160,39 @@ Vector3 KinematicBody::move_and_slide(const Vector3 &p_linear_velocity, const Ve
 		}
 	}
 
+	if (was_on_floor && p_snap != Vector3() && !on_floor) {
+		// Apply snap.
+		Collision col;
+		Transform gt = get_global_transform();
+
+		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) {
+					on_floor = true;
+					floor_normal = col.normal;
+					on_floor_body = col.collider_rid;
+					floor_velocity = col.collider_vel;
+					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.
+						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.
+				}
+			}
+			if (apply) {
+				gt.origin += col.travel;
+				set_global_transform(gt);
+			}
+		}
+	}
+
 	if (!on_floor && !on_wall) {
 		// Add last platform velocity when just left a moving platform.
 		return body_velocity + current_floor_velocity;
@@ -1167,46 +1201,12 @@ Vector3 KinematicBody::move_and_slide(const Vector3 &p_linear_velocity, const Ve
 	return body_velocity;
 }
 
-Vector3 KinematicBody::move_and_slide_with_snap(const Vector3 &p_linear_velocity, const Vector3 &p_snap, const Vector3 &p_up_direction, bool p_stop_on_slope, int p_max_slides, float p_floor_max_angle, bool p_infinite_inertia) {
-	Vector3 up_direction = p_up_direction.normalized();
-	bool was_on_floor = on_floor;
-
-	Vector3 ret = move_and_slide(p_linear_velocity, up_direction, p_stop_on_slope, p_max_slides, p_floor_max_angle, p_infinite_inertia);
-	if (!was_on_floor || p_snap == Vector3() || on_floor) {
-		return ret;
-	}
-
-	Collision col;
-	Transform gt = get_global_transform();
-
-	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) {
-				on_floor = true;
-				floor_normal = col.normal;
-				on_floor_body = col.collider_rid;
-				floor_velocity = col.collider_vel;
-				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.
-					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.
-			}
-		}
-		if (apply) {
-			gt.origin += col.travel;
-			set_global_transform(gt);
-		}
-	}
+Vector3 KinematicBody::move_and_slide(const Vector3 &p_linear_velocity, const Vector3 &p_up_direction, bool p_stop_on_slope, int p_max_slides, float p_floor_max_angle, bool p_infinite_inertia) {
+	return _move_and_slide_internal(p_linear_velocity, Vector3(), p_up_direction, p_stop_on_slope, p_max_slides, p_floor_max_angle, p_infinite_inertia);
+}
 
-	return ret;
+Vector3 KinematicBody::move_and_slide_with_snap(const Vector3 &p_linear_velocity, const Vector3 &p_snap, const Vector3 &p_up_direction, bool p_stop_on_slope, int p_max_slides, float p_floor_max_angle, bool p_infinite_inertia) {
+	return _move_and_slide_internal(p_linear_velocity, p_snap, p_up_direction, p_stop_on_slope, p_max_slides, p_floor_max_angle, p_infinite_inertia);
 }
 
 void KinematicBody::_set_collision_direction(const Collision &p_collision, const Vector3 &p_up_direction, float p_floor_max_angle) {

scene/3d/physics_body.h

@@ -293,13 +293,13 @@ private:
 	Vector<Ref<KinematicCollision>> slide_colliders;
 	Ref<KinematicCollision> motion_cache;
 
-	_FORCE_INLINE_ bool _ignores_mode(PhysicsServer::BodyMode) const;
-
 	Ref<KinematicCollision> _move(const Vector3 &p_motion, bool p_infinite_inertia = true, bool p_exclude_raycast_shapes = true, bool p_test_only = false);
 	Ref<KinematicCollision> _get_slide_collision(int p_bounce);
 
 	Transform last_valid_transform;
 	void _direct_state_changed(Object *p_state);
+
+	Vector3 _move_and_slide_internal(const Vector3 &p_linear_velocity, const Vector3 &p_snap, const Vector3 &p_up_direction = Vector3(0, 0, 0), bool p_stop_on_slope = false, int p_max_slides = 4, float p_floor_max_angle = Math::deg2rad((float)45), bool p_infinite_inertia = true);
 	void _set_collision_direction(const Collision &p_collision, const Vector3 &p_up_direction, float p_floor_max_angle);
 
 protected: