Fix contact generation with backface collision disabled

Replaced the previous implementation for backface collision handling (in
test_axis function from SAT algorithm) with much simpler logic in the
collision generation phase with face shapes, in order to get rid of
wrong contacts when backface collision is disabled.

Now it just ignores the generated collision if the contact normal is
against the face normal, with a threshold to keep edge contacts.

Added a special case for soft bodies to invert the collision instead of
ignoring it, because for now it's the best solution to avoid soft bodies
to go through concave shapes (they use small spheres). This might be
replaced with a better algorithm for soft bodies later.

1 files changed, 66 insertions, 13 deletions

diff --git a/servers/physics_3d/godot_collision_solver_3d_sat.cpp b/servers/physics_3d/godot_collision_solver_3d_sat.cpp
index 0790333f65..4faa07b6c9 100644
--- a/servers/physics_3d/godot_collision_solver_3d_sat.cpp
+++ b/servers/physics_3d/godot_collision_solver_3d_sat.cpp
@@ -36,6 +36,8 @@
 
 #define fallback_collision_solver gjk_epa_calculate_penetration
 
+#define _BACKFACE_NORMAL_THRESHOLD -0.0002
+
 // Cylinder SAT analytic methods and face-circle contact points for cylinder-trimesh and cylinder-box collision are based on ODE colliders.
 
 /*
@@ -612,13 +614,14 @@ class SeparatorAxisTest {
 	const Transform3D *transform_A = nullptr;
 	const Transform3D *transform_B = nullptr;
 	real_t best_depth = 1e15;
-	Vector3 best_axis;
 	_CollectorCallback *callback = nullptr;
 	real_t margin_A = 0.0;
 	real_t margin_B = 0.0;
 	Vector3 separator_axis;
 
 public:
+	Vector3 best_axis;
+
 	_FORCE_INLINE_ bool test_previous_axis() {
 		if (callback && callback->prev_axis && *callback->prev_axis != Vector3()) {
 			return test_axis(*callback->prev_axis);
@@ -627,7 +630,7 @@ public:
 		}
 	}
 
-	_FORCE_INLINE_ bool test_axis(const Vector3 &p_axis, bool p_directional = false) {
+	_FORCE_INLINE_ bool test_axis(const Vector3 &p_axis) {
 		Vector3 axis = p_axis;
 
 		if (axis.is_equal_approx(Vector3())) {
@@ -661,12 +664,7 @@ public:
 		//use the smallest depth
 
 		if (min_B < 0.0) { // could be +0.0, we don't want it to become -0.0
-			if (p_directional) {
-				min_B = max_B;
-				axis = -axis;
-			} else {
-				min_B = -min_B;
-			}
+			min_B = -min_B;
 		}
 
 		if (max_B < min_B) {
@@ -1014,7 +1012,7 @@ static void _collision_sphere_face(const GodotShape3D *p_a, const Transform3D &p
 
 	Vector3 normal = (vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized();
 
-	if (!separator.test_axis(normal, !face_B->backface_collision)) {
+	if (!separator.test_axis(normal)) {
 		return;
 	}
 
@@ -1041,6 +1039,17 @@ static void _collision_sphere_face(const GodotShape3D *p_a, const Transform3D &p
 		}
 	}
 
+	if (!face_B->backface_collision) {
+		if (separator.best_axis.dot(normal) < _BACKFACE_NORMAL_THRESHOLD) {
+			if (face_B->invert_backface_collision) {
+				separator.best_axis = separator.best_axis.bounce(normal);
+			} else {
+				// Just ignore backface collision.
+				return;
+			}
+		}
+	}
+
 	separator.generate_contacts();
 }
 
@@ -1486,7 +1495,7 @@ static void _collision_box_face(const GodotShape3D *p_a, const Transform3D &p_tr
 
 	Vector3 normal = (vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized();
 
-	if (!separator.test_axis(normal, !face_B->backface_collision)) {
+	if (!separator.test_axis(normal)) {
 		return;
 	}
 
@@ -1591,6 +1600,17 @@ static void _collision_box_face(const GodotShape3D *p_a, const Transform3D &p_tr
 		}
 	}
 
+	if (!face_B->backface_collision) {
+		if (separator.best_axis.dot(normal) < _BACKFACE_NORMAL_THRESHOLD) {
+			if (face_B->invert_backface_collision) {
+				separator.best_axis = separator.best_axis.bounce(normal);
+			} else {
+				// Just ignore backface collision.
+				return;
+			}
+		}
+	}
+
 	separator.generate_contacts();
 }
 
@@ -1802,7 +1822,7 @@ static void _collision_capsule_face(const GodotShape3D *p_a, const Transform3D &
 
 	Vector3 normal = (vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized();
 
-	if (!separator.test_axis(normal, !face_B->backface_collision)) {
+	if (!separator.test_axis(normal)) {
 		return;
 	}
 
@@ -1858,6 +1878,17 @@ static void _collision_capsule_face(const GodotShape3D *p_a, const Transform3D &
 		}
 	}
 
+	if (!face_B->backface_collision) {
+		if (separator.best_axis.dot(normal) < _BACKFACE_NORMAL_THRESHOLD) {
+			if (face_B->invert_backface_collision) {
+				separator.best_axis = separator.best_axis.bounce(normal);
+			} else {
+				// Just ignore backface collision.
+				return;
+			}
+		}
+	}
+
 	separator.generate_contacts();
 }
 
@@ -1952,7 +1983,7 @@ static void _collision_cylinder_face(const GodotShape3D *p_a, const Transform3D
 	Vector3 normal = (vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized();
 
 	// Face B normal.
-	if (!separator.test_axis(normal, !face_B->backface_collision)) {
+	if (!separator.test_axis(normal)) {
 		return;
 	}
 
@@ -2034,6 +2065,17 @@ static void _collision_cylinder_face(const GodotShape3D *p_a, const Transform3D
 		}
 	}
 
+	if (!face_B->backface_collision) {
+		if (separator.best_axis.dot(normal) < _BACKFACE_NORMAL_THRESHOLD) {
+			if (face_B->invert_backface_collision) {
+				separator.best_axis = separator.best_axis.bounce(normal);
+			} else {
+				// Just ignore backface collision.
+				return;
+			}
+		}
+	}
+
 	separator.generate_contacts();
 }
 
@@ -2174,7 +2216,7 @@ static void _collision_convex_polygon_face(const GodotShape3D *p_a, const Transf
 
 	Vector3 normal = (vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized();
 
-	if (!separator.test_axis(normal, !face_B->backface_collision)) {
+	if (!separator.test_axis(normal)) {
 		return;
 	}
 
@@ -2266,6 +2308,17 @@ static void _collision_convex_polygon_face(const GodotShape3D *p_a, const Transf
 		}
 	}
 
+	if (!face_B->backface_collision) {
+		if (separator.best_axis.dot(normal) < _BACKFACE_NORMAL_THRESHOLD) {
+			if (face_B->invert_backface_collision) {
+				separator.best_axis = separator.best_axis.bounce(normal);
+			} else {
+				// Just ignore backface collision.
+				return;
+			}
+		}
+	}
+
 	separator.generate_contacts();
 }