-Fixed issue in Kinematicbody2D

5 files changed, 24 insertions, 15 deletions

diff --git a/servers/physics_2d/physics_2d_server_sw.cpp b/servers/physics_2d/physics_2d_server_sw.cpp
index 54cd929c2f..8e92a475ab 100644
--- a/servers/physics_2d/physics_2d_server_sw.cpp
+++ b/servers/physics_2d/physics_2d_server_sw.cpp
@@ -1016,14 +1016,14 @@ void Physics2DServerSW::body_set_pickable(RID p_body,bool p_pickable) {
 
 }
 
-bool Physics2DServerSW::body_test_motion(RID p_body,const Vector2& p_motion,float p_margin,MotionResult *r_result) {
+bool Physics2DServerSW::body_test_motion(RID p_body, const Matrix32 &p_from, const Vector2& p_motion, float p_margin, MotionResult *r_result) {
 
 	Body2DSW *body = body_owner.get(p_body);
 	ERR_FAIL_COND_V(!body,false);
 	ERR_FAIL_COND_V(!body->get_space(),false);
 	ERR_FAIL_COND_V(body->get_space()->is_locked(),false);
 
-	return body->get_space()->test_body_motion(body,p_motion,p_margin,r_result);
+	return body->get_space()->test_body_motion(body,p_from,p_motion,p_margin,r_result);
 
 }
 
diff --git a/servers/physics_2d/physics_2d_server_sw.h b/servers/physics_2d/physics_2d_server_sw.h
index d557688b91..1dc735289a 100644
--- a/servers/physics_2d/physics_2d_server_sw.h
+++ b/servers/physics_2d/physics_2d_server_sw.h
@@ -236,7 +236,7 @@ public:
 
 	virtual void body_set_pickable(RID p_body,bool p_pickable);
 
-	virtual bool body_test_motion(RID p_body,const Vector2& p_motion,float p_margin=0.001,MotionResult *r_result=NULL);
+	virtual bool body_test_motion(RID p_body,const Matrix32& p_from,const Vector2& p_motion,float p_margin=0.001,MotionResult *r_result=NULL);
 
 
 	/* JOINT API */
diff --git a/servers/physics_2d/physics_2d_server_wrap_mt.h b/servers/physics_2d/physics_2d_server_wrap_mt.h
index fd98da2d9c..57da958f9a 100644
--- a/servers/physics_2d/physics_2d_server_wrap_mt.h
+++ b/servers/physics_2d/physics_2d_server_wrap_mt.h
@@ -266,10 +266,10 @@ public:
 
 	FUNC2(body_set_pickable,RID,bool);
 
-	bool body_test_motion(RID p_body,const Vector2& p_motion,float p_margin=0.001,MotionResult *r_result=NULL) {
+	bool body_test_motion(RID p_body,const Matrix32& p_from,const Vector2& p_motion,float p_margin=0.001,MotionResult *r_result=NULL) {
 
 		ERR_FAIL_COND_V(main_thread!=Thread::get_caller_ID(),false);
-		return physics_2d_server->body_test_motion(p_body,p_motion,p_margin,r_result);
+		return physics_2d_server->body_test_motion(p_body,p_from,p_motion,p_margin,r_result);
 	}
 
 	/* JOINT API */
diff --git a/servers/physics_2d/space_2d_sw.cpp b/servers/physics_2d/space_2d_sw.cpp
index 366eda6913..d0dcee7763 100644
--- a/servers/physics_2d/space_2d_sw.cpp
+++ b/servers/physics_2d/space_2d_sw.cpp
@@ -589,7 +589,7 @@ int Space2DSW::_cull_aabb_for_body(Body2DSW *p_body,const Rect2& p_aabb) {
 	return amount;
 }
 
-bool Space2DSW::test_body_motion(Body2DSW *p_body,const Vector2&p_motion,float p_margin,Physics2DServer::MotionResult *r_result) {
+bool Space2DSW::test_body_motion(Body2DSW *p_body, const Matrix32 &p_from, const Vector2&p_motion, float p_margin, Physics2DServer::MotionResult *r_result) {
 
 	//give me back regular physics engine logic
 	//this is madness
@@ -598,6 +598,11 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body,const Vector2&p_motion,float p
 	//this took about a week to get right..
 	//but is it right? who knows at this point..
 
+	if (r_result) {
+		r_result->collider_id=0;
+		r_result->collider_shape=0;
+
+	}
 	Rect2 body_aabb;
 
 	for(int i=0;i<p_body->get_shape_count();i++) {
@@ -610,8 +615,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body,const Vector2&p_motion,float p
 
 	body_aabb=body_aabb.grow(p_margin);
 
-
-	Matrix32 body_transform = p_body->get_transform();
+	Matrix32 body_transform = p_from;
 
 	{
 		//STEP 1, FREE BODY IF STUCK
@@ -681,6 +685,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body,const Vector2&p_motion,float p
 				Vector2 a = sr[i*2+0];
 				Vector2 b = sr[i*2+1];
 
+#if 0
 				Vector2 rel = b-a;
 				float d = rel.length();
 				if (d==0)
@@ -690,12 +695,12 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body,const Vector2&p_motion,float p
 				float traveled = n.dot(recover_motion);
 				a+=n*traveled;
 
-
+#endif
 			//	float d = a.distance_to(b);
 
 				//if (d<margin)
 				///	continue;
-				recover_motion+=(b-a)*0.2;
+				recover_motion+=(b-a)*0.4;
 			}
 
 			if (recover_motion==Vector2()) {
@@ -843,8 +848,9 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body,const Vector2&p_motion,float p
 		collided=false;
 		if (r_result) {
 
-			r_result->motion=p_motion+(body_transform.elements[2]-p_body->get_transform().elements[2]);
-			r_result->remainder=Vector2();
+			r_result->motion=p_motion;
+			r_result->remainder=Vector2();			
+			r_result->motion+=(body_transform.elements[2]-p_from.elements[2]);
 		}
 
 	} else {
@@ -905,16 +911,19 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body,const Vector2&p_motion,float p
 				Vector2 rel_vec = r_result->collision_point-body->get_transform().get_origin();
 				r_result->collider_velocity = Vector2(-body->get_angular_velocity() * rel_vec.y, body->get_angular_velocity() * rel_vec.x) + body->get_linear_velocity();
 
-				r_result->motion=safe*p_motion+(body_transform.elements[2]-p_body->get_transform().elements[2]);
+				r_result->motion=safe*p_motion;
 				r_result->remainder=p_motion - safe * p_motion;
+				r_result->motion+=(body_transform.elements[2]-p_from.elements[2]);
+
 			}
 
 			collided=true;
 		} else {
 			if (r_result) {
 
-				r_result->motion=p_motion+(body_transform.elements[2]-p_body->get_transform().elements[2]);
+				r_result->motion=p_motion;
 				r_result->remainder=Vector2();
+				r_result->motion+=(body_transform.elements[2]-p_from.elements[2]);
 			}
 
 			collided=false;
diff --git a/servers/physics_2d/space_2d_sw.h b/servers/physics_2d/space_2d_sw.h
index f8e1f32838..f58e8c3fe7 100644
--- a/servers/physics_2d/space_2d_sw.h
+++ b/servers/physics_2d/space_2d_sw.h
@@ -184,7 +184,7 @@ public:
 
 	int get_collision_pairs() const { return collision_pairs; }
 
-	bool test_body_motion(Body2DSW *p_body, const Vector2&p_motion, float p_margin, Physics2DServer::MotionResult *r_result);
+	bool test_body_motion(Body2DSW *p_body, const Matrix32 &p_from, const Vector2&p_motion, float p_margin, Physics2DServer::MotionResult *r_result);
 
 
 	void set_debug_contacts(int p_amount) { contact_debug.resize(p_amount); }