Merge pull request #55073 from nekomatata/fix-backface-collision

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;

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();
 }
 

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++) {

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; }