3 files changed, 93 insertions, 24 deletions

diff --git a/servers/physics_3d/godot_body_3d.cpp b/servers/physics_3d/godot_body_3d.cpp
index cec233d95b..53f4ab86f9 100644
--- a/servers/physics_3d/godot_body_3d.cpp
+++ b/servers/physics_3d/godot_body_3d.cpp
@@ -118,7 +118,7 @@ void GodotBody3D::update_mass_properties() {
 					shape_inertia_tensor = shape_basis * shape_inertia_tensor * shape_basis.transposed();
 
 					Vector3 shape_origin = shape_transform.origin - center_of_mass_local;
-					inertia_tensor += shape_inertia_tensor + (Basis() * shape_origin.dot(shape_origin) - shape_origin.outer(shape_origin)) * mass;
+					inertia_tensor += shape_inertia_tensor + (Basis() * shape_origin.dot(shape_origin) - shape_origin.outer(shape_origin)) * mass_new;
 				}
 
 				// Set the inertia to a valid value when there are no valid shapes.
diff --git a/servers/physics_3d/godot_shape_3d.cpp b/servers/physics_3d/godot_shape_3d.cpp
index cd61ceab62..1443cd166b 100644
--- a/servers/physics_3d/godot_shape_3d.cpp
+++ b/servers/physics_3d/godot_shape_3d.cpp
@@ -817,48 +817,78 @@ GodotCylinderShape3D::GodotCylinderShape3D() {}
 /********** CONVEX POLYGON *************/
 
 void GodotConvexPolygonShape3D::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const {
-	int vertex_count = mesh.vertices.size();
+	uint32_t vertex_count = mesh.vertices.size();
 	if (vertex_count == 0) {
 		return;
 	}
 
 	const Vector3 *vrts = &mesh.vertices[0];
 
-	for (int i = 0; i < vertex_count; i++) {
-		real_t d = p_normal.dot(p_transform.xform(vrts[i]));
+	if (vertex_count > 3 * extreme_vertices.size()) {
+		// For a large mesh, two calls to get_support() is faster than a full
+		// scan over all vertices.
 
-		if (i == 0 || d > r_max) {
-			r_max = d;
-		}
-		if (i == 0 || d < r_min) {
-			r_min = d;
+		Vector3 n = p_transform.basis.xform_inv(p_normal).normalized();
+		r_min = p_normal.dot(p_transform.xform(get_support(-n)));
+		r_max = p_normal.dot(p_transform.xform(get_support(n)));
+	} else {
+		for (uint32_t i = 0; i < vertex_count; i++) {
+			real_t d = p_normal.dot(p_transform.xform(vrts[i]));
+
+			if (i == 0 || d > r_max) {
+				r_max = d;
+			}
+			if (i == 0 || d < r_min) {
+				r_min = d;
+			}
 		}
 	}
 }
 
 Vector3 GodotConvexPolygonShape3D::get_support(const Vector3 &p_normal) const {
-	Vector3 n = p_normal;
-
-	int vert_support_idx = -1;
-	real_t support_max = 0;
-
-	int vertex_count = mesh.vertices.size();
-	if (vertex_count == 0) {
+	if (mesh.vertices.size() == 0) {
 		return Vector3();
 	}
 
-	const Vector3 *vrts = &mesh.vertices[0];
-
-	for (int i = 0; i < vertex_count; i++) {
-		real_t d = n.dot(vrts[i]);
+	// Find an initial guess for the support vertex by checking the ones we
+	// found in _setup().
 
-		if (i == 0 || d > support_max) {
-			support_max = d;
-			vert_support_idx = i;
+	int best_vertex = -1;
+	real_t max_support = 0.0;
+	for (uint32_t i = 0; i < extreme_vertices.size(); i++) {
+		real_t s = p_normal.dot(mesh.vertices[extreme_vertices[i]]);
+		if (best_vertex == -1 || s > max_support) {
+			best_vertex = extreme_vertices[i];
+			max_support = s;
 		}
 	}
+	if (extreme_vertices.size() == mesh.vertices.size()) {
+		// We've already checked every vertex, so we can return now.
+		return mesh.vertices[best_vertex];
+	}
+
+	// Move along the surface until we reach the true support vertex.
 
-	return vrts[vert_support_idx];
+	int last_vertex = -1;
+	while (true) {
+		int next_vertex = -1;
+		for (uint32_t i = 0; i < vertex_neighbors[best_vertex].size(); i++) {
+			int vert = vertex_neighbors[best_vertex][i];
+			if (vert != last_vertex) {
+				real_t s = p_normal.dot(mesh.vertices[vert]);
+				if (s > max_support) {
+					next_vertex = vert;
+					max_support = s;
+					break;
+				}
+			}
+		}
+		if (next_vertex == -1) {
+			return mesh.vertices[best_vertex];
+		}
+		last_vertex = best_vertex;
+		best_vertex = next_vertex;
+	}
 }
 
 void GodotConvexPolygonShape3D::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const {
@@ -1067,6 +1097,43 @@ void GodotConvexPolygonShape3D::_setup(const Vector<Vector3> &p_vertices) {
 	}
 
 	configure(_aabb);
+
+	// Pre-compute the extreme vertices in 26 directions.  This will be used
+	// to speed up get_support() by letting us quickly get a good guess for
+	// the support vertex.
+
+	for (int x = -1; x < 2; x++) {
+		for (int y = -1; y < 2; y++) {
+			for (int z = -1; z < 2; z++) {
+				if (x != 0 || y != 0 || z != 0) {
+					Vector3 dir(x, y, z);
+					dir.normalize();
+					real_t max_support = 0.0;
+					int best_vertex = -1;
+					for (uint32_t i = 0; i < mesh.vertices.size(); i++) {
+						real_t s = dir.dot(mesh.vertices[i]);
+						if (best_vertex == -1 || s > max_support) {
+							best_vertex = i;
+							max_support = s;
+						}
+					}
+					if (extreme_vertices.find(best_vertex) == -1)
+						extreme_vertices.push_back(best_vertex);
+				}
+			}
+		}
+	}
+
+	// Record all the neighbors of each vertex.  This is used in get_support().
+
+	if (extreme_vertices.size() < mesh.vertices.size()) {
+		vertex_neighbors.resize(mesh.vertices.size());
+		for (uint32_t i = 0; i < mesh.edges.size(); i++) {
+			Geometry3D::MeshData::Edge &edge = mesh.edges[i];
+			vertex_neighbors[edge.vertex_a].push_back(edge.vertex_b);
+			vertex_neighbors[edge.vertex_b].push_back(edge.vertex_a);
+		}
+	}
 }
 
 void GodotConvexPolygonShape3D::set_data(const Variant &p_data) {
diff --git a/servers/physics_3d/godot_shape_3d.h b/servers/physics_3d/godot_shape_3d.h
index 1fc8f7c711..dc8e34e2bc 100644
--- a/servers/physics_3d/godot_shape_3d.h
+++ b/servers/physics_3d/godot_shape_3d.h
@@ -277,6 +277,8 @@ public:
 
 struct GodotConvexPolygonShape3D : public GodotShape3D {
 	Geometry3D::MeshData mesh;
+	LocalVector<int> extreme_vertices;
+	LocalVector<LocalVector<int>> vertex_neighbors;
 
 	void _setup(const Vector<Vector3> &p_vertices);