Fix test_body_motion recovery

This change makes test_body_motion more reliable when the kinematic body
recovers from being stuck.

- When recovery occurs, the rest information is generated, in order to
make sure collision results from test_move, move_and_collide and
move_and_slide are consistent and return a collision in case of overlap.

- The new calculation for recovery vector makes sure the recovery is
never more than the overlap depth between shapes.
This can help with cases where the kinematic body overlaps with several
shapes.
Recovery is made iteratively, without forcing a full overlap at each
step. This helps with getting proper rest information when recovery
occurs.

- One Way Collision:
When attempting motion, contact direction is checked against motion
before skipping in order to solve cases where kinematic bodies can sink
into one-way collision shapes.
Rest info now sets max contact depth in order to properly handle one-way
collision.

- Low speed motion is now handled in the rest info, by never setting
min_allowed_depth lower than motion length.
Separation is always applied with full margin, otherwise contact is lost
when low speed motion occurs right after higher speed motion.

- Similar changes are applied to 3D in order to make 2D and 3D
consistent.

1 files changed, 61 insertions, 34 deletions

diff --git a/servers/physics_3d/space_3d_sw.cpp b/servers/physics_3d/space_3d_sw.cpp
index 40537c7001..dd5754b9ac 100644
--- a/servers/physics_3d/space_3d_sw.cpp
+++ b/servers/physics_3d/space_3d_sw.cpp
@@ -384,6 +384,8 @@ bool PhysicsDirectSpaceState3DSW::collide_shape(RID p_shape, const Transform &p_
 struct _RestCallbackData {
 	const CollisionObject3DSW *object;
 	const CollisionObject3DSW *best_object;
+	int local_shape;
+	int best_local_shape;
 	int shape;
 	int best_shape;
 	Vector3 best_contact;
@@ -409,6 +411,7 @@ static void _rest_cbk_result(const Vector3 &p_point_A, const Vector3 &p_point_B,
 	rd->best_normal = contact_rel / len;
 	rd->best_object = rd->object;
 	rd->best_shape = rd->shape;
+	rd->best_local_shape = rd->local_shape;
 }
 
 bool PhysicsDirectSpaceState3DSW::rest_info(RID p_shape, const Transform &p_shape_xform, real_t p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) {
@@ -739,8 +742,13 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons
 	body_aabb = p_from.xform(p_body->get_inv_transform().xform(body_aabb));
 	body_aabb = body_aabb.grow(p_margin);
 
+	real_t motion_length = p_motion.length();
+	Vector3 motion_normal = p_motion / motion_length;
+
 	Transform body_transform = p_from;
 
+	bool recovered = false;
+
 	{
 		//STEP 1, FREE BODY IF STUCK
 
@@ -791,7 +799,17 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons
 			for (int i = 0; i < cbk.amount; i++) {
 				Vector3 a = sr[i * 2 + 0];
 				Vector3 b = sr[i * 2 + 1];
-				recover_motion += (b - a) / cbk.amount;
+
+				// Compute plane on b towards a.
+				Vector3 n = (a - b).normalized();
+				real_t d = n.dot(b);
+
+				// Compute depth on recovered motion.
+				real_t depth = n.dot(a + recover_motion) - d;
+				if (depth > 0.0) {
+					// Only recover if there is penetration.
+					recover_motion -= n * depth * 0.4;
+				}
 			}
 
 			if (recover_motion == Vector3()) {
@@ -799,6 +817,8 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons
 				break;
 			}
 
+			recovered = true;
+
 			body_transform.origin += recover_motion;
 			body_aabb.position += recover_motion;
 
@@ -848,14 +868,14 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons
 
 				//test initial overlap, does it collide if going all the way?
 				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?
 				if (CollisionSolver3DSW::solve_distance(&mshape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, motion_aabb, &sep_axis)) {
 					continue;
 				}
-				sep_axis = p_motion.normalized();
+				sep_axis = motion_normal;
 
 				if (!CollisionSolver3DSW::solve_distance(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, motion_aabb, &sep_axis)) {
 					stuck = true;
@@ -865,13 +885,12 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons
 				//just do kinematic solving
 				real_t low = 0;
 				real_t hi = 1;
-				Vector3 mnormal = p_motion.normalized();
 
 				for (int k = 0; k < 8; k++) { //steps should be customizable..
 
 					real_t ofs = (low + hi) * 0.5;
 
-					Vector3 sep = mnormal; //important optimization for this to work fast enough
+					Vector3 sep = motion_normal; //important optimization for this to work fast enough
 
 					mshape.motion = body_shape_xform_inv.basis.xform(p_motion * ofs);
 
@@ -912,16 +931,11 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons
 	}
 
 	bool collided = false;
-	if (safe >= 1) {
-		//not collided
-		collided = false;
-		if (r_result) {
-			r_result->motion = p_motion;
-			r_result->remainder = Vector3();
-			r_result->motion += (body_transform.get_origin() - p_from.get_origin());
+	if (recovered || (safe < 1)) {
+		if (safe >= 1) {
+			best_shape = -1; //no best shape with cast, reset to -1
 		}
 
-	} else {
 		//it collided, let's get the rest info in unsafe advance
 		Transform ugt = body_transform;
 		ugt.origin += p_motion * unsafe;
@@ -930,25 +944,40 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons
 		rcd.best_len = 0;
 		rcd.best_object = nullptr;
 		rcd.best_shape = 0;
-		rcd.min_allowed_depth = test_motion_min_contact_depth;
 
-		Transform body_shape_xform = ugt * p_body->get_shape_transform(best_shape);
-		Shape3DSW *body_shape = p_body->get_shape(best_shape);
+		// Allowed depth can't be lower than motion length, in order to handle contacts at low speed.
+		rcd.min_allowed_depth = MIN(motion_length, test_motion_min_contact_depth);
 
-		body_aabb.position += p_motion * unsafe;
+		int from_shape = best_shape != -1 ? best_shape : 0;
+		int to_shape = best_shape != -1 ? best_shape + 1 : p_body->get_shape_count();
 
-		int amount = _cull_aabb_for_body(p_body, body_aabb);
+		for (int j = from_shape; j < to_shape; j++) {
+			if (p_body->is_shape_set_as_disabled(j)) {
+				continue;
+			}
 
-		for (int i = 0; i < amount; i++) {
-			const CollisionObject3DSW *col_obj = intersection_query_results[i];
-			int shape_idx = intersection_query_subindex_results[i];
+			Transform body_shape_xform = ugt * p_body->get_shape_transform(j);
+			Shape3DSW *body_shape = p_body->get_shape(j);
 
-			rcd.object = col_obj;
-			rcd.shape = shape_idx;
-			bool sc = CollisionSolver3DSW::solve_static(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), _rest_cbk_result, &rcd, nullptr, p_margin);
-			if (!sc) {
+			if (p_exclude_raycast_shapes && body_shape->get_type() == PhysicsServer3D::SHAPE_RAY) {
 				continue;
 			}
+
+			body_aabb.position += p_motion * unsafe;
+
+			int amount = _cull_aabb_for_body(p_body, body_aabb);
+
+			for (int i = 0; i < amount; i++) {
+				const CollisionObject3DSW *col_obj = intersection_query_results[i];
+				int shape_idx = intersection_query_subindex_results[i];
+
+				rcd.object = col_obj;
+				rcd.shape = shape_idx;
+				bool sc = CollisionSolver3DSW::solve_static(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), _rest_cbk_result, &rcd, nullptr, p_margin);
+				if (!sc) {
+					continue;
+				}
+			}
 		}
 
 		if (rcd.best_len != 0) {
@@ -956,7 +985,7 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons
 				r_result->collider = rcd.best_object->get_self();
 				r_result->collider_id = rcd.best_object->get_instance_id();
 				r_result->collider_shape = rcd.best_shape;
-				r_result->collision_local_shape = best_shape;
+				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->collider_metadata = rcd.best_object->get_shape_metadata(rcd.best_shape);
@@ -972,17 +1001,15 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons
 			}
 
 			collided = true;
-		} else {
-			if (r_result) {
-				r_result->motion = p_motion;
-				r_result->remainder = Vector3();
-				r_result->motion += (body_transform.get_origin() - p_from.get_origin());
-			}
-
-			collided = false;
 		}
 	}
 
+	if (!collided && r_result) {
+		r_result->motion = p_motion;
+		r_result->remainder = Vector3();
+		r_result->motion += (body_transform.get_origin() - p_from.get_origin());
+	}
+
 	return collided;
 }