15 files changed, 154 insertions, 132 deletions

diff --git a/servers/physics_3d/godot_area_3d.cpp b/servers/physics_3d/godot_area_3d.cpp
index 9765d0bf58..d4d3b3e6aa 100644
--- a/servers/physics_3d/godot_area_3d.cpp
+++ b/servers/physics_3d/godot_area_3d.cpp
@@ -333,12 +333,12 @@ void GodotArea3D::call_queries() {
 
 void GodotArea3D::compute_gravity(const Vector3 &p_position, Vector3 &r_gravity) const {
 	if (is_gravity_point()) {
-		const real_t gravity_distance_scale = get_gravity_distance_scale();
+		const real_t gr_distance_scale = get_gravity_distance_scale();
 		Vector3 v = get_transform().xform(get_gravity_vector()) - p_position;
-		if (gravity_distance_scale > 0) {
+		if (gr_distance_scale > 0) {
 			const real_t v_length = v.length();
 			if (v_length > 0) {
-				const real_t v_scaled = v_length * gravity_distance_scale;
+				const real_t v_scaled = v_length * gr_distance_scale;
 				r_gravity = (v.normalized() * (get_gravity() / (v_scaled * v_scaled)));
 			} else {
 				r_gravity = Vector3();
diff --git a/servers/physics_3d/godot_body_3d.cpp b/servers/physics_3d/godot_body_3d.cpp
index 19f065c319..cec233d95b 100644
--- a/servers/physics_3d/godot_body_3d.cpp
+++ b/servers/physics_3d/godot_body_3d.cpp
@@ -78,10 +78,10 @@ void GodotBody3D::update_mass_properties() {
 
 						real_t area = get_shape_area(i);
 
-						real_t mass = area * this->mass / total_area;
+						real_t mass_new = area * mass / total_area;
 
 						// NOTE: we assume that the shape origin is also its center of mass.
-						center_of_mass_local += mass * get_shape_transform(i).origin;
+						center_of_mass_local += mass_new * get_shape_transform(i).origin;
 					}
 
 					center_of_mass_local /= mass;
@@ -108,9 +108,9 @@ void GodotBody3D::update_mass_properties() {
 
 					const GodotShape3D *shape = get_shape(i);
 
-					real_t mass = area * this->mass / total_area;
+					real_t mass_new = area * mass / total_area;
 
-					Basis shape_inertia_tensor = Basis::from_scale(shape->get_moment_of_inertia(mass));
+					Basis shape_inertia_tensor = Basis::from_scale(shape->get_moment_of_inertia(mass_new));
 					Transform3D shape_transform = get_shape_transform(i);
 					Basis shape_basis = shape_transform.basis.orthonormalized();
 
@@ -637,14 +637,14 @@ void GodotBody3D::integrate_forces(real_t p_step) {
 				damp = 0;
 			}
 
-			real_t angular_damp = 1.0 - p_step * total_angular_damp;
+			real_t angular_damp_new = 1.0 - p_step * total_angular_damp;
 
-			if (angular_damp < 0) { // reached zero in the given time
-				angular_damp = 0;
+			if (angular_damp_new < 0) { // reached zero in the given time
+				angular_damp_new = 0;
 			}
 
 			linear_velocity *= damp;
-			angular_velocity *= angular_damp;
+			angular_velocity *= angular_damp_new;
 
 			linear_velocity += _inv_mass * force * p_step;
 			angular_velocity += _inv_inertia_tensor.xform(torque) * p_step;
@@ -707,27 +707,27 @@ void GodotBody3D::integrate_velocities(real_t p_step) {
 	Vector3 total_angular_velocity = angular_velocity + biased_angular_velocity;
 
 	real_t ang_vel = total_angular_velocity.length();
-	Transform3D transform = get_transform();
+	Transform3D transform_new = get_transform();
 
 	if (!Math::is_zero_approx(ang_vel)) {
 		Vector3 ang_vel_axis = total_angular_velocity / ang_vel;
 		Basis rot(ang_vel_axis, ang_vel * p_step);
 		Basis identity3(1, 0, 0, 0, 1, 0, 0, 0, 1);
-		transform.origin += ((identity3 - rot) * transform.basis).xform(center_of_mass_local);
-		transform.basis = rot * transform.basis;
-		transform.orthonormalize();
+		transform_new.origin += ((identity3 - rot) * transform_new.basis).xform(center_of_mass_local);
+		transform_new.basis = rot * transform_new.basis;
+		transform_new.orthonormalize();
 	}
 
 	Vector3 total_linear_velocity = linear_velocity + biased_linear_velocity;
 	/*for(int i=0;i<3;i++) {
 		if (axis_lock&(1<<i)) {
-			transform.origin[i]=0.0;
+			transform_new.origin[i]=0.0;
 		}
 	}*/
 
-	transform.origin += total_linear_velocity * p_step;
+	transform_new.origin += total_linear_velocity * p_step;
 
-	_set_transform(transform);
+	_set_transform(transform_new);
 	_set_inv_transform(get_transform().inverse());
 
 	_update_transform_dependent();
diff --git a/servers/physics_3d/godot_body_pair_3d.cpp b/servers/physics_3d/godot_body_pair_3d.cpp
index eebbe0196d..7e6cc6f834 100644
--- a/servers/physics_3d/godot_body_pair_3d.cpp
+++ b/servers/physics_3d/godot_body_pair_3d.cpp
@@ -170,7 +170,7 @@ bool GodotBodyPair3D::_test_ccd(real_t p_step, GodotBody3D *p_A, int p_shape_A,
 
 	Vector3 mnormal = motion / mlen;
 
-	real_t min, max;
+	real_t min = 0.0, max = 0.0;
 	p_A->get_shape(p_shape_A)->project_range(mnormal, p_xform_A, min, max);
 
 	// Did it move enough in this direction to even attempt raycast?
diff --git a/servers/physics_3d/godot_collision_solver_3d.cpp b/servers/physics_3d/godot_collision_solver_3d.cpp
index 9fe0e3eb84..ca76a819ec 100644
--- a/servers/physics_3d/godot_collision_solver_3d.cpp
+++ b/servers/physics_3d/godot_collision_solver_3d.cpp
@@ -48,7 +48,7 @@ bool GodotCollisionSolver3D::solve_static_world_boundary(const GodotShape3D *p_s
 	static const int max_supports = 16;
 	Vector3 supports[max_supports];
 	int support_count;
-	GodotShape3D::FeatureType support_type;
+	GodotShape3D::FeatureType support_type = GodotShape3D::FeatureType::FEATURE_POINT;
 	p_shape_B->get_supports(p_transform_B.basis.xform_inv(-p.normal).normalized(), max_supports, supports, support_count, support_type);
 
 	if (support_type == GodotShape3D::FEATURE_CIRCLE) {
@@ -338,7 +338,7 @@ bool GodotCollisionSolver3D::solve_concave(const GodotShape3D *p_shape_A, const
 		real_t axis_scale = 1.0 / axis.length();
 		axis *= axis_scale;
 
-		real_t smin, smax;
+		real_t smin = 0.0, smax = 0.0;
 		p_shape_A->project_range(axis, rel_transform, smin, smax);
 		smin -= p_margin_A;
 		smax += p_margin_A;
diff --git a/servers/physics_3d/godot_collision_solver_3d_sat.cpp b/servers/physics_3d/godot_collision_solver_3d_sat.cpp
index 56e644b57b..96253cb452 100644
--- a/servers/physics_3d/godot_collision_solver_3d_sat.cpp
+++ b/servers/physics_3d/godot_collision_solver_3d_sat.cpp
@@ -634,7 +634,7 @@ public:
 			axis = Vector3(0.0, 1.0, 0.0);
 		}
 
-		real_t min_A, max_A, min_B, max_B;
+		real_t min_A = 0.0, max_A = 0.0, min_B = 0.0, max_B = 0.0;
 
 		shape_A->project_range(axis, *transform_A, min_A, max_A);
 		shape_B->project_range(axis, *transform_B, min_B, max_B);
@@ -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_physics_server_3d.cpp b/servers/physics_3d/godot_physics_server_3d.cpp
index 7f2ec72921..68db5df144 100644
--- a/servers/physics_3d/godot_physics_server_3d.cpp
+++ b/servers/physics_3d/godot_physics_server_3d.cpp
@@ -387,6 +387,13 @@ void GodotPhysicsServer3D::area_set_collision_layer(RID p_area, uint32_t p_layer
 	area->set_collision_layer(p_layer);
 }
 
+uint32_t GodotPhysicsServer3D::area_get_collision_layer(RID p_area) const {
+	GodotArea3D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_COND_V(!area, 0);
+
+	return area->get_collision_layer();
+}
+
 void GodotPhysicsServer3D::area_set_collision_mask(RID p_area, uint32_t p_mask) {
 	GodotArea3D *area = area_owner.get_or_null(p_area);
 	ERR_FAIL_COND(!area);
@@ -394,6 +401,13 @@ void GodotPhysicsServer3D::area_set_collision_mask(RID p_area, uint32_t p_mask)
 	area->set_collision_mask(p_mask);
 }
 
+uint32_t GodotPhysicsServer3D::area_get_collision_mask(RID p_area) const {
+	GodotArea3D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_COND_V(!area, 0);
+
+	return area->get_collision_mask();
+}
+
 void GodotPhysicsServer3D::area_set_monitorable(RID p_area, bool p_monitorable) {
 	GodotArea3D *area = area_owner.get_or_null(p_area);
 	ERR_FAIL_COND(!area);
@@ -1196,6 +1210,7 @@ RID GodotPhysicsServer3D::joint_create() {
 
 void GodotPhysicsServer3D::joint_clear(RID p_joint) {
 	GodotJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL(joint);
 	if (joint->get_type() != JOINT_TYPE_MAX) {
 		GodotJoint3D *empty_joint = memnew(GodotJoint3D);
 		empty_joint->copy_settings_from(joint);
diff --git a/servers/physics_3d/godot_physics_server_3d.h b/servers/physics_3d/godot_physics_server_3d.h
index 6dc9330caf..e3e649da57 100644
--- a/servers/physics_3d/godot_physics_server_3d.h
+++ b/servers/physics_3d/godot_physics_server_3d.h
@@ -148,8 +148,11 @@ public:
 
 	virtual void area_set_ray_pickable(RID p_area, bool p_enable) override;
 
-	virtual void area_set_collision_mask(RID p_area, uint32_t p_mask) override;
 	virtual void area_set_collision_layer(RID p_area, uint32_t p_layer) override;
+	virtual uint32_t area_get_collision_layer(RID p_area) const override;
+
+	virtual void area_set_collision_mask(RID p_area, uint32_t p_mask) override;
+	virtual uint32_t area_get_collision_mask(RID p_area) const override;
 
 	virtual void area_set_monitorable(RID p_area, bool p_monitorable) override;
 
diff --git a/servers/physics_3d/godot_shape_3d.cpp b/servers/physics_3d/godot_shape_3d.cpp
index 5e310670a5..cd61ceab62 100644
--- a/servers/physics_3d/godot_shape_3d.cpp
+++ b/servers/physics_3d/godot_shape_3d.cpp
@@ -411,9 +411,9 @@ void GodotBoxShape3D::get_supports(const Vector3 &p_normal, int p_max, Vector3 *
 }
 
 bool GodotBoxShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, bool p_hit_back_faces) const {
-	AABB aabb(-half_extents, half_extents * 2.0);
+	AABB aabb_ext(-half_extents, half_extents * 2.0);
 
-	return aabb.intersects_segment(p_begin, p_end, &r_result, &r_normal);
+	return aabb_ext.intersects_segment(p_begin, p_end, &r_result, &r_normal);
 }
 
 bool GodotBoxShape3D::intersect_point(const Vector3 &p_point) const {
@@ -735,29 +735,6 @@ void GodotCylinderShape3D::get_supports(const Vector3 &p_normal, int p_max, Vect
 		r_amount = 1;
 		r_type = FEATURE_POINT;
 		r_supports[0] = get_support(p_normal);
-		return;
-
-		Vector3 n = p_normal;
-		real_t h = n.y * Math::sqrt(0.25 * height * height + radius * radius);
-		if (Math::abs(h) > 1.0) {
-			// Top or bottom surface.
-			n.y = (n.y > 0.0) ? height * 0.5 : -height * 0.5;
-		} else {
-			// Lateral surface.
-			n.y = height * 0.5 * h;
-		}
-
-		real_t s = Math::sqrt(n.x * n.x + n.z * n.z);
-		if (Math::is_zero_approx(s)) {
-			n.x = 0.0;
-			n.z = 0.0;
-		} else {
-			real_t scaled_radius = radius / s;
-			n.x = n.x * scaled_radius;
-			n.z = n.z * scaled_radius;
-		}
-
-		r_supports[0] = n;
 	}
 }
 
@@ -938,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;
 		}
 	}
@@ -1048,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);
@@ -1081,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 {
@@ -1097,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() {
@@ -1282,14 +1264,14 @@ Vector3 GodotConcavePolygonShape3D::get_support(const Vector3 &p_normal) const {
 }
 
 void GodotConcavePolygonShape3D::_cull_segment(int p_idx, _SegmentCullParams *p_params) const {
-	const BVH *bvh = &p_params->bvh[p_idx];
+	const BVH *params_bvh = &p_params->bvh[p_idx];
 
-	if (!bvh->aabb.intersects_segment(p_params->from, p_params->to)) {
+	if (!params_bvh->aabb.intersects_segment(p_params->from, p_params->to)) {
 		return;
 	}
 
-	if (bvh->face_index >= 0) {
-		const Face *f = &p_params->faces[bvh->face_index];
+	if (params_bvh->face_index >= 0) {
+		const Face *f = &p_params->faces[params_bvh->face_index];
 		GodotFaceShape3D *face = p_params->face;
 		face->normal = f->normal;
 		face->vertex[0] = p_params->vertices[f->indices[0]];
@@ -1308,11 +1290,11 @@ void GodotConcavePolygonShape3D::_cull_segment(int p_idx, _SegmentCullParams *p_
 			}
 		}
 	} else {
-		if (bvh->left >= 0) {
-			_cull_segment(bvh->left, p_params);
+		if (params_bvh->left >= 0) {
+			_cull_segment(params_bvh->left, p_params);
 		}
-		if (bvh->right >= 0) {
-			_cull_segment(bvh->right, p_params);
+		if (params_bvh->right >= 0) {
+			_cull_segment(params_bvh->right, p_params);
 		}
 	}
 }
@@ -1362,14 +1344,14 @@ Vector3 GodotConcavePolygonShape3D::get_closest_point_to(const Vector3 &p_point)
 }
 
 bool GodotConcavePolygonShape3D::_cull(int p_idx, _CullParams *p_params) const {
-	const BVH *bvh = &p_params->bvh[p_idx];
+	const BVH *params_bvh = &p_params->bvh[p_idx];
 
-	if (!p_params->aabb.intersects(bvh->aabb)) {
+	if (!p_params->aabb.intersects(params_bvh->aabb)) {
 		return false;
 	}
 
-	if (bvh->face_index >= 0) {
-		const Face *f = &p_params->faces[bvh->face_index];
+	if (params_bvh->face_index >= 0) {
+		const Face *f = &p_params->faces[params_bvh->face_index];
 		GodotFaceShape3D *face = p_params->face;
 		face->normal = f->normal;
 		face->vertex[0] = p_params->vertices[f->indices[0]];
@@ -1379,14 +1361,14 @@ bool GodotConcavePolygonShape3D::_cull(int p_idx, _CullParams *p_params) const {
 			return true;
 		}
 	} else {
-		if (bvh->left >= 0) {
-			if (_cull(bvh->left, p_params)) {
+		if (params_bvh->left >= 0) {
+			if (_cull(params_bvh->left, p_params)) {
 				return true;
 			}
 		}
 
-		if (bvh->right >= 0) {
-			if (_cull(bvh->right, p_params)) {
+		if (params_bvh->right >= 0) {
+			if (_cull(params_bvh->right, p_params)) {
 				return true;
 			}
 		}
@@ -1942,14 +1924,14 @@ Vector3 GodotHeightMapShape3D::get_closest_point_to(const Vector3 &p_point) cons
 }
 
 void GodotHeightMapShape3D::_get_cell(const Vector3 &p_point, int &r_x, int &r_y, int &r_z) const {
-	const AABB &aabb = get_aabb();
+	const AABB &shape_aabb = get_aabb();
 
-	Vector3 pos_local = aabb.position + local_origin;
+	Vector3 pos_local = shape_aabb.position + local_origin;
 
 	Vector3 clamped_point(p_point);
-	clamped_point.x = CLAMP(p_point.x, pos_local.x, pos_local.x + aabb.size.x);
-	clamped_point.y = CLAMP(p_point.y, pos_local.y, pos_local.y + aabb.size.y);
-	clamped_point.z = CLAMP(p_point.z, pos_local.z, pos_local.x + aabb.size.z);
+	clamped_point.x = CLAMP(p_point.x, pos_local.x, pos_local.x + shape_aabb.size.x);
+	clamped_point.y = CLAMP(p_point.y, pos_local.y, pos_local.y + shape_aabb.size.y);
+	clamped_point.z = CLAMP(p_point.z, pos_local.z, pos_local.x + shape_aabb.size.z);
 
 	r_x = (clamped_point.x < 0.0) ? (clamped_point.x - 0.5) : (clamped_point.x + 0.5);
 	r_y = (clamped_point.y < 0.0) ? (clamped_point.y - 0.5) : (clamped_point.y + 0.5);
@@ -2093,19 +2075,19 @@ void GodotHeightMapShape3D::_setup(const Vector<real_t> &p_heights, int p_width,
 	depth = p_depth;
 
 	// Initialize aabb.
-	AABB aabb;
-	aabb.position = Vector3(0.0, p_min_height, 0.0);
-	aabb.size = Vector3(p_width - 1, p_max_height - p_min_height, p_depth - 1);
+	AABB aabb_new;
+	aabb_new.position = Vector3(0.0, p_min_height, 0.0);
+	aabb_new.size = Vector3(p_width - 1, p_max_height - p_min_height, p_depth - 1);
 
 	// Initialize origin as the aabb center.
-	local_origin = aabb.position + 0.5 * aabb.size;
+	local_origin = aabb_new.position + 0.5 * aabb_new.size;
 	local_origin.y = 0.0;
 
-	aabb.position -= local_origin;
+	aabb_new.position -= local_origin;
 
 	_build_accelerator();
 
-	configure(aabb);
+	configure(aabb_new);
 }
 
 void GodotHeightMapShape3D::set_data(const Variant &p_data) {
@@ -2116,11 +2098,11 @@ void GodotHeightMapShape3D::set_data(const Variant &p_data) {
 	ERR_FAIL_COND(!d.has("depth"));
 	ERR_FAIL_COND(!d.has("heights"));
 
-	int width = d["width"];
-	int depth = d["depth"];
+	int width_new = d["width"];
+	int depth_new = d["depth"];
 
-	ERR_FAIL_COND(width <= 0.0);
-	ERR_FAIL_COND(depth <= 0.0);
+	ERR_FAIL_COND(width_new <= 0.0);
+	ERR_FAIL_COND(depth_new <= 0.0);
 
 	Variant heights_variant = d["heights"];
 	Vector<real_t> heights_buffer;
@@ -2174,10 +2156,10 @@ void GodotHeightMapShape3D::set_data(const Variant &p_data) {
 
 	ERR_FAIL_COND(min_height > max_height);
 
-	ERR_FAIL_COND(heights_buffer.size() != (width * depth));
+	ERR_FAIL_COND(heights_buffer.size() != (width_new * depth_new));
 
 	// If specified, min and max height will be used as precomputed values.
-	_setup(heights_buffer, width, depth, min_height, max_height);
+	_setup(heights_buffer, width_new, depth_new, min_height, max_height);
 }
 
 Variant GodotHeightMapShape3D::get_data() const {
@@ -2185,9 +2167,9 @@ Variant GodotHeightMapShape3D::get_data() const {
 	d["width"] = width;
 	d["depth"] = depth;
 
-	const AABB &aabb = get_aabb();
-	d["min_height"] = aabb.position.y;
-	d["max_height"] = aabb.position.y + aabb.size.y;
+	const AABB &shape_aabb = get_aabb();
+	d["min_height"] = shape_aabb.position.y;
+	d["max_height"] = shape_aabb.position.y + shape_aabb.size.y;
 
 	d["heights"] = heights;
 
diff --git a/servers/physics_3d/godot_space_3d.cpp b/servers/physics_3d/godot_space_3d.cpp
index 76d59202c9..c23485279d 100644
--- a/servers/physics_3d/godot_space_3d.cpp
+++ b/servers/physics_3d/godot_space_3d.cpp
@@ -120,8 +120,8 @@ bool GodotPhysicsDirectSpaceState3D::intersect_ray(const RayParameters &p_parame
 
 	bool collided = false;
 	Vector3 res_point, res_normal;
-	int res_shape;
-	const GodotCollisionObject3D *res_obj;
+	int res_shape = -1;
+	const GodotCollisionObject3D *res_obj = nullptr;
 	real_t min_d = 1e10;
 
 	for (int i = 0; i < amount; i++) {
@@ -185,6 +185,7 @@ bool GodotPhysicsDirectSpaceState3D::intersect_ray(const RayParameters &p_parame
 	if (!collided) {
 		return false;
 	}
+	ERR_FAIL_NULL_V(res_obj, false); // Shouldn't happen but silences warning.
 
 	r_result.collider_id = res_obj->get_instance_id();
 	if (r_result.collider_id.is_valid()) {
diff --git a/servers/physics_3d/joints/godot_cone_twist_joint_3d.h b/servers/physics_3d/joints/godot_cone_twist_joint_3d.h
index fdcc2ceea3..1651d34916 100644
--- a/servers/physics_3d/joints/godot_cone_twist_joint_3d.h
+++ b/servers/physics_3d/joints/godot_cone_twist_joint_3d.h
@@ -28,6 +28,9 @@
 /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
 /*************************************************************************/
 
+#ifndef GODOT_CONE_TWIST_JOINT_3D_H
+#define GODOT_CONE_TWIST_JOINT_3D_H
+
 /*
 Adapted to Godot from the Bullet library.
 */
@@ -49,9 +52,6 @@ subject to the following restrictions:
 Written by: Marcus Hennix
 */
 
-#ifndef GODOT_CONE_TWIST_JOINT_3D_H
-#define GODOT_CONE_TWIST_JOINT_3D_H
-
 #include "servers/physics_3d/godot_joint_3d.h"
 #include "servers/physics_3d/joints/godot_jacobian_entry_3d.h"
 
diff --git a/servers/physics_3d/joints/godot_generic_6dof_joint_3d.h b/servers/physics_3d/joints/godot_generic_6dof_joint_3d.h
index bcf2d18647..6a0ab81461 100644
--- a/servers/physics_3d/joints/godot_generic_6dof_joint_3d.h
+++ b/servers/physics_3d/joints/godot_generic_6dof_joint_3d.h
@@ -28,13 +28,13 @@
 /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
 /*************************************************************************/
 
+#ifndef GODOT_GENERIC_6DOF_JOINT_3D_H
+#define GODOT_GENERIC_6DOF_JOINT_3D_H
+
 /*
 Adapted to Godot from the Bullet library.
 */
 
-#ifndef GODOT_GENERIC_6DOF_JOINT_3D_H
-#define GODOT_GENERIC_6DOF_JOINT_3D_H
-
 #include "servers/physics_3d/godot_joint_3d.h"
 #include "servers/physics_3d/joints/godot_jacobian_entry_3d.h"
 
diff --git a/servers/physics_3d/joints/godot_hinge_joint_3d.h b/servers/physics_3d/joints/godot_hinge_joint_3d.h
index b934540e8d..f1187771af 100644
--- a/servers/physics_3d/joints/godot_hinge_joint_3d.h
+++ b/servers/physics_3d/joints/godot_hinge_joint_3d.h
@@ -28,13 +28,13 @@
 /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
 /*************************************************************************/
 
+#ifndef GODOT_HINGE_JOINT_3D_H
+#define GODOT_HINGE_JOINT_3D_H
+
 /*
 Adapted to Godot from the Bullet library.
 */
 
-#ifndef GODOT_HINGE_JOINT_3D_H
-#define GODOT_HINGE_JOINT_3D_H
-
 #include "servers/physics_3d/godot_joint_3d.h"
 #include "servers/physics_3d/joints/godot_jacobian_entry_3d.h"
 
diff --git a/servers/physics_3d/joints/godot_jacobian_entry_3d.h b/servers/physics_3d/joints/godot_jacobian_entry_3d.h
index 0fe15751d5..a46ce830ec 100644
--- a/servers/physics_3d/joints/godot_jacobian_entry_3d.h
+++ b/servers/physics_3d/joints/godot_jacobian_entry_3d.h
@@ -28,13 +28,13 @@
 /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
 /*************************************************************************/
 
+#ifndef GODOT_JACOBIAN_ENTRY_3D_H
+#define GODOT_JACOBIAN_ENTRY_3D_H
+
 /*
 Adapted to Godot from the Bullet library.
 */
 
-#ifndef GODOT_JACOBIAN_ENTRY_3D_H
-#define GODOT_JACOBIAN_ENTRY_3D_H
-
 /*
 Bullet Continuous Collision Detection and Physics Library
 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
diff --git a/servers/physics_3d/joints/godot_pin_joint_3d.h b/servers/physics_3d/joints/godot_pin_joint_3d.h
index eeeaa650bd..b3e2389d5a 100644
--- a/servers/physics_3d/joints/godot_pin_joint_3d.h
+++ b/servers/physics_3d/joints/godot_pin_joint_3d.h
@@ -28,13 +28,13 @@
 /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
 /*************************************************************************/
 
+#ifndef GODOT_PIN_JOINT_3D_H
+#define GODOT_PIN_JOINT_3D_H
+
 /*
 Adapted to Godot from the Bullet library.
 */
 
-#ifndef GODOT_PIN_JOINT_3D_H
-#define GODOT_PIN_JOINT_3D_H
-
 #include "servers/physics_3d/godot_joint_3d.h"
 #include "servers/physics_3d/joints/godot_jacobian_entry_3d.h"
 
diff --git a/servers/physics_3d/joints/godot_slider_joint_3d.h b/servers/physics_3d/joints/godot_slider_joint_3d.h
index f596c9ff75..29d19be0d7 100644
--- a/servers/physics_3d/joints/godot_slider_joint_3d.h
+++ b/servers/physics_3d/joints/godot_slider_joint_3d.h
@@ -28,13 +28,13 @@
 /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
 /*************************************************************************/
 
+#ifndef GODOT_SLIDER_JOINT_3D_H
+#define GODOT_SLIDER_JOINT_3D_H
+
 /*
 Adapted to Godot from the Bullet library.
 */
 
-#ifndef GODOT_SLIDER_JOINT_3D_H
-#define GODOT_SLIDER_JOINT_3D_H
-
 #include "servers/physics_3d/godot_joint_3d.h"
 #include "servers/physics_3d/joints/godot_jacobian_entry_3d.h"