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.

(cherry picked from commit e9fdf3e61fb043f456f1f038f02863fcb0c3f5e1)

servers/physics/step_sw.cpp

@@ -162,6 +162,9 @@ void StepSW::step(SpaceSW *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(SpaceSW::ELAPSED_TIME_INTEGRATE_FORCES, profile_endtime - profile_begtime);
@@ -278,7 +281,6 @@ void StepSW::step(SpaceSW *p_space, real_t p_delta, int p_iterations) {
 		profile_begtime = profile_endtime;
 	}
 
-	p_space->update();
 	p_space->unlock();
 	_step++;
 }

servers/physics_2d/step_2d_sw.cpp

@@ -151,6 +151,9 @@ void Step2DSW::step(Space2DSW *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(Space2DSW::ELAPSED_TIME_INTEGRATE_FORCES, profile_endtime - profile_begtime);
@@ -294,7 +297,6 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) {
 		//profile_begtime=profile_endtime;
 	}
 
-	p_space->update();
 	p_space->unlock();
 	_step++;
 }