Merge pull request #49471 from nekomatata/body-state-sync-callback

Clean physics direct body state usage in 2D and 3D physics

9 files changed, 348 insertions, 143 deletions

diff --git a/servers/physics_3d/body_3d_sw.cpp b/servers/physics_3d/body_3d_sw.cpp
index 397a38079b..7f62e4eb85 100644
--- a/servers/physics_3d/body_3d_sw.cpp
+++ b/servers/physics_3d/body_3d_sw.cpp
@@ -29,7 +29,9 @@
 /*************************************************************************/
 
 #include "body_3d_sw.h"
+
 #include "area_3d_sw.h"
+#include "body_direct_state_3d_sw.h"
 #include "space_3d_sw.h"
 
 void Body3DSW::_update_inertia() {
@@ -536,7 +538,7 @@ void Body3DSW::integrate_velocities(real_t p_step) {
 		return;
 	}
 
-	if (fi_callback) {
+	if (fi_callback_data || body_state_callback) {
 		get_space()->body_add_to_state_query_list(&direct_state_query_list);
 	}
 
@@ -593,11 +595,6 @@ void Body3DSW::integrate_velocities(real_t p_step) {
 	_set_inv_transform(get_transform().inverse());
 
 	_update_transform_dependant();
-
-	/*
-	if (fi_callback) {
-		get_space()->body_add_to_state_query_list(&direct_state_query_list);
-	*/
 }
 
 /*
@@ -655,24 +652,23 @@ void Body3DSW::wakeup_neighbours() {
 }
 
 void Body3DSW::call_queries() {
-	if (fi_callback) {
-		PhysicsDirectBodyState3DSW *dbs = PhysicsDirectBodyState3DSW::singleton;
-		dbs->body = this;
-
-		Variant v = dbs;
-
-		Object *obj = fi_callback->callable.get_object();
-		if (!obj) {
+	if (fi_callback_data) {
+		if (!fi_callback_data->callable.get_object()) {
 			set_force_integration_callback(Callable());
 		} else {
-			const Variant *vp[2] = { &v, &fi_callback->udata };
+			Variant direct_state_variant = get_direct_state();
+			const Variant *vp[2] = { &direct_state_variant, &fi_callback_data->udata };
 
 			Callable::CallError ce;
-			int argc = (fi_callback->udata.get_type() == Variant::NIL) ? 1 : 2;
+			int argc = (fi_callback_data->udata.get_type() == Variant::NIL) ? 1 : 2;
 			Variant rv;
-			fi_callback->callable.call(vp, argc, rv, ce);
+			fi_callback_data->callable.call(vp, argc, rv, ce);
 		}
 	}
+
+	if (body_state_callback_instance) {
+		(body_state_callback)(body_state_callback_instance, get_direct_state());
+	}
 }
 
 bool Body3DSW::sleep_test(real_t p_step) {
@@ -692,22 +688,34 @@ bool Body3DSW::sleep_test(real_t p_step) {
 	}
 }
 
+void Body3DSW::set_state_sync_callback(void *p_instance, PhysicsServer3D::BodyStateCallback p_callback) {
+	body_state_callback_instance = p_instance;
+	body_state_callback = p_callback;
+}
+
 void Body3DSW::set_force_integration_callback(const Callable &p_callable, const Variant &p_udata) {
-	if (fi_callback) {
-		memdelete(fi_callback);
-		fi_callback = nullptr;
+	if (p_callable.get_object()) {
+		if (!fi_callback_data) {
+			fi_callback_data = memnew(ForceIntegrationCallbackData);
+		}
+		fi_callback_data->callable = p_callable;
+		fi_callback_data->udata = p_udata;
+	} else if (fi_callback_data) {
+		memdelete(fi_callback_data);
+		fi_callback_data = nullptr;
 	}
+}
 
-	if (p_callable.get_object()) {
-		fi_callback = memnew(ForceIntegrationCallback);
-		fi_callback->callable = p_callable;
-		fi_callback->udata = p_udata;
+PhysicsDirectBodyState3DSW *Body3DSW::get_direct_state() {
+	if (!direct_state) {
+		direct_state = memnew(PhysicsDirectBodyState3DSW);
+		direct_state->body = this;
 	}
+	return direct_state;
 }
 
 Body3DSW::Body3DSW() :
 		CollisionObject3DSW(TYPE_BODY),
-
 		active_list(this),
 		inertia_update_list(this),
 		direct_state_query_list(this) {
@@ -735,17 +743,13 @@ Body3DSW::Body3DSW() :
 	still_time = 0;
 	continuous_cd = false;
 	can_sleep = true;
-	fi_callback = nullptr;
 }
 
 Body3DSW::~Body3DSW() {
-	if (fi_callback) {
-		memdelete(fi_callback);
+	if (fi_callback_data) {
+		memdelete(fi_callback_data);
+	}
+	if (direct_state) {
+		memdelete(direct_state);
 	}
-}
-
-PhysicsDirectBodyState3DSW *PhysicsDirectBodyState3DSW::singleton = nullptr;
-
-PhysicsDirectSpaceState3D *PhysicsDirectBodyState3DSW::get_space_state() {
-	return body->get_space()->get_direct_state();
 }
diff --git a/servers/physics_3d/body_3d_sw.h b/servers/physics_3d/body_3d_sw.h
index 130be2d42f..f58f40652b 100644
--- a/servers/physics_3d/body_3d_sw.h
+++ b/servers/physics_3d/body_3d_sw.h
@@ -28,14 +28,15 @@
 /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
 /*************************************************************************/
 
-#ifndef BODY_SW_H
-#define BODY_SW_H
+#ifndef BODY_3D_SW_H
+#define BODY_3D_SW_H
 
 #include "area_3d_sw.h"
 #include "collision_object_3d_sw.h"
 #include "core/templates/vset.h"
 
 class Constraint3DSW;
+class PhysicsDirectBodyState3DSW;
 
 class Body3DSW : public CollisionObject3DSW {
 	PhysicsServer3D::BodyMode mode;
@@ -113,12 +114,17 @@ class Body3DSW : public CollisionObject3DSW {
 	Vector<Contact> contacts; //no contacts by default
 	int contact_count;
 
-	struct ForceIntegrationCallback {
+	void *body_state_callback_instance = nullptr;
+	PhysicsServer3D::BodyStateCallback body_state_callback = nullptr;
+
+	struct ForceIntegrationCallbackData {
 		Callable callable;
 		Variant udata;
 	};
 
-	ForceIntegrationCallback *fi_callback;
+	ForceIntegrationCallbackData *fi_callback_data = nullptr;
+
+	PhysicsDirectBodyState3DSW *direct_state = nullptr;
 
 	uint64_t island_step;
 
@@ -129,8 +135,11 @@ class Body3DSW : public CollisionObject3DSW {
 	friend class PhysicsDirectBodyState3DSW; // i give up, too many functions to expose
 
 public:
+	void set_state_sync_callback(void *p_instance, PhysicsServer3D::BodyStateCallback p_callback);
 	void set_force_integration_callback(const Callable &p_callable, const Variant &p_udata = Variant());
 
+	PhysicsDirectBodyState3DSW *get_direct_state();
+
 	_FORCE_INLINE_ void add_area(Area3DSW *p_area) {
 		int index = areas.find(AreaCMP(p_area));
 		if (index > -1) {
@@ -349,96 +358,4 @@ void Body3DSW::add_contact(const Vector3 &p_local_pos, const Vector3 &p_local_no
 	c[idx].collider_velocity_at_pos = p_collider_velocity_at_pos;
 }
 
-class PhysicsDirectBodyState3DSW : public PhysicsDirectBodyState3D {
-	GDCLASS(PhysicsDirectBodyState3DSW, PhysicsDirectBodyState3D);
-
-public:
-	static PhysicsDirectBodyState3DSW *singleton;
-	Body3DSW *body;
-	real_t step;
-
-	virtual Vector3 get_total_gravity() const override { return body->gravity; } // get gravity vector working on this body space/area
-	virtual real_t get_total_angular_damp() const override { return body->area_angular_damp; } // get density of this body space/area
-	virtual real_t get_total_linear_damp() const override { return body->area_linear_damp; } // get density of this body space/area
-
-	virtual Vector3 get_center_of_mass() const override { return body->get_center_of_mass(); }
-	virtual Basis get_principal_inertia_axes() const override { return body->get_principal_inertia_axes(); }
-
-	virtual real_t get_inverse_mass() const override { return body->get_inv_mass(); } // get the mass
-	virtual Vector3 get_inverse_inertia() const override { return body->get_inv_inertia(); } // get density of this body space
-	virtual Basis get_inverse_inertia_tensor() const override { return body->get_inv_inertia_tensor(); } // get density of this body space
-
-	virtual void set_linear_velocity(const Vector3 &p_velocity) override { body->set_linear_velocity(p_velocity); }
-	virtual Vector3 get_linear_velocity() const override { return body->get_linear_velocity(); }
-
-	virtual void set_angular_velocity(const Vector3 &p_velocity) override { body->set_angular_velocity(p_velocity); }
-	virtual Vector3 get_angular_velocity() const override { return body->get_angular_velocity(); }
-
-	virtual void set_transform(const Transform3D &p_transform) override { body->set_state(PhysicsServer3D::BODY_STATE_TRANSFORM, p_transform); }
-	virtual Transform3D get_transform() const override { return body->get_transform(); }
-
-	virtual Vector3 get_velocity_at_local_position(const Vector3 &p_position) const override { return body->get_velocity_in_local_point(p_position); }
-
-	virtual void add_central_force(const Vector3 &p_force) override { body->add_central_force(p_force); }
-	virtual void add_force(const Vector3 &p_force, const Vector3 &p_position = Vector3()) override {
-		body->add_force(p_force, p_position);
-	}
-	virtual void add_torque(const Vector3 &p_torque) override { body->add_torque(p_torque); }
-	virtual void apply_central_impulse(const Vector3 &p_impulse) override { body->apply_central_impulse(p_impulse); }
-	virtual void apply_impulse(const Vector3 &p_impulse, const Vector3 &p_position = Vector3()) override {
-		body->apply_impulse(p_impulse, p_position);
-	}
-	virtual void apply_torque_impulse(const Vector3 &p_impulse) override { body->apply_torque_impulse(p_impulse); }
-
-	virtual void set_sleep_state(bool p_sleep) override { body->set_active(!p_sleep); }
-	virtual bool is_sleeping() const override { return !body->is_active(); }
-
-	virtual int get_contact_count() const override { return body->contact_count; }
-
-	virtual Vector3 get_contact_local_position(int p_contact_idx) const override {
-		ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3());
-		return body->contacts[p_contact_idx].local_pos;
-	}
-	virtual Vector3 get_contact_local_normal(int p_contact_idx) const override {
-		ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3());
-		return body->contacts[p_contact_idx].local_normal;
-	}
-	virtual real_t get_contact_impulse(int p_contact_idx) const override {
-		return 0.0f; // Only implemented for bullet
-	}
-	virtual int get_contact_local_shape(int p_contact_idx) const override {
-		ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, -1);
-		return body->contacts[p_contact_idx].local_shape;
-	}
-
-	virtual RID get_contact_collider(int p_contact_idx) const override {
-		ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, RID());
-		return body->contacts[p_contact_idx].collider;
-	}
-	virtual Vector3 get_contact_collider_position(int p_contact_idx) const override {
-		ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3());
-		return body->contacts[p_contact_idx].collider_pos;
-	}
-	virtual ObjectID get_contact_collider_id(int p_contact_idx) const override {
-		ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, ObjectID());
-		return body->contacts[p_contact_idx].collider_instance_id;
-	}
-	virtual int get_contact_collider_shape(int p_contact_idx) const override {
-		ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, 0);
-		return body->contacts[p_contact_idx].collider_shape;
-	}
-	virtual Vector3 get_contact_collider_velocity_at_position(int p_contact_idx) const override {
-		ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3());
-		return body->contacts[p_contact_idx].collider_velocity_at_pos;
-	}
-
-	virtual PhysicsDirectSpaceState3D *get_space_state() override;
-
-	virtual real_t get_step() const override { return step; }
-	PhysicsDirectBodyState3DSW() {
-		singleton = this;
-		body = nullptr;
-	}
-};
-
-#endif // BODY__SW_H
+#endif // BODY_3D_SW_H
diff --git a/servers/physics_3d/body_direct_state_3d_sw.cpp b/servers/physics_3d/body_direct_state_3d_sw.cpp
new file mode 100644
index 0000000000..d197dd288d
--- /dev/null
+++ b/servers/physics_3d/body_direct_state_3d_sw.cpp
@@ -0,0 +1,182 @@
+/*************************************************************************/
+/*  body_direct_state_3d_sw.cpp                                          */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                      https://godotengine.org                          */
+/*************************************************************************/
+/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md).   */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#include "body_direct_state_3d_sw.h"
+
+#include "body_3d_sw.h"
+#include "space_3d_sw.h"
+
+Vector3 PhysicsDirectBodyState3DSW::get_total_gravity() const {
+	return body->gravity;
+}
+
+real_t PhysicsDirectBodyState3DSW::get_total_angular_damp() const {
+	return body->area_angular_damp;
+}
+
+real_t PhysicsDirectBodyState3DSW::get_total_linear_damp() const {
+	return body->area_linear_damp;
+}
+
+Vector3 PhysicsDirectBodyState3DSW::get_center_of_mass() const {
+	return body->get_center_of_mass();
+}
+
+Basis PhysicsDirectBodyState3DSW::get_principal_inertia_axes() const {
+	return body->get_principal_inertia_axes();
+}
+
+real_t PhysicsDirectBodyState3DSW::get_inverse_mass() const {
+	return body->get_inv_mass();
+}
+
+Vector3 PhysicsDirectBodyState3DSW::get_inverse_inertia() const {
+	return body->get_inv_inertia();
+}
+
+Basis PhysicsDirectBodyState3DSW::get_inverse_inertia_tensor() const {
+	return body->get_inv_inertia_tensor();
+}
+
+void PhysicsDirectBodyState3DSW::set_linear_velocity(const Vector3 &p_velocity) {
+	body->set_linear_velocity(p_velocity);
+}
+
+Vector3 PhysicsDirectBodyState3DSW::get_linear_velocity() const {
+	return body->get_linear_velocity();
+}
+
+void PhysicsDirectBodyState3DSW::set_angular_velocity(const Vector3 &p_velocity) {
+	body->set_angular_velocity(p_velocity);
+}
+
+Vector3 PhysicsDirectBodyState3DSW::get_angular_velocity() const {
+	return body->get_angular_velocity();
+}
+
+void PhysicsDirectBodyState3DSW::set_transform(const Transform3D &p_transform) {
+	body->set_state(PhysicsServer3D::BODY_STATE_TRANSFORM, p_transform);
+}
+
+Transform3D PhysicsDirectBodyState3DSW::get_transform() const {
+	return body->get_transform();
+}
+
+Vector3 PhysicsDirectBodyState3DSW::get_velocity_at_local_position(const Vector3 &p_position) const {
+	return body->get_velocity_in_local_point(p_position);
+}
+
+void PhysicsDirectBodyState3DSW::add_central_force(const Vector3 &p_force) {
+	body->add_central_force(p_force);
+}
+
+void PhysicsDirectBodyState3DSW::add_force(const Vector3 &p_force, const Vector3 &p_position) {
+	body->add_force(p_force, p_position);
+}
+
+void PhysicsDirectBodyState3DSW::add_torque(const Vector3 &p_torque) {
+	body->add_torque(p_torque);
+}
+
+void PhysicsDirectBodyState3DSW::apply_central_impulse(const Vector3 &p_impulse) {
+	body->apply_central_impulse(p_impulse);
+}
+
+void PhysicsDirectBodyState3DSW::apply_impulse(const Vector3 &p_impulse, const Vector3 &p_position) {
+	body->apply_impulse(p_impulse, p_position);
+}
+
+void PhysicsDirectBodyState3DSW::apply_torque_impulse(const Vector3 &p_impulse) {
+	body->apply_torque_impulse(p_impulse);
+}
+
+void PhysicsDirectBodyState3DSW::set_sleep_state(bool p_sleep) {
+	body->set_active(!p_sleep);
+}
+
+bool PhysicsDirectBodyState3DSW::is_sleeping() const {
+	return !body->is_active();
+}
+
+int PhysicsDirectBodyState3DSW::get_contact_count() const {
+	return body->contact_count;
+}
+
+Vector3 PhysicsDirectBodyState3DSW::get_contact_local_position(int p_contact_idx) const {
+	ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3());
+	return body->contacts[p_contact_idx].local_pos;
+}
+
+Vector3 PhysicsDirectBodyState3DSW::get_contact_local_normal(int p_contact_idx) const {
+	ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3());
+	return body->contacts[p_contact_idx].local_normal;
+}
+
+real_t PhysicsDirectBodyState3DSW::get_contact_impulse(int p_contact_idx) const {
+	return 0.0f; // Only implemented for bullet
+}
+
+int PhysicsDirectBodyState3DSW::get_contact_local_shape(int p_contact_idx) const {
+	ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, -1);
+	return body->contacts[p_contact_idx].local_shape;
+}
+
+RID PhysicsDirectBodyState3DSW::get_contact_collider(int p_contact_idx) const {
+	ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, RID());
+	return body->contacts[p_contact_idx].collider;
+}
+
+Vector3 PhysicsDirectBodyState3DSW::get_contact_collider_position(int p_contact_idx) const {
+	ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3());
+	return body->contacts[p_contact_idx].collider_pos;
+}
+
+ObjectID PhysicsDirectBodyState3DSW::get_contact_collider_id(int p_contact_idx) const {
+	ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, ObjectID());
+	return body->contacts[p_contact_idx].collider_instance_id;
+}
+
+int PhysicsDirectBodyState3DSW::get_contact_collider_shape(int p_contact_idx) const {
+	ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, 0);
+	return body->contacts[p_contact_idx].collider_shape;
+}
+
+Vector3 PhysicsDirectBodyState3DSW::get_contact_collider_velocity_at_position(int p_contact_idx) const {
+	ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3());
+	return body->contacts[p_contact_idx].collider_velocity_at_pos;
+}
+
+PhysicsDirectSpaceState3D *PhysicsDirectBodyState3DSW::get_space_state() {
+	return body->get_space()->get_direct_state();
+}
+
+real_t PhysicsDirectBodyState3DSW::get_step() const {
+	return body->get_space()->get_last_step();
+}
diff --git a/servers/physics_3d/body_direct_state_3d_sw.h b/servers/physics_3d/body_direct_state_3d_sw.h
new file mode 100644
index 0000000000..5132376715
--- /dev/null
+++ b/servers/physics_3d/body_direct_state_3d_sw.h
@@ -0,0 +1,94 @@
+/*************************************************************************/
+/*  body_direct_state_3d_sw.h                                            */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                      https://godotengine.org                          */
+/*************************************************************************/
+/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md).   */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#ifndef BODY_DIRECT_STATE_3D_SW_H
+#define BODY_DIRECT_STATE_3D_SW_H
+
+#include "servers/physics_server_3d.h"
+
+class Body3DSW;
+
+class PhysicsDirectBodyState3DSW : public PhysicsDirectBodyState3D {
+	GDCLASS(PhysicsDirectBodyState3DSW, PhysicsDirectBodyState3D);
+
+public:
+	Body3DSW *body = nullptr;
+
+	virtual Vector3 get_total_gravity() const override;
+	virtual real_t get_total_angular_damp() const override;
+	virtual real_t get_total_linear_damp() const override;
+
+	virtual Vector3 get_center_of_mass() const override;
+	virtual Basis get_principal_inertia_axes() const override;
+
+	virtual real_t get_inverse_mass() const override;
+	virtual Vector3 get_inverse_inertia() const override;
+	virtual Basis get_inverse_inertia_tensor() const override;
+
+	virtual void set_linear_velocity(const Vector3 &p_velocity) override;
+	virtual Vector3 get_linear_velocity() const override;
+
+	virtual void set_angular_velocity(const Vector3 &p_velocity) override;
+	virtual Vector3 get_angular_velocity() const override;
+
+	virtual void set_transform(const Transform3D &p_transform) override;
+	virtual Transform3D get_transform() const override;
+
+	virtual Vector3 get_velocity_at_local_position(const Vector3 &p_position) const override;
+
+	virtual void add_central_force(const Vector3 &p_force) override;
+	virtual void add_force(const Vector3 &p_force, const Vector3 &p_position = Vector3()) override;
+	virtual void add_torque(const Vector3 &p_torque) override;
+	virtual void apply_central_impulse(const Vector3 &p_impulse) override;
+	virtual void apply_impulse(const Vector3 &p_impulse, const Vector3 &p_position = Vector3()) override;
+	virtual void apply_torque_impulse(const Vector3 &p_impulse) override;
+
+	virtual void set_sleep_state(bool p_sleep) override;
+	virtual bool is_sleeping() const override;
+
+	virtual int get_contact_count() const override;
+
+	virtual Vector3 get_contact_local_position(int p_contact_idx) const override;
+	virtual Vector3 get_contact_local_normal(int p_contact_idx) const override;
+	virtual real_t get_contact_impulse(int p_contact_idx) const override;
+	virtual int get_contact_local_shape(int p_contact_idx) const override;
+
+	virtual RID get_contact_collider(int p_contact_idx) const override;
+	virtual Vector3 get_contact_collider_position(int p_contact_idx) const override;
+	virtual ObjectID get_contact_collider_id(int p_contact_idx) const override;
+	virtual int get_contact_collider_shape(int p_contact_idx) const override;
+	virtual Vector3 get_contact_collider_velocity_at_position(int p_contact_idx) const override;
+
+	virtual PhysicsDirectSpaceState3D *get_space_state() override;
+
+	virtual real_t get_step() const override;
+};
+
+#endif // BODY_DIRECT_STATE_3D_SW_H
diff --git a/servers/physics_3d/physics_server_3d_sw.cpp b/servers/physics_3d/physics_server_3d_sw.cpp
index fbc6f7eee8..a214e80c6c 100644
--- a/servers/physics_3d/physics_server_3d_sw.cpp
+++ b/servers/physics_3d/physics_server_3d_sw.cpp
@@ -30,6 +30,7 @@
 
 #include "physics_server_3d_sw.h"
 
+#include "body_direct_state_3d_sw.h"
 #include "broad_phase_3d_bvh.h"
 #include "core/debugger/engine_debugger.h"
 #include "core/os/os.h"
@@ -842,6 +843,12 @@ int PhysicsServer3DSW::body_get_max_contacts_reported(RID p_body) const {
 	return body->get_max_contacts_reported();
 }
 
+void PhysicsServer3DSW::body_set_state_sync_callback(RID p_body, void *p_instance, BodyStateCallback p_callback) {
+	Body3DSW *body = body_owner.getornull(p_body);
+	ERR_FAIL_COND(!body);
+	body->set_state_sync_callback(p_instance, p_callback);
+}
+
 void PhysicsServer3DSW::body_set_force_integration_callback(RID p_body, const Callable &p_callable, const Variant &p_udata) {
 	Body3DSW *body = body_owner.getornull(p_body);
 	ERR_FAIL_COND(!body);
@@ -869,11 +876,12 @@ PhysicsDirectBodyState3D *PhysicsServer3DSW::body_get_direct_state(RID p_body) {
 	ERR_FAIL_COND_V_MSG((using_threads && !doing_sync), nullptr, "Body state is inaccessible right now, wait for iteration or physics process notification.");
 
 	Body3DSW *body = body_owner.getornull(p_body);
-	ERR_FAIL_COND_V(!body, nullptr);
+	ERR_FAIL_NULL_V(body, nullptr);
+
+	ERR_FAIL_NULL_V(body->get_space(), nullptr);
 	ERR_FAIL_COND_V_MSG(body->get_space()->is_locked(), nullptr, "Body state is inaccessible right now, wait for iteration or physics process notification.");
 
-	direct_state->body = body;
-	return direct_state;
+	return body->get_direct_state();
 }
 
 /* SOFT BODY */
@@ -1578,10 +1586,8 @@ void PhysicsServer3DSW::set_collision_iterations(int p_iterations) {
 };
 
 void PhysicsServer3DSW::init() {
-	last_step = 0.001;
 	iterations = 8; // 8?
 	stepper = memnew(Step3DSW);
-	direct_state = memnew(PhysicsDirectBodyState3DSW);
 };
 
 void PhysicsServer3DSW::step(real_t p_step) {
@@ -1593,9 +1599,6 @@ void PhysicsServer3DSW::step(real_t p_step) {
 
 	_update_shapes();
 
-	last_step = p_step;
-	PhysicsDirectBodyState3DSW::singleton->step = p_step;
-
 	island_count = 0;
 	active_objects = 0;
 	collision_pairs = 0;
@@ -1671,7 +1674,6 @@ void PhysicsServer3DSW::end_sync() {
 
 void PhysicsServer3DSW::finish() {
 	memdelete(stepper);
-	memdelete(direct_state);
 };
 
 int PhysicsServer3DSW::get_process_info(ProcessInfo p_info) {
diff --git a/servers/physics_3d/physics_server_3d_sw.h b/servers/physics_3d/physics_server_3d_sw.h
index a18593c90c..f283f83112 100644
--- a/servers/physics_3d/physics_server_3d_sw.h
+++ b/servers/physics_3d/physics_server_3d_sw.h
@@ -44,7 +44,6 @@ class PhysicsServer3DSW : public PhysicsServer3D {
 	friend class PhysicsDirectSpaceState3DSW;
 	bool active;
 	int iterations;
-	real_t last_step;
 
 	int island_count;
 	int active_objects;
@@ -57,8 +56,6 @@ class PhysicsServer3DSW : public PhysicsServer3D {
 	Step3DSW *stepper;
 	Set<const Space3DSW *> active_spaces;
 
-	PhysicsDirectBodyState3DSW *direct_state;
-
 	mutable RID_PtrOwner<Shape3DSW, true> shape_owner;
 	mutable RID_PtrOwner<Space3DSW, true> space_owner;
 	mutable RID_PtrOwner<Area3DSW, true> area_owner;
@@ -238,6 +235,7 @@ public:
 	virtual void body_set_max_contacts_reported(RID p_body, int p_contacts) override;
 	virtual int body_get_max_contacts_reported(RID p_body) const override;
 
+	virtual void body_set_state_sync_callback(RID p_body, void *p_instance, BodyStateCallback p_callback) override;
 	virtual void body_set_force_integration_callback(RID p_body, const Callable &p_callable, const Variant &p_udata = Variant()) override;
 
 	virtual void body_set_ray_pickable(RID p_body, bool p_enable) override;
diff --git a/servers/physics_3d/physics_server_3d_wrap_mt.h b/servers/physics_3d/physics_server_3d_wrap_mt.h
index 674c52d4a3..8865bd4bd2 100644
--- a/servers/physics_3d/physics_server_3d_wrap_mt.h
+++ b/servers/physics_3d/physics_server_3d_wrap_mt.h
@@ -246,6 +246,7 @@ public:
 	FUNC2(body_set_omit_force_integration, RID, bool);
 	FUNC1RC(bool, body_is_omitting_force_integration, RID);
 
+	FUNC3(body_set_state_sync_callback, RID, void *, BodyStateCallback);
 	FUNC3(body_set_force_integration_callback, RID, const Callable &, const Variant &);
 
 	FUNC2(body_set_ray_pickable, RID, bool);
diff --git a/servers/physics_3d/space_3d_sw.h b/servers/physics_3d/space_3d_sw.h
index 1c3d1cf9f6..eff494a7bc 100644
--- a/servers/physics_3d/space_3d_sw.h
+++ b/servers/physics_3d/space_3d_sw.h
@@ -112,6 +112,8 @@ private:
 
 	bool locked;
 
+	real_t last_step = 0.001;
+
 	int island_count;
 	int active_objects;
 	int collision_pairs;
@@ -174,6 +176,9 @@ public:
 	void lock();
 	void unlock();
 
+	real_t get_last_step() const { return last_step; }
+	void set_last_step(real_t p_step) { last_step = p_step; }
+
 	void set_param(PhysicsServer3D::SpaceParameter p_param, real_t p_value);
 	real_t get_param(PhysicsServer3D::SpaceParameter p_param) const;
 
diff --git a/servers/physics_3d/step_3d_sw.cpp b/servers/physics_3d/step_3d_sw.cpp
index ba18bac611..d0604b0aa0 100644
--- a/servers/physics_3d/step_3d_sw.cpp
+++ b/servers/physics_3d/step_3d_sw.cpp
@@ -185,6 +185,8 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
 
 	p_space->setup(); //update inertias, etc
 
+	p_space->set_last_step(p_delta);
+
 	iterations = p_iterations;
 	delta = p_delta;