-Fixed issue in Kinematicbody2D

scene/2d/physics_body_2d.cpp

@@ -436,7 +436,7 @@ bool RigidBody2D::_test_motion(const Vector2& p_motion,float p_margin,const Ref<
 	Physics2DServer::MotionResult *r=NULL;
 	if (p_result.is_valid())
 		r=p_result->get_result_ptr();
-	return Physics2DServer::get_singleton()->body_test_motion(get_rid(),p_motion,p_margin,r);
+	return Physics2DServer::get_singleton()->body_test_motion(get_rid(),get_global_transform(),p_motion,p_margin,r);
 
 }
 
@@ -1057,8 +1057,10 @@ Vector2 KinematicBody2D::get_travel() const {
 Vector2 KinematicBody2D::move(const Vector2& p_motion) {
 
 #if 1
+
+	Matrix32 gt = get_global_transform();
 	Physics2DServer::MotionResult result;
-	colliding = Physics2DServer::get_singleton()->body_test_motion(get_rid(),p_motion,margin,&result);
+	colliding = Physics2DServer::get_singleton()->body_test_motion(get_rid(),gt,p_motion,margin,&result);
 
 	collider_metadata=result.collider_metadata;
 	collider_shape=result.collider_shape;
@@ -1067,10 +1069,12 @@ Vector2 KinematicBody2D::move(const Vector2& p_motion) {
 	normal=result.collision_normal;
 	collider=result.collider_id;
 
-	Matrix32 gt = get_global_transform();
+
 	gt.elements[2]+=result.motion;
 	set_global_transform(gt);
 	travel=result.motion;
+
+
 	return result.remainder;
 
 #else
@@ -1081,7 +1085,6 @@ Vector2 KinematicBody2D::move(const Vector2& p_motion) {
 	//this took about a week to get right..
 	//but is it right? who knows at this point..
 
-
 	colliding=false;
 	ERR_FAIL_COND_V(!is_inside_tree(),Vector2());
 	Physics2DDirectSpaceState *dss = Physics2DServer::get_singleton()->space_get_direct_state(get_world_2d()->get_space());
@@ -1099,17 +1102,15 @@ Vector2 KinematicBody2D::move(const Vector2& p_motion) {
 
 	bool collided=false;
 	uint32_t mask=0;
-	if (collide_static)
+	if (true)
 		mask|=Physics2DDirectSpaceState::TYPE_MASK_STATIC_BODY;
-	if (collide_kinematic)
+	if (true)
 		mask|=Physics2DDirectSpaceState::TYPE_MASK_KINEMATIC_BODY;
-	if (collide_rigid)
+	if (true)
 		mask|=Physics2DDirectSpaceState::TYPE_MASK_RIGID_BODY;
-	if (collide_character)
+	if (true)
 		mask|=Physics2DDirectSpaceState::TYPE_MASK_CHARACTER_BODY;
 
-//	print_line("motion: "+p_motion+" margin: "+rtos(margin));
-
 	//print_line("margin: "+rtos(margin));
 	do {
 
@@ -1145,6 +1146,8 @@ Vector2 KinematicBody2D::move(const Vector2& p_motion) {
 			break;
 		}
 
+
+
 		Matrix32 gt = get_global_transform();
 		gt.elements[2]+=recover_motion;
 		set_global_transform(gt);
@@ -1191,6 +1194,8 @@ Vector2 KinematicBody2D::move(const Vector2& p_motion) {
 	if (safe>=1) {
 		//not collided
 		colliding=false;
+
+
 	} else {
 
 		//it collided, let's get the rest info in unsafe advance
@@ -1239,7 +1244,7 @@ Vector2 KinematicBody2D::move_and_slide(const Vector2& p_linear_velocity,const V
 	move_and_slide_colliders.clear();
 	move_and_slide_floor_velocity=Vector2();
 
-	while(motion!=Vector2() && p_max_bounces) {
+	while(p_max_bounces) {
 
 		motion=move(motion);
 
@@ -1280,6 +1285,8 @@ Vector2 KinematicBody2D::move_and_slide(const Vector2& p_linear_velocity,const V
 		}
 
 		p_max_bounces--;
+		if (motion==Vector2())
+			break;
 	}
 
 	return lv;
@@ -1307,11 +1314,11 @@ Vector2 KinematicBody2D::move_to(const Vector2& p_position) {
 	return move(p_position-get_global_pos());
 }
 
-bool KinematicBody2D::test_move(const Vector2& p_motion) {
+bool KinematicBody2D::test_move(const Matrix32& p_from,const Vector2& p_motion) {
 
 	ERR_FAIL_COND_V(!is_inside_tree(),false);
 
-	return Physics2DServer::get_singleton()->body_test_motion(get_rid(),p_motion,margin);
+	return Physics2DServer::get_singleton()->body_test_motion(get_rid(),p_from,p_motion,margin);
 
 
 }
@@ -1378,7 +1385,7 @@ void KinematicBody2D::_bind_methods() {
 	ObjectTypeDB::bind_method(_MD("move_to","position"),&KinematicBody2D::move_to);
 	ObjectTypeDB::bind_method(_MD("move_and_slide","linear_velocity","floor_normal","slope_stop_min_velocity","max_bounces"),&KinematicBody2D::move_and_slide,DEFVAL(Vector2(0,0)),DEFVAL(5),DEFVAL(4));
 
-	ObjectTypeDB::bind_method(_MD("test_move","rel_vec"),&KinematicBody2D::test_move);
+	ObjectTypeDB::bind_method(_MD("test_move","from","rel_vec"),&KinematicBody2D::test_move);
 	ObjectTypeDB::bind_method(_MD("get_travel"),&KinematicBody2D::get_travel);
 	ObjectTypeDB::bind_method(_MD("revert_motion"),&KinematicBody2D::revert_motion);
 

scene/2d/physics_body_2d.h

@@ -319,7 +319,7 @@ public:
 	Vector2 move(const Vector2& p_motion);
 	Vector2 move_to(const Vector2& p_position);
 
-	bool test_move(const Vector2& p_motion);
+	bool test_move(const Matrix32 &p_from, const Vector2& p_motion);
 	bool is_colliding() const;
 
 	Vector2 get_travel() const;

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);
 
 }
 

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 */

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 */

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;

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); }

servers/physics_2d_server.cpp

@@ -493,12 +493,12 @@ Physics2DTestMotionResult::Physics2DTestMotionResult(){
 
 
 
-bool Physics2DServer::_body_test_motion(RID p_body,const Vector2& p_motion,float p_margin,const Ref<Physics2DTestMotionResult>& p_result) {
+bool Physics2DServer::_body_test_motion(RID p_body,const Matrix32& p_from,const Vector2& p_motion,float p_margin,const Ref<Physics2DTestMotionResult>& p_result) {
 
 	MotionResult *r=NULL;
 	if (p_result.is_valid())
 		r=p_result->get_result_ptr();
-	return body_test_motion(p_body,p_motion,p_margin,r);
+	return body_test_motion(p_body,p_from,p_motion,p_margin,r);
 }
 
 void Physics2DServer::_bind_methods() {
@@ -619,7 +619,7 @@ void Physics2DServer::_bind_methods() {
 
 	ObjectTypeDB::bind_method(_MD("body_set_force_integration_callback","body","receiver","method","userdata"),&Physics2DServer::body_set_force_integration_callback,DEFVAL(Variant()));
 
-	ObjectTypeDB::bind_method(_MD("body_test_motion","body","motion","margin","result:Physics2DTestMotionResult"),&Physics2DServer::_body_test_motion,DEFVAL(0.08),DEFVAL(Variant()));
+	ObjectTypeDB::bind_method(_MD("body_test_motion","body","from","motion","margin","result:Physics2DTestMotionResult"),&Physics2DServer::_body_test_motion,DEFVAL(0.08),DEFVAL(Variant()));
 
 	/* JOINT API */
 

servers/physics_2d_server.h

@@ -238,7 +238,7 @@ class Physics2DServer : public Object {
 
 	static Physics2DServer * singleton;
 
-	virtual bool _body_test_motion(RID p_body,const Vector2& p_motion,float p_margin=0.08,const Ref<Physics2DTestMotionResult>& p_result=Ref<Physics2DTestMotionResult>());
+	virtual bool _body_test_motion(RID p_body, const Matrix32 &p_from, const Vector2& p_motion, float p_margin=0.08, const Ref<Physics2DTestMotionResult>& p_result=Ref<Physics2DTestMotionResult>());
 
 protected:
 	static void _bind_methods();
@@ -497,7 +497,7 @@ public:
 		Variant collider_metadata;
 	};
 
-	virtual bool body_test_motion(RID p_body,const Vector2& p_motion,float p_margin=0.001,MotionResult *r_result=NULL)=0;
+	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)=0;
 
 	/* JOINT API */