9 files changed, 141 insertions, 116 deletions

diff --git a/servers/physics_2d/collision_object_2d_sw.h b/servers/physics_2d/collision_object_2d_sw.h
index 2939b4b99f..2db3961f41 100644
--- a/servers/physics_2d/collision_object_2d_sw.h
+++ b/servers/physics_2d/collision_object_2d_sw.h
@@ -61,7 +61,7 @@ private:
 		Variant metadata;
 		bool disabled;
 		bool one_way_collision;
-		float one_way_collision_margin;
+		real_t one_way_collision_margin;
 		Shape() {
 			disabled = false;
 			one_way_collision = false;
@@ -153,7 +153,7 @@ public:
 		return shapes[p_idx].disabled;
 	}
 
-	_FORCE_INLINE_ void set_shape_as_one_way_collision(int p_idx, bool p_one_way_collision, float p_margin) {
+	_FORCE_INLINE_ void set_shape_as_one_way_collision(int p_idx, bool p_one_way_collision, real_t p_margin) {
 		CRASH_BAD_INDEX(p_idx, shapes.size());
 		shapes.write[p_idx].one_way_collision = p_one_way_collision;
 		shapes.write[p_idx].one_way_collision_margin = p_margin;
@@ -163,7 +163,7 @@ public:
 		return shapes[p_idx].one_way_collision;
 	}
 
-	_FORCE_INLINE_ float get_shape_one_way_collision_margin(int p_idx) const {
+	_FORCE_INLINE_ real_t get_shape_one_way_collision_margin(int p_idx) const {
 		CRASH_BAD_INDEX(p_idx, shapes.size());
 		return shapes[p_idx].one_way_collision_margin;
 	}
diff --git a/servers/physics_2d/joints_2d_sw.cpp b/servers/physics_2d/joints_2d_sw.cpp
index 3558848dac..f503868ba5 100644
--- a/servers/physics_2d/joints_2d_sw.cpp
+++ b/servers/physics_2d/joints_2d_sw.cpp
@@ -55,6 +55,14 @@
  * SOFTWARE.
  */
 
+void Joint2DSW::copy_settings_from(Joint2DSW *p_joint) {
+	set_self(p_joint->get_self());
+	set_max_force(p_joint->get_max_force());
+	set_bias(p_joint->get_bias());
+	set_max_bias(p_joint->get_max_bias());
+	disable_collisions_between_bodies(p_joint->is_disabled_collisions_between_bodies());
+}
+
 static inline real_t k_scalar(Body2DSW *a, Body2DSW *b, const Vector2 &rA, const Vector2 &rB, const Vector2 &n) {
 	real_t value = 0;
 
diff --git a/servers/physics_2d/joints_2d_sw.h b/servers/physics_2d/joints_2d_sw.h
index 53e436b539..6050dc2775 100644
--- a/servers/physics_2d/joints_2d_sw.h
+++ b/servers/physics_2d/joints_2d_sw.h
@@ -49,7 +49,12 @@ public:
 	_FORCE_INLINE_ void set_max_bias(real_t p_bias) { max_bias = p_bias; }
 	_FORCE_INLINE_ real_t get_max_bias() const { return max_bias; }
 
-	virtual PhysicsServer2D::JointType get_type() const = 0;
+	virtual bool setup(real_t p_step) { return false; }
+	virtual void solve(real_t p_step) {}
+
+	void copy_settings_from(Joint2DSW *p_joint);
+
+	virtual PhysicsServer2D::JointType get_type() const { return PhysicsServer2D::JOINT_TYPE_MAX; }
 	Joint2DSW(Body2DSW **p_body_ptr = nullptr, int p_body_count = 0) :
 			Constraint2DSW(p_body_ptr, p_body_count) {
 		bias = 0;
@@ -76,7 +81,7 @@ class PinJoint2DSW : public Joint2DSW {
 	real_t softness;
 
 public:
-	virtual PhysicsServer2D::JointType get_type() const { return PhysicsServer2D::JOINT_PIN; }
+	virtual PhysicsServer2D::JointType get_type() const { return PhysicsServer2D::JOINT_TYPE_PIN; }
 
 	virtual bool setup(real_t p_step);
 	virtual void solve(real_t p_step);
@@ -113,7 +118,7 @@ class GrooveJoint2DSW : public Joint2DSW {
 	bool correct;
 
 public:
-	virtual PhysicsServer2D::JointType get_type() const { return PhysicsServer2D::JOINT_GROOVE; }
+	virtual PhysicsServer2D::JointType get_type() const { return PhysicsServer2D::JOINT_TYPE_GROOVE; }
 
 	virtual bool setup(real_t p_step);
 	virtual void solve(real_t p_step);
@@ -146,7 +151,7 @@ class DampedSpringJoint2DSW : public Joint2DSW {
 	real_t v_coef;
 
 public:
-	virtual PhysicsServer2D::JointType get_type() const { return PhysicsServer2D::JOINT_DAMPED_SPRING; }
+	virtual PhysicsServer2D::JointType get_type() const { return PhysicsServer2D::JOINT_TYPE_DAMPED_SPRING; }
 
 	virtual bool setup(real_t p_step);
 	virtual void solve(real_t p_step);
diff --git a/servers/physics_2d/physics_server_2d_sw.cpp b/servers/physics_2d/physics_server_2d_sw.cpp
index c4e2489bef..1040437ca7 100644
--- a/servers/physics_2d/physics_server_2d_sw.cpp
+++ b/servers/physics_2d/physics_server_2d_sw.cpp
@@ -667,7 +667,7 @@ void PhysicsServer2DSW::body_set_shape_disabled(RID p_body, int p_shape_idx, boo
 	body->set_shape_as_disabled(p_shape_idx, p_disabled);
 }
 
-void PhysicsServer2DSW::body_set_shape_as_one_way_collision(RID p_body, int p_shape_idx, bool p_enable, float p_margin) {
+void PhysicsServer2DSW::body_set_shape_as_one_way_collision(RID p_body, int p_shape_idx, bool p_enable, real_t p_margin) {
 	Body2DSW *body = body_owner.getornull(p_body);
 	ERR_FAIL_COND(!body);
 	ERR_FAIL_INDEX(p_shape_idx, body->get_shape_count());
@@ -958,7 +958,7 @@ bool PhysicsServer2DSW::body_test_motion(RID p_body, const Transform2D &p_from,
 	return body->get_space()->test_body_motion(body, p_from, p_motion, p_infinite_inertia, p_margin, r_result, p_exclude_raycast_shapes);
 }
 
-int PhysicsServer2DSW::body_test_ray_separation(RID p_body, const Transform2D &p_transform, bool p_infinite_inertia, Vector2 &r_recover_motion, SeparationResult *r_results, int p_result_max, float p_margin) {
+int PhysicsServer2DSW::body_test_ray_separation(RID p_body, const Transform2D &p_transform, bool p_infinite_inertia, Vector2 &r_recover_motion, SeparationResult *r_results, int p_result_max, real_t p_margin) {
 	Body2DSW *body = body_owner.getornull(p_body);
 	ERR_FAIL_COND_V(!body, false);
 	ERR_FAIL_COND_V(!body->get_space(), false);
@@ -985,6 +985,24 @@ PhysicsDirectBodyState2D *PhysicsServer2DSW::body_get_direct_state(RID p_body) {
 
 /* JOINT API */
 
+RID PhysicsServer2DSW::joint_create() {
+	Joint2DSW *joint = memnew(Joint2DSW);
+	RID joint_rid = joint_owner.make_rid(joint);
+	joint->set_self(joint_rid);
+	return joint_rid;
+}
+
+void PhysicsServer2DSW::joint_clear(RID p_joint) {
+	Joint2DSW *joint = joint_owner.getornull(p_joint);
+	if (joint->get_type() != JOINT_TYPE_MAX) {
+		Joint2DSW *empty_joint = memnew(Joint2DSW);
+		empty_joint->copy_settings_from(joint);
+
+		joint_owner.replace(p_joint, empty_joint);
+		memdelete(joint);
+	}
+}
+
 void PhysicsServer2DSW::joint_set_param(RID p_joint, JointParam p_param, real_t p_value) {
 	Joint2DSW *joint = joint_owner.getornull(p_joint);
 	ERR_FAIL_COND(!joint);
@@ -1048,52 +1066,63 @@ bool PhysicsServer2DSW::joint_is_disabled_collisions_between_bodies(RID p_joint)
 	return joint->is_disabled_collisions_between_bodies();
 }
 
-RID PhysicsServer2DSW::pin_joint_create(const Vector2 &p_pos, RID p_body_a, RID p_body_b) {
+void PhysicsServer2DSW::joint_make_pin(RID p_joint, const Vector2 &p_pos, RID p_body_a, RID p_body_b) {
 	Body2DSW *A = body_owner.getornull(p_body_a);
-	ERR_FAIL_COND_V(!A, RID());
+	ERR_FAIL_COND(!A);
 	Body2DSW *B = nullptr;
 	if (body_owner.owns(p_body_b)) {
 		B = body_owner.getornull(p_body_b);
-		ERR_FAIL_COND_V(!B, RID());
+		ERR_FAIL_COND(!B);
 	}
 
+	Joint2DSW *prev_joint = joint_owner.getornull(p_joint);
+	ERR_FAIL_COND(prev_joint == nullptr);
+
 	Joint2DSW *joint = memnew(PinJoint2DSW(p_pos, A, B));
-	RID self = joint_owner.make_rid(joint);
-	joint->set_self(self);
 
-	return self;
+	joint_owner.replace(p_joint, joint);
+	joint->copy_settings_from(prev_joint);
+	memdelete(prev_joint);
 }
 
-RID PhysicsServer2DSW::groove_joint_create(const Vector2 &p_a_groove1, const Vector2 &p_a_groove2, const Vector2 &p_b_anchor, RID p_body_a, RID p_body_b) {
+void PhysicsServer2DSW::joint_make_groove(RID p_joint, const Vector2 &p_a_groove1, const Vector2 &p_a_groove2, const Vector2 &p_b_anchor, RID p_body_a, RID p_body_b) {
 	Body2DSW *A = body_owner.getornull(p_body_a);
-	ERR_FAIL_COND_V(!A, RID());
+	ERR_FAIL_COND(!A);
 
 	Body2DSW *B = body_owner.getornull(p_body_b);
-	ERR_FAIL_COND_V(!B, RID());
+	ERR_FAIL_COND(!B);
+
+	Joint2DSW *prev_joint = joint_owner.getornull(p_joint);
+	ERR_FAIL_COND(prev_joint == nullptr);
 
 	Joint2DSW *joint = memnew(GrooveJoint2DSW(p_a_groove1, p_a_groove2, p_b_anchor, A, B));
-	RID self = joint_owner.make_rid(joint);
-	joint->set_self(self);
-	return self;
+
+	joint_owner.replace(p_joint, joint);
+	joint->copy_settings_from(prev_joint);
+	memdelete(prev_joint);
 }
 
-RID PhysicsServer2DSW::damped_spring_joint_create(const Vector2 &p_anchor_a, const Vector2 &p_anchor_b, RID p_body_a, RID p_body_b) {
+void PhysicsServer2DSW::joint_make_damped_spring(RID p_joint, const Vector2 &p_anchor_a, const Vector2 &p_anchor_b, RID p_body_a, RID p_body_b) {
 	Body2DSW *A = body_owner.getornull(p_body_a);
-	ERR_FAIL_COND_V(!A, RID());
+	ERR_FAIL_COND(!A);
 
 	Body2DSW *B = body_owner.getornull(p_body_b);
-	ERR_FAIL_COND_V(!B, RID());
+	ERR_FAIL_COND(!B);
+
+	Joint2DSW *prev_joint = joint_owner.getornull(p_joint);
+	ERR_FAIL_COND(prev_joint == nullptr);
 
 	Joint2DSW *joint = memnew(DampedSpringJoint2DSW(p_anchor_a, p_anchor_b, A, B));
-	RID self = joint_owner.make_rid(joint);
-	joint->set_self(self);
-	return self;
+
+	joint_owner.replace(p_joint, joint);
+	joint->copy_settings_from(prev_joint);
+	memdelete(prev_joint);
 }
 
 void PhysicsServer2DSW::pin_joint_set_param(RID p_joint, PinJointParam p_param, real_t p_value) {
 	Joint2DSW *j = joint_owner.getornull(p_joint);
 	ERR_FAIL_COND(!j);
-	ERR_FAIL_COND(j->get_type() != JOINT_PIN);
+	ERR_FAIL_COND(j->get_type() != JOINT_TYPE_PIN);
 
 	PinJoint2DSW *pin_joint = static_cast<PinJoint2DSW *>(j);
 	pin_joint->set_param(p_param, p_value);
@@ -1102,7 +1131,7 @@ void PhysicsServer2DSW::pin_joint_set_param(RID p_joint, PinJointParam p_param,
 real_t PhysicsServer2DSW::pin_joint_get_param(RID p_joint, PinJointParam p_param) const {
 	Joint2DSW *j = joint_owner.getornull(p_joint);
 	ERR_FAIL_COND_V(!j, 0);
-	ERR_FAIL_COND_V(j->get_type() != JOINT_PIN, 0);
+	ERR_FAIL_COND_V(j->get_type() != JOINT_TYPE_PIN, 0);
 
 	PinJoint2DSW *pin_joint = static_cast<PinJoint2DSW *>(j);
 	return pin_joint->get_param(p_param);
@@ -1111,7 +1140,7 @@ real_t PhysicsServer2DSW::pin_joint_get_param(RID p_joint, PinJointParam p_param
 void PhysicsServer2DSW::damped_spring_joint_set_param(RID p_joint, DampedSpringParam p_param, real_t p_value) {
 	Joint2DSW *j = joint_owner.getornull(p_joint);
 	ERR_FAIL_COND(!j);
-	ERR_FAIL_COND(j->get_type() != JOINT_DAMPED_SPRING);
+	ERR_FAIL_COND(j->get_type() != JOINT_TYPE_DAMPED_SPRING);
 
 	DampedSpringJoint2DSW *dsj = static_cast<DampedSpringJoint2DSW *>(j);
 	dsj->set_param(p_param, p_value);
@@ -1120,7 +1149,7 @@ void PhysicsServer2DSW::damped_spring_joint_set_param(RID p_joint, DampedSpringP
 real_t PhysicsServer2DSW::damped_spring_joint_get_param(RID p_joint, DampedSpringParam p_param) const {
 	Joint2DSW *j = joint_owner.getornull(p_joint);
 	ERR_FAIL_COND_V(!j, 0);
-	ERR_FAIL_COND_V(j->get_type() != JOINT_DAMPED_SPRING, 0);
+	ERR_FAIL_COND_V(j->get_type() != JOINT_TYPE_DAMPED_SPRING, 0);
 
 	DampedSpringJoint2DSW *dsj = static_cast<DampedSpringJoint2DSW *>(j);
 	return dsj->get_param(p_param);
@@ -1128,7 +1157,7 @@ real_t PhysicsServer2DSW::damped_spring_joint_get_param(RID p_joint, DampedSprin
 
 PhysicsServer2D::JointType PhysicsServer2DSW::joint_get_type(RID p_joint) const {
 	Joint2DSW *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();
 }
@@ -1325,7 +1354,7 @@ int PhysicsServer2DSW::get_process_info(ProcessInfo p_info) {
 
 PhysicsServer2DSW *PhysicsServer2DSW::singletonsw = nullptr;
 
-PhysicsServer2DSW::PhysicsServer2DSW() {
+PhysicsServer2DSW::PhysicsServer2DSW(bool p_using_threads) {
 	singletonsw = this;
 	BroadPhase2DSW::create_func = BroadPhase2DHashGrid::_create;
 	//BroadPhase2DSW::create_func=BroadPhase2DBasic::_create;
@@ -1334,10 +1363,6 @@ PhysicsServer2DSW::PhysicsServer2DSW() {
 	island_count = 0;
 	active_objects = 0;
 	collision_pairs = 0;
-#ifdef NO_THREADS
-	using_threads = false;
-#else
-	using_threads = int(ProjectSettings::get_singleton()->get("physics/2d/thread_model")) == 2;
-#endif
+	using_threads = p_using_threads;
 	flushing_queries = false;
 };
diff --git a/servers/physics_2d/physics_server_2d_sw.h b/servers/physics_2d/physics_server_2d_sw.h
index 3305c0bd3d..65c5df0fce 100644
--- a/servers/physics_2d/physics_server_2d_sw.h
+++ b/servers/physics_2d/physics_server_2d_sw.h
@@ -61,11 +61,11 @@ class PhysicsServer2DSW : public PhysicsServer2D {
 
 	PhysicsDirectBodyState2DSW *direct_state;
 
-	mutable RID_PtrOwner<Shape2DSW> shape_owner;
-	mutable RID_PtrOwner<Space2DSW> space_owner;
-	mutable RID_PtrOwner<Area2DSW> area_owner;
-	mutable RID_PtrOwner<Body2DSW> body_owner;
-	mutable RID_PtrOwner<Joint2DSW> joint_owner;
+	mutable RID_PtrOwner<Shape2DSW, true> shape_owner;
+	mutable RID_PtrOwner<Space2DSW, true> space_owner;
+	mutable RID_PtrOwner<Area2DSW, true> area_owner;
+	mutable RID_PtrOwner<Body2DSW, true> body_owner;
+	mutable RID_PtrOwner<Joint2DSW, true> joint_owner;
 
 	static PhysicsServer2DSW *singletonsw;
 
@@ -191,7 +191,7 @@ public:
 	virtual void body_clear_shapes(RID p_body) override;
 
 	virtual void body_set_shape_disabled(RID p_body, int p_shape_idx, bool p_disabled) override;
-	virtual void body_set_shape_as_one_way_collision(RID p_body, int p_shape_idx, bool p_enable, float p_margin) override;
+	virtual void body_set_shape_as_one_way_collision(RID p_body, int p_shape_idx, bool p_enable, real_t p_margin) override;
 
 	virtual void body_attach_object_instance_id(RID p_body, ObjectID p_id) override;
 	virtual ObjectID body_get_object_instance_id(RID p_body) const override;
@@ -248,22 +248,27 @@ public:
 	virtual void body_set_pickable(RID p_body, bool p_pickable) override;
 
 	virtual bool body_test_motion(RID p_body, const Transform2D &p_from, const Vector2 &p_motion, bool p_infinite_inertia, real_t p_margin = 0.001, MotionResult *r_result = nullptr, bool p_exclude_raycast_shapes = true) override;
-	virtual int body_test_ray_separation(RID p_body, const Transform2D &p_transform, bool p_infinite_inertia, Vector2 &r_recover_motion, SeparationResult *r_results, int p_result_max, float p_margin = 0.001) override;
+	virtual int body_test_ray_separation(RID p_body, const Transform2D &p_transform, bool p_infinite_inertia, Vector2 &r_recover_motion, SeparationResult *r_results, int p_result_max, real_t p_margin = 0.001) override;
 
 	// this function only works on physics process, errors and returns null otherwise
 	virtual PhysicsDirectBodyState2D *body_get_direct_state(RID p_body) override;
 
 	/* JOINT API */
 
+	virtual RID joint_create() override;
+
+	virtual void joint_clear(RID p_joint) override;
+
 	virtual void joint_set_param(RID p_joint, JointParam p_param, real_t p_value) override;
 	virtual real_t joint_get_param(RID p_joint, JointParam p_param) const override;
 
 	virtual void joint_disable_collisions_between_bodies(RID p_joint, const bool p_disabled) override;
 	virtual bool joint_is_disabled_collisions_between_bodies(RID p_joint) const override;
 
-	virtual RID pin_joint_create(const Vector2 &p_pos, RID p_body_a, RID p_body_b = RID()) override;
-	virtual RID groove_joint_create(const Vector2 &p_a_groove1, const Vector2 &p_a_groove2, const Vector2 &p_b_anchor, RID p_body_a, RID p_body_b) override;
-	virtual RID damped_spring_joint_create(const Vector2 &p_anchor_a, const Vector2 &p_anchor_b, RID p_body_a, RID p_body_b = RID()) override;
+	virtual void joint_make_pin(RID p_joint, const Vector2 &p_anchor, RID p_body_a, RID p_body_b = RID()) override;
+	virtual void joint_make_groove(RID p_joint, const Vector2 &p_a_groove1, const Vector2 &p_a_groove2, const Vector2 &p_b_anchor, RID p_body_a, RID p_body_b) override;
+	virtual void joint_make_damped_spring(RID p_joint, const Vector2 &p_anchor_a, const Vector2 &p_anchor_b, RID p_body_a, RID p_body_b = RID()) override;
+
 	virtual void pin_joint_set_param(RID p_joint, PinJointParam p_param, real_t p_value) override;
 	virtual real_t pin_joint_get_param(RID p_joint, PinJointParam p_param) const override;
 	virtual void damped_spring_joint_set_param(RID p_joint, DampedSpringParam p_param, real_t p_value) override;
@@ -287,7 +292,7 @@ public:
 
 	int get_process_info(ProcessInfo p_info) override;
 
-	PhysicsServer2DSW();
+	PhysicsServer2DSW(bool p_using_threads = false);
 	~PhysicsServer2DSW() {}
 };
 
diff --git a/servers/physics_2d/physics_server_2d_wrap_mt.cpp b/servers/physics_2d/physics_server_2d_wrap_mt.cpp
index 15d875b3b7..c2557d8f7f 100644
--- a/servers/physics_2d/physics_server_2d_wrap_mt.cpp
+++ b/servers/physics_2d/physics_server_2d_wrap_mt.cpp
@@ -76,7 +76,7 @@ void PhysicsServer2DWrapMT::step(real_t p_step) {
 }
 
 void PhysicsServer2DWrapMT::sync() {
-	if (thread) {
+	if (create_thread) {
 		if (first_frame) {
 			first_frame = false;
 		} else {
@@ -97,7 +97,7 @@ void PhysicsServer2DWrapMT::end_sync() {
 void PhysicsServer2DWrapMT::init() {
 	if (create_thread) {
 		//OS::get_singleton()->release_rendering_thread();
-		thread = Thread::create(_thread_callback, this);
+		thread.start(_thread_callback, this);
 		while (!step_thread_up) {
 			OS::get_singleton()->delay_usec(1000);
 		}
@@ -107,35 +107,18 @@ void PhysicsServer2DWrapMT::init() {
 }
 
 void PhysicsServer2DWrapMT::finish() {
-	if (thread) {
+	if (thread.is_started()) {
 		command_queue.push(this, &PhysicsServer2DWrapMT::thread_exit);
-		Thread::wait_to_finish(thread);
-		memdelete(thread);
-
-		thread = nullptr;
+		thread.wait_to_finish();
 	} else {
 		physics_2d_server->finish();
 	}
-
-	line_shape_free_cached_ids();
-	ray_shape_free_cached_ids();
-	segment_shape_free_cached_ids();
-	circle_shape_free_cached_ids();
-	rectangle_shape_free_cached_ids();
-	capsule_shape_free_cached_ids();
-	convex_polygon_shape_free_cached_ids();
-	concave_polygon_shape_free_cached_ids();
-
-	space_free_cached_ids();
-	area_free_cached_ids();
-	body_free_cached_ids();
 }
 
 PhysicsServer2DWrapMT::PhysicsServer2DWrapMT(PhysicsServer2D *p_contained, bool p_create_thread) :
 		command_queue(p_create_thread) {
 	physics_2d_server = p_contained;
 	create_thread = p_create_thread;
-	thread = nullptr;
 	step_pending = 0;
 	step_thread_up = false;
 
diff --git a/servers/physics_2d/physics_server_2d_wrap_mt.h b/servers/physics_2d/physics_server_2d_wrap_mt.h
index 9207081a51..a6f0b1d4f1 100644
--- a/servers/physics_2d/physics_server_2d_wrap_mt.h
+++ b/servers/physics_2d/physics_server_2d_wrap_mt.h
@@ -53,7 +53,7 @@ class PhysicsServer2DWrapMT : public PhysicsServer2D {
 	Thread::ID server_thread;
 	Thread::ID main_thread;
 	volatile bool exit;
-	Thread *thread;
+	Thread thread;
 	volatile bool step_thread_up;
 	bool create_thread;
 
@@ -73,6 +73,8 @@ public:
 #define ServerName PhysicsServer2D
 #define ServerNameWrapMT PhysicsServer2DWrapMT
 #define server_name physics_2d_server
+#define WRITE_ACTION
+
 #include "servers/server_wrap_mt_common.h"
 
 	//FUNC1RID(shape,ShapeType); todo fix
@@ -93,7 +95,7 @@ public:
 	FUNC1RC(real_t, shape_get_custom_solver_bias, RID);
 
 	//these work well, but should be used from the main thread only
-	bool shape_collide(RID p_shape_A, const Transform2D &p_xform_A, const Vector2 &p_motion_A, RID p_shape_B, const Transform2D &p_xform_B, const Vector2 &p_motion_B, Vector2 *r_results, int p_result_max, int &r_result_count) {
+	bool shape_collide(RID p_shape_A, const Transform2D &p_xform_A, const Vector2 &p_motion_A, RID p_shape_B, const Transform2D &p_xform_B, const Vector2 &p_motion_B, Vector2 *r_results, int p_result_max, int &r_result_count) override {
 		ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), false);
 		return physics_2d_server->shape_collide(p_shape_A, p_xform_A, p_motion_A, p_shape_B, p_xform_B, p_motion_B, r_results, p_result_max, r_result_count);
 	}
@@ -108,18 +110,18 @@ public:
 	FUNC2RC(real_t, space_get_param, RID, SpaceParameter);
 
 	// this function only works on physics process, errors and returns null otherwise
-	PhysicsDirectSpaceState2D *space_get_direct_state(RID p_space) {
+	PhysicsDirectSpaceState2D *space_get_direct_state(RID p_space) override {
 		ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), nullptr);
 		return physics_2d_server->space_get_direct_state(p_space);
 	}
 
 	FUNC2(space_set_debug_contacts, RID, int);
-	virtual Vector<Vector2> space_get_contacts(RID p_space) const {
+	virtual Vector<Vector2> space_get_contacts(RID p_space) const override {
 		ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), Vector<Vector2>());
 		return physics_2d_server->space_get_contacts(p_space);
 	}
 
-	virtual int space_get_contact_count(RID p_space) const {
+	virtual int space_get_contact_count(RID p_space) const override {
 		ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), 0);
 		return physics_2d_server->space_get_contact_count(p_space);
 	}
@@ -189,7 +191,7 @@ public:
 	FUNC2RC(RID, body_get_shape, RID, int);
 
 	FUNC3(body_set_shape_disabled, RID, int, bool);
-	FUNC4(body_set_shape_as_one_way_collision, RID, int, bool, float);
+	FUNC4(body_set_shape_as_one_way_collision, RID, int, bool, real_t);
 
 	FUNC2(body_remove_shape, RID, int);
 	FUNC1(body_clear_shapes, RID);
@@ -244,30 +246,34 @@ public:
 
 	FUNC4(body_set_force_integration_callback, RID, Object *, const StringName &, 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) {
+	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 {
 		return physics_2d_server->body_collide_shape(p_body, p_body_shape, p_shape, p_shape_xform, p_motion, r_results, p_result_max, r_result_count);
 	}
 
 	FUNC2(body_set_pickable, RID, bool);
 
-	bool body_test_motion(RID p_body, const Transform2D &p_from, const Vector2 &p_motion, bool p_infinite_inertia, real_t p_margin = 0.001, MotionResult *r_result = nullptr, bool p_exclude_raycast_shapes = true) {
+	bool body_test_motion(RID p_body, const Transform2D &p_from, const Vector2 &p_motion, bool p_infinite_inertia, real_t p_margin = 0.001, MotionResult *r_result = nullptr, bool p_exclude_raycast_shapes = true) override {
 		ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), false);
 		return physics_2d_server->body_test_motion(p_body, p_from, p_motion, p_infinite_inertia, p_margin, r_result, p_exclude_raycast_shapes);
 	}
 
-	int body_test_ray_separation(RID p_body, const Transform2D &p_transform, bool p_infinite_inertia, Vector2 &r_recover_motion, SeparationResult *r_results, int p_result_max, float p_margin = 0.001) {
+	int body_test_ray_separation(RID p_body, const Transform2D &p_transform, bool p_infinite_inertia, Vector2 &r_recover_motion, SeparationResult *r_results, int p_result_max, real_t p_margin = 0.001) override {
 		ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), false);
 		return physics_2d_server->body_test_ray_separation(p_body, p_transform, p_infinite_inertia, r_recover_motion, r_results, p_result_max, p_margin);
 	}
 
 	// this function only works on physics process, errors and returns null otherwise
-	PhysicsDirectBodyState2D *body_get_direct_state(RID p_body) {
+	PhysicsDirectBodyState2D *body_get_direct_state(RID p_body) override {
 		ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), nullptr);
 		return physics_2d_server->body_get_direct_state(p_body);
 	}
 
 	/* JOINT API */
 
+	FUNCRID(joint)
+
+	FUNC1(joint_clear, RID)
+
 	FUNC3(joint_set_param, RID, JointParam, real_t);
 	FUNC2RC(real_t, joint_get_param, RID, JointParam);
 
@@ -280,9 +286,9 @@ public:
 
 	//TODO need to convert this to FUNCRID, but it's a hassle..
 
-	FUNC3R(RID, pin_joint_create, const Vector2 &, RID, RID);
-	FUNC5R(RID, groove_joint_create, const Vector2 &, const Vector2 &, const Vector2 &, RID, RID);
-	FUNC4R(RID, damped_spring_joint_create, const Vector2 &, const Vector2 &, RID, RID);
+	FUNC4(joint_make_pin, RID, const Vector2 &, RID, RID);
+	FUNC6(joint_make_groove, RID, const Vector2 &, const Vector2 &, const Vector2 &, RID, RID);
+	FUNC5(joint_make_damped_spring, RID, const Vector2 &, const Vector2 &, RID, RID);
 
 	FUNC3(pin_joint_set_param, RID, PinJointParam, real_t);
 	FUNC2RC(real_t, pin_joint_get_param, RID, PinJointParam);
@@ -297,43 +303,28 @@ public:
 	FUNC1(free, RID);
 	FUNC1(set_active, bool);
 
-	virtual void init();
-	virtual void step(real_t p_step);
-	virtual void sync();
-	virtual void end_sync();
-	virtual void flush_queries();
-	virtual void finish();
+	virtual void init() override;
+	virtual void step(real_t p_step) override;
+	virtual void sync() override;
+	virtual void end_sync() override;
+	virtual void flush_queries() override;
+	virtual void finish() override;
 
-	virtual bool is_flushing_queries() const {
+	virtual bool is_flushing_queries() const override {
 		return physics_2d_server->is_flushing_queries();
 	}
 
-	int get_process_info(ProcessInfo p_info) {
+	int get_process_info(ProcessInfo p_info) override {
 		return physics_2d_server->get_process_info(p_info);
 	}
 
 	PhysicsServer2DWrapMT(PhysicsServer2D *p_contained, bool p_create_thread);
 	~PhysicsServer2DWrapMT();
 
-	template <class T>
-	static PhysicsServer2D *init_server() {
-#ifdef NO_THREADS
-		return memnew(T); // Always single unsafe when no threads are available.
-#else
-		int tm = GLOBAL_DEF("physics/2d/thread_model", 1);
-		if (tm == 0) { // single unsafe
-			return memnew(T);
-		} else if (tm == 1) { // single safe
-			return memnew(PhysicsServer2DWrapMT(memnew(T), false));
-		} else { // multi threaded
-			return memnew(PhysicsServer2DWrapMT(memnew(T), true));
-		}
-#endif
-	}
-
 #undef ServerNameWrapMT
 #undef ServerName
 #undef server_name
+#undef WRITE_ACTION
 };
 
 #ifdef DEBUG_SYNC
diff --git a/servers/physics_2d/shape_2d_sw.cpp b/servers/physics_2d/shape_2d_sw.cpp
index 24c73314d8..6e7e802a8b 100644
--- a/servers/physics_2d/shape_2d_sw.cpp
+++ b/servers/physics_2d/shape_2d_sw.cpp
@@ -339,10 +339,10 @@ void RectangleShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_suppor
 }
 
 bool RectangleShape2DSW::contains_point(const Vector2 &p_point) const {
-	float x = p_point.x;
-	float y = p_point.y;
-	float edge_x = half_extents.x;
-	float edge_y = half_extents.y;
+	real_t x = p_point.x;
+	real_t y = p_point.y;
+	real_t edge_x = half_extents.x;
+	real_t edge_y = half_extents.y;
 	return (x >= -edge_x) && (x < edge_x) && (y >= -edge_y) && (y < edge_y);
 }
 
@@ -590,7 +590,11 @@ real_t ConvexPolygonShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2
 }
 
 void ConvexPolygonShape2DSW::set_data(const Variant &p_data) {
+#ifdef REAL_T_IS_DOUBLE
+	ERR_FAIL_COND(p_data.get_type() != Variant::PACKED_VECTOR2_ARRAY && p_data.get_type() != Variant::PACKED_FLOAT64_ARRAY);
+#else
 	ERR_FAIL_COND(p_data.get_type() != Variant::PACKED_VECTOR2_ARRAY && p_data.get_type() != Variant::PACKED_FLOAT32_ARRAY);
+#endif
 
 	if (points) {
 		memdelete_arr(points);
@@ -829,7 +833,11 @@ int ConcavePolygonShape2DSW::_generate_bvh(BVH *p_bvh, int p_len, int p_depth) {
 }
 
 void ConcavePolygonShape2DSW::set_data(const Variant &p_data) {
+#ifdef REAL_T_IS_DOUBLE
+	ERR_FAIL_COND(p_data.get_type() != Variant::PACKED_VECTOR2_ARRAY && p_data.get_type() != Variant::PACKED_FLOAT64_ARRAY);
+#else
 	ERR_FAIL_COND(p_data.get_type() != Variant::PACKED_VECTOR2_ARRAY && p_data.get_type() != Variant::PACKED_FLOAT32_ARRAY);
+#endif
 
 	Rect2 aabb;
 
diff --git a/servers/physics_2d/space_2d_sw.cpp b/servers/physics_2d/space_2d_sw.cpp
index 068bc7fc0a..05136e2501 100644
--- a/servers/physics_2d/space_2d_sw.cpp
+++ b/servers/physics_2d/space_2d_sw.cpp
@@ -644,7 +644,7 @@ int Space2DSW::test_body_ray_separation(Body2DSW *p_body, const Transform2D &p_t
 
 								recover_motion += (b - a) / cbk.amount;
 
-								float depth = a.distance_to(b);
+								real_t depth = a.distance_to(b);
 								if (depth > result.collision_depth) {
 									result.collision_depth = depth;
 									result.collision_point = b;
@@ -739,7 +739,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
 	ExcludedShapeSW excluded_shape_pairs[max_excluded_shape_pairs];
 	int excluded_shape_pair_count = 0;
 
-	float separation_margin = MIN(p_margin, MAX(0.0, p_motion.length() - CMP_EPSILON)); //don't separate by more than the intended motion
+	real_t separation_margin = MIN(p_margin, MAX(0.0, p_motion.length() - CMP_EPSILON)); //don't separate by more than the intended motion
 
 	Transform2D body_transform = p_from;
 
@@ -793,7 +793,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
 					if (col_obj->is_shape_set_as_one_way_collision(shape_idx)) {
 						cbk.valid_dir = col_obj_shape_xform.get_axis(1).normalized();
 
-						float owc_margin = col_obj->get_shape_one_way_collision_margin(shape_idx);
+						real_t owc_margin = col_obj->get_shape_one_way_collision_margin(shape_idx);
 						cbk.valid_depth = MAX(owc_margin, p_margin); //user specified, but never less than actual margin or it won't work
 						cbk.invalid_by_dir = 0;
 
@@ -804,7 +804,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
 								Vector2 lv = b->get_linear_velocity();
 								//compute displacement from linear velocity
 								Vector2 motion = lv * PhysicsDirectBodyState2DSW::singleton->step;
-								float motion_len = motion.length();
+								real_t motion_len = motion.length();
 								motion.normalize();
 								cbk.valid_depth += motion_len * MAX(motion.dot(-cbk.valid_dir), 0.0);
 							}
@@ -1328,7 +1328,7 @@ Space2DSW::Space2DSW() {
 
 	constraint_bias = 0.2;
 	body_linear_velocity_sleep_threshold = GLOBAL_DEF("physics/2d/sleep_threshold_linear", 2.0);
-	body_angular_velocity_sleep_threshold = GLOBAL_DEF("physics/2d/sleep_threshold_angular", (8.0 / 180.0 * Math_PI));
+	body_angular_velocity_sleep_threshold = GLOBAL_DEF("physics/2d/sleep_threshold_angular", Math::deg2rad(8.0));
 	body_time_to_sleep = GLOBAL_DEF("physics/2d/time_before_sleep", 0.5);
 	ProjectSettings::get_singleton()->set_custom_property_info("physics/2d/time_before_sleep", PropertyInfo(Variant::FLOAT, "physics/2d/time_before_sleep", PROPERTY_HINT_RANGE, "0,5,0.01,or_greater"));