1 files changed, 37 insertions, 30 deletions

diff --git a/servers/physics_2d/space_2d_sw.cpp b/servers/physics_2d/space_2d_sw.cpp
index 40e7b19f6f..b38cf0c2df 100644
--- a/servers/physics_2d/space_2d_sw.cpp
+++ b/servers/physics_2d/space_2d_sw.cpp
@@ -555,38 +555,10 @@ Physics2DDirectSpaceStateSW::Physics2DDirectSpaceStateSW() {
 
 
 
+int Space2DSW::_cull_aabb_for_body(Body2DSW *p_body,const Rect2& p_aabb) {
 
-bool Space2DSW::test_body_motion(Body2DSW *p_body,const Vector2&p_motion,float p_margin,Physics2DServer::MotionResult *r_result) {
-
-	//give me back regular physics engine logic
-	//this is madness
-	//and most people using this function will think
-	//what it does is simpler than using physics
-	//this took about a week to get right..
-	//but is it right? who knows at this point..
-
-	Rect2 body_aabb;
-
-	for(int i=0;i<p_body->get_shape_count();i++) {
-
-		if (i==0)
-			body_aabb=p_body->get_shape_aabb(i);
-		else
-			body_aabb=body_aabb.merge(p_body->get_shape_aabb(i));
-	}
-
-	body_aabb=body_aabb.grow(p_margin);
-
-	{
-		//add motion
 
-		Rect2 motion_aabb=body_aabb;
-		motion_aabb.pos+=p_motion;
-		body_aabb=body_aabb.merge(motion_aabb);
-	}
-
-
-	int amount = broadphase->cull_aabb(body_aabb,intersection_query_results,INTERSECTION_QUERY_MAX,intersection_query_subindex_results);
+	int amount = broadphase->cull_aabb(p_aabb,intersection_query_results,INTERSECTION_QUERY_MAX,intersection_query_subindex_results);
 
 	for(int i=0;i<amount;i++) {
 
@@ -617,6 +589,31 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body,const Vector2&p_motion,float p
 		}
 	}
 
+	return amount;
+}
+
+bool Space2DSW::test_body_motion(Body2DSW *p_body,const Vector2&p_motion,float p_margin,Physics2DServer::MotionResult *r_result) {
+
+	//give me back regular physics engine logic
+	//this is madness
+	//and most people using this function will think
+	//what it does is simpler than using physics
+	//this took about a week to get right..
+	//but is it right? who knows at this point..
+
+	Rect2 body_aabb;
+
+	for(int i=0;i<p_body->get_shape_count();i++) {
+
+		if (i==0)
+			body_aabb=p_body->get_shape_aabb(i);
+		else
+			body_aabb=body_aabb.merge(p_body->get_shape_aabb(i));
+	}
+
+	body_aabb=body_aabb.grow(p_margin);
+
+
 	Matrix32 body_transform = p_body->get_transform();
 
 	{
@@ -642,6 +639,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body,const Vector2&p_motion,float p
 
 			bool collided=false;
 
+			int amount = _cull_aabb_for_body(p_body,body_aabb);
 
 			for(int j=0;j<p_body->get_shape_count();j++) {
 				if (p_body->is_shape_set_as_trigger(j))
@@ -694,6 +692,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body,const Vector2&p_motion,float p
 			}
 
 			body_transform.elements[2]+=recover_motion;
+			body_aabb.pos+=recover_motion;
 
 			recover_attempts--;
 
@@ -709,7 +708,11 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body,const Vector2&p_motion,float p
 	{
 		// STEP 2 ATTEMPT MOTION
 
+		Rect2 motion_aabb=body_aabb;
+		motion_aabb.pos+=p_motion;
+		motion_aabb=motion_aabb.merge(body_aabb);
 
+		int amount = _cull_aabb_for_body(p_body,motion_aabb);
 
 		for(int j=0;j<p_body->get_shape_count();j++) {
 
@@ -847,6 +850,10 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body,const Vector2&p_motion,float p
 		Matrix32 body_shape_xform = ugt * p_body->get_shape_transform(best_shape);
 		Shape2DSW *body_shape = p_body->get_shape(best_shape);
 
+		body_aabb.pos+=p_motion*unsafe;
+
+		int amount = _cull_aabb_for_body(p_body,body_aabb);
+
 
 		for(int i=0;i<amount;i++) {