6 files changed, 5 insertions, 33 deletions

diff --git a/servers/physics_3d/gjk_epa.cpp b/servers/physics_3d/gjk_epa.cpp
index 928ffe0980..23c8079538 100644
--- a/servers/physics_3d/gjk_epa.cpp
+++ b/servers/physics_3d/gjk_epa.cpp
@@ -918,7 +918,7 @@ bool Distance(	const GodotShape3D*	shape0,
 	{
 		results.status	=	gjk_status==GJK::eStatus::Inside?
 			sResults::Penetrating	:
-		sResults::GJK_Failed	;
+		sResults::GJK_Failed;
 		return(false);
 	}
 }
diff --git a/servers/physics_3d/godot_collision_solver_3d_sat.cpp b/servers/physics_3d/godot_collision_solver_3d_sat.cpp
index 82e04a7e31..ca429040f5 100644
--- a/servers/physics_3d/godot_collision_solver_3d_sat.cpp
+++ b/servers/physics_3d/godot_collision_solver_3d_sat.cpp
@@ -345,10 +345,6 @@ static void _generate_contacts_face_face(const Vector3 *p_points_A, int p_point_
 
 	for (int i = 0; i < clipbuf_len; i++) {
 		real_t d = plane_B.distance_to(clipbuf_src[i]);
-		/*
-		if (d>CMP_EPSILON)
-			continue;
-		*/
 
 		Vector3 closest_B = clipbuf_src[i] - plane_B.normal * d;
 
diff --git a/servers/physics_3d/godot_physics_server_3d.cpp b/servers/physics_3d/godot_physics_server_3d.cpp
index 4e1680e6ac..a1912dc660 100644
--- a/servers/physics_3d/godot_physics_server_3d.cpp
+++ b/servers/physics_3d/godot_physics_server_3d.cpp
@@ -1540,14 +1540,6 @@ void GodotPhysicsServer3D::free(RID p_rid) {
 	} else if (body_owner.owns(p_rid)) {
 		GodotBody3D *body = body_owner.get_or_null(p_rid);
 
-		/*
-		if (body->get_state_query())
-			_clear_query(body->get_state_query());
-
-		if (body->get_direct_state_query())
-			_clear_query(body->get_direct_state_query());
-		*/
-
 		body->set_space(nullptr);
 
 		while (body->get_shape_count()) {
@@ -1566,11 +1558,6 @@ void GodotPhysicsServer3D::free(RID p_rid) {
 	} else if (area_owner.owns(p_rid)) {
 		GodotArea3D *area = area_owner.get_or_null(p_rid);
 
-		/*
-		if (area->get_monitor_query())
-			_clear_query(area->get_monitor_query());
-		*/
-
 		area->set_space(nullptr);
 
 		while (area->get_shape_count()) {
diff --git a/servers/physics_3d/godot_shape_3d.cpp b/servers/physics_3d/godot_shape_3d.cpp
index 666e773c1c..7762c4829e 100644
--- a/servers/physics_3d/godot_shape_3d.cpp
+++ b/servers/physics_3d/godot_shape_3d.cpp
@@ -1284,12 +1284,6 @@ 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];
 
-	/*
-	if (p_params->dir.dot(bvh->aabb.get_support(-p_params->dir))>p_params->min_d)
-		return; //test against whole AABB, which isn't very costly
-	*/
-
-	//printf("addr: %p\n",bvh);
 	if (!bvh->aabb.intersects_segment(p_params->from, p_params->to)) {
 		return;
 	}
diff --git a/servers/physics_3d/godot_space_3d.cpp b/servers/physics_3d/godot_space_3d.cpp
index ed756a7f9d..2490a2f506 100644
--- a/servers/physics_3d/godot_space_3d.cpp
+++ b/servers/physics_3d/godot_space_3d.cpp
@@ -926,6 +926,9 @@ bool GodotSpace3D::test_body_motion(GodotBody3D *p_body, const PhysicsServer3D::
 		// Allowed depth can't be lower than motion length, in order to handle contacts at low speed.
 		rcd.min_allowed_depth = MIN(motion_length, min_contact_depth);
 
+		body_aabb.position += p_parameters.motion * unsafe;
+		int amount = _cull_aabb_for_body(p_body, body_aabb);
+
 		int from_shape = best_shape != -1 ? best_shape : 0;
 		int to_shape = best_shape != -1 ? best_shape + 1 : p_body->get_shape_count();
 
@@ -937,10 +940,6 @@ bool GodotSpace3D::test_body_motion(GodotBody3D *p_body, const PhysicsServer3D::
 			Transform3D body_shape_xform = ugt * p_body->get_shape_transform(j);
 			GodotShape3D *body_shape = p_body->get_shape(j);
 
-			body_aabb.position += p_parameters.motion * unsafe;
-
-			int amount = _cull_aabb_for_body(p_body, body_aabb);
-
 			for (int i = 0; i < amount; i++) {
 				const GodotCollisionObject3D *col_obj = intersection_query_results[i];
 				if (p_parameters.exclude_bodies.has(col_obj->get_self())) {
diff --git a/servers/physics_3d/joints/godot_hinge_joint_3d.cpp b/servers/physics_3d/joints/godot_hinge_joint_3d.cpp
index a7a7843499..1c4d5dec23 100644
--- a/servers/physics_3d/joints/godot_hinge_joint_3d.cpp
+++ b/servers/physics_3d/joints/godot_hinge_joint_3d.cpp
@@ -213,16 +213,12 @@ bool GodotHingeJoint3D::setup(real_t p_step) {
 	m_solveLimit = false;
 	m_accLimitImpulse = real_t(0.);
 
-	//if (m_lowerLimit < m_upperLimit)
 	if (m_useLimit && m_lowerLimit <= m_upperLimit) {
-		//if (hingeAngle <= m_lowerLimit*m_limitSoftness)
 		if (hingeAngle <= m_lowerLimit) {
 			m_correction = (m_lowerLimit - hingeAngle);
 			m_limitSign = 1.0f;
 			m_solveLimit = true;
-		}
-		//else if (hingeAngle >= m_upperLimit*m_limitSoftness)
-		else if (hingeAngle >= m_upperLimit) {
+		} else if (hingeAngle >= m_upperLimit) {
 			m_correction = m_upperLimit - hingeAngle;
 			m_limitSign = -1.0f;
 			m_solveLimit = true;