Ensure move and slide snap respects stop on slope, fixes #26180

2 files changed, 11 insertions, 0 deletions

diff --git a/scene/2d/physics_body_2d.cpp b/scene/2d/physics_body_2d.cpp
index ce80bc508e..eeabe15b08 100644
--- a/scene/2d/physics_body_2d.cpp
+++ b/scene/2d/physics_body_2d.cpp
@@ -1341,6 +1341,12 @@ Vector2 KinematicBody2D::move_and_slide_with_snap(const Vector2 &p_linear_veloci
 				on_floor = true;
 				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.
+					col.travel = p_floor_direction * p_floor_direction.dot(col.travel);
+				}
+
 			} else {
 				apply = false;
 			}
diff --git a/scene/3d/physics_body.cpp b/scene/3d/physics_body.cpp
index f85b51af08..ab1eed0859 100644
--- a/scene/3d/physics_body.cpp
+++ b/scene/3d/physics_body.cpp
@@ -1288,6 +1288,11 @@ Vector3 KinematicBody::move_and_slide_with_snap(const Vector3 &p_linear_velocity
 				on_floor = true;
 				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.
+					col.travel = p_floor_direction * p_floor_direction.dot(col.travel);
+				}
 			} else {
 				apply = false; //snapped with floor direction, but did not snap to a floor, do not snap.
 			}