Merge pull request #42929 from madmiraal/fix-42877-3.2

modules/bullet/bullet_physics_server.cpp

@@ -856,13 +856,13 @@ PhysicsDirectBodyState *BulletPhysicsServer::body_get_direct_state(RID p_body) {
 	}
 
 	RigidBodyBullet *body = rigid_body_owner.get(p_body);
-	ERR_FAIL_COND_V(!body, nullptr);
+	ERR_FAIL_COND_V_MSG(!body, nullptr, "Body with RID " + itos(p_body.get_id()) + " not owned by this server.");
 
 	if (!body->get_space()) {
 		return nullptr;
 	}
 
-	return BulletPhysicsDirectBodyState::get_singleton(body);
+	return body->get_direct_state();
 }
 
 bool BulletPhysicsServer::body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, MotionResult *r_result, bool p_exclude_raycast_shapes, const Set<RID> &p_exclude) {
@@ -1533,7 +1533,6 @@ void BulletPhysicsServer::free(RID p_rid) {
 }
 
 void BulletPhysicsServer::init() {
-	BulletPhysicsDirectBodyState::initSingleton();
 }
 
 void BulletPhysicsServer::step(float p_deltaTime) {
@@ -1541,8 +1540,6 @@ void BulletPhysicsServer::step(float p_deltaTime) {
 		return;
 	}
 
-	BulletPhysicsDirectBodyState::singleton_setDeltaTime(p_deltaTime);
-
 	for (int i = 0; i < active_spaces_count; ++i) {
 		active_spaces[i]->step(p_deltaTime);
 	}
@@ -1552,7 +1549,6 @@ void BulletPhysicsServer::flush_queries() {
 }
 
 void BulletPhysicsServer::finish() {
-	BulletPhysicsDirectBodyState::destroySingleton();
 }
 
 void BulletPhysicsServer::set_collision_iterations(int p_iterations) {

modules/bullet/rigid_body_bullet.cpp

@@ -48,8 +48,6 @@
 	@author AndreaCatania
 */
 
-BulletPhysicsDirectBodyState *BulletPhysicsDirectBodyState::singleton = nullptr;
-
 Vector3 BulletPhysicsDirectBodyState::get_total_gravity() const {
 	Vector3 gVec;
 	B_TO_G(body->btBody->getGravity(), gVec);
@@ -204,6 +202,10 @@ Vector3 BulletPhysicsDirectBodyState::get_contact_collider_velocity_at_position(
 	return velocityAtPoint;
 }
 
+real_t BulletPhysicsDirectBodyState::get_step() const {
+	return body->get_space()->get_delta_time();
+}
+
 PhysicsDirectSpaceState *BulletPhysicsDirectBodyState::get_space_state() {
 	return body->get_space()->get_direct_state();
 }
@@ -306,11 +308,14 @@ RigidBodyBullet::RigidBodyBullet() :
 
 	prev_collision_traces = &collision_traces_1;
 	curr_collision_traces = &collision_traces_2;
+
+	direct_access = memnew(BulletPhysicsDirectBodyState);
+	direct_access->body = this;
 }
 
 RigidBodyBullet::~RigidBodyBullet() {
 	bulletdelete(godotMotionState);
-
+	memdelete(direct_access);
 	if (force_integration_callback) {
 		memdelete(force_integration_callback);
 	}
@@ -370,9 +375,7 @@ void RigidBodyBullet::dispatch_callbacks() {
 			btBody->clearForces();
 		}
 
-		BulletPhysicsDirectBodyState *bodyDirect = BulletPhysicsDirectBodyState::get_singleton(this);
-
-		Variant variantBodyDirect = bodyDirect;
+		Variant variantBodyDirect = direct_access;
 
 		Object *obj = ObjectDB::get_instance(force_integration_callback->id);
 		if (!obj) {

modules/bullet/rigid_body_bullet.h

@@ -45,49 +45,15 @@ class AreaBullet;
 class SpaceBullet;
 class btRigidBody;
 class GodotMotionState;
-class BulletPhysicsDirectBodyState;
-
-/// This class could be used in multi thread with few changes but currently
-/// is set to be only in one single thread.
-///
-/// In the system there is only one object at a time that manage all bodies and is
-/// created by BulletPhysicsServer and is held by the "singleton" variable of this class
-/// Each time something require it, the body must be set again.
+
 class BulletPhysicsDirectBodyState : public PhysicsDirectBodyState {
 	GDCLASS(BulletPhysicsDirectBodyState, PhysicsDirectBodyState);
 
-	static BulletPhysicsDirectBodyState *singleton;
-
-public:
-	/// This class avoid the creation of more object of this class
-	static void initSingleton() {
-		if (!singleton) {
-			singleton = memnew(BulletPhysicsDirectBodyState);
-		}
-	}
-
-	static void destroySingleton() {
-		memdelete(singleton);
-		singleton = nullptr;
-	}
-
-	static void singleton_setDeltaTime(real_t p_deltaTime) {
-		singleton->deltaTime = p_deltaTime;
-	}
-
-	static BulletPhysicsDirectBodyState *get_singleton(RigidBodyBullet *p_body) {
-		singleton->body = p_body;
-		return singleton;
-	}
-
 public:
-	RigidBodyBullet *body;
-	real_t deltaTime;
+	RigidBodyBullet *body = nullptr;
 
-private:
 	BulletPhysicsDirectBodyState() {}
 
-public:
 	virtual Vector3 get_total_gravity() const;
 	virtual float get_total_angular_damp() const;
 	virtual float get_total_linear_damp() const;
@@ -135,7 +101,7 @@ public:
 	virtual int get_contact_collider_shape(int p_contact_idx) const;
 	virtual Vector3 get_contact_collider_velocity_at_position(int p_contact_idx) const;
 
-	virtual real_t get_step() const { return deltaTime; }
+	virtual real_t get_step() const;
 	virtual void integrate_forces() {
 		// Skip the execution of this function
 	}
@@ -188,6 +154,7 @@ public:
 	};
 
 private:
+	BulletPhysicsDirectBodyState *direct_access = nullptr;
 	friend class BulletPhysicsDirectBodyState;
 
 	// This is required only for Kinematic movement
@@ -232,6 +199,8 @@ public:
 	RigidBodyBullet();
 	~RigidBodyBullet();
 
+	BulletPhysicsDirectBodyState *get_direct_state() const { return direct_access; }
+
 	void init_kinematic_utilities();
 	void destroy_kinematic_utilities();
 	_FORCE_INLINE_ KinematicUtilities *get_kinematic_utilities() const { return kinematic_utilities; }

servers/physics/body_sw.cpp

@@ -703,10 +703,7 @@ void BodySW::wakeup_neighbours() {
 
 void BodySW::call_queries() {
 	if (fi_callback) {
-		PhysicsDirectBodyStateSW *dbs = PhysicsDirectBodyStateSW::singleton;
-		dbs->body = this;
-
-		Variant v = dbs;
+		Variant v = direct_access;
 
 		Object *obj = ObjectDB::get_instance(fi_callback->id);
 		if (!obj) {
@@ -793,16 +790,22 @@ BodySW::BodySW() :
 	continuous_cd = false;
 	can_sleep = true;
 	fi_callback = nullptr;
+
+	direct_access = memnew(PhysicsDirectBodyStateSW);
+	direct_access->body = this;
 }
 
 BodySW::~BodySW() {
+	memdelete(direct_access);
 	if (fi_callback) {
 		memdelete(fi_callback);
 	}
 }
 
-PhysicsDirectBodyStateSW *PhysicsDirectBodyStateSW::singleton = nullptr;
-
 PhysicsDirectSpaceState *PhysicsDirectBodyStateSW::get_space_state() {
 	return body->get_space()->get_direct_state();
 }
+
+real_t PhysicsDirectBodyStateSW::get_step() const {
+	return body->get_space()->get_step();
+}

servers/physics/body_sw.h

@@ -36,6 +36,7 @@
 #include "core/vset.h"
 
 class ConstraintSW;
+class PhysicsDirectBodyStateSW;
 
 class BodySW : public CollisionObjectSW {
 	PhysicsServer::BodyMode mode;
@@ -142,6 +143,7 @@ class BodySW : public CollisionObjectSW {
 
 	_FORCE_INLINE_ void _update_transform_dependant();
 
+	PhysicsDirectBodyStateSW *direct_access = nullptr;
 	friend class PhysicsDirectBodyStateSW; // i give up, too many functions to expose
 
 public:
@@ -326,6 +328,8 @@ public:
 
 	bool sleep_test(real_t p_step);
 
+	PhysicsDirectBodyStateSW *get_direct_state() const { return direct_access; }
+
 	BodySW();
 	~BodySW();
 };
@@ -378,9 +382,7 @@ class PhysicsDirectBodyStateSW : public PhysicsDirectBodyState {
 	GDCLASS(PhysicsDirectBodyStateSW, PhysicsDirectBodyState);
 
 public:
-	static PhysicsDirectBodyStateSW *singleton;
-	BodySW *body;
-	real_t step;
+	BodySW *body = nullptr;
 
 	virtual Vector3 get_total_gravity() const { return body->gravity; } // get gravity vector working on this body space/area
 	virtual real_t get_total_angular_damp() const { return body->area_angular_damp; } // get density of this body space/area
@@ -479,11 +481,9 @@ public:
 
 	virtual PhysicsDirectSpaceState *get_space_state();
 
-	virtual real_t get_step() const { return step; }
-	PhysicsDirectBodyStateSW() {
-		singleton = this;
-		body = nullptr;
-	}
+	virtual real_t get_step() const;
+
+	PhysicsDirectBodyStateSW() {}
 };
 
 #endif // BODY__SW_H

servers/physics/physics_server_sw.cpp

@@ -886,16 +886,14 @@ PhysicsDirectBodyState *PhysicsServerSW::body_get_direct_state(RID p_body) {
 	}
 
 	BodySW *body = body_owner.get(p_body);
-	ERR_FAIL_COND_V(!body, nullptr);
+	ERR_FAIL_COND_V_MSG(!body, nullptr, "Body with RID " + itos(p_body.get_id()) + " not owned by this server.");
 
 	if (!body->get_space()) {
 		return 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 */
@@ -1286,10 +1284,8 @@ void PhysicsServerSW::set_collision_iterations(int p_iterations) {
 };
 
 void PhysicsServerSW::init() {
-	last_step = 0.001;
 	iterations = 8; // 8?
 	stepper = memnew(StepSW);
-	direct_state = memnew(PhysicsDirectBodyStateSW);
 };
 
 void PhysicsServerSW::step(real_t p_step) {
@@ -1301,9 +1297,6 @@ void PhysicsServerSW::step(real_t p_step) {
 
 	_update_shapes();
 
-	last_step = p_step;
-	PhysicsDirectBodyStateSW::singleton->step = p_step;
-
 	island_count = 0;
 	active_objects = 0;
 	collision_pairs = 0;
@@ -1370,7 +1363,6 @@ void PhysicsServerSW::flush_queries() {
 
 void PhysicsServerSW::finish() {
 	memdelete(stepper);
-	memdelete(direct_state);
 };
 
 int PhysicsServerSW::get_process_info(ProcessInfo p_info) {

servers/physics/physics_server_sw.h

@@ -43,7 +43,6 @@ class PhysicsServerSW : public PhysicsServer {
 	friend class PhysicsDirectSpaceStateSW;
 	bool active;
 	int iterations;
-	real_t last_step;
 
 	int island_count;
 	int active_objects;
@@ -54,8 +53,6 @@ class PhysicsServerSW : public PhysicsServer {
 	StepSW *stepper;
 	Set<const SpaceSW *> active_spaces;
 
-	PhysicsDirectBodyStateSW *direct_state;
-
 	mutable RID_Owner<ShapeSW> shape_owner;
 	mutable RID_Owner<SpaceSW> space_owner;
 	mutable RID_Owner<AreaSW> area_owner;

servers/physics/space_sw.h

@@ -110,6 +110,7 @@ private:
 
 	bool locked;
 
+	real_t step;
 	int island_count;
 	int active_objects;
 	int collision_pairs;
@@ -127,6 +128,9 @@ public:
 	_FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; }
 	_FORCE_INLINE_ RID get_self() const { return self; }
 
+	_FORCE_INLINE_ void set_step(const real_t &p_step) { step = p_step; }
+	_FORCE_INLINE_ real_t get_step() const { return step; }
+
 	void set_default_area(AreaSW *p_area) { area = p_area; }
 	AreaSW *get_default_area() const { return area; }
 

servers/physics/step_sw.cpp

@@ -141,7 +141,7 @@ void StepSW::_check_suspend(BodySW *p_island, real_t p_delta) {
 
 void StepSW::step(SpaceSW *p_space, real_t p_delta, int p_iterations) {
 	p_space->lock(); // can't access space during this
-
+	p_space->set_step(p_delta);
 	p_space->setup(); //update inertias, etc
 
 	const SelfList<BodySW>::List *body_list = &p_space->get_active_body_list();

servers/physics_2d/body_2d_sw.cpp

@@ -588,10 +588,7 @@ void Body2DSW::wakeup_neighbours() {
 
 void Body2DSW::call_queries() {
 	if (fi_callback) {
-		Physics2DDirectBodyStateSW *dbs = Physics2DDirectBodyStateSW::singleton;
-		dbs->body = this;
-
-		Variant v = dbs;
+		Variant v = direct_access;
 		const Variant *vp[2] = { &v, &fi_callback->callback_udata };
 
 		Object *obj = ObjectDB::get_instance(fi_callback->id);
@@ -677,20 +674,26 @@ Body2DSW::Body2DSW() :
 	continuous_cd_mode = Physics2DServer::CCD_MODE_DISABLED;
 	can_sleep = true;
 	fi_callback = nullptr;
+
+	direct_access = memnew(Physics2DDirectBodyStateSW);
+	direct_access->body = this;
 }
 
 Body2DSW::~Body2DSW() {
+	memdelete(direct_access);
 	if (fi_callback) {
 		memdelete(fi_callback);
 	}
 }
 
-Physics2DDirectBodyStateSW *Physics2DDirectBodyStateSW::singleton = nullptr;
-
 Physics2DDirectSpaceState *Physics2DDirectBodyStateSW::get_space_state() {
 	return body->get_space()->get_direct_state();
 }
 
+real_t Physics2DDirectBodyStateSW::get_step() const {
+	return body->get_space()->get_step();
+}
+
 Variant Physics2DDirectBodyStateSW::get_contact_collider_shape_metadata(int p_contact_idx) const {
 	ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Variant());
 

servers/physics_2d/body_2d_sw.h

@@ -36,6 +36,7 @@
 #include "core/vset.h"
 
 class Constraint2DSW;
+class Physics2DDirectBodyStateSW;
 
 class Body2DSW : public CollisionObject2DSW {
 	Physics2DServer::BodyMode mode;
@@ -128,6 +129,7 @@ class Body2DSW : public CollisionObject2DSW {
 
 	_FORCE_INLINE_ void _compute_area_gravity_and_dampenings(const Area2DSW *p_area);
 
+	Physics2DDirectBodyStateSW *direct_access = nullptr;
 	friend class Physics2DDirectBodyStateSW; // i give up, too many functions to expose
 
 public:
@@ -291,6 +293,8 @@ public:
 
 	bool sleep_test(real_t p_step);
 
+	Physics2DDirectBodyStateSW *get_direct_state() const { return direct_access; }
+
 	Body2DSW();
 	~Body2DSW();
 };
@@ -343,9 +347,7 @@ class Physics2DDirectBodyStateSW : public Physics2DDirectBodyState {
 	GDCLASS(Physics2DDirectBodyStateSW, Physics2DDirectBodyState);
 
 public:
-	static Physics2DDirectBodyStateSW *singleton;
-	Body2DSW *body;
-	real_t step;
+	Body2DSW *body = nullptr;
 
 	virtual Vector2 get_total_gravity() const { return body->gravity; } // get gravity vector working on this body space/area
 	virtual real_t get_total_angular_damp() const { return body->area_angular_damp; } // get density of this body space/area
@@ -439,11 +441,9 @@ public:
 
 	virtual Physics2DDirectSpaceState *get_space_state();
 
-	virtual real_t get_step() const { return step; }
-	Physics2DDirectBodyStateSW() {
-		singleton = this;
-		body = nullptr;
-	}
+	virtual real_t get_step() const;
+
+	Physics2DDirectBodyStateSW() {}
 };
 
 #endif // BODY_2D_SW_H

servers/physics_2d/physics_2d_server_sw.cpp

@@ -954,23 +954,20 @@ int Physics2DServerSW::body_test_ray_separation(RID p_body, const Transform2D &p
 }
 
 Physics2DDirectBodyState *Physics2DServerSW::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.get(p_body);
-	ERR_FAIL_COND_V(!body, nullptr);
+	ERR_FAIL_COND_V_MSG(!body, nullptr, "Body with RID " + itos(p_body.get_id()) + " not owned by this server.");
 
 	if (!body->get_space()) {
 		return 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.");
+	ERR_FAIL_COND_V_MSG((using_threads && !doing_sync) || 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 */
@@ -1205,10 +1202,8 @@ void Physics2DServerSW::set_collision_iterations(int p_iterations) {
 
 void Physics2DServerSW::init() {
 	doing_sync = false;
-	last_step = 0.001;
 	iterations = 8; // 8?
 	stepper = memnew(Step2DSW);
-	direct_state = memnew(Physics2DDirectBodyStateSW);
 };
 
 void Physics2DServerSW::step(real_t p_step) {
@@ -1218,8 +1213,6 @@ void Physics2DServerSW::step(real_t p_step) {
 
 	_update_shapes();
 
-	last_step = p_step;
-	Physics2DDirectBodyStateSW::singleton->step = p_step;
 	island_count = 0;
 	active_objects = 0;
 	collision_pairs = 0;
@@ -1290,7 +1283,6 @@ void Physics2DServerSW::end_sync() {
 
 void Physics2DServerSW::finish() {
 	memdelete(stepper);
-	memdelete(direct_state);
 };
 
 void Physics2DServerSW::_update_shapes() {

servers/physics_2d/physics_2d_server_sw.h

@@ -45,7 +45,6 @@ class Physics2DServerSW : public Physics2DServer {
 	bool active;
 	int iterations;
 	bool doing_sync;
-	real_t last_step;
 
 	int island_count;
 	int active_objects;
@@ -58,8 +57,6 @@ class Physics2DServerSW : public Physics2DServer {
 	Step2DSW *stepper;
 	Set<const Space2DSW *> active_spaces;
 
-	Physics2DDirectBodyStateSW *direct_state;
-
 	mutable RID_Owner<Shape2DSW> shape_owner;
 	mutable RID_Owner<Space2DSW> space_owner;
 	mutable RID_Owner<Area2DSW> area_owner;

servers/physics_2d/space_2d_sw.cpp

@@ -838,7 +838,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 * Physics2DDirectBodyStateSW::singleton->step;
+								Vector2 motion = lv * step;
 								float motion_len = motion.length();
 								motion.normalize();
 								cbk.valid_depth += motion_len * MAX(motion.dot(-cbk.valid_dir), 0.0);
@@ -1141,7 +1141,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 * Physics2DDirectBodyStateSW::singleton->step;
+							Vector2 motion = lv * step;
 							float 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,7 @@ private:
 
 	bool locked;
 
+	real_t step;
 	int island_count;
 	int active_objects;
 	int collision_pairs;
@@ -132,6 +133,9 @@ public:
 	_FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; }
 	_FORCE_INLINE_ RID get_self() const { return self; }
 
+	_FORCE_INLINE_ void set_step(const real_t &p_step) { step = p_step; }
+	_FORCE_INLINE_ real_t get_step() const { return step; }
+
 	void set_default_area(Area2DSW *p_area) { area = p_area; }
 	Area2DSW *get_default_area() const { return area; }
 

servers/physics_2d/step_2d_sw.cpp

@@ -130,7 +130,7 @@ void Step2DSW::_check_suspend(Body2DSW *p_island, real_t p_delta) {
 
 void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) {
 	p_space->lock(); // can't access space during this
-
+	p_space->set_step(p_delta);
 	p_space->setup(); //update inertias, etc
 
 	const SelfList<Body2DSW>::List *body_list = &p_space->get_active_body_list();