9 files changed, 53 insertions, 0 deletions
diff --git a/servers/physics/physics_server_sw.cpp b/servers/physics/physics_server_sw.cpp
index 1553c3c8e6..a7c31cf16c 100644
--- a/servers/physics/physics_server_sw.cpp
+++ b/servers/physics/physics_server_sw.cpp
@@ -914,6 +914,21 @@ bool PhysicsServerSW::body_test_motion(RID p_body, const Transform &p_from, cons
 	return body->get_space()->test_body_motion(body, p_from, p_motion, p_margin, r_result);
 }
 
+PhysicsDirectBodyState *PhysicsServerSW::body_get_direct_state(RID p_body) {
+
+	BodySW *body = body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body, NULL);
+
+	if (!doing_sync || body->get_space()->is_locked()) {
+
+		ERR_EXPLAIN("Body state is inaccessible right now, wait for iteration or physics process notification.");
+		ERR_FAIL_V(NULL);
+	}
+
+	direct_state->body = body;
+	return direct_state;
+}
+
 /* JOINT API */
 
 RID PhysicsServerSW::joint_create_pin(RID p_body_A, const Vector3 &p_local_A, RID p_body_B, const Vector3 &p_local_B) {
diff --git a/servers/physics/physics_server_sw.h b/servers/physics/physics_server_sw.h
index aea3e150b0..f9eb8fa454 100644
--- a/servers/physics/physics_server_sw.h
+++ b/servers/physics/physics_server_sw.h
@@ -223,6 +223,9 @@ public:
 
 	virtual bool body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, float p_margin = 0.001, MotionResult *r_result = NULL);
 
+	// this function only works on physics process, errors and returns null otherwise
+	virtual PhysicsDirectBodyState *body_get_direct_state(RID p_body);
+
 	/* JOINT API */
 
 	virtual RID joint_create_pin(RID p_body_A, const Vector3 &p_local_A, RID p_body_B, const Vector3 &p_local_B);
diff --git a/servers/physics_2d/physics_2d_server_sw.cpp b/servers/physics_2d/physics_2d_server_sw.cpp
index 2b7505169a..df3bf72a31 100644
--- a/servers/physics_2d/physics_2d_server_sw.cpp
+++ b/servers/physics_2d/physics_2d_server_sw.cpp
@@ -954,6 +954,21 @@ bool Physics2DServerSW::body_test_motion(RID p_body, const Transform2D &p_from,
 	return body->get_space()->test_body_motion(body, p_from, p_motion, p_margin, r_result);
 }
 
+Physics2DDirectBodyState *Physics2DServerSW::body_get_direct_state(RID p_body) {
+
+	Body2DSW *body = body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body, NULL);
+
+	if ((using_threads && !doing_sync) || body->get_space()->is_locked()) {
+
+		ERR_EXPLAIN("Body state is inaccessible right now, wait for iteration or physics process notification.");
+		ERR_FAIL_V(NULL);
+	}
+
+	direct_state->body = body;
+	return direct_state;
+}
+
 /* JOINT API */
 
 void Physics2DServerSW::joint_set_param(RID p_joint, JointParam p_param, real_t p_value) {
diff --git a/servers/physics_2d/physics_2d_server_sw.h b/servers/physics_2d/physics_2d_server_sw.h
index 0ebd43be53..c40cf0e3e0 100644
--- a/servers/physics_2d/physics_2d_server_sw.h
+++ b/servers/physics_2d/physics_2d_server_sw.h
@@ -222,6 +222,9 @@ public:
 
 	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);
 
+	// this function only works on physics process, errors and returns null otherwise
+	virtual Physics2DDirectBodyState *body_get_direct_state(RID p_body);
+
 	/* JOINT API */
 
 	virtual void joint_set_param(RID p_joint, JointParam p_param, real_t p_value);
diff --git a/servers/physics_2d/physics_2d_server_wrap_mt.h b/servers/physics_2d/physics_2d_server_wrap_mt.h
index 5bec2f7cbd..50e9ab1005 100644
--- a/servers/physics_2d/physics_2d_server_wrap_mt.h
+++ b/servers/physics_2d/physics_2d_server_wrap_mt.h
@@ -253,6 +253,13 @@ public:
 		return physics_2d_server->body_test_motion(p_body, p_from, p_motion, p_margin, r_result);
 	}
 
+	// this function only works on physics process, errors and returns null otherwise
+	Physics2DDirectBodyState *body_get_direct_state(RID p_body) {
+
+		ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), NULL);
+		return physics_2d_server->body_get_direct_state(p_body);
+	}
+
 	/* JOINT API */
 
 	FUNC3(joint_set_param, RID, JointParam, real_t);
diff --git a/servers/physics_2d_server.cpp b/servers/physics_2d_server.cpp
index b42b85b1be..671c31e6a3 100644
--- a/servers/physics_2d_server.cpp
+++ b/servers/physics_2d_server.cpp
@@ -579,6 +579,8 @@ void Physics2DServer::_bind_methods() {
 
 	ClassDB::bind_method(D_METHOD("body_test_motion", "body", "from", "motion", "margin", "result"), &Physics2DServer::_body_test_motion, DEFVAL(0.08), DEFVAL(Variant()));
 
+	ClassDB::bind_method(D_METHOD("body_get_direct_state", "body"), &Physics2DServer::body_get_direct_state);
+
 	/* JOINT API */
 
 	ClassDB::bind_method(D_METHOD("joint_set_param", "joint", "param", "value"), &Physics2DServer::joint_set_param);
diff --git a/servers/physics_2d_server.h b/servers/physics_2d_server.h
index 34f885db1d..18f4f460b6 100644
--- a/servers/physics_2d_server.h
+++ b/servers/physics_2d_server.h
@@ -468,6 +468,9 @@ public:
 
 	virtual void body_set_pickable(RID p_body, bool p_pickable) = 0;
 
+	// this function only works on physics process, errors and returns null otherwise
+	virtual Physics2DDirectBodyState *body_get_direct_state(RID p_body) = 0;
+
 	struct MotionResult {
 
 		Vector2 motion;
diff --git a/servers/physics_server.cpp b/servers/physics_server.cpp
index 0e54867ee1..6d192886a5 100644
--- a/servers/physics_server.cpp
+++ b/servers/physics_server.cpp
@@ -504,6 +504,8 @@ void PhysicsServer::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("body_set_ray_pickable", "body", "enable"), &PhysicsServer::body_set_ray_pickable);
 	ClassDB::bind_method(D_METHOD("body_is_ray_pickable", "body"), &PhysicsServer::body_is_ray_pickable);
 
+	ClassDB::bind_method(D_METHOD("body_get_direct_state", "body"), &PhysicsServer::body_get_direct_state);
+
 	/* JOINT API */
 
 	BIND_ENUM_CONSTANT(JOINT_PIN);
diff --git a/servers/physics_server.h b/servers/physics_server.h
index 7012caeae6..8cec125646 100644
--- a/servers/physics_server.h
+++ b/servers/physics_server.h
@@ -464,6 +464,9 @@ public:
 	virtual void body_set_ray_pickable(RID p_body, bool p_enable) = 0;
 	virtual bool body_is_ray_pickable(RID p_body) const = 0;
 
+	// this function only works on physics process, errors and returns null otherwise
+	virtual PhysicsDirectBodyState *body_get_direct_state(RID p_body) = 0;
+
 	struct MotionResult {
 
 		Vector3 motion;