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.

5 files changed, 102 insertions, 39 deletions

diff --git a/core/math/convex_hull.cpp b/core/math/convex_hull.cpp
index 996f4f4d67..561970d2ee 100644
--- a/core/math/convex_hull.cpp
+++ b/core/math/convex_hull.cpp
@@ -62,6 +62,7 @@ subject to the following restrictions:
 #include "core/math/aabb.h"
 #include "core/math/math_defs.h"
 #include "core/os/memory.h"
+#include "core/templates/oa_hash_map.h"
 #include "core/templates/paged_allocator.h"
 
 #include <string.h>
@@ -2252,19 +2253,62 @@ Error ConvexHullComputer::convex_hull(const Vector<Vector3> &p_points, Geometry3
 
 	r_mesh.vertices = ch.vertices;
 
+	// Tag which face each edge belongs to
+	LocalVector<int32_t> edge_faces;
+	edge_faces.resize(ch.edges.size());
+
+	for (uint32_t i = 0; i < ch.edges.size(); i++) {
+		edge_faces[i] = -1;
+	}
+
+	for (uint32_t i = 0; i < ch.faces.size(); i++) {
+		const Edge *e_start = &ch.edges[ch.faces[i]];
+		const Edge *e = e_start;
+		do {
+			int64_t ofs = e - ch.edges.ptr();
+			edge_faces[ofs] = i;
+
+			e = e->get_next_edge_of_face();
+		} while (e != e_start);
+	}
+
 	// Copy the edges over. There's two "half-edges" for every edge, so we pick only one of them.
 	r_mesh.edges.resize(ch.edges.size() / 2);
+	OAHashMap<uint64_t, int32_t> edge_map;
+	edge_map.reserve(ch.edges.size() * 4); // The higher the capacity, the faster the insert
+
 	uint32_t edges_copied = 0;
 	for (uint32_t i = 0; i < ch.edges.size(); i++) {
+		ERR_CONTINUE(edge_faces[i] == -1); // Sanity check
+
 		uint32_t a = (&ch.edges[i])->get_source_vertex();
 		uint32_t b = (&ch.edges[i])->get_target_vertex();
 		if (a < b) { // Copy only the "canonical" edge. For the reverse edge, this will be false.
 			ERR_BREAK(edges_copied >= (uint32_t)r_mesh.edges.size());
-			r_mesh.edges.write[edges_copied].a = a;
-			r_mesh.edges.write[edges_copied].b = b;
+			r_mesh.edges[edges_copied].vertex_a = a;
+			r_mesh.edges[edges_copied].vertex_b = b;
+			r_mesh.edges[edges_copied].face_a = edge_faces[i];
+			r_mesh.edges[edges_copied].face_b = -1;
+
+			uint64_t key = a;
+			key <<= 32;
+			key |= b;
+			edge_map.insert(key, edges_copied);
+
 			edges_copied++;
+		} else {
+			uint64_t key = b;
+			key <<= 32;
+			key |= a;
+			int32_t index;
+			if (!edge_map.lookup(key, index)) {
+				ERR_PRINT("Invalid edge");
+			} else {
+				r_mesh.edges[index].face_b = edge_faces[i];
+			}
 		}
 	}
+
 	if (edges_copied != (uint32_t)r_mesh.edges.size()) {
 		ERR_PRINT("Invalid edge count.");
 	}
@@ -2273,7 +2317,7 @@ Error ConvexHullComputer::convex_hull(const Vector<Vector3> &p_points, Geometry3
 	for (uint32_t i = 0; i < ch.faces.size(); i++) {
 		const Edge *e_start = &ch.edges[ch.faces[i]];
 		const Edge *e = e_start;
-		Geometry3D::MeshData::Face &face = r_mesh.faces.write[i];
+		Geometry3D::MeshData::Face &face = r_mesh.faces[i];
 
 		do {
 			face.indices.push_back(e->get_target_vertex());
@@ -2284,8 +2328,8 @@ Error ConvexHullComputer::convex_hull(const Vector<Vector3> &p_points, Geometry3
 		// reverse indices: Godot wants clockwise, but this is counter-clockwise
 		if (face.indices.size() > 2) {
 			// reverse all but the first index.
-			int *indices = face.indices.ptrw();
-			for (int c = 0; c < (face.indices.size() - 1) / 2; c++) {
+			int *indices = face.indices.ptr();
+			for (uint32_t c = 0; c < (face.indices.size() - 1) / 2; c++) {
 				SWAP(indices[c + 1], indices[face.indices.size() - 1 - c]);
 			}
 		}
diff --git a/core/math/convex_hull.h b/core/math/convex_hull.h
index cc41a794bd..ab6671a7d0 100644
--- a/core/math/convex_hull.h
+++ b/core/math/convex_hull.h
@@ -62,6 +62,10 @@ public:
 		friend class ConvexHullComputer;
 
 	public:
+		int32_t get_next_relative() const {
+			return next;
+		}
+
 		int32_t get_source_vertex() const {
 			return (this + reverse)->target_vertex;
 		}
@@ -86,7 +90,7 @@ public:
 	};
 
 	// Vertices of the output hull
-	Vector<Vector3> vertices;
+	LocalVector<Vector3> vertices;
 
 	// Edges of the output hull
 	LocalVector<Edge> edges;
diff --git a/core/math/geometry_3d.cpp b/core/math/geometry_3d.cpp
index c5871358ed..548b9e4620 100644
--- a/core/math/geometry_3d.cpp
+++ b/core/math/geometry_3d.cpp
@@ -141,21 +141,21 @@ real_t Geometry3D::get_closest_distance_between_segments(const Vector3 &p_p0, co
 void Geometry3D::MeshData::optimize_vertices() {
 	HashMap<int, int> vtx_remap;
 
-	for (int i = 0; i < faces.size(); i++) {
-		for (int j = 0; j < faces[i].indices.size(); j++) {
+	for (uint32_t i = 0; i < faces.size(); i++) {
+		for (uint32_t j = 0; j < faces[i].indices.size(); j++) {
 			int idx = faces[i].indices[j];
 			if (!vtx_remap.has(idx)) {
 				int ni = vtx_remap.size();
 				vtx_remap[idx] = ni;
 			}
 
-			faces.write[i].indices.write[j] = vtx_remap[idx];
+			faces[i].indices[j] = vtx_remap[idx];
 		}
 	}
 
-	for (int i = 0; i < edges.size(); i++) {
-		int a = edges[i].a;
-		int b = edges[i].b;
+	for (uint32_t i = 0; i < edges.size(); i++) {
+		int a = edges[i].vertex_a;
+		int b = edges[i].vertex_b;
 
 		if (!vtx_remap.has(a)) {
 			int ni = vtx_remap.size();
@@ -166,16 +166,16 @@ void Geometry3D::MeshData::optimize_vertices() {
 			vtx_remap[b] = ni;
 		}
 
-		edges.write[i].a = vtx_remap[a];
-		edges.write[i].b = vtx_remap[b];
+		edges[i].vertex_a = vtx_remap[a];
+		edges[i].vertex_b = vtx_remap[b];
 	}
 
-	Vector<Vector3> new_vertices;
+	LocalVector<Vector3> new_vertices;
 	new_vertices.resize(vtx_remap.size());
 
-	for (int i = 0; i < vertices.size(); i++) {
+	for (uint32_t i = 0; i < vertices.size(); i++) {
 		if (vtx_remap.has(i)) {
-			new_vertices.write[vtx_remap[i]] = vertices[i];
+			new_vertices[vtx_remap[i]] = vertices[i];
 		}
 	}
 	vertices = new_vertices;
@@ -751,7 +751,7 @@ Geometry3D::MeshData Geometry3D::build_convex_mesh(const Vector<Plane> &p_planes
 		Vector3 center = p.center();
 
 		// make a quad clockwise
-		Vector<Vector3> vertices = {
+		LocalVector<Vector3> vertices = {
 			center - up * subplane_size + right * subplane_size,
 			center - up * subplane_size - right * subplane_size,
 			center + up * subplane_size - right * subplane_size,
@@ -763,7 +763,7 @@ Geometry3D::MeshData Geometry3D::build_convex_mesh(const Vector<Plane> &p_planes
 				continue;
 			}
 
-			Vector<Vector3> new_vertices;
+			LocalVector<Vector3> new_vertices;
 			Plane clip = p_planes[j];
 
 			if (clip.normal.dot(p.normal) > 0.95f) {
@@ -774,7 +774,7 @@ Geometry3D::MeshData Geometry3D::build_convex_mesh(const Vector<Plane> &p_planes
 				break;
 			}
 
-			for (int k = 0; k < vertices.size(); k++) {
+			for (uint32_t k = 0; k < vertices.size(); k++) {
 				int k_n = (k + 1) % vertices.size();
 
 				Vector3 edge0_A = vertices[k];
@@ -816,9 +816,9 @@ Geometry3D::MeshData Geometry3D::build_convex_mesh(const Vector<Plane> &p_planes
 		MeshData::Face face;
 
 		// Add face indices.
-		for (int j = 0; j < vertices.size(); j++) {
+		for (uint32_t j = 0; j < vertices.size(); j++) {
 			int idx = -1;
-			for (int k = 0; k < mesh.vertices.size(); k++) {
+			for (uint32_t k = 0; k < mesh.vertices.size(); k++) {
 				if (mesh.vertices[k].distance_to(vertices[j]) < 0.001f) {
 					idx = k;
 					break;
@@ -837,28 +837,34 @@ Geometry3D::MeshData Geometry3D::build_convex_mesh(const Vector<Plane> &p_planes
 
 		// Add edge.
 
-		for (int j = 0; j < face.indices.size(); j++) {
+		for (uint32_t j = 0; j < face.indices.size(); j++) {
 			int a = face.indices[j];
 			int b = face.indices[(j + 1) % face.indices.size()];
 
 			bool found = false;
-			for (int k = 0; k < mesh.edges.size(); k++) {
-				if (mesh.edges[k].a == a && mesh.edges[k].b == b) {
+			int found_idx = -1;
+			for (uint32_t k = 0; k < mesh.edges.size(); k++) {
+				if (mesh.edges[k].vertex_a == a && mesh.edges[k].vertex_b == b) {
 					found = true;
+					found_idx = k;
 					break;
 				}
-				if (mesh.edges[k].b == a && mesh.edges[k].a == b) {
+				if (mesh.edges[k].vertex_b == a && mesh.edges[k].vertex_a == b) {
 					found = true;
+					found_idx = k;
 					break;
 				}
 			}
 
 			if (found) {
+				mesh.edges[found_idx].face_b = j;
 				continue;
 			}
 			MeshData::Edge edge;
-			edge.a = a;
-			edge.b = b;
+			edge.vertex_a = a;
+			edge.vertex_b = b;
+			edge.face_a = j;
+			edge.face_b = -1;
 			mesh.edges.push_back(edge);
 		}
 	}
diff --git a/core/math/geometry_3d.h b/core/math/geometry_3d.h
index e5ace9db72..4b4c173a1e 100644
--- a/core/math/geometry_3d.h
+++ b/core/math/geometry_3d.h
@@ -33,6 +33,7 @@
 
 #include "core/math/face3.h"
 #include "core/object/object.h"
+#include "core/templates/local_vector.h"
 #include "core/templates/vector.h"
 
 class Geometry3D {
@@ -539,18 +540,19 @@ public:
 	struct MeshData {
 		struct Face {
 			Plane plane;
-			Vector<int> indices;
+			LocalVector<int> indices;
 		};
 
-		Vector<Face> faces;
+		LocalVector<Face> faces;
 
 		struct Edge {
-			int a, b;
+			int vertex_a, vertex_b;
+			int face_a, face_b;
 		};
 
-		Vector<Edge> edges;
+		LocalVector<Edge> edges;
 
-		Vector<Vector3> vertices;
+		LocalVector<Vector3> vertices;
 
 		void optimize_vertices();
 	};
diff --git a/core/math/quick_hull.cpp b/core/math/quick_hull.cpp
index c7727a44a1..c194e1cc21 100644
--- a/core/math/quick_hull.cpp
+++ b/core/math/quick_hull.cpp
@@ -369,7 +369,7 @@ Error QuickHull::build(const Vector<Vector3> &p_points, Geometry3D::MeshData &r_
 	for (List<Geometry3D::MeshData::Face>::Element *E = ret_faces.front(); E; E = E->next()) {
 		Geometry3D::MeshData::Face &f = E->get();
 
-		for (int i = 0; i < f.indices.size(); i++) {
+		for (uint32_t i = 0; i < f.indices.size(); i++) {
 			int a = E->get().indices[i];
 			int b = E->get().indices[(i + 1) % f.indices.size()];
 			Edge e(a, b);
@@ -436,17 +436,24 @@ Error QuickHull::build(const Vector<Vector3> &p_points, Geometry3D::MeshData &r_
 	r_mesh.faces.clear();
 	r_mesh.faces.resize(ret_faces.size());
 
+	HashMap<List<Geometry3D::MeshData::Face>::Element *, int> face_indices;
+
 	int idx = 0;
-	for (const Geometry3D::MeshData::Face &E : ret_faces) {
-		r_mesh.faces.write[idx++] = E;
+	for (List<Geometry3D::MeshData::Face>::Element *E = ret_faces.front(); E; E = E->next()) {
+		face_indices[E] = idx;
+		r_mesh.faces[idx++] = E->get();
 	}
 	r_mesh.edges.resize(ret_edges.size());
 	idx = 0;
 	for (const KeyValue<Edge, RetFaceConnect> &E : ret_edges) {
 		Geometry3D::MeshData::Edge e;
-		e.a = E.key.vertices[0];
-		e.b = E.key.vertices[1];
-		r_mesh.edges.write[idx++] = e;
+		e.vertex_a = E.key.vertices[0];
+		e.vertex_b = E.key.vertices[1];
+		ERR_CONTINUE(!face_indices.has(E.value.left));
+		ERR_CONTINUE(!face_indices.has(E.value.right));
+		e.face_a = face_indices[E.value.left];
+		e.face_b = face_indices[E.value.right];
+		r_mesh.edges[idx++] = e;
 	}
 
 	r_mesh.vertices = p_points;