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.

4 files changed, 78 insertions, 22 deletions

diff --git a/servers/physics_3d/godot_collision_solver_3d.cpp b/servers/physics_3d/godot_collision_solver_3d.cpp
index 686f3e4d59..540b16c6e3 100644
--- a/servers/physics_3d/godot_collision_solver_3d.cpp
+++ b/servers/physics_3d/godot_collision_solver_3d.cpp
@@ -264,7 +264,7 @@ bool GodotCollisionSolver3D::solve_soft_body(const GodotShape3D *p_shape_A, cons
 			local_aabb.size[i] = smax - smin;
 		}
 
-		concave_shape_A->cull(local_aabb, soft_body_concave_callback, &query_cinfo);
+		concave_shape_A->cull(local_aabb, soft_body_concave_callback, &query_cinfo, true);
 	} else {
 		AABB shape_aabb = p_transform_A.xform(p_shape_A->get_aabb());
 		shape_aabb.grow_by(collision_margin);
@@ -346,7 +346,7 @@ bool GodotCollisionSolver3D::solve_concave(const GodotShape3D *p_shape_A, const
 		local_aabb.size[i] = smax - smin;
 	}
 
-	concave_B->cull(local_aabb, concave_callback, &cinfo);
+	concave_B->cull(local_aabb, concave_callback, &cinfo, false);
 
 	return cinfo.collided;
 }
@@ -559,7 +559,7 @@ bool GodotCollisionSolver3D::solve_distance(const GodotShape3D *p_shape_A, const
 			local_aabb.size[i] = smax - smin;
 		}
 
-		concave_B->cull(local_aabb, concave_distance_callback, &cinfo);
+		concave_B->cull(local_aabb, concave_distance_callback, &cinfo, false);
 		if (!cinfo.collided) {
 			r_point_A = cinfo.close_A;
 			r_point_B = cinfo.close_B;
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();
 }
 
diff --git a/servers/physics_3d/godot_shape_3d.cpp b/servers/physics_3d/godot_shape_3d.cpp
index d1e919ab6a..5364a9833d 100644
--- a/servers/physics_3d/godot_shape_3d.cpp
+++ b/servers/physics_3d/godot_shape_3d.cpp
@@ -1401,7 +1401,7 @@ bool GodotConcavePolygonShape3D::_cull(int p_idx, _CullParams *p_params) const {
 	return false;
 }
 
-void GodotConcavePolygonShape3D::cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata) const {
+void GodotConcavePolygonShape3D::cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata, bool p_invert_backface_collision) const {
 	// make matrix local to concave
 	if (faces.size() == 0) {
 		return;
@@ -1416,6 +1416,7 @@ void GodotConcavePolygonShape3D::cull(const AABB &p_local_aabb, QueryCallback p_
 
 	GodotFaceShape3D face; // use this to send in the callback
 	face.backface_collision = backface_collision;
+	face.invert_backface_collision = p_invert_backface_collision;
 
 	_CullParams params;
 	params.aabb = local_aabb;
@@ -1961,7 +1962,7 @@ void GodotHeightMapShape3D::_get_cell(const Vector3 &p_point, int &r_x, int &r_y
 	r_z = (clamped_point.z < 0.0) ? (clamped_point.z - 0.5) : (clamped_point.z + 0.5);
 }
 
-void GodotHeightMapShape3D::cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata) const {
+void GodotHeightMapShape3D::cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata, bool p_invert_backface_collision) const {
 	if (heights.is_empty()) {
 		return;
 	}
@@ -1988,7 +1989,8 @@ void GodotHeightMapShape3D::cull(const AABB &p_local_aabb, QueryCallback p_callb
 	int end_z = MIN(depth - 1, aabb_max[2]);
 
 	GodotFaceShape3D face;
-	face.backface_collision = true;
+	face.backface_collision = !p_invert_backface_collision;
+	face.invert_backface_collision = p_invert_backface_collision;
 
 	for (int z = start_z; z < end_z; z++) {
 		for (int x = start_x; x < end_x; x++) {
diff --git a/servers/physics_3d/godot_shape_3d.h b/servers/physics_3d/godot_shape_3d.h
index 7a32b69166..43319510d4 100644
--- a/servers/physics_3d/godot_shape_3d.h
+++ b/servers/physics_3d/godot_shape_3d.h
@@ -107,7 +107,7 @@ public:
 	// Returns true to stop the query.
 	typedef bool (*QueryCallback)(void *p_userdata, GodotShape3D *p_convex);
 
-	virtual void cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata) const = 0;
+	virtual void cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata, bool p_invert_backface_collision) const = 0;
 
 	GodotConcaveShape3D() {}
 };
@@ -370,7 +370,7 @@ public:
 	virtual bool intersect_point(const Vector3 &p_point) const override;
 	virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override;
 
-	virtual void cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata) const override;
+	virtual void cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata, bool p_invert_backface_collision) const override;
 
 	virtual Vector3 get_moment_of_inertia(real_t p_mass) const override;
 
@@ -433,7 +433,7 @@ public:
 	virtual bool intersect_point(const Vector3 &p_point) const override;
 
 	virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override;
-	virtual void cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata) const override;
+	virtual void cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata, bool p_invert_backface_collision) const override;
 
 	virtual Vector3 get_moment_of_inertia(real_t p_mass) const override;
 
@@ -448,6 +448,7 @@ struct GodotFaceShape3D : public GodotShape3D {
 	Vector3 normal; //cache
 	Vector3 vertex[3];
 	bool backface_collision = false;
+	bool invert_backface_collision = false;
 
 	virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_CONCAVE_POLYGON; }