Fix physics BVH pairing for teleported or fast moving objects

Updating the broadphase to find new collision pairs was done after
checking for collision islands, so it was working in most cases due to
the pairing margin used in the BVH, but in case of teleported objects
the narrowphase collision could be skipped.

Now it's done before checking for collision islands, so we can ensure
that broadphase pairing has been done at the same time as objects are
marked as moved so their collision can be checked properly.

This issue didn't happen in the Octree/HashGrid because they do nothing
on update and trigger pairs directly when objects move instead.

servers/physics_2d/godot_step_2d.cpp

@@ -152,6 +152,9 @@ void GodotStep2D::step(GodotSpace2D *p_space, real_t p_delta, int p_iterations)
 
 	p_space->set_active_objects(active_count);
 
+	// Update the broadphase to register collision pairs.
+	p_space->update();
+
 	{ //profile
 		profile_endtime = OS::get_singleton()->get_ticks_usec();
 		p_space->set_elapsed_time(GodotSpace2D::ELAPSED_TIME_INTEGRATE_FORCES, profile_endtime - profile_begtime);
@@ -286,7 +289,6 @@ void GodotStep2D::step(GodotSpace2D *p_space, real_t p_delta, int p_iterations)
 
 	all_constraints.clear();
 
-	p_space->update();
 	p_space->unlock();
 	_step++;
 }

servers/physics_3d/godot_step_3d.cpp

@@ -220,6 +220,9 @@ void GodotStep3D::step(GodotSpace3D *p_space, real_t p_delta, int p_iterations)
 
 	p_space->set_active_objects(active_count);
 
+	// Update the broadphase to register collision pairs.
+	p_space->update();
+
 	{ //profile
 		profile_endtime = OS::get_singleton()->get_ticks_usec();
 		p_space->set_elapsed_time(GodotSpace3D::ELAPSED_TIME_INTEGRATE_FORCES, profile_endtime - profile_begtime);
@@ -398,7 +401,6 @@ void GodotStep3D::step(GodotSpace3D *p_space, real_t p_delta, int p_iterations)
 
 	all_constraints.clear();
 
-	p_space->update();
 	p_space->unlock();
 	_step++;
 }