More accurate unsafe motion calculation

* Safe and unsafe motion are calculated by dichotomy with a limited
number of steps. It's good for performance, but on long motions that
either collide near the beginning or near the end, the result can be
very imprecise.
* Now a factor 0.25 or 0.75 is used to converge faster when this case
happens, which allows longer motions to get more accurate collision
detection.
* Makes snap collision more precise, and helps with cases where diagonal collision on the border of a platform can lead to the character being stuck.

Additional improvements to move_and_slide:
* Handle slide canceling in move_and_collide with 0 velocity instead of
not applying it.
* Better handling of snap with custom logic to cancel sliding.
* Remove small jittering when using stop on slope, by canceling the
motion completely when the resulting motion is less than margin instead
of always projecting to the up direction (in both body motion and snap).

Co-authored-by: fabriceci <fabricecipolla@gmail.com>

scene/2d/physics_body_2d.cpp

@@ -1042,11 +1042,11 @@ bool KinematicBody2D::move_and_collide(const Vector2 &p_motion, bool p_infinite_
 	// 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();
-	if (motion_length > CMP_EPSILON) {
 		real_t precision = 0.001;
 
-		if (colliding && p_cancel_sliding) {
+		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);
@@ -1057,16 +1057,21 @@ bool KinematicBody2D::move_and_collide(const Vector2 &p_motion, bool p_infinite_
 		}
 
 		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.
-			Vector2 motion_normal = p_motion / motion_length;
+			real_t projected_length = result.motion.dot(motion_normal);
-			real_t dot = result.motion.dot(motion_normal);
+			Vector2 recovery = result.motion - motion_normal * projected_length;
-			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.
+			// 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 * dot;
+				result.motion = motion_normal * projected_length;
 				result.remainder = p_motion - result.motion;
 			}
 		}
@@ -1134,8 +1139,9 @@ Vector2 KinematicBody2D::move_and_slide(const Vector2 &p_linear_velocity, const
 	on_floor_body = RID();
 	Vector2 motion = body_velocity * delta;
 
-	// No sliding on first attempt to keep floor motion stable when possible.
+	// No sliding on first attempt to keep floor motion stable when possible,
-	bool sliding_enabled = false;
+	// 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;
@@ -1165,7 +1171,11 @@ Vector2 KinematicBody2D::move_and_slide(const Vector2 &p_linear_velocity, const
 				if (on_floor && p_stop_on_slope) {
 					if ((body_velocity_normal + up_direction).length() < 0.01) {
 						Transform2D gt = get_global_transform();
+						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();
 					}
@@ -1207,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) {
@@ -1218,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.
+					if (col.travel.length() > margin) {
 						col.travel = up_direction * up_direction.dot(col.travel);
+					} else {
+						col.travel = Vector2();
+					}
 				}
-
 			} else {
 				apply = false;
 			}

scene/3d/physics_body.cpp

@@ -985,11 +985,11 @@ bool KinematicBody::move_and_collide(const Vector3 &p_motion, bool p_infinite_in
 	// 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();
-	if (motion_length > CMP_EPSILON) {
+		real_t precision = 0.001;
-		real_t precision = CMP_EPSILON;
 
-		if (colliding && p_cancel_sliding) {
+		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);
@@ -1000,16 +1000,21 @@ bool KinematicBody::move_and_collide(const Vector3 &p_motion, bool p_infinite_in
 		}
 
 		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.
-			Vector3 motion_normal = p_motion / motion_length;
+			real_t projected_length = result.motion.dot(motion_normal);
-			real_t dot = result.motion.dot(motion_normal);
+			Vector3 recovery = result.motion - motion_normal * projected_length;
-			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.
+			// 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 * dot;
+				result.motion = motion_normal * projected_length;
 				result.remainder = p_motion - result.motion;
 			}
 		}
@@ -1076,8 +1081,9 @@ Vector3 KinematicBody::move_and_slide(const Vector3 &p_linear_velocity, const Ve
 	floor_normal = Vector3();
 	floor_velocity = Vector3();
 
-	// No sliding on first attempt to keep motion stable when possible.
+	// No sliding on first attempt to keep floor motion stable when possible,
-	bool sliding_enabled = false;
+	// 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;
@@ -1116,7 +1122,11 @@ Vector3 KinematicBody::move_and_slide(const Vector3 &p_linear_velocity, const Ve
 						if (p_stop_on_slope) {
 							if ((body_velocity_normal + up_direction).length() < 0.01) {
 								Transform gt = get_global_transform();
+								if (collision.travel.length() > margin) {
 									gt.origin -= collision.travel.slide(up_direction);
+								} else {
+									gt.origin -= collision.travel;
+								}
 								set_global_transform(gt);
 								return Vector3();
 							}
@@ -1165,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) {
@@ -1176,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.
+					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.

servers/physics/space_sw.cpp

@@ -251,6 +251,8 @@ bool PhysicsDirectSpaceStateSW::cast_motion(const RID &p_shape, const Transform
 
 	bool best_first = true;
 
+	Vector3 motion_normal = p_motion.normalized();
+
 	Vector3 closest_A, closest_B;
 
 	for (int i = 0; i < amount; i++) {
@@ -266,7 +268,7 @@ bool PhysicsDirectSpaceStateSW::cast_motion(const RID &p_shape, const Transform
 		int shape_idx = space->intersection_query_subindex_results[i];
 
 		Vector3 point_A, point_B;
-		Vector3 sep_axis = p_motion.normalized();
+		Vector3 sep_axis = motion_normal;
 
 		Transform col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
 		//test initial overlap, does it collide if going all the way?
@@ -275,35 +277,47 @@ bool PhysicsDirectSpaceStateSW::cast_motion(const RID &p_shape, const Transform
 		}
 
 		//test initial overlap, ignore objects it's inside of.
-		sep_axis = p_motion.normalized();
+		sep_axis = motion_normal;
 
 		if (!CollisionSolverSW::solve_distance(shape, p_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, aabb, &sep_axis)) {
 			continue;
 		}
 
 		//just do kinematic solving
-		real_t low = 0;
+		real_t low = 0.0;
-		real_t hi = 1;
+		real_t hi = 1.0;
-		Vector3 mnormal = p_motion.normalized();
+		real_t fraction_coeff = 0.5;
-
 		for (int j = 0; j < 8; j++) { //steps should be customizable..
+			real_t fraction = low + (hi - low) * fraction_coeff;
 
-			real_t ofs = (low + hi) * 0.5;
+			mshape.motion = xform_inv.basis.xform(p_motion * fraction);
-
-			Vector3 sep = mnormal; //important optimization for this to work fast enough
-
-			mshape.motion = xform_inv.basis.xform(p_motion * ofs);
 
 			Vector3 lA, lB;
-
+			Vector3 sep = motion_normal; //important optimization for this to work fast enough
 			bool collided = !CollisionSolverSW::solve_distance(&mshape, p_xform, col_obj->get_shape(shape_idx), col_obj_xform, lA, lB, aabb, &sep);
 
 			if (collided) {
-				hi = ofs;
+				hi = fraction;
+				if ((j == 0) || (low > 0.0)) { // Did it not collide before?
+					// When alternating or first iteration, use dichotomy.
+					fraction_coeff = 0.5;
+				} else {
+					// When colliding again, converge faster towards low fraction
+					// for more accurate results with long motions that collide near the start.
+					fraction_coeff = 0.25;
+				}
 			} else {
 				point_A = lA;
 				point_B = lB;
-				low = ofs;
+				low = fraction;
+				if ((j == 0) || (hi < 1.0)) { // Did it collide before?
+					// When alternating or first iteration, use dichotomy.
+					fraction_coeff = 0.5;
+				} else {
+					// When not colliding again, converge faster towards high fraction
+					// for more accurate results with long motions that collide near the end.
+					fraction_coeff = 0.75;
+				}
 			}
 		}
 
@@ -894,27 +908,40 @@ bool SpaceSW::test_body_motion(BodySW *p_body, const Transform &p_from, const Ve
 				}
 
 				//just do kinematic solving
-				real_t low = 0;
+				real_t low = 0.0;
-				real_t hi = 1;
+				real_t hi = 1.0;
-
+				real_t fraction_coeff = 0.5;
 				for (int k = 0; k < 8; k++) { //steps should be customizable..
+					real_t fraction = low + (hi - low) * fraction_coeff;
 
-					real_t ofs = (low + hi) * 0.5;
+					mshape.motion = body_shape_xform_inv.basis.xform(p_motion * fraction);
-
-					Vector3 sep = motion_normal; //important optimization for this to work fast enough
-
-					mshape.motion = body_shape_xform_inv.basis.xform(p_motion * ofs);
 
 					Vector3 lA, lB;
-
+					Vector3 sep = motion_normal; //important optimization for this to work fast enough
 					bool collided = !CollisionSolverSW::solve_distance(&mshape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj_xform, lA, lB, motion_aabb, &sep);
 
 					if (collided) {
-						hi = ofs;
+						hi = fraction;
+						if ((k == 0) || (low > 0.0)) { // Did it not collide before?
+							// When alternating or first iteration, use dichotomy.
+							fraction_coeff = 0.5;
+						} else {
+							// When colliding again, converge faster towards low fraction
+							// for more accurate results with long motions that collide near the start.
+							fraction_coeff = 0.25;
+						}
 					} else {
 						point_A = lA;
 						point_B = lB;
-						low = ofs;
+						low = fraction;
+						if ((k == 0) || (hi < 1.0)) { // Did it collide before?
+							// When alternating or first iteration, use dichotomy.
+							fraction_coeff = 0.5;
+						} else {
+							// When not colliding again, converge faster towards high fraction
+							// for more accurate results with long motions that collide near the end.
+							fraction_coeff = 0.75;
+						}
 					}
 				}
 

servers/physics_2d/space_2d_sw.cpp

@@ -283,22 +283,38 @@ bool Physics2DDirectSpaceStateSW::cast_motion(const RID &p_shape, const Transfor
 			continue;
 		}
 
-		//just do kinematic solving
-		real_t low = 0;
-		real_t hi = 1;
 		Vector2 mnormal = p_motion.normalized();
 
+		//just do kinematic solving
+		real_t low = 0.0;
+		real_t hi = 1.0;
+		real_t fraction_coeff = 0.5;
 		for (int j = 0; j < 8; j++) { //steps should be customizable..
-
+			real_t fraction = low + (hi - low) * fraction_coeff;
-			real_t ofs = (low + hi) * 0.5;
 
 			Vector2 sep = mnormal; //important optimization for this to work fast enough
-			bool collided = CollisionSolver2DSW::solve(shape, p_xform, p_motion * ofs, col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), nullptr, nullptr, &sep, p_margin);
+			bool collided = CollisionSolver2DSW::solve(shape, p_xform, p_motion * fraction, col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), nullptr, nullptr, &sep, p_margin);
 
 			if (collided) {
-				hi = ofs;
+				hi = fraction;
+				if ((j == 0) || (low > 0.0)) { // Did it not collide before?
+					// When alternating or first iteration, use dichotomy.
+					fraction_coeff = 0.5;
 				} else {
-				low = ofs;
+					// When colliding again, converge faster towards low fraction
+					// for more accurate results with long motions that collide near the start.
+					fraction_coeff = 0.25;
+				}
+			} else {
+				low = fraction;
+				if ((j == 0) || (hi < 1.0)) { // Did it collide before?
+					// When alternating or first iteration, use dichotomy.
+					fraction_coeff = 0.5;
+				} else {
+					// When not colliding again, converge faster towards high fraction
+					// for more accurate results with long motions that collide near the end.
+					fraction_coeff = 0.75;
+				}
 			}
 		}
 
@@ -962,20 +978,35 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
 				}
 
 				//just do kinematic solving
-				real_t low = 0;
+				real_t low = 0.0;
-				real_t hi = 1;
+				real_t hi = 1.0;
-
+				real_t fraction_coeff = 0.5;
 				for (int k = 0; k < 8; k++) { //steps should be customizable..
-
+					real_t fraction = low + (hi - low) * fraction_coeff;
-					real_t ofs = (low + hi) * 0.5;
 
 					Vector2 sep = motion_normal; //important optimization for this to work fast enough
-					bool collided = CollisionSolver2DSW::solve(body_shape, body_shape_xform, p_motion * ofs, against_shape, col_obj_shape_xform, Vector2(), nullptr, nullptr, &sep, 0);
+					bool collided = CollisionSolver2DSW::solve(body_shape, body_shape_xform, p_motion * fraction, against_shape, col_obj_shape_xform, Vector2(), nullptr, nullptr, &sep, 0);
 
 					if (collided) {
-						hi = ofs;
+						hi = fraction;
+						if ((k == 0) || (low > 0.0)) { // Did it not collide before?
+							// When alternating or first iteration, use dichotomy.
+							fraction_coeff = 0.5;
 						} else {
-						low = ofs;
+							// When colliding again, converge faster towards low fraction
+							// for more accurate results with long motions that collide near the start.
+							fraction_coeff = 0.25;
+						}
+					} else {
+						low = fraction;
+						if ((k == 0) || (hi < 1.0)) { // Did it collide before?
+							// When alternating or first iteration, use dichotomy.
+							fraction_coeff = 0.5;
+						} else {
+							// When not colliding again, converge faster towards high fraction
+							// for more accurate results with long motions that collide near the end.
+							fraction_coeff = 0.75;
+						}
 					}
 				}