1 files changed, 18 insertions, 13 deletions

diff --git a/servers/physics_2d/physics_server_2d_sw.cpp b/servers/physics_2d/physics_server_2d_sw.cpp
index 813e5ddc17..d0a42ca95b 100644
--- a/servers/physics_2d/physics_server_2d_sw.cpp
+++ b/servers/physics_2d/physics_server_2d_sw.cpp
@@ -30,6 +30,7 @@
 
 #include "physics_server_2d_sw.h"
 
+#include "body_direct_state_2d_sw.h"
 #include "broad_phase_2d_bvh.h"
 #include "collision_solver_2d_sw.h"
 #include "core/config/project_settings.h"
@@ -742,20 +743,27 @@ uint32_t PhysicsServer2DSW::body_get_collision_mask(RID p_body) const {
 	return body->get_collision_mask();
 };
 
-void PhysicsServer2DSW::body_set_param(RID p_body, BodyParameter p_param, real_t p_value) {
+void PhysicsServer2DSW::body_set_param(RID p_body, BodyParameter p_param, const Variant &p_value) {
 	Body2DSW *body = body_owner.getornull(p_body);
 	ERR_FAIL_COND(!body);
 
 	body->set_param(p_param, p_value);
 };
 
-real_t PhysicsServer2DSW::body_get_param(RID p_body, BodyParameter p_param) const {
+Variant PhysicsServer2DSW::body_get_param(RID p_body, BodyParameter p_param) const {
 	Body2DSW *body = body_owner.getornull(p_body);
 	ERR_FAIL_COND_V(!body, 0);
 
 	return body->get_param(p_param);
 };
 
+void PhysicsServer2DSW::body_reset_mass_properties(RID p_body) {
+	Body2DSW *body = body_owner.getornull(p_body);
+	ERR_FAIL_COND(!body);
+
+	return body->reset_mass_properties();
+}
+
 void PhysicsServer2DSW::body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) {
 	Body2DSW *body = body_owner.getornull(p_body);
 	ERR_FAIL_COND(!body);
@@ -926,6 +934,12 @@ int PhysicsServer2DSW::body_get_max_contacts_reported(RID p_body) const {
 	return body->get_max_contacts_reported();
 }
 
+void PhysicsServer2DSW::body_set_state_sync_callback(RID p_body, void *p_instance, BodyStateCallback p_callback) {
+	Body2DSW *body = body_owner.getornull(p_body);
+	ERR_FAIL_COND(!body);
+	body->set_state_sync_callback(p_instance, p_callback);
+}
+
 void PhysicsServer2DSW::body_set_force_integration_callback(RID p_body, const Callable &p_callable, const Variant &p_udata) {
 	Body2DSW *body = body_owner.getornull(p_body);
 	ERR_FAIL_COND(!body);
@@ -960,17 +974,13 @@ bool PhysicsServer2DSW::body_test_motion(RID p_body, const Transform2D &p_from,
 PhysicsDirectBodyState2D *PhysicsServer2DSW::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.");
 
-	if (!body_owner.owns(p_body)) {
-		return nullptr;
-	}
-
 	Body2DSW *body = body_owner.getornull(p_body);
 	ERR_FAIL_COND_V(!body, nullptr);
+
 	ERR_FAIL_COND_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();
 }
 
 /* JOINT API */
@@ -1234,10 +1244,8 @@ void PhysicsServer2DSW::set_collision_iterations(int p_iterations) {
 
 void PhysicsServer2DSW::init() {
 	doing_sync = false;
-	last_step = 0.001;
 	iterations = 8; // 8?
 	stepper = memnew(Step2DSW);
-	direct_state = memnew(PhysicsDirectBodyState2DSW);
 };
 
 void PhysicsServer2DSW::step(real_t p_step) {
@@ -1247,8 +1255,6 @@ void PhysicsServer2DSW::step(real_t p_step) {
 
 	_update_shapes();
 
-	last_step = p_step;
-	PhysicsDirectBodyState2DSW::singleton->step = p_step;
 	island_count = 0;
 	active_objects = 0;
 	collision_pairs = 0;
@@ -1320,7 +1326,6 @@ void PhysicsServer2DSW::end_sync() {
 
 void PhysicsServer2DSW::finish() {
 	memdelete(stepper);
-	memdelete(direct_state);
 };
 
 void PhysicsServer2DSW::_update_shapes() {