Added new API to get body direct state

2 files changed, 18 insertions, 0 deletions

diff --git a/servers/physics/physics_server_sw.cpp b/servers/physics/physics_server_sw.cpp
index a0c9f9cd42..8cc4929a56 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 99ba302acd..9d42761c69 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);