Merge pull request #16757 from AndreaCatania/kinpush

Improved kinematic body, Now can move rigid body

5 files changed, 28 insertions, 7 deletions

diff --git a/servers/physics_2d/physics_2d_server_sw.cpp b/servers/physics_2d/physics_2d_server_sw.cpp
index 0603287a79..a14fed8184 100644
--- a/servers/physics_2d/physics_2d_server_sw.cpp
+++ b/servers/physics_2d/physics_2d_server_sw.cpp
@@ -962,14 +962,14 @@ void Physics2DServerSW::body_set_pickable(RID p_body, bool p_pickable) {
 	body->set_pickable(p_pickable);
 }
 
-bool Physics2DServerSW::body_test_motion(RID p_body, const Transform2D &p_from, const Vector2 &p_motion, real_t p_margin, MotionResult *r_result) {
+bool Physics2DServerSW::body_test_motion(RID p_body, const Transform2D &p_from, const Vector2 &p_motion, bool p_infinite_inertia, real_t 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);
+	return body->get_space()->test_body_motion(body, p_from, p_motion, p_infinite_inertia, p_margin, r_result);
 }
 
 Physics2DDirectBodyState *Physics2DServerSW::body_get_direct_state(RID p_body) {
diff --git a/servers/physics_2d/physics_2d_server_sw.h b/servers/physics_2d/physics_2d_server_sw.h
index cf9c2957bf..036eb934e1 100644
--- a/servers/physics_2d/physics_2d_server_sw.h
+++ b/servers/physics_2d/physics_2d_server_sw.h
@@ -232,7 +232,7 @@ public:
 
 	virtual void body_set_pickable(RID p_body, bool p_pickable);
 
-	virtual bool body_test_motion(RID p_body, const Transform2D &p_from, const Vector2 &p_motion, real_t p_margin = 0.001, MotionResult *r_result = NULL);
+	virtual bool body_test_motion(RID p_body, const Transform2D &p_from, const Vector2 &p_motion, bool p_infinite_inertia, real_t p_margin = 0.001, MotionResult *r_result = NULL);
 
 	// this function only works on physics process, errors and returns null otherwise
 	virtual Physics2DDirectBodyState *body_get_direct_state(RID p_body);
diff --git a/servers/physics_2d/physics_2d_server_wrap_mt.h b/servers/physics_2d/physics_2d_server_wrap_mt.h
index d625bc9892..a15e8bde8b 100644
--- a/servers/physics_2d/physics_2d_server_wrap_mt.h
+++ b/servers/physics_2d/physics_2d_server_wrap_mt.h
@@ -245,10 +245,10 @@ public:
 
 	FUNC2(body_set_pickable, RID, bool);
 
-	bool body_test_motion(RID p_body, const Transform2D &p_from, const Vector2 &p_motion, real_t p_margin = 0.001, MotionResult *r_result = NULL) {
+	bool body_test_motion(RID p_body, const Transform2D &p_from, const Vector2 &p_motion, bool p_infinite_inertia, real_t 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_from, p_motion, p_margin, r_result);
+		return physics_2d_server->body_test_motion(p_body, p_from, p_motion, p_infinite_inertia, p_margin, r_result);
 	}
 
 	// this function only works on physics process, errors and returns null otherwise
diff --git a/servers/physics_2d/space_2d_sw.cpp b/servers/physics_2d/space_2d_sw.cpp
index d3b81c627a..c29093d1af 100644
--- a/servers/physics_2d/space_2d_sw.cpp
+++ b/servers/physics_2d/space_2d_sw.cpp
@@ -483,7 +483,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 Transform2D &p_from, const Vector2 &p_motion, real_t p_margin, Physics2DServer::MotionResult *r_result) {
+bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, const Vector2 &p_motion, bool p_infinite_inertia, real_t p_margin, Physics2DServer::MotionResult *r_result) {
 
 	//give me back regular physics engine logic
 	//this is madness
@@ -550,6 +550,13 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
 					const CollisionObject2DSW *col_obj = intersection_query_results[i];
 					int shape_idx = intersection_query_subindex_results[i];
 
+					if (CollisionObject2DSW::TYPE_BODY == col_obj->get_type()) {
+						const Body2DSW *b = static_cast<const Body2DSW *>(col_obj);
+						if (p_infinite_inertia && Physics2DServer::BODY_MODE_STATIC != b->get_mode() && Physics2DServer::BODY_MODE_KINEMATIC != b->get_mode()) {
+							continue;
+						}
+					}
+
 					if (col_obj->is_shape_set_as_one_way_collision(shape_idx)) {
 
 						cbk.valid_dir = body_shape_xform.get_axis(1).normalized();
@@ -638,6 +645,13 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
 				int col_shape_idx = intersection_query_subindex_results[i];
 				Shape2DSW *against_shape = col_obj->get_shape(col_shape_idx);
 
+				if (CollisionObject2DSW::TYPE_BODY == col_obj->get_type()) {
+					const Body2DSW *b = static_cast<const Body2DSW *>(col_obj);
+					if (p_infinite_inertia && Physics2DServer::BODY_MODE_STATIC != b->get_mode() && Physics2DServer::BODY_MODE_KINEMATIC != b->get_mode()) {
+						continue;
+					}
+				}
+
 				bool excluded = false;
 
 				for (int k = 0; k < excluded_shape_pair_count; k++) {
@@ -768,6 +782,13 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
 			const CollisionObject2DSW *col_obj = intersection_query_results[i];
 			int shape_idx = intersection_query_subindex_results[i];
 
+			if (CollisionObject2DSW::TYPE_BODY == col_obj->get_type()) {
+				const Body2DSW *b = static_cast<const Body2DSW *>(col_obj);
+				if (p_infinite_inertia && Physics2DServer::BODY_MODE_STATIC != b->get_mode() && Physics2DServer::BODY_MODE_KINEMATIC != b->get_mode()) {
+					continue;
+				}
+			}
+
 			Shape2DSW *against_shape = col_obj->get_shape(shape_idx);
 
 			bool excluded = false;
diff --git a/servers/physics_2d/space_2d_sw.h b/servers/physics_2d/space_2d_sw.h
index a18bb2be2d..79349c46f3 100644
--- a/servers/physics_2d/space_2d_sw.h
+++ b/servers/physics_2d/space_2d_sw.h
@@ -182,7 +182,7 @@ public:
 
 	int get_collision_pairs() const { return collision_pairs; }
 
-	bool test_body_motion(Body2DSW *p_body, const Transform2D &p_from, const Vector2 &p_motion, real_t p_margin, Physics2DServer::MotionResult *r_result);
+	bool test_body_motion(Body2DSW *p_body, const Transform2D &p_from, const Vector2 &p_motion, bool p_infinite_inertia, real_t p_margin, Physics2DServer::MotionResult *r_result);
 
 	void set_debug_contacts(int p_amount) { contact_debug.resize(p_amount); }
 	_FORCE_INLINE_ bool is_debugging_contacts() const { return !contact_debug.empty(); }