Merge pull request #42575 from madmiraal/fix-one-way-collisions-3.2

[3.2] Fix multiple issues with one-way collisions

servers/physics/space_sw.cpp

@@ -645,7 +645,7 @@ int SpaceSW::test_body_ray_separation(BodySW *p_body, const Transform &p_transfo
 								Vector3 a = sr[k * 2 + 0];
 								Vector3 b = sr[k * 2 + 1];
 
-								recover_motion += (b - a) * 0.4;
+								recover_motion += (b - a) / cbk.amount;
 
 								float depth = a.distance_to(b);
 								if (depth > result.collision_depth) {
@@ -791,7 +791,7 @@ 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) * 0.4;
+				recover_motion += (b - a) / cbk.amount;
 			}
 
 			if (recover_motion == Vector3()) {

servers/physics_2d/body_pair_2d_sw.cpp

@@ -295,19 +295,15 @@ bool BodyPair2DSW::setup(real_t p_step) {
 		if (A->is_shape_set_as_one_way_collision(shape_A)) {
 			Vector2 direction = xform_A.get_axis(1).normalized();
 			bool valid = false;
-			if (B->get_linear_velocity().dot(direction) >= 0) {
-				for (int i = 0; i < contact_count; i++) {
-					Contact &c = contacts[i];
-					if (!c.reused)
-						continue;
-					if (c.normal.dot(direction) > 0) //greater (normal inverted)
-						continue;
-
-					valid = true;
-					break;
-				}
+			for (int i = 0; i < contact_count; i++) {
+				Contact &c = contacts[i];
+				if (!c.reused)
+					continue;
+				if (c.normal.dot(direction) > -CMP_EPSILON) //greater (normal inverted)
+					continue;
+				valid = true;
+				break;
 			}
-
 			if (!valid) {
 				collided = false;
 				oneway_disabled = true;
@@ -318,17 +314,14 @@ bool BodyPair2DSW::setup(real_t p_step) {
 		if (B->is_shape_set_as_one_way_collision(shape_B)) {
 			Vector2 direction = xform_B.get_axis(1).normalized();
 			bool valid = false;
-			if (A->get_linear_velocity().dot(direction) >= 0) {
-				for (int i = 0; i < contact_count; i++) {
-					Contact &c = contacts[i];
-					if (!c.reused)
-						continue;
-					if (c.normal.dot(direction) < 0) //less (normal ok)
-						continue;
-
-					valid = true;
-					break;
-				}
+			for (int i = 0; i < contact_count; i++) {
+				Contact &c = contacts[i];
+				if (!c.reused)
+					continue;
+				if (c.normal.dot(direction) < CMP_EPSILON) //less (normal ok)
+					continue;
+				valid = true;
+				break;
 			}
 			if (!valid) {
 				collided = false;

servers/physics_2d/physics_2d_server_sw.cpp

@@ -167,24 +167,19 @@ void Physics2DServerSW::_shape_col_cbk(const Vector2 &p_point_A, const Vector2 &
 	if (cbk->max == 0)
 		return;
 
+	Vector2 rel_dir = (p_point_A - p_point_B);
+	real_t rel_length2 = rel_dir.length_squared();
 	if (cbk->valid_dir != Vector2()) {
-		if (p_point_A.distance_squared_to(p_point_B) > cbk->valid_depth * cbk->valid_depth) {
-			cbk->invalid_by_dir++;
-			return;
-		}
-		Vector2 rel_dir = (p_point_A - p_point_B).normalized();
-
-		if (cbk->valid_dir.dot(rel_dir) < Math_SQRT12) { //sqrt(2)/2.0 - 45 degrees
-			cbk->invalid_by_dir++;
-
-			/*
-			print_line("A: "+p_point_A);
-			print_line("B: "+p_point_B);
-			print_line("discard too angled "+rtos(cbk->valid_dir.dot((p_point_A-p_point_B))));
-			print_line("resnorm: "+(p_point_A-p_point_B).normalized());
-			print_line("distance: "+rtos(p_point_A.distance_to(p_point_B)));
-			*/
-			return;
+		if (cbk->valid_depth < 10e20) {
+			if (rel_length2 > cbk->valid_depth * cbk->valid_depth ||
+					(rel_length2 > CMP_EPSILON && cbk->valid_dir.dot(rel_dir.normalized()) < CMP_EPSILON)) {
+				cbk->invalid_by_dir++;
+				return;
+			}
+		} else {
+			if (rel_length2 > 0 && cbk->valid_dir.dot(rel_dir.normalized()) < CMP_EPSILON) {
+				return;
+			}
 		}
 	}
 
@@ -201,8 +196,7 @@ void Physics2DServerSW::_shape_col_cbk(const Vector2 &p_point_A, const Vector2 &
 			}
 		}
 
-		real_t d = p_point_A.distance_squared_to(p_point_B);
-		if (d < min_depth)
+		if (rel_length2 < min_depth)
 			return;
 		cbk->ptr[min_depth_idx * 2 + 0] = p_point_A;
 		cbk->ptr[min_depth_idx * 2 + 1] = p_point_B;

servers/physics_2d/space_2d_sw.cpp

@@ -370,7 +370,6 @@ struct _RestCallbackData2D {
 	Vector2 best_normal;
 	real_t best_len;
 	Vector2 valid_dir;
-	real_t valid_depth;
 	real_t min_allowed_depth;
 };
 
@@ -378,16 +377,17 @@ static void _rest_cbk_result(const Vector2 &p_point_A, const Vector2 &p_point_B,
 
 	_RestCallbackData2D *rd = (_RestCallbackData2D *)p_userdata;
 
-	if (rd->valid_dir != Vector2()) {
-		if (p_point_A.distance_squared_to(p_point_B) > rd->valid_depth * rd->valid_depth)
-			return;
-		if (rd->valid_dir.dot((p_point_A - p_point_B).normalized()) < Math_PI * 0.25)
-			return;
-	}
-
 	Vector2 contact_rel = p_point_B - p_point_A;
 	real_t len = contact_rel.length();
 
+	if (len == 0)
+		return;
+
+	Vector2 normal = contact_rel / len;
+
+	if (rd->valid_dir != Vector2() && rd->valid_dir.dot(normal) > -CMP_EPSILON)
+		return;
+
 	if (len < rd->min_allowed_depth)
 		return;
 
@@ -396,7 +396,7 @@ static void _rest_cbk_result(const Vector2 &p_point_A, const Vector2 &p_point_B,
 
 	rd->best_len = len;
 	rd->best_contact = p_point_B;
-	rd->best_normal = contact_rel / len;
+	rd->best_normal = normal;
 	rd->best_object = rd->object;
 	rd->best_shape = rd->shape;
 	rd->best_local_shape = rd->local_shape;
@@ -431,7 +431,6 @@ bool Physics2DDirectSpaceStateSW::rest_info(RID p_shape, const Transform2D &p_sh
 			continue;
 
 		rcd.valid_dir = Vector2();
-		rcd.valid_depth = 0;
 		rcd.object = col_obj;
 		rcd.shape = shape_idx;
 		rcd.local_shape = 0;
@@ -638,7 +637,7 @@ int Space2DSW::test_body_ray_separation(Body2DSW *p_body, const Transform2D &p_t
 								Vector2 a = sr[k * 2 + 0];
 								Vector2 b = sr[k * 2 + 1];
 
-								recover_motion += (b - a) * 0.4;
+								recover_motion += (b - a) / cbk.amount;
 
 								float depth = a.distance_to(b);
 								if (depth > result.collision_depth) {
@@ -849,7 +848,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
 
 				Vector2 a = sr[i * 2 + 0];
 				Vector2 b = sr[i * 2 + 1];
-				recover_motion += (b - a) * 0.4;
+				recover_motion += (b - a) / cbk.amount;
 			}
 
 			if (recover_motion == Vector2()) {
@@ -1010,7 +1009,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
 		best_shape = -1; //no best shape with cast, reset to -1
 	}
 
-	{
+	if (safe < 1) {
 
 		//it collided, let's get the rest info in unsafe advance
 		Transform2D ugt = body_transform;
@@ -1070,12 +1069,9 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
 				Transform2D col_obj_shape_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
 
 				if (col_obj->is_shape_set_as_one_way_collision(shape_idx)) {
-
 					rcd.valid_dir = col_obj_shape_xform.get_axis(1).normalized();
-					rcd.valid_depth = 10e20;
 				} else {
 					rcd.valid_dir = Vector2();
-					rcd.valid_depth = 0;
 				}
 
 				rcd.object = col_obj;