Make Servers truly Thread Safe

-Rendering server now uses a split RID allocate/initialize internally, this allows generating RIDs immediately but initialization to happen later on the proper thread (as rendering APIs generally requiere to call on the right thread).
-RenderingServerWrapMT is no more, multithreading is done in RenderingServerDefault.
-Some functions like texture or mesh creation, when renderer supports it, can register and return immediately (so no waiting for server API to flush, and saving staging and command buffer memory).
-3D physics server changed to be made multithread friendly.
-Added PhysicsServer3DWrapMT to use 3D physics server from multiple threads.
-Disablet Bullet (too much effort to make multithread friendly, this needs to be fixed eventually).

1 files changed, 182 insertions, 135 deletions

diff --git a/servers/physics_3d/physics_server_3d_sw.cpp b/servers/physics_3d/physics_server_3d_sw.cpp
index b554a23bf2..d5d40c181f 100644
--- a/servers/physics_3d/physics_server_3d_sw.cpp
+++ b/servers/physics_3d/physics_server_3d_sw.cpp
@@ -43,47 +43,60 @@
 #define FLUSH_QUERY_CHECK(m_object) \
 	ERR_FAIL_COND_MSG(m_object->get_space() && flushing_queries, "Can't change this state while flushing queries. Use call_deferred() or set_deferred() to change monitoring state instead.");
 
-RID PhysicsServer3DSW::shape_create(ShapeType p_shape) {
-	Shape3DSW *shape = nullptr;
-	switch (p_shape) {
-		case SHAPE_PLANE: {
-			shape = memnew(PlaneShape3DSW);
-		} break;
-		case SHAPE_RAY: {
-			shape = memnew(RayShape3DSW);
-		} break;
-		case SHAPE_SPHERE: {
-			shape = memnew(SphereShape3DSW);
-		} break;
-		case SHAPE_BOX: {
-			shape = memnew(BoxShape3DSW);
-		} break;
-		case SHAPE_CAPSULE: {
-			shape = memnew(CapsuleShape3DSW);
-		} break;
-		case SHAPE_CYLINDER: {
-			ERR_FAIL_V_MSG(RID(), "CylinderShape3D is not supported in GodotPhysics3D. Please switch to Bullet in the Project Settings.");
-		} break;
-		case SHAPE_CONVEX_POLYGON: {
-			shape = memnew(ConvexPolygonShape3DSW);
-		} break;
-		case SHAPE_CONCAVE_POLYGON: {
-			shape = memnew(ConcavePolygonShape3DSW);
-		} break;
-		case SHAPE_HEIGHTMAP: {
-			shape = memnew(HeightMapShape3DSW);
-		} break;
-		case SHAPE_CUSTOM: {
-			ERR_FAIL_V(RID());
-
-		} break;
-	}
-
-	RID id = shape_owner.make_rid(shape);
-	shape->set_self(id);
-
-	return id;
-};
+RID PhysicsServer3DSW::plane_shape_create() {
+	Shape3DSW *shape = memnew(PlaneShape3DSW);
+	RID rid = shape_owner.make_rid(shape);
+	shape->set_self(rid);
+	return rid;
+}
+RID PhysicsServer3DSW::ray_shape_create() {
+	Shape3DSW *shape = memnew(RayShape3DSW);
+	RID rid = shape_owner.make_rid(shape);
+	shape->set_self(rid);
+	return rid;
+}
+RID PhysicsServer3DSW::sphere_shape_create() {
+	Shape3DSW *shape = memnew(SphereShape3DSW);
+	RID rid = shape_owner.make_rid(shape);
+	shape->set_self(rid);
+	return rid;
+}
+RID PhysicsServer3DSW::box_shape_create() {
+	Shape3DSW *shape = memnew(BoxShape3DSW);
+	RID rid = shape_owner.make_rid(shape);
+	shape->set_self(rid);
+	return rid;
+}
+RID PhysicsServer3DSW::capsule_shape_create() {
+	Shape3DSW *shape = memnew(CapsuleShape3DSW);
+	RID rid = shape_owner.make_rid(shape);
+	shape->set_self(rid);
+	return rid;
+}
+RID PhysicsServer3DSW::cylinder_shape_create() {
+	ERR_FAIL_V(RID());
+}
+RID PhysicsServer3DSW::convex_polygon_shape_create() {
+	Shape3DSW *shape = memnew(ConvexPolygonShape3DSW);
+	RID rid = shape_owner.make_rid(shape);
+	shape->set_self(rid);
+	return rid;
+}
+RID PhysicsServer3DSW::concave_polygon_shape_create() {
+	Shape3DSW *shape = memnew(ConcavePolygonShape3DSW);
+	RID rid = shape_owner.make_rid(shape);
+	shape->set_self(rid);
+	return rid;
+}
+RID PhysicsServer3DSW::heightmap_shape_create() {
+	Shape3DSW *shape = memnew(HeightMapShape3DSW);
+	RID rid = shape_owner.make_rid(shape);
+	shape->set_self(rid);
+	return rid;
+}
+RID PhysicsServer3DSW::custom_shape_create() {
+	ERR_FAIL_V(RID());
+}
 
 void PhysicsServer3DSW::shape_set_data(RID p_shape, const Variant &p_data) {
 	Shape3DSW *shape = shape_owner.getornull(p_shape);
@@ -174,7 +187,7 @@ real_t PhysicsServer3DSW::space_get_param(RID p_space, SpaceParameter p_param) c
 PhysicsDirectSpaceState3D *PhysicsServer3DSW::space_get_direct_state(RID p_space) {
 	Space3DSW *space = space_owner.getornull(p_space);
 	ERR_FAIL_COND_V(!space, nullptr);
-	ERR_FAIL_COND_V_MSG(space->is_locked(), nullptr, "Space state is inaccessible right now, wait for iteration or physics process notification.");
+	ERR_FAIL_COND_V_MSG((using_threads && !doing_sync) || space->is_locked(), nullptr, "Space state is inaccessible right now, wait for iteration or physics process notification.");
 
 	return space->get_direct_state();
 }
@@ -413,13 +426,6 @@ void PhysicsServer3DSW::area_set_ray_pickable(RID p_area, bool p_enable) {
 	area->set_ray_pickable(p_enable);
 }
 
-bool PhysicsServer3DSW::area_is_ray_pickable(RID p_area) const {
-	Area3DSW *area = area_owner.getornull(p_area);
-	ERR_FAIL_COND_V(!area, false);
-
-	return area->is_ray_pickable();
-}
-
 void PhysicsServer3DSW::area_set_area_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) {
 	Area3DSW *area = area_owner.getornull(p_area);
 	ERR_FAIL_COND(!area);
@@ -429,14 +435,8 @@ void PhysicsServer3DSW::area_set_area_monitor_callback(RID p_area, Object *p_rec
 
 /* BODY API */
 
-RID PhysicsServer3DSW::body_create(BodyMode p_mode, bool p_init_sleeping) {
+RID PhysicsServer3DSW::body_create() {
 	Body3DSW *body = memnew(Body3DSW);
-	if (p_mode != BODY_MODE_RIGID) {
-		body->set_mode(p_mode);
-	}
-	if (p_init_sleeping) {
-		body->set_state(BODY_STATE_SLEEPING, p_init_sleeping);
-	}
 	RID rid = body_owner.make_rid(body);
 	body->set_self(rid);
 	return rid;
@@ -857,12 +857,6 @@ void PhysicsServer3DSW::body_set_ray_pickable(RID p_body, bool p_enable) {
 	body->set_ray_pickable(p_enable);
 }
 
-bool PhysicsServer3DSW::body_is_ray_pickable(RID p_body) const {
-	Body3DSW *body = body_owner.getornull(p_body);
-	ERR_FAIL_COND_V(!body, false);
-	return body->is_ray_pickable();
-}
-
 bool PhysicsServer3DSW::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) {
 	Body3DSW *body = body_owner.getornull(p_body);
 	ERR_FAIL_COND_V(!body, false);
@@ -886,6 +880,8 @@ int PhysicsServer3DSW::body_test_ray_separation(RID p_body, const Transform &p_t
 }
 
 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_COND_V_MSG(body->get_space()->is_locked(), nullptr, "Body state is inaccessible right now, wait for iteration or physics process notification.");
@@ -896,30 +892,52 @@ PhysicsDirectBodyState3D *PhysicsServer3DSW::body_get_direct_state(RID p_body) {
 
 /* JOINT API */
 
-RID PhysicsServer3DSW::joint_create_pin(RID p_body_A, const Vector3 &p_local_A, RID p_body_B, const Vector3 &p_local_B) {
+RID PhysicsServer3DSW::joint_create() {
+	Joint3DSW *joint = memnew(Joint3DSW);
+	RID rid = joint_owner.make_rid(joint);
+	joint->set_self(rid);
+	return rid;
+}
+
+void PhysicsServer3DSW::joint_clear(RID p_joint) {
+	Joint3DSW *joint = joint_owner.getornull(p_joint);
+	if (joint->get_type() != JOINT_TYPE_MAX) {
+		Joint3DSW *empty_joint = memnew(Joint3DSW);
+		empty_joint->copy_settings_from(joint);
+
+		joint_owner.replace(p_joint, empty_joint);
+		memdelete(joint);
+	}
+}
+
+void PhysicsServer3DSW::joint_make_pin(RID p_joint, RID p_body_A, const Vector3 &p_local_A, RID p_body_B, const Vector3 &p_local_B) {
 	Body3DSW *body_A = body_owner.getornull(p_body_A);
-	ERR_FAIL_COND_V(!body_A, RID());
+	ERR_FAIL_COND(!body_A);
 
 	if (!p_body_B.is_valid()) {
-		ERR_FAIL_COND_V(!body_A->get_space(), RID());
+		ERR_FAIL_COND(!body_A->get_space());
 		p_body_B = body_A->get_space()->get_static_global_body();
 	}
 
 	Body3DSW *body_B = body_owner.getornull(p_body_B);
-	ERR_FAIL_COND_V(!body_B, RID());
+	ERR_FAIL_COND(!body_B);
+
+	ERR_FAIL_COND(body_A == body_B);
 
-	ERR_FAIL_COND_V(body_A == body_B, RID());
+	Joint3DSW *prev_joint = joint_owner.getornull(p_joint);
+	ERR_FAIL_COND(prev_joint == nullptr);
 
 	Joint3DSW *joint = memnew(PinJoint3DSW(body_A, p_local_A, body_B, p_local_B));
-	RID rid = joint_owner.make_rid(joint);
-	joint->set_self(rid);
-	return rid;
+
+	joint->copy_settings_from(prev_joint);
+	joint_owner.replace(p_joint, joint);
+	memdelete(prev_joint);
 }
 
 void PhysicsServer3DSW::pin_joint_set_param(RID p_joint, PinJointParam p_param, real_t p_value) {
 	Joint3DSW *joint = joint_owner.getornull(p_joint);
 	ERR_FAIL_COND(!joint);
-	ERR_FAIL_COND(joint->get_type() != JOINT_PIN);
+	ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_PIN);
 	PinJoint3DSW *pin_joint = static_cast<PinJoint3DSW *>(joint);
 	pin_joint->set_param(p_param, p_value);
 }
@@ -927,7 +945,7 @@ void PhysicsServer3DSW::pin_joint_set_param(RID p_joint, PinJointParam p_param,
 real_t PhysicsServer3DSW::pin_joint_get_param(RID p_joint, PinJointParam p_param) const {
 	Joint3DSW *joint = joint_owner.getornull(p_joint);
 	ERR_FAIL_COND_V(!joint, 0);
-	ERR_FAIL_COND_V(joint->get_type() != JOINT_PIN, 0);
+	ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_PIN, 0);
 	PinJoint3DSW *pin_joint = static_cast<PinJoint3DSW *>(joint);
 	return pin_joint->get_param(p_param);
 }
@@ -935,7 +953,7 @@ real_t PhysicsServer3DSW::pin_joint_get_param(RID p_joint, PinJointParam p_param
 void PhysicsServer3DSW::pin_joint_set_local_a(RID p_joint, const Vector3 &p_A) {
 	Joint3DSW *joint = joint_owner.getornull(p_joint);
 	ERR_FAIL_COND(!joint);
-	ERR_FAIL_COND(joint->get_type() != JOINT_PIN);
+	ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_PIN);
 	PinJoint3DSW *pin_joint = static_cast<PinJoint3DSW *>(joint);
 	pin_joint->set_pos_a(p_A);
 }
@@ -943,7 +961,7 @@ void PhysicsServer3DSW::pin_joint_set_local_a(RID p_joint, const Vector3 &p_A) {
 Vector3 PhysicsServer3DSW::pin_joint_get_local_a(RID p_joint) const {
 	Joint3DSW *joint = joint_owner.getornull(p_joint);
 	ERR_FAIL_COND_V(!joint, Vector3());
-	ERR_FAIL_COND_V(joint->get_type() != JOINT_PIN, Vector3());
+	ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_PIN, Vector3());
 	PinJoint3DSW *pin_joint = static_cast<PinJoint3DSW *>(joint);
 	return pin_joint->get_position_a();
 }
@@ -951,7 +969,7 @@ Vector3 PhysicsServer3DSW::pin_joint_get_local_a(RID p_joint) const {
 void PhysicsServer3DSW::pin_joint_set_local_b(RID p_joint, const Vector3 &p_B) {
 	Joint3DSW *joint = joint_owner.getornull(p_joint);
 	ERR_FAIL_COND(!joint);
-	ERR_FAIL_COND(joint->get_type() != JOINT_PIN);
+	ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_PIN);
 	PinJoint3DSW *pin_joint = static_cast<PinJoint3DSW *>(joint);
 	pin_joint->set_pos_b(p_B);
 }
@@ -959,55 +977,63 @@ void PhysicsServer3DSW::pin_joint_set_local_b(RID p_joint, const Vector3 &p_B) {
 Vector3 PhysicsServer3DSW::pin_joint_get_local_b(RID p_joint) const {
 	Joint3DSW *joint = joint_owner.getornull(p_joint);
 	ERR_FAIL_COND_V(!joint, Vector3());
-	ERR_FAIL_COND_V(joint->get_type() != JOINT_PIN, Vector3());
+	ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_PIN, Vector3());
 	PinJoint3DSW *pin_joint = static_cast<PinJoint3DSW *>(joint);
 	return pin_joint->get_position_b();
 }
 
-RID PhysicsServer3DSW::joint_create_hinge(RID p_body_A, const Transform &p_frame_A, RID p_body_B, const Transform &p_frame_B) {
+void PhysicsServer3DSW::joint_make_hinge(RID p_joint, RID p_body_A, const Transform &p_frame_A, RID p_body_B, const Transform &p_frame_B) {
 	Body3DSW *body_A = body_owner.getornull(p_body_A);
-	ERR_FAIL_COND_V(!body_A, RID());
+	ERR_FAIL_COND(!body_A);
 
 	if (!p_body_B.is_valid()) {
-		ERR_FAIL_COND_V(!body_A->get_space(), RID());
+		ERR_FAIL_COND(!body_A->get_space());
 		p_body_B = body_A->get_space()->get_static_global_body();
 	}
 
 	Body3DSW *body_B = body_owner.getornull(p_body_B);
-	ERR_FAIL_COND_V(!body_B, RID());
+	ERR_FAIL_COND(!body_B);
 
-	ERR_FAIL_COND_V(body_A == body_B, RID());
+	ERR_FAIL_COND(body_A == body_B);
+
+	Joint3DSW *prev_joint = joint_owner.getornull(p_joint);
+	ERR_FAIL_COND(prev_joint == nullptr);
 
 	Joint3DSW *joint = memnew(HingeJoint3DSW(body_A, body_B, p_frame_A, p_frame_B));
-	RID rid = joint_owner.make_rid(joint);
-	joint->set_self(rid);
-	return rid;
+
+	joint->copy_settings_from(prev_joint);
+	joint_owner.replace(p_joint, joint);
+	memdelete(prev_joint);
 }
 
-RID PhysicsServer3DSW::joint_create_hinge_simple(RID p_body_A, const Vector3 &p_pivot_A, const Vector3 &p_axis_A, RID p_body_B, const Vector3 &p_pivot_B, const Vector3 &p_axis_B) {
+void PhysicsServer3DSW::joint_make_hinge_simple(RID p_joint, RID p_body_A, const Vector3 &p_pivot_A, const Vector3 &p_axis_A, RID p_body_B, const Vector3 &p_pivot_B, const Vector3 &p_axis_B) {
 	Body3DSW *body_A = body_owner.getornull(p_body_A);
-	ERR_FAIL_COND_V(!body_A, RID());
+	ERR_FAIL_COND(!body_A);
 
 	if (!p_body_B.is_valid()) {
-		ERR_FAIL_COND_V(!body_A->get_space(), RID());
+		ERR_FAIL_COND(!body_A->get_space());
 		p_body_B = body_A->get_space()->get_static_global_body();
 	}
 
 	Body3DSW *body_B = body_owner.getornull(p_body_B);
-	ERR_FAIL_COND_V(!body_B, RID());
+	ERR_FAIL_COND(!body_B);
+
+	ERR_FAIL_COND(body_A == body_B);
 
-	ERR_FAIL_COND_V(body_A == body_B, RID());
+	Joint3DSW *prev_joint = joint_owner.getornull(p_joint);
+	ERR_FAIL_COND(prev_joint == nullptr);
 
 	Joint3DSW *joint = memnew(HingeJoint3DSW(body_A, body_B, p_pivot_A, p_pivot_B, p_axis_A, p_axis_B));
-	RID rid = joint_owner.make_rid(joint);
-	joint->set_self(rid);
-	return rid;
+
+	joint->copy_settings_from(prev_joint);
+	joint_owner.replace(p_joint, joint);
+	memdelete(prev_joint);
 }
 
 void PhysicsServer3DSW::hinge_joint_set_param(RID p_joint, HingeJointParam p_param, real_t p_value) {
 	Joint3DSW *joint = joint_owner.getornull(p_joint);
 	ERR_FAIL_COND(!joint);
-	ERR_FAIL_COND(joint->get_type() != JOINT_HINGE);
+	ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_HINGE);
 	HingeJoint3DSW *hinge_joint = static_cast<HingeJoint3DSW *>(joint);
 	hinge_joint->set_param(p_param, p_value);
 }
@@ -1015,7 +1041,7 @@ void PhysicsServer3DSW::hinge_joint_set_param(RID p_joint, HingeJointParam p_par
 real_t PhysicsServer3DSW::hinge_joint_get_param(RID p_joint, HingeJointParam p_param) const {
 	Joint3DSW *joint = joint_owner.getornull(p_joint);
 	ERR_FAIL_COND_V(!joint, 0);
-	ERR_FAIL_COND_V(joint->get_type() != JOINT_HINGE, 0);
+	ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_HINGE, 0);
 	HingeJoint3DSW *hinge_joint = static_cast<HingeJoint3DSW *>(joint);
 	return hinge_joint->get_param(p_param);
 }
@@ -1023,7 +1049,7 @@ real_t PhysicsServer3DSW::hinge_joint_get_param(RID p_joint, HingeJointParam p_p
 void PhysicsServer3DSW::hinge_joint_set_flag(RID p_joint, HingeJointFlag p_flag, bool p_value) {
 	Joint3DSW *joint = joint_owner.getornull(p_joint);
 	ERR_FAIL_COND(!joint);
-	ERR_FAIL_COND(joint->get_type() != JOINT_HINGE);
+	ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_HINGE);
 	HingeJoint3DSW *hinge_joint = static_cast<HingeJoint3DSW *>(joint);
 	hinge_joint->set_flag(p_flag, p_value);
 }
@@ -1031,7 +1057,7 @@ void PhysicsServer3DSW::hinge_joint_set_flag(RID p_joint, HingeJointFlag p_flag,
 bool PhysicsServer3DSW::hinge_joint_get_flag(RID p_joint, HingeJointFlag p_flag) const {
 	Joint3DSW *joint = joint_owner.getornull(p_joint);
 	ERR_FAIL_COND_V(!joint, false);
-	ERR_FAIL_COND_V(joint->get_type() != JOINT_HINGE, false);
+	ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_HINGE, false);
 	HingeJoint3DSW *hinge_joint = static_cast<HingeJoint3DSW *>(joint);
 	return hinge_joint->get_flag(p_flag);
 }
@@ -1077,34 +1103,38 @@ bool PhysicsServer3DSW::joint_is_disabled_collisions_between_bodies(RID p_joint)
 
 PhysicsServer3DSW::JointType PhysicsServer3DSW::joint_get_type(RID p_joint) const {
 	Joint3DSW *joint = joint_owner.getornull(p_joint);
-	ERR_FAIL_COND_V(!joint, JOINT_PIN);
+	ERR_FAIL_COND_V(!joint, JOINT_TYPE_PIN);
 	return joint->get_type();
 }
 
-RID PhysicsServer3DSW::joint_create_slider(RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) {
+void PhysicsServer3DSW::joint_make_slider(RID p_joint, RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) {
 	Body3DSW *body_A = body_owner.getornull(p_body_A);
-	ERR_FAIL_COND_V(!body_A, RID());
+	ERR_FAIL_COND(!body_A);
 
 	if (!p_body_B.is_valid()) {
-		ERR_FAIL_COND_V(!body_A->get_space(), RID());
+		ERR_FAIL_COND(!body_A->get_space());
 		p_body_B = body_A->get_space()->get_static_global_body();
 	}
 
 	Body3DSW *body_B = body_owner.getornull(p_body_B);
-	ERR_FAIL_COND_V(!body_B, RID());
+	ERR_FAIL_COND(!body_B);
+
+	ERR_FAIL_COND(body_A == body_B);
 
-	ERR_FAIL_COND_V(body_A == body_B, RID());
+	Joint3DSW *prev_joint = joint_owner.getornull(p_joint);
+	ERR_FAIL_COND(prev_joint == nullptr);
 
 	Joint3DSW *joint = memnew(SliderJoint3DSW(body_A, body_B, p_local_frame_A, p_local_frame_B));
-	RID rid = joint_owner.make_rid(joint);
-	joint->set_self(rid);
-	return rid;
+
+	joint->copy_settings_from(prev_joint);
+	joint_owner.replace(p_joint, joint);
+	memdelete(prev_joint);
 }
 
 void PhysicsServer3DSW::slider_joint_set_param(RID p_joint, SliderJointParam p_param, real_t p_value) {
 	Joint3DSW *joint = joint_owner.getornull(p_joint);
 	ERR_FAIL_COND(!joint);
-	ERR_FAIL_COND(joint->get_type() != JOINT_SLIDER);
+	ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_SLIDER);
 	SliderJoint3DSW *slider_joint = static_cast<SliderJoint3DSW *>(joint);
 	slider_joint->set_param(p_param, p_value);
 }
@@ -1112,35 +1142,39 @@ void PhysicsServer3DSW::slider_joint_set_param(RID p_joint, SliderJointParam p_p
 real_t PhysicsServer3DSW::slider_joint_get_param(RID p_joint, SliderJointParam p_param) const {
 	Joint3DSW *joint = joint_owner.getornull(p_joint);
 	ERR_FAIL_COND_V(!joint, 0);
-	ERR_FAIL_COND_V(joint->get_type() != JOINT_CONE_TWIST, 0);
+	ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_CONE_TWIST, 0);
 	SliderJoint3DSW *slider_joint = static_cast<SliderJoint3DSW *>(joint);
 	return slider_joint->get_param(p_param);
 }
 
-RID PhysicsServer3DSW::joint_create_cone_twist(RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) {
+void PhysicsServer3DSW::joint_make_cone_twist(RID p_joint, RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) {
 	Body3DSW *body_A = body_owner.getornull(p_body_A);
-	ERR_FAIL_COND_V(!body_A, RID());
+	ERR_FAIL_COND(!body_A);
 
 	if (!p_body_B.is_valid()) {
-		ERR_FAIL_COND_V(!body_A->get_space(), RID());
+		ERR_FAIL_COND(!body_A->get_space());
 		p_body_B = body_A->get_space()->get_static_global_body();
 	}
 
 	Body3DSW *body_B = body_owner.getornull(p_body_B);
-	ERR_FAIL_COND_V(!body_B, RID());
+	ERR_FAIL_COND(!body_B);
+
+	ERR_FAIL_COND(body_A == body_B);
 
-	ERR_FAIL_COND_V(body_A == body_B, RID());
+	Joint3DSW *prev_joint = joint_owner.getornull(p_joint);
+	ERR_FAIL_COND(prev_joint == nullptr);
 
 	Joint3DSW *joint = memnew(ConeTwistJoint3DSW(body_A, body_B, p_local_frame_A, p_local_frame_B));
-	RID rid = joint_owner.make_rid(joint);
-	joint->set_self(rid);
-	return rid;
+
+	joint->copy_settings_from(prev_joint);
+	joint_owner.replace(p_joint, joint);
+	memdelete(prev_joint);
 }
 
 void PhysicsServer3DSW::cone_twist_joint_set_param(RID p_joint, ConeTwistJointParam p_param, real_t p_value) {
 	Joint3DSW *joint = joint_owner.getornull(p_joint);
 	ERR_FAIL_COND(!joint);
-	ERR_FAIL_COND(joint->get_type() != JOINT_CONE_TWIST);
+	ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_CONE_TWIST);
 	ConeTwistJoint3DSW *cone_twist_joint = static_cast<ConeTwistJoint3DSW *>(joint);
 	cone_twist_joint->set_param(p_param, p_value);
 }
@@ -1148,43 +1182,47 @@ void PhysicsServer3DSW::cone_twist_joint_set_param(RID p_joint, ConeTwistJointPa
 real_t PhysicsServer3DSW::cone_twist_joint_get_param(RID p_joint, ConeTwistJointParam p_param) const {
 	Joint3DSW *joint = joint_owner.getornull(p_joint);
 	ERR_FAIL_COND_V(!joint, 0);
-	ERR_FAIL_COND_V(joint->get_type() != JOINT_CONE_TWIST, 0);
+	ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_CONE_TWIST, 0);
 	ConeTwistJoint3DSW *cone_twist_joint = static_cast<ConeTwistJoint3DSW *>(joint);
 	return cone_twist_joint->get_param(p_param);
 }
 
-RID PhysicsServer3DSW::joint_create_generic_6dof(RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) {
+void PhysicsServer3DSW::joint_make_generic_6dof(RID p_joint, RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) {
 	Body3DSW *body_A = body_owner.getornull(p_body_A);
-	ERR_FAIL_COND_V(!body_A, RID());
+	ERR_FAIL_COND(!body_A);
 
 	if (!p_body_B.is_valid()) {
-		ERR_FAIL_COND_V(!body_A->get_space(), RID());
+		ERR_FAIL_COND(!body_A->get_space());
 		p_body_B = body_A->get_space()->get_static_global_body();
 	}
 
 	Body3DSW *body_B = body_owner.getornull(p_body_B);
-	ERR_FAIL_COND_V(!body_B, RID());
+	ERR_FAIL_COND(!body_B);
+
+	ERR_FAIL_COND(body_A == body_B);
 
-	ERR_FAIL_COND_V(body_A == body_B, RID());
+	Joint3DSW *prev_joint = joint_owner.getornull(p_joint);
+	ERR_FAIL_COND(prev_joint == nullptr);
 
 	Joint3DSW *joint = memnew(Generic6DOFJoint3DSW(body_A, body_B, p_local_frame_A, p_local_frame_B, true));
-	RID rid = joint_owner.make_rid(joint);
-	joint->set_self(rid);
-	return rid;
+
+	joint->copy_settings_from(prev_joint);
+	joint_owner.replace(p_joint, joint);
+	memdelete(prev_joint);
 }
 
 void PhysicsServer3DSW::generic_6dof_joint_set_param(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisParam p_param, real_t p_value) {
 	Joint3DSW *joint = joint_owner.getornull(p_joint);
 	ERR_FAIL_COND(!joint);
-	ERR_FAIL_COND(joint->get_type() != JOINT_6DOF);
+	ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_6DOF);
 	Generic6DOFJoint3DSW *generic_6dof_joint = static_cast<Generic6DOFJoint3DSW *>(joint);
 	generic_6dof_joint->set_param(p_axis, p_param, p_value);
 }
 
-real_t PhysicsServer3DSW::generic_6dof_joint_get_param(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisParam p_param) {
+real_t PhysicsServer3DSW::generic_6dof_joint_get_param(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisParam p_param) const {
 	Joint3DSW *joint = joint_owner.getornull(p_joint);
 	ERR_FAIL_COND_V(!joint, 0);
-	ERR_FAIL_COND_V(joint->get_type() != JOINT_6DOF, 0);
+	ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_6DOF, 0);
 	Generic6DOFJoint3DSW *generic_6dof_joint = static_cast<Generic6DOFJoint3DSW *>(joint);
 	return generic_6dof_joint->get_param(p_axis, p_param);
 }
@@ -1192,15 +1230,15 @@ real_t PhysicsServer3DSW::generic_6dof_joint_get_param(RID p_joint, Vector3::Axi
 void PhysicsServer3DSW::generic_6dof_joint_set_flag(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisFlag p_flag, bool p_enable) {
 	Joint3DSW *joint = joint_owner.getornull(p_joint);
 	ERR_FAIL_COND(!joint);
-	ERR_FAIL_COND(joint->get_type() != JOINT_6DOF);
+	ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_6DOF);
 	Generic6DOFJoint3DSW *generic_6dof_joint = static_cast<Generic6DOFJoint3DSW *>(joint);
 	generic_6dof_joint->set_flag(p_axis, p_flag, p_enable);
 }
 
-bool PhysicsServer3DSW::generic_6dof_joint_get_flag(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisFlag p_flag) {
+bool PhysicsServer3DSW::generic_6dof_joint_get_flag(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisFlag p_flag) const {
 	Joint3DSW *joint = joint_owner.getornull(p_joint);
 	ERR_FAIL_COND_V(!joint, false);
-	ERR_FAIL_COND_V(joint->get_type() != JOINT_6DOF, false);
+	ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_6DOF, false);
 	Generic6DOFJoint3DSW *generic_6dof_joint = static_cast<Generic6DOFJoint3DSW *>(joint);
 	return generic_6dof_joint->get_flag(p_axis, p_flag);
 }
@@ -1317,6 +1355,10 @@ void PhysicsServer3DSW::step(real_t p_step) {
 #endif
 }
 
+void PhysicsServer3DSW::sync() {
+	doing_sync = true;
+};
+
 void PhysicsServer3DSW::flush_queries() {
 #ifndef _3D_DISABLED
 
@@ -1370,6 +1412,10 @@ void PhysicsServer3DSW::flush_queries() {
 #endif
 };
 
+void PhysicsServer3DSW::end_sync() {
+	doing_sync = false;
+};
+
 void PhysicsServer3DSW::finish() {
 	memdelete(stepper);
 	memdelete(direct_state);
@@ -1431,14 +1477,15 @@ void PhysicsServer3DSW::_shape_col_cbk(const Vector3 &p_point_A, const Vector3 &
 	}
 }
 
-PhysicsServer3DSW *PhysicsServer3DSW::singleton = nullptr;
-PhysicsServer3DSW::PhysicsServer3DSW() {
-	singleton = this;
+PhysicsServer3DSW *PhysicsServer3DSW::singletonsw = nullptr;
+PhysicsServer3DSW::PhysicsServer3DSW(bool p_using_threads) {
+	singletonsw = this;
 	BroadPhase3DSW::create_func = BroadPhaseOctree::_create;
 	island_count = 0;
 	active_objects = 0;
 	collision_pairs = 0;
-
+	using_threads = p_using_threads;
 	active = true;
 	flushing_queries = false;
+	doing_sync = false;
 };