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

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

doc/classes/PhysicsServer2D.xml

@@ -489,6 +489,9 @@
 			<argument index="2" name="userdata" type="Variant" default="null" />
 			<description>
 				Sets the function used to calculate physics for an object, if that object allows it (see [method body_set_omit_force_integration]).
+				The force integration function takes 2 arguments:
+				[code]state:[/code] [PhysicsDirectBodyState2D] used to retrieve and modify the body's state.
+				[code]userdata:[/code] Optional user data, if it was passed when calling [code]body_set_force_integration_callback[/code].
 			</description>
 		</method>
 		<method name="body_set_max_contacts_reported">

doc/classes/PhysicsServer3D.xml

@@ -478,6 +478,9 @@
 			<argument index="2" name="userdata" type="Variant" default="null" />
 			<description>
 				Sets the function used to calculate physics for an object, if that object allows it (see [method body_set_omit_force_integration]).
+				The force integration function takes 2 arguments:
+				[code]state:[/code] [PhysicsDirectBodyState3D] used to retrieve and modify the body's state.
+				[code]userdata:[/code] Optional user data, if it was passed when calling [code]body_set_force_integration_callback[/code].
 			</description>
 		</method>
 		<method name="body_set_max_contacts_reported">

scene/2d/physics_body_2d.cpp

@@ -259,15 +259,17 @@ bool StaticBody2D::is_sync_to_physics_enabled() const {
 	return sync_to_physics;
 }
 
-void StaticBody2D::_direct_state_changed(Object *p_state) {
+void StaticBody2D::_body_state_changed_callback(void *p_instance, PhysicsDirectBodyState2D *p_state) {
+	StaticBody2D *body = (StaticBody2D *)p_instance;
+	body->_body_state_changed(p_state);
+}
+
+void StaticBody2D::_body_state_changed(PhysicsDirectBodyState2D *p_state) {
 	if (!sync_to_physics) {
 		return;
 	}
 
-	PhysicsDirectBodyState2D *state = Object::cast_to<PhysicsDirectBodyState2D>(p_state);
-	ERR_FAIL_NULL_MSG(state, "Method '_direct_state_changed' must receive a valid PhysicsDirectBodyState2D object as argument");
-
-	last_valid_transform = state->get_transform();
+	last_valid_transform = p_state->get_transform();
 	set_notify_local_transform(false);
 	set_global_transform(last_valid_transform);
 	set_notify_local_transform(true);
@@ -379,11 +381,11 @@ void StaticBody2D::_update_kinematic_motion() {
 #endif
 
 	if (kinematic_motion && sync_to_physics) {
-		PhysicsServer2D::get_singleton()->body_set_force_integration_callback(get_rid(), callable_mp(this, &StaticBody2D::_direct_state_changed));
+		PhysicsServer2D::get_singleton()->body_set_state_sync_callback(get_rid(), this, _body_state_changed_callback);
 		set_only_update_transform_changes(true);
 		set_notify_local_transform(true);
 	} else {
-		PhysicsServer2D::get_singleton()->body_set_force_integration_callback(get_rid(), Callable());
+		PhysicsServer2D::get_singleton()->body_set_state_sync_callback(get_rid(), nullptr, nullptr);
 		set_only_update_transform_changes(false);
 		set_notify_local_transform(false);
 	}
@@ -511,26 +513,26 @@ struct _RigidBody2DInOut {
 	int local_shape = 0;
 };
 
-void RigidBody2D::_direct_state_changed(Object *p_state) {
-#ifdef DEBUG_ENABLED
-	state = Object::cast_to<PhysicsDirectBodyState2D>(p_state);
-	ERR_FAIL_NULL_MSG(state, "Method '_direct_state_changed' must receive a valid PhysicsDirectBodyState2D object as argument");
-#else
-	state = (PhysicsDirectBodyState2D *)p_state; //trust it
-#endif
+void RigidBody2D::_body_state_changed_callback(void *p_instance, PhysicsDirectBodyState2D *p_state) {
+	RigidBody2D *body = (RigidBody2D *)p_instance;
+	body->_body_state_changed(p_state);
+}
 
+void RigidBody2D::_body_state_changed(PhysicsDirectBodyState2D *p_state) {
 	set_block_transform_notify(true); // don't want notify (would feedback loop)
 	if (mode != MODE_KINEMATIC) {
-		set_global_transform(state->get_transform());
+		set_global_transform(p_state->get_transform());
 	}
-	linear_velocity = state->get_linear_velocity();
-	angular_velocity = state->get_angular_velocity();
-	if (sleeping != state->is_sleeping()) {
-		sleeping = state->is_sleeping();
+
+	linear_velocity = p_state->get_linear_velocity();
+	angular_velocity = p_state->get_angular_velocity();
+
+	if (sleeping != p_state->is_sleeping()) {
+		sleeping = p_state->is_sleeping();
 		emit_signal(SceneStringNames::get_singleton()->sleeping_state_changed);
 	}
 
-	GDVIRTUAL_CALL(_integrate_forces, state);
+	GDVIRTUAL_CALL(_integrate_forces, p_state);
 
 	set_block_transform_notify(false); // want it back
 
@@ -546,20 +548,18 @@ void RigidBody2D::_direct_state_changed(Object *p_state) {
 			}
 		}
 
-		_RigidBody2DInOut *toadd = (_RigidBody2DInOut *)alloca(state->get_contact_count() * sizeof(_RigidBody2DInOut));
+		_RigidBody2DInOut *toadd = (_RigidBody2DInOut *)alloca(p_state->get_contact_count() * sizeof(_RigidBody2DInOut));
 		int toadd_count = 0; //state->get_contact_count();
 		RigidBody2D_RemoveAction *toremove = (RigidBody2D_RemoveAction *)alloca(rc * sizeof(RigidBody2D_RemoveAction));
 		int toremove_count = 0;
 
 		//put the ones to add
 
-		for (int i = 0; i < state->get_contact_count(); i++) {
-			RID rid = state->get_contact_collider(i);
-			ObjectID obj = state->get_contact_collider_id(i);
-			int local_shape = state->get_contact_local_shape(i);
-			int shape = state->get_contact_collider_shape(i);
-
-			//bool found=false;
+		for (int i = 0; i < p_state->get_contact_count(); i++) {
+			RID rid = p_state->get_contact_collider(i);
+			ObjectID obj = p_state->get_contact_collider_id(i);
+			int local_shape = p_state->get_contact_local_shape(i);
+			int shape = p_state->get_contact_collider_shape(i);
 
 			Map<ObjectID, BodyState>::Element *E = contact_monitor->body_map.find(obj);
 			if (!E) {
@@ -612,8 +612,6 @@ void RigidBody2D::_direct_state_changed(Object *p_state) {
 
 		contact_monitor->locked = false;
 	}
-
-	state = nullptr;
 }
 
 void RigidBody2D::set_mode(Mode p_mode) {
@@ -709,25 +707,15 @@ real_t RigidBody2D::get_angular_damp() const {
 }
 
 void RigidBody2D::set_axis_velocity(const Vector2 &p_axis) {
-	Vector2 v = state ? state->get_linear_velocity() : linear_velocity;
 	Vector2 axis = p_axis.normalized();
-	v -= axis * axis.dot(v);
-	v += p_axis;
-	if (state) {
-		set_linear_velocity(v);
-	} else {
-		PhysicsServer2D::get_singleton()->body_set_axis_velocity(get_rid(), p_axis);
-		linear_velocity = v;
-	}
+	linear_velocity -= axis * axis.dot(linear_velocity);
+	linear_velocity += p_axis;
+	PhysicsServer2D::get_singleton()->body_set_state(get_rid(), PhysicsServer2D::BODY_STATE_LINEAR_VELOCITY, linear_velocity);
 }
 
 void RigidBody2D::set_linear_velocity(const Vector2 &p_velocity) {
 	linear_velocity = p_velocity;
-	if (state) {
-		state->set_linear_velocity(linear_velocity);
-	} else {
-		PhysicsServer2D::get_singleton()->body_set_state(get_rid(), PhysicsServer2D::BODY_STATE_LINEAR_VELOCITY, linear_velocity);
-	}
+	PhysicsServer2D::get_singleton()->body_set_state(get_rid(), PhysicsServer2D::BODY_STATE_LINEAR_VELOCITY, linear_velocity);
 }
 
 Vector2 RigidBody2D::get_linear_velocity() const {
@@ -736,11 +724,7 @@ Vector2 RigidBody2D::get_linear_velocity() const {
 
 void RigidBody2D::set_angular_velocity(real_t p_velocity) {
 	angular_velocity = p_velocity;
-	if (state) {
-		state->set_angular_velocity(angular_velocity);
-	} else {
-		PhysicsServer2D::get_singleton()->body_set_state(get_rid(), PhysicsServer2D::BODY_STATE_ANGULAR_VELOCITY, angular_velocity);
-	}
+	PhysicsServer2D::get_singleton()->body_set_state(get_rid(), PhysicsServer2D::BODY_STATE_ANGULAR_VELOCITY, angular_velocity);
 }
 
 real_t RigidBody2D::get_angular_velocity() const {
@@ -1019,7 +1003,7 @@ void RigidBody2D::_bind_methods() {
 
 RigidBody2D::RigidBody2D() :
 		PhysicsBody2D(PhysicsServer2D::BODY_MODE_DYNAMIC) {
-	PhysicsServer2D::get_singleton()->body_set_force_integration_callback(get_rid(), callable_mp(this, &RigidBody2D::_direct_state_changed));
+	PhysicsServer2D::get_singleton()->body_set_state_sync_callback(get_rid(), this, _body_state_changed_callback);
 }
 
 RigidBody2D::~RigidBody2D() {
@@ -1057,7 +1041,7 @@ bool CharacterBody2D::move_and_slide() {
 			excluded = (moving_platform_wall_layers & platform_layer) == 0;
 		}
 		if (!excluded) {
-			// This approach makes sure there is less delay between the actual body velocity and the one we saved.
+			//this approach makes sure there is less delay between the actual body velocity and the one we saved
 			PhysicsDirectBodyState2D *bs = PhysicsServer2D::get_singleton()->body_get_direct_state(platform_rid);
 			if (bs) {
 				Transform2D gt = get_global_transform();

scene/2d/physics_body_2d.h

@@ -73,7 +73,8 @@ class StaticBody2D : public PhysicsBody2D {
 
 	Transform2D last_valid_transform;
 
-	void _direct_state_changed(Object *p_state);
+	static void _body_state_changed_callback(void *p_instance, PhysicsDirectBodyState2D *p_state);
+	void _body_state_changed(PhysicsDirectBodyState2D *p_state);
 
 protected:
 	void _notification(int p_what);
@@ -124,7 +125,6 @@ public:
 
 private:
 	bool can_sleep = true;
-	PhysicsDirectBodyState2D *state = nullptr;
 	Mode mode = MODE_DYNAMIC;
 
 	real_t mass = 1.0;
@@ -183,7 +183,9 @@ private:
 	void _body_exit_tree(ObjectID p_id);
 
 	void _body_inout(int p_status, const RID &p_body, ObjectID p_instance, int p_body_shape, int p_local_shape);
-	void _direct_state_changed(Object *p_state);
+
+	static void _body_state_changed_callback(void *p_instance, PhysicsDirectBodyState2D *p_state);
+	void _body_state_changed(PhysicsDirectBodyState2D *p_state);
 
 protected:
 	void _notification(int p_what);

scene/3d/physics_body_3d.cpp

@@ -283,18 +283,20 @@ bool StaticBody3D::is_sync_to_physics_enabled() const {
 	return sync_to_physics;
 }
 
-void StaticBody3D::_direct_state_changed(Object *p_state) {
-	PhysicsDirectBodyState3D *state = Object::cast_to<PhysicsDirectBodyState3D>(p_state);
-	ERR_FAIL_NULL_MSG(state, "Method '_direct_state_changed' must receive a valid PhysicsDirectBodyState3D object as argument");
+void StaticBody3D::_body_state_changed_callback(void *p_instance, PhysicsDirectBodyState3D *p_state) {
+	StaticBody3D *body = (StaticBody3D *)p_instance;
+	body->_body_state_changed(p_state);
+}
 
-	linear_velocity = state->get_linear_velocity();
-	angular_velocity = state->get_angular_velocity();
+void StaticBody3D::_body_state_changed(PhysicsDirectBodyState3D *p_state) {
+	linear_velocity = p_state->get_linear_velocity();
+	angular_velocity = p_state->get_angular_velocity();
 
 	if (!sync_to_physics) {
 		return;
 	}
 
-	last_valid_transform = state->get_transform();
+	last_valid_transform = p_state->get_transform();
 	set_notify_local_transform(false);
 	set_global_transform(last_valid_transform);
 	set_notify_local_transform(true);
@@ -458,13 +460,13 @@ void StaticBody3D::_update_kinematic_motion() {
 
 	bool needs_physics_process = false;
 	if (kinematic_motion) {
-		PhysicsServer3D::get_singleton()->body_set_force_integration_callback(get_rid(), callable_mp(this, &StaticBody3D::_direct_state_changed));
+		PhysicsServer3D::get_singleton()->body_set_state_sync_callback(get_rid(), this, &StaticBody3D::_body_state_changed_callback);
 
 		if (!constant_angular_velocity.is_equal_approx(Vector3()) || !constant_linear_velocity.is_equal_approx(Vector3())) {
 			needs_physics_process = true;
 		}
 	} else {
-		PhysicsServer3D::get_singleton()->body_set_force_integration_callback(get_rid(), Callable());
+		PhysicsServer3D::get_singleton()->body_set_state_sync_callback(get_rid(), nullptr, nullptr);
 	}
 
 	set_physics_process_internal(needs_physics_process);
@@ -582,25 +584,26 @@ struct _RigidBodyInOut {
 	int local_shape = 0;
 };
 
-void RigidBody3D::_direct_state_changed(Object *p_state) {
-#ifdef DEBUG_ENABLED
-	state = Object::cast_to<PhysicsDirectBodyState3D>(p_state);
-	ERR_FAIL_NULL_MSG(state, "Method '_direct_state_changed' must receive a valid PhysicsDirectBodyState3D object as argument");
-#else
-	state = (PhysicsDirectBodyState3D *)p_state; //trust it
-#endif
+void RigidBody3D::_body_state_changed_callback(void *p_instance, PhysicsDirectBodyState3D *p_state) {
+	RigidBody3D *body = (RigidBody3D *)p_instance;
+	body->_body_state_changed(p_state);
+}
 
+void RigidBody3D::_body_state_changed(PhysicsDirectBodyState3D *p_state) {
 	set_ignore_transform_notification(true);
-	set_global_transform(state->get_transform());
-	linear_velocity = state->get_linear_velocity();
-	angular_velocity = state->get_angular_velocity();
-	inverse_inertia_tensor = state->get_inverse_inertia_tensor();
-	if (sleeping != state->is_sleeping()) {
-		sleeping = state->is_sleeping();
+	set_global_transform(p_state->get_transform());
+
+	linear_velocity = p_state->get_linear_velocity();
+	angular_velocity = p_state->get_angular_velocity();
+
+	inverse_inertia_tensor = p_state->get_inverse_inertia_tensor();
+
+	if (sleeping != p_state->is_sleeping()) {
+		sleeping = p_state->is_sleeping();
 		emit_signal(SceneStringNames::get_singleton()->sleeping_state_changed);
 	}
 
-	GDVIRTUAL_CALL(_integrate_forces, state);
+	GDVIRTUAL_CALL(_integrate_forces, p_state);
 
 	set_ignore_transform_notification(false);
 	_on_transform_changed();
@@ -617,18 +620,18 @@ void RigidBody3D::_direct_state_changed(Object *p_state) {
 			}
 		}
 
-		_RigidBodyInOut *toadd = (_RigidBodyInOut *)alloca(state->get_contact_count() * sizeof(_RigidBodyInOut));
+		_RigidBodyInOut *toadd = (_RigidBodyInOut *)alloca(p_state->get_contact_count() * sizeof(_RigidBodyInOut));
 		int toadd_count = 0; //state->get_contact_count();
 		RigidBody3D_RemoveAction *toremove = (RigidBody3D_RemoveAction *)alloca(rc * sizeof(RigidBody3D_RemoveAction));
 		int toremove_count = 0;
 
 		//put the ones to add
 
-		for (int i = 0; i < state->get_contact_count(); i++) {
-			RID rid = state->get_contact_collider(i);
-			ObjectID obj = state->get_contact_collider_id(i);
-			int local_shape = state->get_contact_local_shape(i);
-			int shape = state->get_contact_collider_shape(i);
+		for (int i = 0; i < p_state->get_contact_count(); i++) {
+			RID rid = p_state->get_contact_collider(i);
+			ObjectID obj = p_state->get_contact_collider_id(i);
+			int local_shape = p_state->get_contact_local_shape(i);
+			int shape = p_state->get_contact_collider_shape(i);
 
 			//bool found=false;
 
@@ -683,8 +686,6 @@ void RigidBody3D::_direct_state_changed(Object *p_state) {
 
 		contact_monitor->locked = false;
 	}
-
-	state = nullptr;
 }
 
 void RigidBody3D::_notification(int p_what) {
@@ -787,25 +788,15 @@ real_t RigidBody3D::get_angular_damp() const {
 }
 
 void RigidBody3D::set_axis_velocity(const Vector3 &p_axis) {
-	Vector3 v = state ? state->get_linear_velocity() : linear_velocity;
 	Vector3 axis = p_axis.normalized();
-	v -= axis * axis.dot(v);
-	v += p_axis;
-	if (state) {
-		set_linear_velocity(v);
-	} else {
-		PhysicsServer3D::get_singleton()->body_set_axis_velocity(get_rid(), p_axis);
-		linear_velocity = v;
-	}
+	linear_velocity -= axis * axis.dot(linear_velocity);
+	linear_velocity += p_axis;
+	PhysicsServer3D::get_singleton()->body_set_state(get_rid(), PhysicsServer3D::BODY_STATE_LINEAR_VELOCITY, linear_velocity);
 }
 
 void RigidBody3D::set_linear_velocity(const Vector3 &p_velocity) {
 	linear_velocity = p_velocity;
-	if (state) {
-		state->set_linear_velocity(linear_velocity);
-	} else {
-		PhysicsServer3D::get_singleton()->body_set_state(get_rid(), PhysicsServer3D::BODY_STATE_LINEAR_VELOCITY, linear_velocity);
-	}
+	PhysicsServer3D::get_singleton()->body_set_state(get_rid(), PhysicsServer3D::BODY_STATE_LINEAR_VELOCITY, linear_velocity);
 }
 
 Vector3 RigidBody3D::get_linear_velocity() const {
@@ -814,11 +805,7 @@ Vector3 RigidBody3D::get_linear_velocity() const {
 
 void RigidBody3D::set_angular_velocity(const Vector3 &p_velocity) {
 	angular_velocity = p_velocity;
-	if (state) {
-		state->set_angular_velocity(angular_velocity);
-	} else {
-		PhysicsServer3D::get_singleton()->body_set_state(get_rid(), PhysicsServer3D::BODY_STATE_ANGULAR_VELOCITY, angular_velocity);
-	}
+	PhysicsServer3D::get_singleton()->body_set_state(get_rid(), PhysicsServer3D::BODY_STATE_ANGULAR_VELOCITY, angular_velocity);
 }
 
 Vector3 RigidBody3D::get_angular_velocity() const {
@@ -1055,7 +1042,7 @@ void RigidBody3D::_bind_methods() {
 
 RigidBody3D::RigidBody3D() :
 		PhysicsBody3D(PhysicsServer3D::BODY_MODE_DYNAMIC) {
-	PhysicsServer3D::get_singleton()->body_set_force_integration_callback(get_rid(), callable_mp(this, &RigidBody3D::_direct_state_changed));
+	PhysicsServer3D::get_singleton()->body_set_state_sync_callback(get_rid(), this, &RigidBody3D::_body_state_changed_callback);
 }
 
 RigidBody3D::~RigidBody3D() {
@@ -2252,23 +2239,19 @@ void PhysicalBone3D::_notification(int p_what) {
 	}
 }
 
-void PhysicalBone3D::_direct_state_changed(Object *p_state) {
+void PhysicalBone3D::_body_state_changed_callback(void *p_instance, PhysicsDirectBodyState3D *p_state) {
+	PhysicalBone3D *bone = (PhysicalBone3D *)p_instance;
+	bone->_body_state_changed(p_state);
+}
+
+void PhysicalBone3D::_body_state_changed(PhysicsDirectBodyState3D *p_state) {
 	if (!simulate_physics || !_internal_simulate_physics) {
 		return;
 	}
 
-	/// Update bone transform
-
-	PhysicsDirectBodyState3D *state;
-
-#ifdef DEBUG_ENABLED
-	state = Object::cast_to<PhysicsDirectBodyState3D>(p_state);
-	ERR_FAIL_NULL_MSG(state, "Method '_direct_state_changed' must receive a valid PhysicsDirectBodyState3D object as argument");
-#else
-	state = (PhysicsDirectBodyState3D *)p_state; //trust it
-#endif
+	/// Update bone transform.
 
-	Transform3D global_transform(state->get_transform());
+	Transform3D global_transform(p_state->get_transform());
 
 	set_ignore_transform_notification(true);
 	set_global_transform(global_transform);
@@ -2730,7 +2713,7 @@ void PhysicalBone3D::_start_physics_simulation() {
 	set_body_mode(PhysicsServer3D::BODY_MODE_DYNAMIC);
 	PhysicsServer3D::get_singleton()->body_set_collision_layer(get_rid(), get_collision_layer());
 	PhysicsServer3D::get_singleton()->body_set_collision_mask(get_rid(), get_collision_mask());
-	PhysicsServer3D::get_singleton()->body_set_force_integration_callback(get_rid(), callable_mp(this, &PhysicalBone3D::_direct_state_changed));
+	PhysicsServer3D::get_singleton()->body_set_state_sync_callback(get_rid(), this, &PhysicalBone3D::_body_state_changed_callback);
 	set_as_top_level(true);
 	_internal_simulate_physics = true;
 }
@@ -2749,7 +2732,7 @@ void PhysicalBone3D::_stop_physics_simulation() {
 		PhysicsServer3D::get_singleton()->body_set_collision_mask(get_rid(), 0);
 	}
 	if (_internal_simulate_physics) {
-		PhysicsServer3D::get_singleton()->body_set_force_integration_callback(get_rid(), Callable());
+		PhysicsServer3D::get_singleton()->body_set_state_sync_callback(get_rid(), nullptr, nullptr);
 		parent_skeleton->set_bone_global_pose_override(bone_id, Transform3D(), 0.0, false);
 		set_as_top_level(false);
 		_internal_simulate_physics = false;

scene/3d/physics_body_3d.h

@@ -86,7 +86,8 @@ class StaticBody3D : public PhysicsBody3D {
 
 	Transform3D last_valid_transform;
 
-	void _direct_state_changed(Object *p_state);
+	static void _body_state_changed_callback(void *p_instance, PhysicsDirectBodyState3D *p_state);
+	void _body_state_changed(PhysicsDirectBodyState3D *p_state);
 
 protected:
 	void _notification(int p_what);
@@ -136,7 +137,6 @@ public:
 
 protected:
 	bool can_sleep = true;
-	PhysicsDirectBodyState3D *state = nullptr;
 	Mode mode = MODE_DYNAMIC;
 
 	real_t mass = 1.0;
@@ -197,7 +197,8 @@ protected:
 	void _body_exit_tree(ObjectID p_id);
 
 	void _body_inout(int p_status, const RID &p_body, ObjectID p_instance, int p_body_shape, int p_local_shape);
-	virtual void _direct_state_changed(Object *p_state);
+	static void _body_state_changed_callback(void *p_instance, PhysicsDirectBodyState3D *p_state);
+	virtual void _body_state_changed(PhysicsDirectBodyState3D *p_state);
 
 	void _notification(int p_what);
 	static void _bind_methods();
@@ -525,7 +526,8 @@ protected:
 	bool _get(const StringName &p_name, Variant &r_ret) const;
 	void _get_property_list(List<PropertyInfo> *p_list) const;
 	void _notification(int p_what);
-	void _direct_state_changed(Object *p_state);
+	static void _body_state_changed_callback(void *p_instance, PhysicsDirectBodyState3D *p_state);
+	void _body_state_changed(PhysicsDirectBodyState3D *p_state);
 
 	static void _bind_methods();
 

scene/3d/vehicle_body_3d.cpp

@@ -802,24 +802,21 @@ void VehicleBody3D::_update_friction(PhysicsDirectBodyState3D *s) {
 	}
 }
 
-void VehicleBody3D::_direct_state_changed(Object *p_state) {
-	RigidBody3D::_direct_state_changed(p_state);
+void VehicleBody3D::_body_state_changed(PhysicsDirectBodyState3D *p_state) {
+	RigidBody3D::_body_state_changed(p_state);
 
-	state = Object::cast_to<PhysicsDirectBodyState3D>(p_state);
-	ERR_FAIL_NULL_MSG(state, "Method '_direct_state_changed' must receive a valid PhysicsDirectBodyState3D object as argument");
-
-	real_t step = state->get_step();
+	real_t step = p_state->get_step();
 
 	for (int i = 0; i < wheels.size(); i++) {
-		_update_wheel(i, state);
+		_update_wheel(i, p_state);
 	}
 
 	for (int i = 0; i < wheels.size(); i++) {
-		_ray_cast(i, state);
-		wheels[i]->set_transform(state->get_transform().inverse() * wheels[i]->m_worldTransform);
+		_ray_cast(i, p_state);
+		wheels[i]->set_transform(p_state->get_transform().inverse() * wheels[i]->m_worldTransform);
 	}
 
-	_update_suspension(state);
+	_update_suspension(p_state);
 
 	for (int i = 0; i < wheels.size(); i++) {
 		//apply suspension force
@@ -831,20 +828,20 @@ void VehicleBody3D::_direct_state_changed(Object *p_state) {
 			suspensionForce = wheel.m_maxSuspensionForce;
 		}
 		Vector3 impulse = wheel.m_raycastInfo.m_contactNormalWS * suspensionForce * step;
-		Vector3 relative_position = wheel.m_raycastInfo.m_contactPointWS - state->get_transform().origin;
+		Vector3 relative_position = wheel.m_raycastInfo.m_contactPointWS - p_state->get_transform().origin;
 
-		state->apply_impulse(impulse, relative_position);
+		p_state->apply_impulse(impulse, relative_position);
 	}
 
-	_update_friction(state);
+	_update_friction(p_state);
 
 	for (int i = 0; i < wheels.size(); i++) {
 		VehicleWheel3D &wheel = *wheels[i];
-		Vector3 relpos = wheel.m_raycastInfo.m_hardPointWS - state->get_transform().origin;
-		Vector3 vel = state->get_linear_velocity() + (state->get_angular_velocity()).cross(relpos); // * mPos);
+		Vector3 relpos = wheel.m_raycastInfo.m_hardPointWS - p_state->get_transform().origin;
+		Vector3 vel = p_state->get_linear_velocity() + (p_state->get_angular_velocity()).cross(relpos); // * mPos);
 
 		if (wheel.m_raycastInfo.m_isInContact) {
-			const Transform3D &chassisWorldTransform = state->get_transform();
+			const Transform3D &chassisWorldTransform = p_state->get_transform();
 
 			Vector3 fwd(
 					chassisWorldTransform.basis[0][Vector3::AXIS_Z],
@@ -864,8 +861,6 @@ void VehicleBody3D::_direct_state_changed(Object *p_state) {
 
 		wheel.m_deltaRotation *= real_t(0.99); //damping of rotation when not in contact
 	}
-
-	state = nullptr;
 }
 
 void VehicleBody3D::set_engine_force(real_t p_engine_force) {
@@ -926,7 +921,5 @@ void VehicleBody3D::_bind_methods() {
 
 VehicleBody3D::VehicleBody3D() {
 	exclude.insert(get_rid());
-	//PhysicsServer3D::get_singleton()->body_set_force_integration_callback(get_rid(), callable_mp(this, &VehicleBody3D::_direct_state_changed));
-
 	set_mass(40);
 }

scene/3d/vehicle_body_3d.h

@@ -192,7 +192,8 @@ class VehicleBody3D : public RigidBody3D {
 
 	static void _bind_methods();
 
-	void _direct_state_changed(Object *p_state) override;
+	static void _body_state_changed_callback(void *p_instance, PhysicsDirectBodyState3D *p_state);
+	virtual void _body_state_changed(PhysicsDirectBodyState3D *p_state) override;
 
 public:
 	void set_engine_force(real_t p_engine_force);

servers/physics_2d/body_2d_sw.cpp

@@ -29,8 +29,9 @@
 /*************************************************************************/
 
 #include "body_2d_sw.h"
+
 #include "area_2d_sw.h"
-#include "physics_server_2d_sw.h"
+#include "body_direct_state_2d_sw.h"
 #include "space_2d_sw.h"
 
 void Body2DSW::_update_inertia() {
@@ -495,7 +496,7 @@ void Body2DSW::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);
 	}
 
@@ -547,27 +548,27 @@ void Body2DSW::wakeup_neighbours() {
 }
 
 void Body2DSW::call_queries() {
-	if (fi_callback) {
-		PhysicsDirectBodyState2DSW *dbs = PhysicsDirectBodyState2DSW::singleton;
-		dbs->body = this;
-
-		Variant v = dbs;
-		const Variant *vp[2] = { &v, &fi_callback->callback_udata };
-
-		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 {
+			Variant direct_state_variant = get_direct_state();
+			const Variant *vp[2] = { &direct_state_variant, &fi_callback_data->udata };
+
 			Callable::CallError ce;
 			Variant rv;
-			if (fi_callback->callback_udata.get_type() != Variant::NIL) {
-				fi_callback->callable.call(vp, 2, rv, ce);
+			if (fi_callback_data->udata.get_type() != Variant::NIL) {
+				fi_callback_data->callable.call(vp, 2, rv, ce);
 
 			} else {
-				fi_callback->callable.call(vp, 1, rv, ce);
+				fi_callback_data->callable.call(vp, 1, rv, ce);
 			}
 		}
 	}
+
+	if (body_state_callback) {
+		(body_state_callback)(body_state_callback_instance, get_direct_state());
+	}
 }
 
 bool Body2DSW::sleep_test(real_t p_step) {
@@ -587,17 +588,30 @@ bool Body2DSW::sleep_test(real_t p_step) {
 	}
 }
 
+void Body2DSW::set_state_sync_callback(void *p_instance, PhysicsServer2D::BodyStateCallback p_callback) {
+	body_state_callback_instance = p_instance;
+	body_state_callback = p_callback;
+}
+
 void Body2DSW::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->callback_udata = p_udata;
+PhysicsDirectBodyState2DSW *Body2DSW::get_direct_state() {
+	if (!direct_state) {
+		direct_state = memnew(PhysicsDirectBodyState2DSW);
+		direct_state->body = this;
 	}
+	return direct_state;
 }
 
 Body2DSW::Body2DSW() :
@@ -632,33 +646,13 @@ Body2DSW::Body2DSW() :
 	still_time = 0;
 	continuous_cd_mode = PhysicsServer2D::CCD_MODE_DISABLED;
 	can_sleep = true;
-	fi_callback = nullptr;
 }
 
 Body2DSW::~Body2DSW() {
-	if (fi_callback) {
-		memdelete(fi_callback);
-	}
-}
-
-PhysicsDirectBodyState2DSW *PhysicsDirectBodyState2DSW::singleton = nullptr;
-
-PhysicsDirectSpaceState2D *PhysicsDirectBodyState2DSW::get_space_state() {
-	return body->get_space()->get_direct_state();
-}
-
-Variant PhysicsDirectBodyState2DSW::get_contact_collider_shape_metadata(int p_contact_idx) const {
-	ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Variant());
-
-	if (!PhysicsServer2DSW::singletonsw->body_owner.owns(body->contacts[p_contact_idx].collider)) {
-		return Variant();
+	if (fi_callback_data) {
+		memdelete(fi_callback_data);
 	}
-	Body2DSW *other = PhysicsServer2DSW::singletonsw->body_owner.getornull(body->contacts[p_contact_idx].collider);
-
-	int sidx = body->contacts[p_contact_idx].collider_shape;
-	if (sidx < 0 || sidx >= other->get_shape_count()) {
-		return Variant();
+	if (direct_state) {
+		memdelete(direct_state);
 	}
-
-	return other->get_shape_metadata(sidx);
 }

servers/physics_2d/body_2d_sw.h

@@ -38,6 +38,7 @@
 #include "core/templates/vset.h"
 
 class Constraint2DSW;
+class PhysicsDirectBodyState2DSW;
 
 class Body2DSW : public CollisionObject2DSW {
 	PhysicsServer2D::BodyMode mode;
@@ -116,12 +117,17 @@ class Body2DSW : public CollisionObject2DSW {
 	Vector<Contact> contacts; //no contacts by default
 	int contact_count;
 
-	struct ForceIntegrationCallback {
+	void *body_state_callback_instance = nullptr;
+	PhysicsServer2D::BodyStateCallback body_state_callback = nullptr;
+
+	struct ForceIntegrationCallbackData {
 		Callable callable;
-		Variant callback_udata;
+		Variant udata;
 	};
 
-	ForceIntegrationCallback *fi_callback;
+	ForceIntegrationCallbackData *fi_callback_data = nullptr;
+
+	PhysicsDirectBodyState2DSW *direct_state = nullptr;
 
 	uint64_t island_step;
 
@@ -130,8 +136,11 @@ class Body2DSW : public CollisionObject2DSW {
 	friend class PhysicsDirectBodyState2DSW; // i give up, too many functions to expose
 
 public:
+	void set_state_sync_callback(void *p_instance, PhysicsServer2D::BodyStateCallback p_callback);
 	void set_force_integration_callback(const Callable &p_callable, const Variant &p_udata = Variant());
 
+	PhysicsDirectBodyState2DSW *get_direct_state();
+
 	_FORCE_INLINE_ void add_area(Area2DSW *p_area) {
 		int index = areas.find(AreaCMP(p_area));
 		if (index > -1) {
@@ -332,87 +341,4 @@ void Body2DSW::add_contact(const Vector2 &p_local_pos, const Vector2 &p_local_no
 	c[idx].collider_velocity_at_pos = p_collider_velocity_at_pos;
 }
 
-class PhysicsDirectBodyState2DSW : public PhysicsDirectBodyState2D {
-	GDCLASS(PhysicsDirectBodyState2DSW, PhysicsDirectBodyState2D);
-
-public:
-	static PhysicsDirectBodyState2DSW *singleton;
-	Body2DSW *body;
-	real_t step;
-
-	virtual Vector2 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 real_t get_inverse_mass() const override { return body->get_inv_mass(); } // get the mass
-	virtual real_t get_inverse_inertia() const override { return body->get_inv_inertia(); } // get density of this body space
-
-	virtual void set_linear_velocity(const Vector2 &p_velocity) override { body->set_linear_velocity(p_velocity); }
-	virtual Vector2 get_linear_velocity() const override { return body->get_linear_velocity(); }
-
-	virtual void set_angular_velocity(real_t p_velocity) override { body->set_angular_velocity(p_velocity); }
-	virtual real_t get_angular_velocity() const override { return body->get_angular_velocity(); }
-
-	virtual void set_transform(const Transform2D &p_transform) override { body->set_state(PhysicsServer2D::BODY_STATE_TRANSFORM, p_transform); }
-	virtual Transform2D get_transform() const override { return body->get_transform(); }
-
-	virtual Vector2 get_velocity_at_local_position(const Vector2 &p_position) const override { return body->get_velocity_in_local_point(p_position); }
-
-	virtual void add_central_force(const Vector2 &p_force) override { body->add_central_force(p_force); }
-	virtual void add_force(const Vector2 &p_force, const Vector2 &p_position = Vector2()) override { body->add_force(p_force, p_position); }
-	virtual void add_torque(real_t p_torque) override { body->add_torque(p_torque); }
-	virtual void apply_central_impulse(const Vector2 &p_impulse) override { body->apply_central_impulse(p_impulse); }
-	virtual void apply_impulse(const Vector2 &p_impulse, const Vector2 &p_position = Vector2()) override { body->apply_impulse(p_impulse, p_position); }
-	virtual void apply_torque_impulse(real_t p_torque) override { body->apply_torque_impulse(p_torque); }
-
-	virtual void set_sleep_state(bool p_enable) override { body->set_active(!p_enable); }
-	virtual bool is_sleeping() const override { return !body->is_active(); }
-
-	virtual int get_contact_count() const override { return body->contact_count; }
-
-	virtual Vector2 get_contact_local_position(int p_contact_idx) const override {
-		ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2());
-		return body->contacts[p_contact_idx].local_pos;
-	}
-	virtual Vector2 get_contact_local_normal(int p_contact_idx) const override {
-		ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2());
-		return body->contacts[p_contact_idx].local_normal;
-	}
-	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 Vector2 get_contact_collider_position(int p_contact_idx) const override {
-		ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2());
-		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 Variant get_contact_collider_shape_metadata(int p_contact_idx) const override;
-
-	virtual Vector2 get_contact_collider_velocity_at_position(int p_contact_idx) const override {
-		ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2());
-		return body->contacts[p_contact_idx].collider_velocity_at_pos;
-	}
-
-	virtual PhysicsDirectSpaceState2D *get_space_state() override;
-
-	virtual real_t get_step() const override { return step; }
-	PhysicsDirectBodyState2DSW() {
-		singleton = this;
-		body = nullptr;
-	}
-};
-
 #endif // BODY_2D_SW_H

servers/physics_2d/body_direct_state_2d_sw.cpp

@@ -0,0 +1,182 @@
+/*************************************************************************/
+/*  body_direct_state_2d_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_2d_sw.h"
+
+#include "body_2d_sw.h"
+#include "physics_server_2d_sw.h"
+#include "space_2d_sw.h"
+
+Vector2 PhysicsDirectBodyState2DSW::get_total_gravity() const {
+	return body->gravity;
+}
+
+real_t PhysicsDirectBodyState2DSW::get_total_angular_damp() const {
+	return body->area_angular_damp;
+}
+
+real_t PhysicsDirectBodyState2DSW::get_total_linear_damp() const {
+	return body->area_linear_damp;
+}
+
+real_t PhysicsDirectBodyState2DSW::get_inverse_mass() const {
+	return body->get_inv_mass();
+}
+
+real_t PhysicsDirectBodyState2DSW::get_inverse_inertia() const {
+	return body->get_inv_inertia();
+}
+
+void PhysicsDirectBodyState2DSW::set_linear_velocity(const Vector2 &p_velocity) {
+	body->set_linear_velocity(p_velocity);
+}
+
+Vector2 PhysicsDirectBodyState2DSW::get_linear_velocity() const {
+	return body->get_linear_velocity();
+}
+
+void PhysicsDirectBodyState2DSW::set_angular_velocity(real_t p_velocity) {
+	body->set_angular_velocity(p_velocity);
+}
+
+real_t PhysicsDirectBodyState2DSW::get_angular_velocity() const {
+	return body->get_angular_velocity();
+}
+
+void PhysicsDirectBodyState2DSW::set_transform(const Transform2D &p_transform) {
+	body->set_state(PhysicsServer2D::BODY_STATE_TRANSFORM, p_transform);
+}
+
+Transform2D PhysicsDirectBodyState2DSW::get_transform() const {
+	return body->get_transform();
+}
+
+Vector2 PhysicsDirectBodyState2DSW::get_velocity_at_local_position(const Vector2 &p_position) const {
+	return body->get_velocity_in_local_point(p_position);
+}
+
+void PhysicsDirectBodyState2DSW::add_central_force(const Vector2 &p_force) {
+	body->add_central_force(p_force);
+}
+
+void PhysicsDirectBodyState2DSW::add_force(const Vector2 &p_force, const Vector2 &p_position) {
+	body->add_force(p_force, p_position);
+}
+
+void PhysicsDirectBodyState2DSW::add_torque(real_t p_torque) {
+	body->add_torque(p_torque);
+}
+
+void PhysicsDirectBodyState2DSW::apply_central_impulse(const Vector2 &p_impulse) {
+	body->apply_central_impulse(p_impulse);
+}
+
+void PhysicsDirectBodyState2DSW::apply_impulse(const Vector2 &p_impulse, const Vector2 &p_position) {
+	body->apply_impulse(p_impulse, p_position);
+}
+
+void PhysicsDirectBodyState2DSW::apply_torque_impulse(real_t p_torque) {
+	body->apply_torque_impulse(p_torque);
+}
+
+void PhysicsDirectBodyState2DSW::set_sleep_state(bool p_enable) {
+	body->set_active(!p_enable);
+}
+
+bool PhysicsDirectBodyState2DSW::is_sleeping() const {
+	return !body->is_active();
+}
+
+int PhysicsDirectBodyState2DSW::get_contact_count() const {
+	return body->contact_count;
+}
+
+Vector2 PhysicsDirectBodyState2DSW::get_contact_local_position(int p_contact_idx) const {
+	ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2());
+	return body->contacts[p_contact_idx].local_pos;
+}
+
+Vector2 PhysicsDirectBodyState2DSW::get_contact_local_normal(int p_contact_idx) const {
+	ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2());
+	return body->contacts[p_contact_idx].local_normal;
+}
+
+int PhysicsDirectBodyState2DSW::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 PhysicsDirectBodyState2DSW::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;
+}
+Vector2 PhysicsDirectBodyState2DSW::get_contact_collider_position(int p_contact_idx) const {
+	ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2());
+	return body->contacts[p_contact_idx].collider_pos;
+}
+
+ObjectID PhysicsDirectBodyState2DSW::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 PhysicsDirectBodyState2DSW::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;
+}
+
+Vector2 PhysicsDirectBodyState2DSW::get_contact_collider_velocity_at_position(int p_contact_idx) const {
+	ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2());
+	return body->contacts[p_contact_idx].collider_velocity_at_pos;
+}
+
+Variant PhysicsDirectBodyState2DSW::get_contact_collider_shape_metadata(int p_contact_idx) const {
+	ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Variant());
+
+	if (!PhysicsServer2DSW::singletonsw->body_owner.owns(body->contacts[p_contact_idx].collider)) {
+		return Variant();
+	}
+	Body2DSW *other = PhysicsServer2DSW::singletonsw->body_owner.getornull(body->contacts[p_contact_idx].collider);
+
+	int sidx = body->contacts[p_contact_idx].collider_shape;
+	if (sidx < 0 || sidx >= other->get_shape_count()) {
+		return Variant();
+	}
+
+	return other->get_shape_metadata(sidx);
+}
+
+PhysicsDirectSpaceState2D *PhysicsDirectBodyState2DSW::get_space_state() {
+	return body->get_space()->get_direct_state();
+}
+
+real_t PhysicsDirectBodyState2DSW::get_step() const {
+	return body->get_space()->get_last_step();
+}

servers/physics_2d/body_direct_state_2d_sw.h

@@ -0,0 +1,91 @@
+/*************************************************************************/
+/*  body_direct_state_2d_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_2D_SW_H
+#define BODY_DIRECT_STATE_2D_SW_H
+
+#include "servers/physics_server_2d.h"
+
+class Body2DSW;
+
+class PhysicsDirectBodyState2DSW : public PhysicsDirectBodyState2D {
+	GDCLASS(PhysicsDirectBodyState2DSW, PhysicsDirectBodyState2D);
+
+public:
+	Body2DSW *body = nullptr;
+
+	virtual Vector2 get_total_gravity() const override;
+	virtual real_t get_total_angular_damp() const override;
+	virtual real_t get_total_linear_damp() const override;
+
+	virtual real_t get_inverse_mass() const override;
+	virtual real_t get_inverse_inertia() const override;
+
+	virtual void set_linear_velocity(const Vector2 &p_velocity) override;
+	virtual Vector2 get_linear_velocity() const override;
+
+	virtual void set_angular_velocity(real_t p_velocity) override;
+	virtual real_t get_angular_velocity() const override;
+
+	virtual void set_transform(const Transform2D &p_transform) override;
+	virtual Transform2D get_transform() const override;
+
+	virtual Vector2 get_velocity_at_local_position(const Vector2 &p_position) const override;
+
+	virtual void add_central_force(const Vector2 &p_force) override;
+	virtual void add_force(const Vector2 &p_force, const Vector2 &p_position = Vector2()) override;
+	virtual void add_torque(real_t p_torque) override;
+	virtual void apply_central_impulse(const Vector2 &p_impulse) override;
+	virtual void apply_impulse(const Vector2 &p_impulse, const Vector2 &p_position = Vector2()) override;
+	virtual void apply_torque_impulse(real_t p_torque) override;
+
+	virtual void set_sleep_state(bool p_enable) override;
+	virtual bool is_sleeping() const override;
+
+	virtual int get_contact_count() const override;
+
+	virtual Vector2 get_contact_local_position(int p_contact_idx) const override;
+	virtual Vector2 get_contact_local_normal(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 Vector2 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 Variant get_contact_collider_shape_metadata(int p_contact_idx) const override;
+
+	virtual Vector2 get_contact_collider_velocity_at_position(int p_contact_idx) const override;
+
+	virtual PhysicsDirectSpaceState2D *get_space_state() override;
+
+	virtual real_t get_step() const override;
+};
+
+#endif // BODY_2D_SW_H

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"
@@ -926,6 +927,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 +967,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 +1237,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 +1248,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 +1319,6 @@ void PhysicsServer2DSW::end_sync() {
 
 void PhysicsServer2DSW::finish() {
 	memdelete(stepper);
-	memdelete(direct_state);
 };
 
 void PhysicsServer2DSW::_update_shapes() {

servers/physics_2d/physics_server_2d_sw.h

@@ -46,7 +46,6 @@ class PhysicsServer2DSW : public PhysicsServer2D {
 	bool active;
 	int iterations;
 	bool doing_sync;
-	real_t last_step;
 
 	int island_count;
 	int active_objects;
@@ -59,8 +58,6 @@ class PhysicsServer2DSW : public PhysicsServer2D {
 	Step2DSW *stepper;
 	Set<const Space2DSW *> active_spaces;
 
-	PhysicsDirectBodyState2DSW *direct_state;
-
 	mutable RID_PtrOwner<Shape2DSW, true> shape_owner;
 	mutable RID_PtrOwner<Space2DSW, true> space_owner;
 	mutable RID_PtrOwner<Area2DSW, true> area_owner;
@@ -242,7 +239,9 @@ 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 bool body_collide_shape(RID p_body, int p_body_shape, RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, Vector2 *r_results, int p_result_max, int &r_result_count) override;
 
 	virtual void body_set_pickable(RID p_body, bool p_pickable) override;

servers/physics_2d/physics_server_2d_wrap_mt.h

@@ -245,6 +245,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 &);
 
 	bool body_collide_shape(RID p_body, int p_body_shape, RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, Vector2 *r_results, int p_result_max, int &r_result_count) override {

servers/physics_2d/space_2d_sw.cpp

@@ -634,7 +634,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
 								//fix for moving platforms (kinematic and dynamic), margin is increased by how much it moved in the given direction
 								Vector2 lv = b->get_linear_velocity();
 								//compute displacement from linear velocity
-								Vector2 motion = lv * PhysicsDirectBodyState2DSW::singleton->step;
+								Vector2 motion = lv * last_step;
 								real_t motion_len = motion.length();
 								motion.normalize();
 								cbk.valid_depth += motion_len * MAX(motion.dot(-cbk.valid_dir), 0.0);
@@ -926,7 +926,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
 							//fix for moving platforms (kinematic and dynamic), margin is increased by how much it moved in the given direction
 							Vector2 lv = b->get_linear_velocity();
 							//compute displacement from linear velocity
-							Vector2 motion = lv * PhysicsDirectBodyState2DSW::singleton->step;
+							Vector2 motion = lv * last_step;
 							real_t motion_len = motion.length();
 							motion.normalize();
 							rcd.valid_depth += motion_len * MAX(motion.dot(-rcd.valid_dir), 0.0);

servers/physics_2d/space_2d_sw.h

@@ -117,6 +117,8 @@ private:
 
 	bool locked;
 
+	real_t last_step = 0.001;
+
 	int island_count;
 	int active_objects;
 	int collision_pairs;
@@ -172,6 +174,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(PhysicsServer2D::SpaceParameter p_param, real_t p_value);
 	real_t get_param(PhysicsServer2D::SpaceParameter p_param) const;
 

servers/physics_2d/step_2d_sw.cpp

@@ -129,6 +129,8 @@ void Step2DSW::step(Space2DSW *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;
 

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();
 }

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

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();
+}

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

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) {

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;

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);

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;
 

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;
 

servers/physics_server_2d.h

@@ -457,6 +457,10 @@ public:
 	virtual void body_set_omit_force_integration(RID p_body, bool p_omit) = 0;
 	virtual bool body_is_omitting_force_integration(RID p_body) const = 0;
 
+	// Callback for C++ use only.
+	typedef void (*BodyStateCallback)(void *p_instance, PhysicsDirectBodyState2D *p_state);
+	virtual void body_set_state_sync_callback(RID p_body, void *p_instance, BodyStateCallback p_callback) = 0;
+
 	virtual void body_set_force_integration_callback(RID p_body, const Callable &p_callable, const Variant &p_udata = Variant()) = 0;
 
 	virtual bool body_collide_shape(RID p_body, int p_body_shape, RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, Vector2 *r_results, int p_result_max, int &r_result_count) = 0;

servers/physics_server_3d.h

@@ -471,6 +471,10 @@ public:
 	virtual void body_set_omit_force_integration(RID p_body, bool p_omit) = 0;
 	virtual bool body_is_omitting_force_integration(RID p_body) const = 0;
 
+	// Callback for C++ use only.
+	typedef void (*BodyStateCallback)(void *p_instance, PhysicsDirectBodyState3D *p_state);
+	virtual void body_set_state_sync_callback(RID p_body, void *p_instance, BodyStateCallback p_callback) = 0;
+
 	virtual void body_set_force_integration_callback(RID p_body, const Callable &p_callable, const Variant &p_udata = Variant()) = 0;
 
 	virtual void body_set_ray_pickable(RID p_body, bool p_enable) = 0;