Merge pull request #50495 from nekomatata/move-and-slide-fixes-3.x

[3.x] Backport KinematicBody move_and_slide and move_and_collide fixes

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 )">

doc/classes/PhysicsTestMotionResult.xml

@@ -19,10 +19,16 @@
  </member>
  <member name="collider_velocity" type="Vector3" setter="" getter="get_collider_velocity" default="Vector3( 0, 0, 0 )">
  </member>
+ <member name="collision_depth" type="float" setter="" getter="get_collision_depth" default="0.0">
+ </member>
  <member name="collision_normal" type="Vector3" setter="" getter="get_collision_normal" default="Vector3( 0, 0, 0 )">
  </member>
  <member name="collision_point" type="Vector3" setter="" getter="get_collision_point" default="Vector3( 0, 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="Vector3" setter="" getter="get_motion" default="Vector3( 0, 0, 0 )">
  </member>
  <member name="motion_remainder" type="Vector3" setter="" getter="get_motion_remainder" default="Vector3( 0, 0, 0 )">

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<RID> &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<RID> &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,44 @@ 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.
+ if (p_cancel_sliding) {
+ real_t motion_length = p_motion.length();
+ real_t precision = 0.001;
+
+ if (colliding) {
+ // 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) {
+ // When motion is null, recovery is the resulting motion.
+ Vector2 motion_normal;
+ if (motion_length > CMP_EPSILON) {
+ motion_normal = p_motion / motion_length;
+ }
+
+ // Check depth of recovery.
+ real_t projected_length = result.motion.dot(motion_normal);
+ Vector2 recovery = result.motion - motion_normal * projected_length;
+ real_t recovery_length = recovery.length();
+ // Fixes cases where canceling slide causes the motion to go too deep into the ground,
+ // because 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 * projected_length;
+ result.remainder = p_motion - result.motion;
+ }
+ }
+ }
+
  if (colliding) {
  r_collision.collider_metadata = result.collider_metadata;
  r_collision.collider_shape = result.collider_shape;
@@ -1092,7 +1130,7 @@ Vector2 KinematicBody2D::move_and_slide(const Vector2 &p_linear_velocity, const
  Collision floor_collision;
  Set<RID> 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 +1139,17 @@ 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,
+ // when stop on slope is enabled.
+ bool sliding_enabled = !p_stop_on_slope;
+ 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,29 +1165,36 @@ 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);
+ if (collision.travel.length() > margin) {
+ gt.elements[2] -= collision.travel.slide(up_direction);
+ } else {
+ gt.elements[2] -= collision.travel;
+ }
  set_global_transform(gt);
  return Vector2();
  }
  }
 
- 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) {
@@ -1169,7 +1217,7 @@ Vector2 KinematicBody2D::move_and_slide_with_snap(const Vector2 &p_linear_veloci
  Collision col;
  Transform2D gt = get_global_transform();
 
- if (move_and_collide(p_snap, p_infinite_inertia, col, false, true)) {
+ 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) {
@@ -1180,9 +1228,12 @@ Vector2 KinematicBody2D::move_and_slide_with_snap(const Vector2 &p_linear_veloci
  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 = up_direction * up_direction.dot(col.travel);
+ if (col.travel.length() > margin) {
+ col.travel = up_direction * up_direction.dot(col.travel);
+ } else {
+ col.travel = Vector2();
+ }
  }
-
  } else {
  apply = false;
  }

scene/2d/physics_body_2d.h

@@ -311,7 +311,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, const Set<RID> &p_exclude = Set<RID>());
+ 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<RID> &p_exclude = Set<RID>());
 
  bool test_move(const Transform2D &p_from, const Vector2 &p_motion, bool p_infinite_inertia = true);
 

scene/3d/physics_body.cpp

@@ -973,7 +973,7 @@ 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) {
  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,44 @@ 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.
+ if (p_cancel_sliding) {
+ real_t motion_length = p_motion.length();
+ real_t precision = 0.001;
+
+ if (colliding) {
+ // 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) {
+ // When motion is null, recovery is the resulting motion.
+ Vector3 motion_normal;
+ if (motion_length > CMP_EPSILON) {
+ motion_normal = p_motion / motion_length;
+ }
+
+ // Check depth of recovery.
+ real_t projected_length = result.motion.dot(motion_normal);
+ Vector3 recovery = result.motion - motion_normal * projected_length;
+ real_t recovery_length = recovery.length();
+ // Fixes cases where canceling slide causes the motion to go too deep into the ground,
+ // because 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 * projected_length;
+ result.remainder = p_motion - result.motion;
+ }
+ }
+ }
+
  if (colliding) {
  r_collision.collider_metadata = result.collider_metadata;
  r_collision.collider_shape = result.collider_shape;
@@ -1043,14 +1081,17 @@ 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 floor motion stable when possible,
+ // when stop on slope is enabled.
+ bool sliding_enabled = !p_stop_on_slope;
+ 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 +1107,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,9 +1120,13 @@ 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);
+ if (collision.travel.length() > margin) {
+ gt.origin -= collision.travel.slide(up_direction);
+ } else {
+ gt.origin -= collision.travel;
+ }
  set_global_transform(gt);
  return Vector3();
  }
@@ -1094,22 +1138,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;
@@ -1127,7 +1175,7 @@ Vector3 KinematicBody::move_and_slide_with_snap(const Vector3 &p_linear_velocity
  Collision col;
  Transform gt = get_global_transform();
 
- if (move_and_collide(p_snap, p_infinite_inertia, col, false, true)) {
+ 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) {
@@ -1138,7 +1186,11 @@ Vector3 KinematicBody::move_and_slide_with_snap(const Vector3 &p_linear_velocity
  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 = col.travel.project(up_direction);
+ 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.

scene/3d/physics_body.h

@@ -306,7 +306,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);