6 files changed, 103 insertions, 27 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);
 
diff --git a/servers/physics_3d/godot_soft_body_3d.cpp b/servers/physics_3d/godot_soft_body_3d.cpp
index 173843072a..77110c5fbc 100644
--- a/servers/physics_3d/godot_soft_body_3d.cpp
+++ b/servers/physics_3d/godot_soft_body_3d.cpp
@@ -722,7 +722,14 @@ void GodotSoftBody3D::reoptimize_link_order() {
 	const int reop_not_dependent = -1;
 	const int reop_node_complete = -2;
 
-	uint32_t i, link_count = links.size(), node_count = nodes.size();
+	uint32_t link_count = links.size();
+	uint32_t node_count = nodes.size();
+
+	if (link_count < 1 || node_count < 2) {
+		return;
+	}
+
+	uint32_t i;
 	Link *lr;
 	int ar, br;
 	Node *node0 = &(nodes[0]);
diff --git a/servers/physics_3d/godot_space_3d.cpp b/servers/physics_3d/godot_space_3d.cpp
index c23485279d..c98409e2c4 100644
--- a/servers/physics_3d/godot_space_3d.cpp
+++ b/servers/physics_3d/godot_space_3d.cpp
@@ -1258,7 +1258,7 @@ GodotSpace3D::GodotSpace3D() {
 	ProjectSettings::get_singleton()->set_custom_property_info("physics/3d/solver/solver_iterations", PropertyInfo(Variant::INT, "physics/3d/solver/solver_iterations", PROPERTY_HINT_RANGE, "1,32,1,or_greater"));
 
 	contact_recycle_radius = GLOBAL_DEF("physics/3d/solver/contact_recycle_radius", 0.01);
-	ProjectSettings::get_singleton()->set_custom_property_info("physics/3d/solver/contact_recycle_radius", PropertyInfo(Variant::FLOAT, "physics/3d/solver/contact_max_separation", PROPERTY_HINT_RANGE, "0,0.1,0.01,or_greater"));
+	ProjectSettings::get_singleton()->set_custom_property_info("physics/3d/solver/contact_recycle_radius", PropertyInfo(Variant::FLOAT, "physics/3d/solver/contact_recycle_radius", PROPERTY_HINT_RANGE, "0,0.1,0.01,or_greater"));
 
 	contact_max_separation = GLOBAL_DEF("physics/3d/solver/contact_max_separation", 0.05);
 	ProjectSettings::get_singleton()->set_custom_property_info("physics/3d/solver/contact_max_separation", PropertyInfo(Variant::FLOAT, "physics/3d/solver/contact_max_separation", PROPERTY_HINT_RANGE, "0,0.1,0.01,or_greater"));
diff --git a/servers/physics_3d/joints/godot_generic_6dof_joint_3d.cpp b/servers/physics_3d/joints/godot_generic_6dof_joint_3d.cpp
index e0fa940104..dfe2bfa4d3 100644
--- a/servers/physics_3d/joints/godot_generic_6dof_joint_3d.cpp
+++ b/servers/physics_3d/joints/godot_generic_6dof_joint_3d.cpp
@@ -232,7 +232,7 @@ GodotGeneric6DOFJoint3D::GodotGeneric6DOFJoint3D(GodotBody3D *rbA, GodotBody3D *
 void GodotGeneric6DOFJoint3D::calculateAngleInfo() {
 	Basis relative_frame = m_calculatedTransformB.basis.inverse() * m_calculatedTransformA.basis;
 
-	m_calculatedAxisAngleDiff = relative_frame.get_euler(Basis::EULER_ORDER_XYZ);
+	m_calculatedAxisAngleDiff = relative_frame.get_euler(EulerOrder::XYZ);
 
 	// in euler angle mode we do not actually constrain the angular velocity
 	// along the axes axis[0] and axis[2] (although we do use axis[1]) :