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.

servers/physics/space_sw.cpp

@@ -378,6 +378,8 @@ struct _RestCallbackData {
 
 	const CollisionObjectSW *object;
 	const CollisionObjectSW *best_object;
+	int local_shape;
+	int best_local_shape;
 	int shape;
 	int best_shape;
 	Vector3 best_contact;
@@ -402,6 +404,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 PhysicsDirectSpaceStateSW::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) {
 
@@ -737,8 +740,13 @@ bool SpaceSW::test_body_motion(BodySW *p_body, const Transform &p_from, const Ve
 	body_aabb = p_from.xform(p_body->get_inv_transform().xform(body_aabb));
 	body_aabb = body_aabb.grow(p_margin);
 
+	float 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 SpaceSW::test_body_motion(BodySW *p_body, const Transform &p_from, const Ve
 
 				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();
+				float d = n.dot(b);
+
+				// Compute depth on recovered motion.
+				float 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 SpaceSW::test_body_motion(BodySW *p_body, const Transform &p_from, const Ve
 				break;
 			}
 
+			recovered = true;
+
 			body_transform.origin += recover_motion;
 			body_aabb.position += recover_motion;
 
@@ -849,14 +869,14 @@ bool SpaceSW::test_body_motion(BodySW *p_body, const Transform &p_from, const Ve
 
 				//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 (CollisionSolverSW::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 (!CollisionSolverSW::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;
@@ -866,13 +886,12 @@ bool SpaceSW::test_body_motion(BodySW *p_body, const Transform &p_from, const Ve
 				//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);
 
@@ -917,18 +936,12 @@ bool SpaceSW::test_body_motion(BodySW *p_body, const Transform &p_from, const Ve
 	}
 
 	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;
@@ -937,25 +950,41 @@ bool SpaceSW::test_body_motion(BodySW *p_body, const Transform &p_from, const Ve
 		rcd.best_len = 0;
 		rcd.best_object = NULL;
 		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);
-		ShapeSW *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++) {
 
-		for (int i = 0; i < amount; i++) {
+			if (p_body->is_shape_set_as_disabled(j))
+				continue;
 
-			const CollisionObjectSW *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);
+			ShapeSW *body_shape = p_body->get_shape(j);
 
-			rcd.object = col_obj;
-			rcd.shape = shape_idx;
-			bool sc = CollisionSolverSW::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, NULL, p_margin);
-			if (!sc)
+			if (p_exclude_raycast_shapes && body_shape->get_type() == PhysicsServer::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 CollisionObjectSW *col_obj = intersection_query_results[i];
+				int shape_idx = intersection_query_subindex_results[i];
+
+				rcd.object = col_obj;
+				rcd.shape = shape_idx;
+				rcd.local_shape = j;
+				bool sc = CollisionSolverSW::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, NULL, p_margin);
+				if (!sc)
+					continue;
+			}
 		}
 
 		if (rcd.best_len != 0) {
@@ -964,7 +993,7 @@ bool SpaceSW::test_body_motion(BodySW *p_body, const Transform &p_from, const Ve
 				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);
@@ -980,16 +1009,14 @@ bool SpaceSW::test_body_motion(BodySW *p_body, const Transform &p_from, const Ve
 			}
 
 			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());
-			}
+	if (!collided && r_result) {
 
-			collided = false;
-		}
+		r_result->motion = p_motion;
+		r_result->remainder = Vector3();
+		r_result->motion += (body_transform.get_origin() - p_from.get_origin());
 	}
 
 	return collided;

servers/physics_2d/space_2d_sw.cpp

@@ -370,6 +370,7 @@ struct _RestCallbackData2D {
 	Vector2 best_normal;
 	real_t best_len;
 	Vector2 valid_dir;
+	real_t valid_depth;
 	real_t min_allowed_depth;
 };
 
@@ -380,19 +381,25 @@ static void _rest_cbk_result(const Vector2 &p_point_A, const Vector2 &p_point_B,
 	Vector2 contact_rel = p_point_B - p_point_A;
 	real_t len = contact_rel.length();
 
-	if (len == 0)
+	if (len < rd->min_allowed_depth) {
 		return;
+	}
 
-	Vector2 normal = contact_rel / len;
-
-	if (rd->valid_dir != Vector2() && rd->valid_dir.dot(normal) > -CMP_EPSILON)
+	if (len <= rd->best_len) {
 		return;
+	}
 
-	if (len < rd->min_allowed_depth)
-		return;
+	Vector2 normal = contact_rel / len;
 
-	if (len <= rd->best_len)
-		return;
+	if (rd->valid_dir != Vector2()) {
+		if (len > rd->valid_depth) {
+			return;
+		}
+
+		if (rd->valid_dir.dot(normal) > -CMP_EPSILON) {
+			return;
+		}
+	}
 
 	rd->best_len = len;
 	rd->best_contact = p_point_B;
@@ -735,10 +742,13 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
 	ExcludedShapeSW excluded_shape_pairs[max_excluded_shape_pairs];
 	int excluded_shape_pair_count = 0;
 
-	float separation_margin = MIN(p_margin, MAX(0.0, p_motion.length() - CMP_EPSILON)); //don't separate by more than the intended motion
+	float motion_length = p_motion.length();
+	Vector2 motion_normal = p_motion / motion_length;
 
 	Transform2D body_transform = p_from;
 
+	bool recovered = false;
+
 	{
 		//STEP 1, FREE BODY IF STUCK
 
@@ -817,7 +827,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
 					bool did_collide = false;
 
 					Shape2DSW *against_shape = col_obj->get_shape(shape_idx);
-					if (CollisionSolver2DSW::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), cbkres, cbkptr, NULL, separation_margin)) {
+					if (CollisionSolver2DSW::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), cbkres, cbkptr, NULL, p_margin)) {
 						did_collide = cbk.passed > current_passed; //more passed, so collision actually existed
 					}
 
@@ -843,12 +853,20 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
 			}
 
 			Vector2 recover_motion;
-
 			for (int i = 0; i < cbk.amount; i++) {
-
 				Vector2 a = sr[i * 2 + 0];
 				Vector2 b = sr[i * 2 + 1];
-				recover_motion += (b - a) / cbk.amount;
+
+				// Compute plane on b towards a.
+				Vector2 n = (a - b).normalized();
+				float d = n.dot(b);
+
+				// Compute depth on recovered motion.
+				float 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 == Vector2()) {
@@ -856,6 +874,8 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
 				break;
 			}
 
+			recovered = true;
+
 			body_transform.elements[2] += recover_motion;
 			body_aabb.position += recover_motion;
 
@@ -932,7 +952,10 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
 				if (CollisionSolver2DSW::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), NULL, NULL, NULL, 0)) {
 
 					if (col_obj->is_shape_set_as_one_way_collision(col_shape_idx)) {
-						continue;
+						Vector2 direction = col_obj_shape_xform.get_axis(1).normalized();
+						if (motion_normal.dot(direction) < 0) {
+							continue;
+						}
 					}
 
 					stuck = true;
@@ -942,13 +965,12 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
 				//just do kinematic solving
 				real_t low = 0;
 				real_t hi = 1;
-				Vector2 mnormal = p_motion.normalized();
 
 				for (int k = 0; k < 8; k++) { //steps should be customizable..
 
 					real_t ofs = (low + hi) * 0.5;
 
-					Vector2 sep = mnormal; //important optimization for this to work fast enough
+					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(), NULL, NULL, &sep, 0);
 
 					if (collided) {
@@ -972,7 +994,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
 
 					cbk.valid_depth = 10e20;
 
-					Vector2 sep = mnormal; //important optimization for this to work fast enough
+					Vector2 sep = motion_normal; //important optimization for this to work fast enough
 					bool collided = CollisionSolver2DSW::solve(body_shape, body_shape_xform, p_motion * (hi + contact_max_allowed_penetration), col_obj->get_shape(col_shape_idx), col_obj_shape_xform, Vector2(), Physics2DServerSW::_shape_col_cbk, &cbk, &sep, 0);
 					if (!collided || cbk.amount == 0) {
 						continue;
@@ -1005,11 +1027,11 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
 	}
 
 	bool collided = false;
-	if (safe >= 1) {
-		best_shape = -1; //no best shape with cast, reset to -1
-	}
 
-	if (safe < 1) {
+	if (recovered || (safe < 1)) {
+		if (safe >= 1) {
+			best_shape = -1; //no best shape with cast, reset to -1
+		}
 
 		//it collided, let's get the rest info in unsafe advance
 		Transform2D ugt = body_transform;
@@ -1019,9 +1041,10 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
 		rcd.best_len = 0;
 		rcd.best_object = NULL;
 		rcd.best_shape = 0;
-		rcd.min_allowed_depth = test_motion_min_contact_depth;
 
-		//optimization
+		// 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);
+
 		int from_shape = best_shape != -1 ? best_shape : 0;
 		int to_shape = best_shape != -1 ? best_shape + 1 : p_body->get_shape_count();
 
@@ -1070,8 +1093,25 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
 
 				if (col_obj->is_shape_set_as_one_way_collision(shape_idx)) {
 					rcd.valid_dir = col_obj_shape_xform.get_axis(1).normalized();
+
+					float owc_margin = col_obj->get_shape_one_way_collision_margin(shape_idx);
+					rcd.valid_depth = MAX(owc_margin, p_margin); //user specified, but never less than actual margin or it won't work
+
+					if (col_obj->get_type() == CollisionObject2DSW::TYPE_BODY) {
+						const Body2DSW *b = static_cast<const Body2DSW *>(col_obj);
+						if (b->get_mode() == Physics2DServer::BODY_MODE_KINEMATIC || b->get_mode() == Physics2DServer::BODY_MODE_RIGID) {
+							//fix for moving platforms (kinematic and dynamic), margin is increased by how much it moved in the given direction
+							Vector2 lv = b->get_linear_velocity();
+							//compute displacement from linear velocity
+							Vector2 motion = lv * Physics2DDirectBodyStateSW::singleton->step;
+							float motion_len = motion.length();
+							motion.normalize();
+							rcd.valid_depth += motion_len * MAX(motion.dot(-rcd.valid_dir), 0.0);
+						}
+					}
 				} else {
 					rcd.valid_dir = Vector2();
+					rcd.valid_depth = 0;
 				}
 
 				rcd.object = col_obj;