Optimize Convex Collision

Implements the Gauss Mapping optimization to SAT convex collision test.

* Described [here](https://ubm-twvideo01.s3.amazonaws.com/o1/vault/gdc2013/slides/822403Gregorius_Dirk_TheSeparatingAxisTest.pdf) by Dirk Gregorius.
* Requires adding of face information to edges in MeshData
* Took the chance to convert MeshData to LocalVector for performance.

2 files changed, 54 insertions, 28 deletions

diff --git a/servers/physics_3d/godot_collision_solver_3d_sat.cpp b/servers/physics_3d/godot_collision_solver_3d_sat.cpp
index 933a5e28df..96253cb452 100644
--- a/servers/physics_3d/godot_collision_solver_3d_sat.cpp
+++ b/servers/physics_3d/godot_collision_solver_3d_sat.cpp
@@ -964,8 +964,8 @@ static void _collision_sphere_convex_polygon(const GodotShape3D *p_a, const Tran
 
 	// edges of B
 	for (int i = 0; i < edge_count; i++) {
-		Vector3 v1 = p_transform_b.xform(vertices[edges[i].a]);
-		Vector3 v2 = p_transform_b.xform(vertices[edges[i].b]);
+		Vector3 v1 = p_transform_b.xform(vertices[edges[i].vertex_a]);
+		Vector3 v2 = p_transform_b.xform(vertices[edges[i].vertex_b]);
 		Vector3 v3 = p_transform_a.origin;
 
 		Vector3 n1 = v2 - v1;
@@ -1404,7 +1404,7 @@ static void _collision_box_convex_polygon(const GodotShape3D *p_a, const Transfo
 		Vector3 e1 = p_transform_a.basis.get_column(i);
 
 		for (int j = 0; j < edge_count; j++) {
-			Vector3 e2 = p_transform_b.basis.xform(vertices[edges[j].a]) - p_transform_b.basis.xform(vertices[edges[j].b]);
+			Vector3 e2 = p_transform_b.basis.xform(vertices[edges[j].vertex_a]) - p_transform_b.basis.xform(vertices[edges[j].vertex_b]);
 
 			Vector3 axis = e1.cross(e2).normalized();
 
@@ -1460,8 +1460,8 @@ static void _collision_box_convex_polygon(const GodotShape3D *p_a, const Transfo
 					}
 
 					for (int e = 0; e < edge_count; e++) {
-						Vector3 p1 = p_transform_b.xform(vertices[edges[e].a]);
-						Vector3 p2 = p_transform_b.xform(vertices[edges[e].b]);
+						Vector3 p1 = p_transform_b.xform(vertices[edges[e].vertex_a]);
+						Vector3 p2 = p_transform_b.xform(vertices[edges[e].vertex_b]);
 						Vector3 n = (p2 - p1);
 
 						if (!separator.test_axis((point - p2).cross(n).cross(n).normalized())) {
@@ -1771,7 +1771,7 @@ static void _collision_capsule_convex_polygon(const GodotShape3D *p_a, const Tra
 
 	for (int i = 0; i < edge_count; i++) {
 		// cylinder
-		Vector3 edge_axis = p_transform_b.basis.xform(vertices[edges[i].a]) - p_transform_b.basis.xform(vertices[edges[i].b]);
+		Vector3 edge_axis = p_transform_b.basis.xform(vertices[edges[i].vertex_a]) - p_transform_b.basis.xform(vertices[edges[i].vertex_b]);
 		Vector3 axis = edge_axis.cross(p_transform_a.basis.get_column(1)).normalized();
 
 		if (!separator.test_axis(axis)) {
@@ -1789,8 +1789,8 @@ static void _collision_capsule_convex_polygon(const GodotShape3D *p_a, const Tra
 		Vector3 sphere_pos = p_transform_a.origin + ((i == 0) ? capsule_axis : -capsule_axis);
 
 		for (int j = 0; j < edge_count; j++) {
-			Vector3 n1 = sphere_pos - p_transform_b.xform(vertices[edges[j].a]);
-			Vector3 n2 = p_transform_b.basis.xform(vertices[edges[j].a]) - p_transform_b.basis.xform(vertices[edges[j].b]);
+			Vector3 n1 = sphere_pos - p_transform_b.xform(vertices[edges[j].vertex_a]);
+			Vector3 n2 = p_transform_b.basis.xform(vertices[edges[j].vertex_a]) - p_transform_b.basis.xform(vertices[edges[j].vertex_b]);
 
 			Vector3 axis = n1.cross(n2).cross(n2).normalized();
 
@@ -2075,6 +2075,16 @@ static void _collision_cylinder_face(const GodotShape3D *p_a, const Transform3D
 	separator.generate_contacts();
 }
 
+static _FORCE_INLINE_ bool is_minkowski_face(const Vector3 &A, const Vector3 &B, const Vector3 &B_x_A, const Vector3 &C, const Vector3 &D, const Vector3 &D_x_C) {
+	// Test if arcs AB and CD intersect on the unit sphere
+	real_t CBA = C.dot(B_x_A);
+	real_t DBA = D.dot(B_x_A);
+	real_t ADC = A.dot(D_x_C);
+	real_t BDC = B.dot(D_x_C);
+
+	return (CBA * DBA < 0.0f) && (ADC * BDC < 0.0f) && (CBA * BDC > 0.0f);
+}
+
 template <bool withMargin>
 static void _collision_convex_polygon_convex_polygon(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
 	const GodotConvexPolygonShape3D *convex_polygon_A = static_cast<const GodotConvexPolygonShape3D *>(p_a);
@@ -2129,16 +2139,27 @@ static void _collision_convex_polygon_convex_polygon(const GodotShape3D *p_a, co
 	}
 
 	// A<->B edges
+
 	for (int i = 0; i < edge_count_A; i++) {
-		Vector3 e1 = p_transform_a.basis.xform(vertices_A[edges_A[i].a]) - p_transform_a.basis.xform(vertices_A[edges_A[i].b]);
+		Vector3 p1 = p_transform_a.xform(vertices_A[edges_A[i].vertex_a]);
+		Vector3 q1 = p_transform_a.xform(vertices_A[edges_A[i].vertex_b]);
+		Vector3 e1 = q1 - p1;
+		Vector3 u1 = p_transform_a.basis.xform(faces_A[edges_A[i].face_a].plane.normal).normalized();
+		Vector3 v1 = p_transform_a.basis.xform(faces_A[edges_A[i].face_b].plane.normal).normalized();
 
 		for (int j = 0; j < edge_count_B; j++) {
-			Vector3 e2 = p_transform_b.basis.xform(vertices_B[edges_B[j].a]) - p_transform_b.basis.xform(vertices_B[edges_B[j].b]);
+			Vector3 p2 = p_transform_b.xform(vertices_B[edges_B[j].vertex_a]);
+			Vector3 q2 = p_transform_b.xform(vertices_B[edges_B[j].vertex_b]);
+			Vector3 e2 = q2 - p2;
+			Vector3 u2 = p_transform_b.basis.xform(faces_B[edges_B[j].face_a].plane.normal).normalized();
+			Vector3 v2 = p_transform_b.basis.xform(faces_B[edges_B[j].face_b].plane.normal).normalized();
 
-			Vector3 axis = e1.cross(e2).normalized();
+			if (is_minkowski_face(u1, v1, -e1, -u2, -v2, -e2)) {
+				Vector3 axis = e1.cross(e2).normalized();
 
-			if (!separator.test_axis(axis)) {
-				return;
+				if (!separator.test_axis(axis)) {
+					return;
+				}
 			}
 		}
 	}
@@ -2157,8 +2178,8 @@ static void _collision_convex_polygon_convex_polygon(const GodotShape3D *p_a, co
 		//edge-vertex (shell)
 
 		for (int i = 0; i < edge_count_A; i++) {
-			Vector3 e1 = p_transform_a.basis.xform(vertices_A[edges_A[i].a]);
-			Vector3 e2 = p_transform_a.basis.xform(vertices_A[edges_A[i].b]);
+			Vector3 e1 = p_transform_a.basis.xform(vertices_A[edges_A[i].vertex_a]);
+			Vector3 e2 = p_transform_a.basis.xform(vertices_A[edges_A[i].vertex_b]);
 			Vector3 n = (e2 - e1);
 
 			for (int j = 0; j < vertex_count_B; j++) {
@@ -2171,8 +2192,8 @@ static void _collision_convex_polygon_convex_polygon(const GodotShape3D *p_a, co
 		}
 
 		for (int i = 0; i < edge_count_B; i++) {
-			Vector3 e1 = p_transform_b.basis.xform(vertices_B[edges_B[i].a]);
-			Vector3 e2 = p_transform_b.basis.xform(vertices_B[edges_B[i].b]);
+			Vector3 e1 = p_transform_b.basis.xform(vertices_B[edges_B[i].vertex_a]);
+			Vector3 e2 = p_transform_b.basis.xform(vertices_B[edges_B[i].vertex_b]);
 			Vector3 n = (e2 - e1);
 
 			for (int j = 0; j < vertex_count_A; j++) {
@@ -2231,7 +2252,7 @@ static void _collision_convex_polygon_face(const GodotShape3D *p_a, const Transf
 
 	// A<->B edges
 	for (int i = 0; i < edge_count; i++) {
-		Vector3 e1 = p_transform_a.xform(vertices[edges[i].a]) - p_transform_a.xform(vertices[edges[i].b]);
+		Vector3 e1 = p_transform_a.xform(vertices[edges[i].vertex_a]) - p_transform_a.xform(vertices[edges[i].vertex_b]);
 
 		for (int j = 0; j < 3; j++) {
 			Vector3 e2 = vertex[j] - vertex[(j + 1) % 3];
@@ -2266,8 +2287,8 @@ static void _collision_convex_polygon_face(const GodotShape3D *p_a, const Transf
 		//edge-vertex (shell)
 
 		for (int i = 0; i < edge_count; i++) {
-			Vector3 e1 = p_transform_a.basis.xform(vertices[edges[i].a]);
-			Vector3 e2 = p_transform_a.basis.xform(vertices[edges[i].b]);
+			Vector3 e1 = p_transform_a.basis.xform(vertices[edges[i].vertex_a]);
+			Vector3 e2 = p_transform_a.basis.xform(vertices[edges[i].vertex_b]);
 			Vector3 n = (e2 - e1);
 
 			for (int j = 0; j < 3; j++) {
diff --git a/servers/physics_3d/godot_shape_3d.cpp b/servers/physics_3d/godot_shape_3d.cpp
index e051c688fa..cd61ceab62 100644
--- a/servers/physics_3d/godot_shape_3d.cpp
+++ b/servers/physics_3d/godot_shape_3d.cpp
@@ -915,13 +915,13 @@ void GodotConvexPolygonShape3D::get_supports(const Vector3 &p_normal, int p_max,
 	}
 
 	for (int i = 0; i < ec; i++) {
-		real_t dot = (vertices[edges[i].a] - vertices[edges[i].b]).normalized().dot(p_normal);
+		real_t dot = (vertices[edges[i].vertex_a] - vertices[edges[i].vertex_b]).normalized().dot(p_normal);
 		dot = ABS(dot);
-		if (dot < edge_support_threshold && (edges[i].a == vtx || edges[i].b == vtx)) {
+		if (dot < edge_support_threshold && (edges[i].vertex_a == vtx || edges[i].vertex_b == vtx)) {
 			r_amount = 2;
 			r_type = FEATURE_EDGE;
-			r_supports[0] = vertices[edges[i].a];
-			r_supports[1] = vertices[edges[i].b];
+			r_supports[0] = vertices[edges[i].vertex_a];
+			r_supports[1] = vertices[edges[i].vertex_b];
 			return;
 		}
 	}
@@ -1025,8 +1025,8 @@ Vector3 GodotConvexPolygonShape3D::get_closest_point_to(const Vector3 &p_point)
 	int ec = mesh.edges.size();
 	for (int i = 0; i < ec; i++) {
 		Vector3 s[2] = {
-			vertices[edges[i].a],
-			vertices[edges[i].b]
+			vertices[edges[i].vertex_a],
+			vertices[edges[i].vertex_b]
 		};
 
 		Vector3 closest = Geometry3D::get_closest_point_to_segment(p_point, s);
@@ -1058,7 +1058,7 @@ void GodotConvexPolygonShape3D::_setup(const Vector<Vector3> &p_vertices) {
 
 	AABB _aabb;
 
-	for (int i = 0; i < mesh.vertices.size(); i++) {
+	for (uint32_t i = 0; i < mesh.vertices.size(); i++) {
 		if (i == 0) {
 			_aabb.position = mesh.vertices[i];
 		} else {
@@ -1074,7 +1074,12 @@ void GodotConvexPolygonShape3D::set_data(const Variant &p_data) {
 }
 
 Variant GodotConvexPolygonShape3D::get_data() const {
-	return mesh.vertices;
+	Vector<Vector3> vertices;
+	vertices.resize(mesh.vertices.size());
+	for (uint32_t i = 0; i < mesh.vertices.size(); i++) {
+		vertices.write[i] = mesh.vertices[i];
+	}
+	return vertices;
 }
 
 GodotConvexPolygonShape3D::GodotConvexPolygonShape3D() {