Merge pull request #7957 from RandomShaper/fix-kb-2d-motion-2.1

Fix KinematicBody2D motion issues + KinematicBody2D.test_move_from() (2.1)

scene/2d/physics_body_2d.cpp

@@ -29,6 +29,8 @@
 #include "physics_body_2d.h"
 #include "scene/scene_string_names.h"
 
+bool PhysicsBody2D::motion_fix_enabled=false;
+
 void PhysicsBody2D::_notification(int p_what) {
 
 /*
@@ -436,7 +438,11 @@ 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);
+	if (motion_fix_enabled) {
+		return Physics2DServer::get_singleton()->body_test_motion_from(get_rid(),get_global_transform(),p_motion,p_margin,r);
+	} else {
+		return Physics2DServer::get_singleton()->body_test_motion(get_rid(),p_motion,p_margin,r);
+	}
 
 }
 
@@ -1022,7 +1028,6 @@ RigidBody2D::~RigidBody2D() {
 
 //////////////////////////
 
-
 Variant KinematicBody2D::_get_collider() const {
 
 	ObjectID oid=get_collider();
@@ -1044,9 +1049,8 @@ void KinematicBody2D::revert_motion() {
 
 	Matrix32 gt = get_global_transform();
 	gt.elements[2]-=travel;
-	travel=Vector2();
 	set_global_transform(gt);
-
+	travel=Vector2();
 }
 
 Vector2 KinematicBody2D::get_travel() const {
@@ -1057,8 +1061,14 @@ 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);
+	if (motion_fix_enabled) {
+		colliding = Physics2DServer::get_singleton()->body_test_motion_from(get_rid(),gt,p_motion,margin,&result);
+	} else {
+		colliding = Physics2DServer::get_singleton()->body_test_motion(get_rid(),p_motion,margin,&result);
+	}
 
 	collider_metadata=result.collider_metadata;
 	collider_shape=result.collider_shape;
@@ -1067,10 +1077,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 +1093,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 +1110,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 +1154,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 +1202,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
@@ -1235,9 +1248,18 @@ bool KinematicBody2D::test_move(const Vector2& p_motion) {
 
 	ERR_FAIL_COND_V(!is_inside_tree(),false);
 
-	return Physics2DServer::get_singleton()->body_test_motion(get_rid(),p_motion,margin);
+	if (motion_fix_enabled) {
+		return Physics2DServer::get_singleton()->body_test_motion_from(get_rid(),get_global_transform(),p_motion,margin);
+	} else {
+		return Physics2DServer::get_singleton()->body_test_motion(get_rid(),p_motion,margin);
+	}
+}
+
+bool KinematicBody2D::test_move_from(const Matrix32& p_from,const Vector2& p_motion) {
 
+	ERR_FAIL_COND_V(!is_inside_tree(),false);
 
+	return Physics2DServer::get_singleton()->body_test_motion_from(get_rid(),p_from,p_motion,margin);
 }
 
 Vector2 KinematicBody2D::get_collision_pos() const {
@@ -1302,6 +1324,7 @@ void KinematicBody2D::_bind_methods() {
 	ObjectTypeDB::bind_method(_MD("move_to","position"),&KinematicBody2D::move_to);
 
 	ObjectTypeDB::bind_method(_MD("test_move","rel_vec"),&KinematicBody2D::test_move);
+	ObjectTypeDB::bind_method(_MD("test_move_from","from","rel_vec"),&KinematicBody2D::test_move_from);
 	ObjectTypeDB::bind_method(_MD("get_travel"),&KinematicBody2D::get_travel);
 	ObjectTypeDB::bind_method(_MD("revert_motion"),&KinematicBody2D::revert_motion);
 

scene/2d/physics_body_2d.h

@@ -33,7 +33,6 @@
 #include "servers/physics_2d_server.h"
 #include "vset.h"
 
-
 class PhysicsBody2D : public CollisionObject2D {
 
 	OBJ_TYPE(PhysicsBody2D,CollisionObject2D);
@@ -49,6 +48,10 @@ class PhysicsBody2D : public CollisionObject2D {
 
 protected:
 
+	// So this flag can be set at startup and cached for every body
+	friend void register_scene_types();
+	static bool motion_fix_enabled;
+
 	void _notification(int p_what);
 	PhysicsBody2D(Physics2DServer::BodyMode p_mode);
 
@@ -314,6 +317,7 @@ public:
 	Vector2 move_to(const Vector2& p_position);
 
 	bool test_move(const Vector2& p_motion);
+	bool test_move_from(const Matrix32 &p_from, const Vector2& p_motion);
 	bool is_colliding() const;
 
 	Vector2 get_travel() const;

scene/register_scene_types.cpp

@@ -497,6 +497,7 @@ void register_scene_types() {
 	ObjectTypeDB::register_type<StaticBody2D>();
 	ObjectTypeDB::register_type<RigidBody2D>();
 	ObjectTypeDB::register_type<KinematicBody2D>();
+	KinematicBody2D::motion_fix_enabled=bool(GLOBAL_DEF("physics_2d/motion_fix_enabled",false));
 	ObjectTypeDB::register_type<Area2D>();
 	ObjectTypeDB::register_type<CollisionShape2D>();
 	ObjectTypeDB::register_type<CollisionPolygon2D>();

servers/physics_2d/physics_2d_server_sw.cpp

@@ -1016,14 +1016,27 @@ 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 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);
+	// Since this is the old-style, broken version, the transform to be used
+	// is that the physics server is aware of
+	return body->get_space()->test_body_motion(body,body->get_transform(),p_motion,p_margin,r_result);
+
+}
+
+bool Physics2DServerSW::body_test_motion_from(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_from,p_motion,p_margin,r_result);
 
 }
 

servers/physics_2d/physics_2d_server_sw.h

@@ -237,6 +237,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_from(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

@@ -272,6 +272,12 @@ public:
 		return physics_2d_server->body_test_motion(p_body,p_motion,p_margin,r_result);
 	}
 
+	bool body_test_motion_from(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_from(p_body,p_from,p_motion,p_margin,r_result);
+	}
+
 	/* JOINT API */
 
 

servers/physics_2d/space_2d_sw.cpp

@@ -592,7 +592,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
@@ -601,6 +601,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++) {
@@ -611,10 +616,12 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body,const Vector2&p_motion,float p
 			body_aabb=body_aabb.merge(p_body->get_shape_aabb(i));
 	}
 
-	body_aabb=body_aabb.grow(p_margin);
+	// Undo the currently transform the physics server is aware of and apply the provided one
+	body_aabb=p_from.xform(p_body->get_inv_transform().xform(body_aabb));
 
+	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
@@ -684,6 +691,17 @@ 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)
+					continue;
+
+				Vector2 n = rel/d;
+				float traveled = n.dot(recover_motion);
+				a+=n*traveled;
+
+#endif
 			//	float d = a.distance_to(b);
 
 				//if (d<margin)
@@ -836,8 +854,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 {
@@ -898,16 +917,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

@@ -185,7 +185,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

@@ -501,6 +501,14 @@ bool Physics2DServer::_body_test_motion(RID p_body,const Vector2& p_motion,float
 	return body_test_motion(p_body,p_motion,p_margin,r);
 }
 
+bool Physics2DServer::_body_test_motion_from(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_from(p_body,p_from,p_motion,p_margin,r);
+}
+
 void Physics2DServer::_bind_methods() {
 
 
@@ -620,6 +628,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_from","body","from","motion","margin","result:Physics2DTestMotionResult"),&Physics2DServer::_body_test_motion_from,DEFVAL(0.08),DEFVAL(Variant()));
 
 	/* JOINT API */
 

servers/physics_2d_server.h

@@ -238,7 +238,8 @@ 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 Vector2& p_motion, float p_margin=0.08, const Ref<Physics2DTestMotionResult>& p_result=Ref<Physics2DTestMotionResult>());
+	virtual bool _body_test_motion_from(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();
@@ -498,6 +499,7 @@ public:
 	};
 
 	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_from(RID p_body,const Matrix32& p_from,const Vector2& p_motion,float p_margin=0.001,MotionResult *r_result=NULL)=0;
 
 	/* JOINT API */