Merge pull request #47139 from nekomatata/concave-backface-collision

Disable backface collision with ConcavePolygonShape by default

3 files changed, 169 insertions, 67 deletions

diff --git a/servers/physics_3d/collision_solver_3d_sat.cpp b/servers/physics_3d/collision_solver_3d_sat.cpp
index 33075a38be..db48f011c6 100644
--- a/servers/physics_3d/collision_solver_3d_sat.cpp
+++ b/servers/physics_3d/collision_solver_3d_sat.cpp
@@ -626,7 +626,7 @@ public:
 		}
 	}
 
-	_FORCE_INLINE_ bool test_axis(const Vector3 &p_axis) {
+	_FORCE_INLINE_ bool test_axis(const Vector3 &p_axis, bool p_directional = false) {
 		Vector3 axis = p_axis;
 
 		if (Math::abs(axis.x) < CMP_EPSILON &&
@@ -662,7 +662,12 @@ public:
 		//use the smallest depth
 
 		if (min_B < 0.0) { // could be +0.0, we don't want it to become -0.0
-			min_B = -min_B;
+			if (p_directional) {
+				min_B = max_B;
+				axis = -axis;
+			} else {
+				min_B = -min_B;
+			}
 		}
 
 		if (max_B < min_B) {
@@ -1006,23 +1011,31 @@ static void _collision_sphere_face(const Shape3DSW *p_a, const Transform &p_tran
 		p_transform_b.xform(face_B->vertex[2]),
 	};
 
-	if (!separator.test_axis((vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized())) {
+	Vector3 normal = (vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized();
+
+	if (!separator.test_axis(normal, !face_B->backface_collision)) {
 		return;
 	}
 
 	// edges and points of B
 	for (int i = 0; i < 3; i++) {
 		Vector3 n1 = vertex[i] - p_transform_a.origin;
+		if (n1.dot(normal) < 0.0) {
+			n1 *= -1.0;
+		}
 
-		if (!separator.test_axis(n1.normalized())) {
+		if (!separator.test_axis(n1.normalized(), !face_B->backface_collision)) {
 			return;
 		}
 
 		Vector3 n2 = vertex[(i + 1) % 3] - vertex[i];
 
 		Vector3 axis = n1.cross(n2).cross(n2).normalized();
+		if (axis.dot(normal) < 0.0) {
+			axis *= -1.0;
+		}
 
-		if (!separator.test_axis(axis)) {
+		if (!separator.test_axis(axis, !face_B->backface_collision)) {
 			return;
 		}
 	}
@@ -1467,15 +1480,20 @@ static void _collision_box_face(const Shape3DSW *p_a, const Transform &p_transfo
 		p_transform_b.xform(face_B->vertex[2]),
 	};
 
-	if (!separator.test_axis((vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized())) {
+	Vector3 normal = (vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized();
+
+	if (!separator.test_axis(normal, !face_B->backface_collision)) {
 		return;
 	}
 
 	// faces of A
 	for (int i = 0; i < 3; i++) {
 		Vector3 axis = p_transform_a.basis.get_axis(i).normalized();
+		if (axis.dot(normal) < 0.0) {
+			axis *= -1.0;
+		}
 
-		if (!separator.test_axis(axis)) {
+		if (!separator.test_axis(axis, !face_B->backface_collision)) {
 			return;
 		}
 	}
@@ -1486,9 +1504,12 @@ static void _collision_box_face(const Shape3DSW *p_a, const Transform &p_transfo
 		Vector3 e = vertex[i] - vertex[(i + 1) % 3];
 
 		for (int j = 0; j < 3; j++) {
-			Vector3 axis = p_transform_a.basis.get_axis(j);
+			Vector3 axis = e.cross(p_transform_a.basis.get_axis(j)).normalized();
+			if (axis.dot(normal) < 0.0) {
+				axis *= -1.0;
+			}
 
-			if (!separator.test_axis(e.cross(axis).normalized())) {
+			if (!separator.test_axis(axis, !face_B->backface_collision)) {
 				return;
 			}
 		}
@@ -1508,8 +1529,11 @@ static void _collision_box_face(const Shape3DSW *p_a, const Transform &p_transfo
 					(cnormal_a.z < 0) ? -box_A->get_half_extents().z : box_A->get_half_extents().z));
 
 			Vector3 axis_ab = support_a - vertex[v];
+			if (axis_ab.dot(normal) < 0.0) {
+				axis_ab *= -1.0;
+			}
 
-			if (!separator.test_axis(axis_ab.normalized())) {
+			if (!separator.test_axis(axis_ab.normalized(), !face_B->backface_collision)) {
 				return;
 			}
 
@@ -1519,7 +1543,12 @@ static void _collision_box_face(const Shape3DSW *p_a, const Transform &p_transfo
 				//a ->b
 				Vector3 axis_a = p_transform_a.basis.get_axis(i);
 
-				if (!separator.test_axis(axis_ab.cross(axis_a).cross(axis_a).normalized())) {
+				Vector3 axis = axis_ab.cross(axis_a).cross(axis_a).normalized();
+				if (axis.dot(normal) < 0.0) {
+					axis *= -1.0;
+				}
+
+				if (!separator.test_axis(axis, !face_B->backface_collision)) {
 					return;
 				}
 			}
@@ -1544,7 +1573,12 @@ static void _collision_box_face(const Shape3DSW *p_a, const Transform &p_transfo
 
 						Vector3 n = (p2 - p1);
 
-						if (!separator.test_axis((point - p2).cross(n).cross(n).normalized())) {
+						Vector3 axis = (point - p2).cross(n).cross(n).normalized();
+						if (axis.dot(normal) < 0.0) {
+							axis *= -1.0;
+						}
+
+						if (!separator.test_axis(axis, !face_B->backface_collision)) {
 							return;
 						}
 					}
@@ -1759,7 +1793,9 @@ static void _collision_capsule_face(const Shape3DSW *p_a, const Transform &p_tra
 		p_transform_b.xform(face_B->vertex[2]),
 	};
 
-	if (!separator.test_axis((vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized())) {
+	Vector3 normal = (vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized();
+
+	if (!separator.test_axis(normal, !face_B->backface_collision)) {
 		return;
 	}
 
@@ -1770,13 +1806,22 @@ static void _collision_capsule_face(const Shape3DSW *p_a, const Transform &p_tra
 	for (int i = 0; i < 3; i++) {
 		// edge-cylinder
 		Vector3 edge_axis = vertex[i] - vertex[(i + 1) % 3];
+
 		Vector3 axis = edge_axis.cross(capsule_axis).normalized();
+		if (axis.dot(normal) < 0.0) {
+			axis *= -1.0;
+		}
 
-		if (!separator.test_axis(axis)) {
+		if (!separator.test_axis(axis, !face_B->backface_collision)) {
 			return;
 		}
 
-		if (!separator.test_axis((p_transform_a.origin - vertex[i]).cross(capsule_axis).cross(capsule_axis).normalized())) {
+		Vector3 dir_axis = (p_transform_a.origin - vertex[i]).cross(capsule_axis).cross(capsule_axis).normalized();
+		if (dir_axis.dot(normal) < 0.0) {
+			dir_axis *= -1.0;
+		}
+
+		if (!separator.test_axis(dir_axis, !face_B->backface_collision)) {
 			return;
 		}
 
@@ -1785,16 +1830,22 @@ static void _collision_capsule_face(const Shape3DSW *p_a, const Transform &p_tra
 			Vector3 sphere_pos = p_transform_a.origin + ((j == 0) ? capsule_axis : -capsule_axis);
 
 			Vector3 n1 = sphere_pos - vertex[i];
+			if (n1.dot(normal) < 0.0) {
+				n1 *= -1.0;
+			}
 
-			if (!separator.test_axis(n1.normalized())) {
+			if (!separator.test_axis(n1.normalized(), !face_B->backface_collision)) {
 				return;
 			}
 
 			Vector3 n2 = edge_axis;
 
 			axis = n1.cross(n2).cross(n2);
+			if (axis.dot(normal) < 0.0) {
+				axis *= -1.0;
+			}
 
-			if (!separator.test_axis(axis.normalized())) {
+			if (!separator.test_axis(axis.normalized(), !face_B->backface_collision)) {
 				return;
 			}
 		}
@@ -1891,18 +1942,21 @@ static void _collision_cylinder_face(const Shape3DSW *p_a, const Transform &p_tr
 		p_transform_b.xform(face_B->vertex[2]),
 	};
 
+	Vector3 normal = (vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized();
+
 	// Face B normal.
-	if (!separator.test_axis((vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized())) {
+	if (!separator.test_axis(normal, !face_B->backface_collision)) {
 		return;
 	}
 
 	Vector3 cyl_axis = p_transform_a.basis.get_axis(1).normalized();
+	if (cyl_axis.dot(normal) < 0.0) {
+		cyl_axis *= -1.0;
+	}
 
 	// Cylinder end caps.
-	{
-		if (!separator.test_axis(cyl_axis)) {
-			return;
-		}
+	if (!separator.test_axis(cyl_axis, !face_B->backface_collision)) {
+		return;
 	}
 
 	// Edges of B, cylinder lateral surface.
@@ -1913,7 +1967,11 @@ static void _collision_cylinder_face(const Shape3DSW *p_a, const Transform &p_tr
 			continue;
 		}
 
-		if (!separator.test_axis(axis.normalized())) {
+		if (axis.dot(normal) < 0.0) {
+			axis *= -1.0;
+		}
+
+		if (!separator.test_axis(axis.normalized(), !face_B->backface_collision)) {
 			return;
 		}
 	}
@@ -1922,8 +1980,11 @@ static void _collision_cylinder_face(const Shape3DSW *p_a, const Transform &p_tr
 	for (int i = 0; i < 3; i++) {
 		const Vector3 &point = vertex[i];
 		Vector3 axis = Plane(cyl_axis, 0).project(point).normalized();
+		if (axis.dot(normal) < 0.0) {
+			axis *= -1.0;
+		}
 
-		if (!separator.test_axis(axis)) {
+		if (!separator.test_axis(axis, !face_B->backface_collision)) {
 			return;
 		}
 	}
@@ -1956,8 +2017,11 @@ static void _collision_cylinder_face(const Shape3DSW *p_a, const Transform &p_tr
 
 			// Axis is orthogonal both to tangent and edge direction.
 			Vector3 axis = tangent.cross(edge_dir);
+			if (axis.dot(normal) < 0.0) {
+				axis *= -1.0;
+			}
 
-			if (!separator.test_axis(axis.normalized())) {
+			if (!separator.test_axis(axis.normalized(), !face_B->backface_collision)) {
 				return;
 			}
 		}
@@ -2097,7 +2161,9 @@ static void _collision_convex_polygon_face(const Shape3DSW *p_a, const Transform
 		p_transform_b.xform(face_B->vertex[2]),
 	};
 
-	if (!separator.test_axis((vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized())) {
+	Vector3 normal = (vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized();
+
+	if (!separator.test_axis(normal, !face_B->backface_collision)) {
 		return;
 	}
 
@@ -2105,8 +2171,11 @@ static void _collision_convex_polygon_face(const Shape3DSW *p_a, const Transform
 	for (int i = 0; i < face_count; i++) {
 		//Vector3 axis = p_transform_a.xform( faces[i].plane ).normal;
 		Vector3 axis = p_transform_a.basis.xform(faces[i].plane.normal).normalized();
+		if (axis.dot(normal) < 0.0) {
+			axis *= -1.0;
+		}
 
-		if (!separator.test_axis(axis)) {
+		if (!separator.test_axis(axis, !face_B->backface_collision)) {
 			return;
 		}
 	}
@@ -2119,8 +2188,11 @@ static void _collision_convex_polygon_face(const Shape3DSW *p_a, const Transform
 			Vector3 e2 = vertex[j] - vertex[(j + 1) % 3];
 
 			Vector3 axis = e1.cross(e2).normalized();
+			if (axis.dot(normal) < 0.0) {
+				axis *= -1.0;
+			}
 
-			if (!separator.test_axis(axis)) {
+			if (!separator.test_axis(axis, !face_B->backface_collision)) {
 				return;
 			}
 		}
@@ -2132,7 +2204,12 @@ static void _collision_convex_polygon_face(const Shape3DSW *p_a, const Transform
 			Vector3 va = p_transform_a.xform(vertices[i]);
 
 			for (int j = 0; j < 3; j++) {
-				if (!separator.test_axis((va - vertex[j]).normalized())) {
+				Vector3 axis = (va - vertex[j]).normalized();
+				if (axis.dot(normal) < 0.0) {
+					axis *= -1.0;
+				}
+
+				if (!separator.test_axis(axis, !face_B->backface_collision)) {
 					return;
 				}
 			}
@@ -2147,7 +2224,12 @@ static void _collision_convex_polygon_face(const Shape3DSW *p_a, const Transform
 			for (int j = 0; j < 3; j++) {
 				Vector3 e3 = vertex[j];
 
-				if (!separator.test_axis((e1 - e3).cross(n).cross(n).normalized())) {
+				Vector3 axis = (e1 - e3).cross(n).cross(n).normalized();
+				if (axis.dot(normal) < 0.0) {
+					axis *= -1.0;
+				}
+
+				if (!separator.test_axis(axis, !face_B->backface_collision)) {
 					return;
 				}
 			}
@@ -2161,7 +2243,12 @@ static void _collision_convex_polygon_face(const Shape3DSW *p_a, const Transform
 			for (int j = 0; j < vertex_count; j++) {
 				Vector3 e3 = p_transform_a.xform(vertices[j]);
 
-				if (!separator.test_axis((e1 - e3).cross(n).cross(n).normalized())) {
+				Vector3 axis = (e1 - e3).cross(n).cross(n).normalized();
+				if (axis.dot(normal) < 0.0) {
+					axis *= -1.0;
+				}
+
+				if (!separator.test_axis(axis, !face_B->backface_collision)) {
 					return;
 				}
 			}
diff --git a/servers/physics_3d/shape_3d_sw.cpp b/servers/physics_3d/shape_3d_sw.cpp
index 02d0c66215..687a7b288f 100644
--- a/servers/physics_3d/shape_3d_sw.cpp
+++ b/servers/physics_3d/shape_3d_sw.cpp
@@ -1134,7 +1134,7 @@ void FaceShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_
 	Vector3 n = p_normal;
 
 	/** TEST FACE AS SUPPORT **/
-	if (normal.dot(n) > _FACE_IS_VALID_SUPPORT_THRESHOLD) {
+	if (Math::abs(normal.dot(n)) > _FACE_IS_VALID_SUPPORT_THRESHOLD) {
 		r_amount = 3;
 		r_type = FEATURE_FACE;
 		for (int i = 0; i < 3; i++) {
@@ -1187,7 +1187,11 @@ bool FaceShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_e
 	if (c) {
 		r_normal = Plane(vertex[0], vertex[1], vertex[2]).normal;
 		if (r_normal.dot(p_end - p_begin) > 0) {
-			r_normal = -r_normal;
+			if (backface_collision) {
+				r_normal = -r_normal;
+			} else {
+				c = false;
+			}
 		}
 	}
 
@@ -1285,30 +1289,24 @@ void ConcavePolygonShape3DSW::_cull_segment(int p_idx, _SegmentCullParams *p_par
 	}
 
 	if (bvh->face_index >= 0) {
-		Vector3 res;
-		Vector3 vertices[3] = {
-			p_params->vertices[p_params->faces[bvh->face_index].indices[0]],
-			p_params->vertices[p_params->faces[bvh->face_index].indices[1]],
-			p_params->vertices[p_params->faces[bvh->face_index].indices[2]]
-		};
+		const Face *f = &p_params->faces[bvh->face_index];
+		FaceShape3DSW *face = p_params->face;
+		face->normal = f->normal;
+		face->vertex[0] = p_params->vertices[f->indices[0]];
+		face->vertex[1] = p_params->vertices[f->indices[1]];
+		face->vertex[2] = p_params->vertices[f->indices[2]];
 
-		if (Geometry3D::segment_intersects_triangle(
-					p_params->from,
-					p_params->to,
-					vertices[0],
-					vertices[1],
-					vertices[2],
-					&res)) {
+		Vector3 res;
+		Vector3 normal;
+		if (face->intersect_segment(p_params->from, p_params->to, res, normal)) {
 			real_t d = p_params->dir.dot(res) - p_params->dir.dot(p_params->from);
-			//TODO, seems segmen/triangle intersection is broken :(
-			if (d > 0 && d < p_params->min_d) {
+			if ((d > 0) && (d < p_params->min_d)) {
 				p_params->min_d = d;
 				p_params->result = res;
-				p_params->normal = Plane(vertices[0], vertices[1], vertices[2]).normal;
+				p_params->normal = normal;
 				p_params->collisions++;
 			}
 		}
-
 	} else {
 		if (bvh->left >= 0) {
 			_cull_segment(bvh->left, p_params);
@@ -1329,17 +1327,20 @@ bool ConcavePolygonShape3DSW::intersect_segment(const Vector3 &p_begin, const Ve
 	const Vector3 *vr = vertices.ptr();
 	const BVH *br = bvh.ptr();
 
+	FaceShape3DSW face;
+	face.backface_collision = backface_collision;
+
 	_SegmentCullParams params;
 	params.from = p_begin;
 	params.to = p_end;
-	params.collisions = 0;
 	params.dir = (p_end - p_begin).normalized();
 
 	params.faces = fr;
 	params.vertices = vr;
 	params.bvh = br;
 
-	params.min_d = 1e20;
+	params.face = &face;
+
 	// cull
 	_cull_segment(0, &params);
 
@@ -1401,6 +1402,7 @@ void ConcavePolygonShape3DSW::cull(const AABB &p_local_aabb, Callback p_callback
 	const BVH *br = bvh.ptr();
 
 	FaceShape3DSW face; // use this to send in the callback
+	face.backface_collision = backface_collision;
 
 	_CullParams params;
 	params.aabb = local_aabb;
@@ -1532,7 +1534,7 @@ void ConcavePolygonShape3DSW::_fill_bvh(_VolumeSW_BVH *p_bvh_tree, BVH *p_bvh_ar
 	memdelete(p_bvh_tree);
 }
 
-void ConcavePolygonShape3DSW::_setup(Vector<Vector3> p_faces) {
+void ConcavePolygonShape3DSW::_setup(const Vector<Vector3> &p_faces, bool p_backface_collision) {
 	int src_face_count = p_faces.size();
 	if (src_face_count == 0) {
 		configure(AABB());
@@ -1587,15 +1589,24 @@ void ConcavePolygonShape3DSW::_setup(Vector<Vector3> p_faces) {
 	int idx = 0;
 	_fill_bvh(bvh_tree, bvh_arrayw2, idx);
 
+	backface_collision = p_backface_collision;
+
 	configure(_aabb); // this type of shape has no margin
 }
 
 void ConcavePolygonShape3DSW::set_data(const Variant &p_data) {
-	_setup(p_data);
+	Dictionary d = p_data;
+	ERR_FAIL_COND(!d.has("faces"));
+
+	_setup(d["faces"], d["backface_collision"]);
 }
 
 Variant ConcavePolygonShape3DSW::get_data() const {
-	return get_faces();
+	Dictionary d;
+	d["faces"] = get_faces();
+	d["backface_collision"] = backface_collision;
+
+	return d;
 }
 
 ConcavePolygonShape3DSW::ConcavePolygonShape3DSW() {
diff --git a/servers/physics_3d/shape_3d_sw.h b/servers/physics_3d/shape_3d_sw.h
index cafe978abb..988e76c699 100644
--- a/servers/physics_3d/shape_3d_sw.h
+++ b/servers/physics_3d/shape_3d_sw.h
@@ -334,34 +334,37 @@ struct ConcavePolygonShape3DSW : public ConcaveShape3DSW {
 
 	struct _CullParams {
 		AABB aabb;
-		Callback callback;
-		void *userdata;
-		const Face *faces;
-		const Vector3 *vertices;
-		const BVH *bvh;
-		FaceShape3DSW *face;
+		Callback callback = nullptr;
+		void *userdata = nullptr;
+		const Face *faces = nullptr;
+		const Vector3 *vertices = nullptr;
+		const BVH *bvh = nullptr;
+		FaceShape3DSW *face = nullptr;
 	};
 
 	struct _SegmentCullParams {
 		Vector3 from;
 		Vector3 to;
-		const Face *faces;
-		const Vector3 *vertices;
-		const BVH *bvh;
 		Vector3 dir;
+		const Face *faces = nullptr;
+		const Vector3 *vertices = nullptr;
+		const BVH *bvh = nullptr;
+		FaceShape3DSW *face = nullptr;
 
 		Vector3 result;
 		Vector3 normal;
-		real_t min_d;
-		int collisions;
+		real_t min_d = 1e20;
+		int collisions = 0;
 	};
 
+	bool backface_collision = false;
+
 	void _cull_segment(int p_idx, _SegmentCullParams *p_params) const;
 	void _cull(int p_idx, _CullParams *p_params) const;
 
 	void _fill_bvh(_VolumeSW_BVH *p_bvh_tree, BVH *p_bvh_array, int &p_idx);
 
-	void _setup(Vector<Vector3> p_faces);
+	void _setup(const Vector<Vector3> &p_faces, bool p_backface_collision);
 
 public:
 	Vector<Vector3> get_faces() const;
@@ -424,6 +427,7 @@ public:
 struct FaceShape3DSW : public Shape3DSW {
 	Vector3 normal; //cache
 	Vector3 vertex[3];
+	bool backface_collision = false;
 
 	virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_CONCAVE_POLYGON; }