In final phase of test_body_motion, move and cull AABB for body once instead of for every shape

servers/physics/space_sw.cpp

@@ -1006,6 +1006,9 @@ bool SpaceSW::test_body_motion(BodySW *p_body, const Transform &p_from, const Ve
 		// 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_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();
 
@@ -1021,10 +1024,6 @@ bool SpaceSW::test_body_motion(BodySW *p_body, const Transform &p_from, const Ve
 				continue;
 			}
 
-			body_aabb.position += p_motion * unsafe;
-
-			int amount = _cull_aabb_for_body(p_body, body_aabb);
-
 			for (int i = 0; i < amount; i++) {
 				const CollisionObjectSW *col_obj = intersection_query_results[i];
 				if (p_exclude.has(col_obj->get_self())) {

servers/physics_2d/space_2d_sw.cpp

@@ -1087,6 +1087,9 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
 		// 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_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();
 
@@ -1102,10 +1105,6 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
 				continue;
 			}
 
-			body_aabb.position += p_motion * unsafe;
-
-			int amount = _cull_aabb_for_body(p_body, body_aabb);
-
 			for (int i = 0; i < amount; i++) {
 				const CollisionObject2DSW *col_obj = intersection_query_results[i];
 				if (p_exclude.has(col_obj->get_self())) {