48 files changed, 4602 insertions, 4855 deletions

diff --git a/servers/physics_3d/gjk_epa.cpp b/servers/physics_3d/gjk_epa.cpp
index f2f712193a..ef6535a878 100644
--- a/servers/physics_3d/gjk_epa.cpp
+++ b/servers/physics_3d/gjk_epa.cpp
@@ -37,7 +37,7 @@
 
 /*
 Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2008 Erwin Coumans  https://bulletphysics.org
+Copyright (c) 2003-2008 Erwin Coumans  http://continuousphysics.com/Bullet/
 
 This software is provided 'as-is', without any express or implied warranty.
 In no event will the authors be held liable for any damages arising from the
@@ -96,7 +96,7 @@ struct sResults	{
 
 	Vector3	witnesses[2];
 	Vector3	normal;
-	real_t	distance;
+	real_t	distance = 0.0;
 };
 
 // Shorthands
@@ -105,7 +105,7 @@ typedef unsigned char	U1;
 
 // MinkowskiDiff
 struct	MinkowskiDiff {
-	const Shape3DSW* m_shapes[2];
+	const GodotShape3D* m_shapes[2];
 
 	Transform3D transform_A;
 	Transform3D transform_B;
@@ -113,10 +113,10 @@ struct	MinkowskiDiff {
 	real_t margin_A = 0.0;
 	real_t margin_B = 0.0;
 
-	Vector3 (*get_support)(const Shape3DSW*, const Vector3&, real_t);
+	Vector3 (*get_support)(const GodotShape3D*, const Vector3&, real_t);
 
-	void Initialize(const Shape3DSW* shape0, const Transform3D& wtrs0, const real_t margin0,
-		const Shape3DSW* shape1, const Transform3D& wtrs1, const real_t margin1) {
+	void Initialize(const GodotShape3D* shape0, const Transform3D& wtrs0, const real_t margin0,
+		const GodotShape3D* shape1, const Transform3D& wtrs1, const real_t margin1) {
 		m_shapes[0]		=	shape0;
 		m_shapes[1]		=	shape1;
 		transform_A		=	wtrs0;
@@ -131,11 +131,11 @@ struct	MinkowskiDiff {
 		}
 	}
 
-	static Vector3 get_support_without_margin(const Shape3DSW* p_shape, const Vector3& p_dir, real_t p_margin) {
+	static Vector3 get_support_without_margin(const GodotShape3D* p_shape, const Vector3& p_dir, real_t p_margin) {
 		return p_shape->get_support(p_dir.normalized());
 	}
 
-	static Vector3 get_support_with_margin(const Shape3DSW* p_shape, const Vector3& p_dir, real_t p_margin) {
+	static Vector3 get_support_with_margin(const GodotShape3D* p_shape, const Vector3& p_dir, real_t p_margin) {
 		Vector3 local_dir_norm = p_dir;
 		if (local_dir_norm.length_squared() < CMP_EPSILON2) {
 			local_dir_norm = Vector3(-1.0, -1.0, -1.0);
@@ -862,8 +862,8 @@ struct	GJK
 	};
 
 	//
-	static void	Initialize(	const Shape3DSW* shape0, const Transform3D& wtrs0, real_t margin0,
-		const Shape3DSW* shape1, const Transform3D& wtrs1, real_t margin1,
+	static void	Initialize(	const GodotShape3D* shape0, const Transform3D& wtrs0, real_t margin0,
+		const GodotShape3D* shape1, const Transform3D& wtrs1, real_t margin1,
 		sResults& results,
 		tShape& shape)
 	{
@@ -884,10 +884,10 @@ struct	GJK
 //
 
 //
-bool Distance(	const Shape3DSW*	shape0,
+bool Distance(	const GodotShape3D*	shape0,
 									  const Transform3D&		wtrs0,
 									  real_t				margin0,
-									  const Shape3DSW*		shape1,
+									  const GodotShape3D*		shape1,
 									  const Transform3D&		wtrs1,
 									  real_t				margin1,
 									  const Vector3&		guess,
@@ -925,10 +925,10 @@ bool Distance(	const Shape3DSW*	shape0,
 
 
 //
-bool Penetration(	const Shape3DSW*	shape0,
+bool Penetration(	const GodotShape3D*	shape0,
 									 const Transform3D&		wtrs0,
 									 real_t					margin0,
-									 const Shape3DSW*		shape1,
+									 const GodotShape3D*		shape1,
 									 const Transform3D&		wtrs1,
 									 real_t					margin1,
 									 const Vector3&			guess,
@@ -993,7 +993,7 @@ bool Penetration(	const Shape3DSW*	shape0,
 
 /* clang-format on */
 
-bool gjk_epa_calculate_distance(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_result_A, Vector3 &r_result_B) {
+bool gjk_epa_calculate_distance(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_result_A, Vector3 &r_result_B) {
 	GjkEpa2::sResults res;
 
 	if (GjkEpa2::Distance(p_shape_A, p_transform_A, 0.0, p_shape_B, p_transform_B, 0.0, p_transform_B.origin - p_transform_A.origin, res)) {
@@ -1005,7 +1005,7 @@ bool gjk_epa_calculate_distance(const Shape3DSW *p_shape_A, const Transform3D &p
 	return false;
 }
 
-bool gjk_epa_calculate_penetration(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CollisionSolver3DSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap, real_t p_margin_A, real_t p_margin_B) {
+bool gjk_epa_calculate_penetration(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, GodotCollisionSolver3D::CallbackResult p_result_callback, void *p_userdata, bool p_swap, real_t p_margin_A, real_t p_margin_B) {
 	GjkEpa2::sResults res;
 
 	if (GjkEpa2::Penetration(p_shape_A, p_transform_A, p_margin_A, p_shape_B, p_transform_B, p_margin_B, p_transform_B.origin - p_transform_A.origin, res)) {
diff --git a/servers/physics_3d/gjk_epa.h b/servers/physics_3d/gjk_epa.h
index 69e85d2bc0..39a7d03435 100644
--- a/servers/physics_3d/gjk_epa.h
+++ b/servers/physics_3d/gjk_epa.h
@@ -31,10 +31,10 @@
 #ifndef GJK_EPA_H
 #define GJK_EPA_H
 
-#include "collision_solver_3d_sw.h"
-#include "shape_3d_sw.h"
+#include "godot_collision_solver_3d.h"
+#include "godot_shape_3d.h"
 
-bool gjk_epa_calculate_penetration(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CollisionSolver3DSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap = false, real_t p_margin_A = 0.0, real_t p_margin_B = 0.0);
-bool gjk_epa_calculate_distance(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_result_A, Vector3 &r_result_B);
+bool gjk_epa_calculate_penetration(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, GodotCollisionSolver3D::CallbackResult p_result_callback, void *p_userdata, bool p_swap = false, real_t p_margin_A = 0.0, real_t p_margin_B = 0.0);
+bool gjk_epa_calculate_distance(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_result_A, Vector3 &r_result_B);
 
 #endif
diff --git a/servers/physics_3d/area_3d_sw.cpp b/servers/physics_3d/godot_area_3d.cpp
index c9e8bcb8ca..d4e14b8d85 100644
--- a/servers/physics_3d/area_3d_sw.cpp
+++ b/servers/physics_3d/godot_area_3d.cpp
@@ -1,5 +1,5 @@
 /*************************************************************************/
-/*  area_3d_sw.cpp                                                       */
+/*  godot_area_3d.cpp                                                    */
 /*************************************************************************/
 /*                       This file is part of:                           */
 /*                           GODOT ENGINE                                */
@@ -28,39 +28,40 @@
 /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
 /*************************************************************************/
 
-#include "area_3d_sw.h"
-#include "body_3d_sw.h"
-#include "soft_body_3d_sw.h"
-#include "space_3d_sw.h"
+#include "godot_area_3d.h"
 
-Area3DSW::BodyKey::BodyKey(SoftBody3DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape) {
+#include "godot_body_3d.h"
+#include "godot_soft_body_3d.h"
+#include "godot_space_3d.h"
+
+GodotArea3D::BodyKey::BodyKey(GodotSoftBody3D *p_body, uint32_t p_body_shape, uint32_t p_area_shape) {
 	rid = p_body->get_self();
 	instance_id = p_body->get_instance_id();
 	body_shape = p_body_shape;
 	area_shape = p_area_shape;
 }
 
-Area3DSW::BodyKey::BodyKey(Body3DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape) {
+GodotArea3D::BodyKey::BodyKey(GodotBody3D *p_body, uint32_t p_body_shape, uint32_t p_area_shape) {
 	rid = p_body->get_self();
 	instance_id = p_body->get_instance_id();
 	body_shape = p_body_shape;
 	area_shape = p_area_shape;
 }
 
-Area3DSW::BodyKey::BodyKey(Area3DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape) {
+GodotArea3D::BodyKey::BodyKey(GodotArea3D *p_body, uint32_t p_body_shape, uint32_t p_area_shape) {
 	rid = p_body->get_self();
 	instance_id = p_body->get_instance_id();
 	body_shape = p_body_shape;
 	area_shape = p_area_shape;
 }
 
-void Area3DSW::_shapes_changed() {
+void GodotArea3D::_shapes_changed() {
 	if (!moved_list.in_list() && get_space()) {
 		get_space()->area_add_to_moved_list(&moved_list);
 	}
 }
 
-void Area3DSW::set_transform(const Transform3D &p_transform) {
+void GodotArea3D::set_transform(const Transform3D &p_transform) {
 	if (!moved_list.in_list() && get_space()) {
 		get_space()->area_add_to_moved_list(&moved_list);
 	}
@@ -69,7 +70,7 @@ void Area3DSW::set_transform(const Transform3D &p_transform) {
 	_set_inv_transform(p_transform.affine_inverse());
 }
 
-void Area3DSW::set_space(Space3DSW *p_space) {
+void GodotArea3D::set_space(GodotSpace3D *p_space) {
 	if (get_space()) {
 		if (monitor_query_list.in_list()) {
 			get_space()->area_remove_from_monitor_query_list(&monitor_query_list);
@@ -85,16 +86,16 @@ void Area3DSW::set_space(Space3DSW *p_space) {
 	_set_space(p_space);
 }
 
-void Area3DSW::set_monitor_callback(ObjectID p_id, const StringName &p_method) {
-	if (p_id == monitor_callback_id) {
-		monitor_callback_method = p_method;
+void GodotArea3D::set_monitor_callback(const Callable &p_callback) {
+	ObjectID id = p_callback.get_object_id();
+	if (id == monitor_callback.get_object_id()) {
+		monitor_callback = p_callback;
 		return;
 	}
 
 	_unregister_shapes();
 
-	monitor_callback_id = p_id;
-	monitor_callback_method = p_method;
+	monitor_callback = p_callback;
 
 	monitored_bodies.clear();
 	monitored_areas.clear();
@@ -106,16 +107,16 @@ void Area3DSW::set_monitor_callback(ObjectID p_id, const StringName &p_method) {
 	}
 }
 
-void Area3DSW::set_area_monitor_callback(ObjectID p_id, const StringName &p_method) {
-	if (p_id == area_monitor_callback_id) {
-		area_monitor_callback_method = p_method;
+void GodotArea3D::set_area_monitor_callback(const Callable &p_callback) {
+	ObjectID id = p_callback.get_object_id();
+	if (id == area_monitor_callback.get_object_id()) {
+		area_monitor_callback = p_callback;
 		return;
 	}
 
 	_unregister_shapes();
 
-	area_monitor_callback_id = p_id;
-	area_monitor_callback_method = p_method;
+	area_monitor_callback = p_callback;
 
 	monitored_bodies.clear();
 	monitored_areas.clear();
@@ -127,18 +128,21 @@ void Area3DSW::set_area_monitor_callback(ObjectID p_id, const StringName &p_meth
 	}
 }
 
-void Area3DSW::set_space_override_mode(PhysicsServer3D::AreaSpaceOverrideMode p_mode) {
-	bool do_override = p_mode != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED;
-	if (do_override == (space_override_mode != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED)) {
+void GodotArea3D::_set_space_override_mode(PhysicsServer3D::AreaSpaceOverrideMode &r_mode, PhysicsServer3D::AreaSpaceOverrideMode p_new_mode) {
+	bool do_override = p_new_mode != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED;
+	if (do_override == (r_mode != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED)) {
 		return;
 	}
 	_unregister_shapes();
-	space_override_mode = p_mode;
+	r_mode = p_new_mode;
 	_shape_changed();
 }
 
-void Area3DSW::set_param(PhysicsServer3D::AreaParameter p_param, const Variant &p_value) {
+void GodotArea3D::set_param(PhysicsServer3D::AreaParameter p_param, const Variant &p_value) {
 	switch (p_param) {
+		case PhysicsServer3D::AREA_PARAM_GRAVITY_OVERRIDE_MODE:
+			_set_space_override_mode(gravity_override_mode, (PhysicsServer3D::AreaSpaceOverrideMode)(int)p_value);
+			break;
 		case PhysicsServer3D::AREA_PARAM_GRAVITY:
 			gravity = p_value;
 			break;
@@ -154,9 +158,15 @@ void Area3DSW::set_param(PhysicsServer3D::AreaParameter p_param, const Variant &
 		case PhysicsServer3D::AREA_PARAM_GRAVITY_POINT_ATTENUATION:
 			point_attenuation = p_value;
 			break;
+		case PhysicsServer3D::AREA_PARAM_LINEAR_DAMP_OVERRIDE_MODE:
+			_set_space_override_mode(linear_damping_override_mode, (PhysicsServer3D::AreaSpaceOverrideMode)(int)p_value);
+			break;
 		case PhysicsServer3D::AREA_PARAM_LINEAR_DAMP:
 			linear_damp = p_value;
 			break;
+		case PhysicsServer3D::AREA_PARAM_ANGULAR_DAMP_OVERRIDE_MODE:
+			_set_space_override_mode(angular_damping_override_mode, (PhysicsServer3D::AreaSpaceOverrideMode)(int)p_value);
+			break;
 		case PhysicsServer3D::AREA_PARAM_ANGULAR_DAMP:
 			angular_damp = p_value;
 			break;
@@ -180,8 +190,10 @@ void Area3DSW::set_param(PhysicsServer3D::AreaParameter p_param, const Variant &
 	}
 }
 
-Variant Area3DSW::get_param(PhysicsServer3D::AreaParameter p_param) const {
+Variant GodotArea3D::get_param(PhysicsServer3D::AreaParameter p_param) const {
 	switch (p_param) {
+		case PhysicsServer3D::AREA_PARAM_GRAVITY_OVERRIDE_MODE:
+			return gravity_override_mode;
 		case PhysicsServer3D::AREA_PARAM_GRAVITY:
 			return gravity;
 		case PhysicsServer3D::AREA_PARAM_GRAVITY_VECTOR:
@@ -192,8 +204,12 @@ Variant Area3DSW::get_param(PhysicsServer3D::AreaParameter p_param) const {
 			return gravity_distance_scale;
 		case PhysicsServer3D::AREA_PARAM_GRAVITY_POINT_ATTENUATION:
 			return point_attenuation;
+		case PhysicsServer3D::AREA_PARAM_LINEAR_DAMP_OVERRIDE_MODE:
+			return linear_damping_override_mode;
 		case PhysicsServer3D::AREA_PARAM_LINEAR_DAMP:
 			return linear_damp;
+		case PhysicsServer3D::AREA_PARAM_ANGULAR_DAMP_OVERRIDE_MODE:
+			return angular_damping_override_mode;
 		case PhysicsServer3D::AREA_PARAM_ANGULAR_DAMP:
 			return angular_damp;
 		case PhysicsServer3D::AREA_PARAM_PRIORITY:
@@ -211,7 +227,7 @@ Variant Area3DSW::get_param(PhysicsServer3D::AreaParameter p_param) const {
 	return Variant();
 }
 
-void Area3DSW::_queue_monitor_update() {
+void GodotArea3D::_queue_monitor_update() {
 	ERR_FAIL_COND(!get_space());
 
 	if (!monitor_query_list.in_list()) {
@@ -219,92 +235,91 @@ void Area3DSW::_queue_monitor_update() {
 	}
 }
 
-void Area3DSW::set_monitorable(bool p_monitorable) {
+void GodotArea3D::set_monitorable(bool p_monitorable) {
 	if (monitorable == p_monitorable) {
 		return;
 	}
 
 	monitorable = p_monitorable;
 	_set_static(!monitorable);
+	_shapes_changed();
 }
 
-void Area3DSW::call_queries() {
-	if (monitor_callback_id.is_valid() && !monitored_bodies.is_empty()) {
-		Variant res[5];
-		Variant *resptr[5];
-		for (int i = 0; i < 5; i++) {
-			resptr[i] = &res[i];
-		}
+void GodotArea3D::call_queries() {
+	if (!monitor_callback.is_null() && !monitored_bodies.is_empty()) {
+		if (monitor_callback.is_valid()) {
+			Variant res[5];
+			Variant *resptr[5];
+			for (int i = 0; i < 5; i++) {
+				resptr[i] = &res[i];
+			}
 
-		Object *obj = ObjectDB::get_instance(monitor_callback_id);
-		if (!obj) {
-			monitored_bodies.clear();
-			monitor_callback_id = ObjectID();
-			return;
-		}
+			for (Map<BodyKey, BodyState>::Element *E = monitored_bodies.front(); E;) {
+				if (E->get().state == 0) { // Nothing happened
+					Map<BodyKey, BodyState>::Element *next = E->next();
+					monitored_bodies.erase(E);
+					E = next;
+					continue;
+				}
+
+				res[0] = E->get().state > 0 ? PhysicsServer3D::AREA_BODY_ADDED : PhysicsServer3D::AREA_BODY_REMOVED;
+				res[1] = E->key().rid;
+				res[2] = E->key().instance_id;
+				res[3] = E->key().body_shape;
+				res[4] = E->key().area_shape;
 
-		for (Map<BodyKey, BodyState>::Element *E = monitored_bodies.front(); E;) {
-			if (E->get().state == 0) { // Nothing happened
 				Map<BodyKey, BodyState>::Element *next = E->next();
 				monitored_bodies.erase(E);
 				E = next;
-				continue;
-			}
 
-			res[0] = E->get().state > 0 ? PhysicsServer3D::AREA_BODY_ADDED : PhysicsServer3D::AREA_BODY_REMOVED;
-			res[1] = E->key().rid;
-			res[2] = E->key().instance_id;
-			res[3] = E->key().body_shape;
-			res[4] = E->key().area_shape;
-
-			Map<BodyKey, BodyState>::Element *next = E->next();
-			monitored_bodies.erase(E);
-			E = next;
-
-			Callable::CallError ce;
-			obj->call(monitor_callback_method, (const Variant **)resptr, 5, ce);
+				Callable::CallError ce;
+				Variant ret;
+				monitor_callback.call((const Variant **)resptr, 5, ret, ce);
+			}
+		} else {
+			monitored_bodies.clear();
+			monitor_callback = Callable();
 		}
 	}
 
-	if (area_monitor_callback_id.is_valid() && !monitored_areas.is_empty()) {
-		Variant res[5];
-		Variant *resptr[5];
-		for (int i = 0; i < 5; i++) {
-			resptr[i] = &res[i];
-		}
+	if (!area_monitor_callback.is_null() && !monitored_areas.is_empty()) {
+		if (area_monitor_callback.is_valid()) {
+			Variant res[5];
+			Variant *resptr[5];
+			for (int i = 0; i < 5; i++) {
+				resptr[i] = &res[i];
+			}
 
-		Object *obj = ObjectDB::get_instance(area_monitor_callback_id);
-		if (!obj) {
-			monitored_areas.clear();
-			area_monitor_callback_id = ObjectID();
-			return;
-		}
+			for (Map<BodyKey, BodyState>::Element *E = monitored_areas.front(); E;) {
+				if (E->get().state == 0) { // Nothing happened
+					Map<BodyKey, BodyState>::Element *next = E->next();
+					monitored_areas.erase(E);
+					E = next;
+					continue;
+				}
+
+				res[0] = E->get().state > 0 ? PhysicsServer3D::AREA_BODY_ADDED : PhysicsServer3D::AREA_BODY_REMOVED;
+				res[1] = E->key().rid;
+				res[2] = E->key().instance_id;
+				res[3] = E->key().body_shape;
+				res[4] = E->key().area_shape;
 
-		for (Map<BodyKey, BodyState>::Element *E = monitored_areas.front(); E;) {
-			if (E->get().state == 0) { // Nothing happened
 				Map<BodyKey, BodyState>::Element *next = E->next();
 				monitored_areas.erase(E);
 				E = next;
-				continue;
-			}
-
-			res[0] = E->get().state > 0 ? PhysicsServer3D::AREA_BODY_ADDED : PhysicsServer3D::AREA_BODY_REMOVED;
-			res[1] = E->key().rid;
-			res[2] = E->key().instance_id;
-			res[3] = E->key().body_shape;
-			res[4] = E->key().area_shape;
-
-			Map<BodyKey, BodyState>::Element *next = E->next();
-			monitored_areas.erase(E);
-			E = next;
 
-			Callable::CallError ce;
-			obj->call(area_monitor_callback_method, (const Variant **)resptr, 5, ce);
+				Callable::CallError ce;
+				Variant ret;
+				area_monitor_callback.call((const Variant **)resptr, 5, ret, ce);
+			}
+		} else {
+			monitored_areas.clear();
+			area_monitor_callback = Callable();
 		}
 	}
 }
 
-void Area3DSW::compute_gravity(const Vector3 &p_position, Vector3 &r_gravity) const {
+void GodotArea3D::compute_gravity(const Vector3 &p_position, Vector3 &r_gravity) const {
 	if (is_gravity_point()) {
 		const real_t gravity_distance_scale = get_gravity_distance_scale();
 		Vector3 v = get_transform().xform(get_gravity_vector()) - p_position;
@@ -324,23 +339,13 @@ void Area3DSW::compute_gravity(const Vector3 &p_position, Vector3 &r_gravity) co
 	}
 }
 
-Area3DSW::Area3DSW() :
-		CollisionObject3DSW(TYPE_AREA),
+GodotArea3D::GodotArea3D() :
+		GodotCollisionObject3D(TYPE_AREA),
 		monitor_query_list(this),
 		moved_list(this) {
 	_set_static(true); //areas are never active
-	space_override_mode = PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED;
-	gravity = 9.80665;
-	gravity_vector = Vector3(0, -1, 0);
-	gravity_is_point = false;
-	gravity_distance_scale = 0;
-	point_attenuation = 1;
-	angular_damp = 0.1;
-	linear_damp = 0.1;
-	priority = 0;
 	set_ray_pickable(false);
-	monitorable = false;
 }
 
-Area3DSW::~Area3DSW() {
+GodotArea3D::~GodotArea3D() {
 }
diff --git a/servers/physics_3d/area_3d_sw.h b/servers/physics_3d/godot_area_3d.h
index d5f1e60119..0dcf89b2b4 100644
--- a/servers/physics_3d/area_3d_sw.h
+++ b/servers/physics_3d/godot_area_3d.h
@@ -1,5 +1,5 @@
 /*************************************************************************/
-/*  area_3d_sw.h                                                         */
+/*  godot_area_3d.h                                                      */
 /*************************************************************************/
 /*                       This file is part of:                           */
 /*                           GODOT ENGINE                                */
@@ -28,48 +28,49 @@
 /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
 /*************************************************************************/
 
-#ifndef AREA_SW_H
-#define AREA_SW_H
+#ifndef GODOT_AREA_3D_H
+#define GODOT_AREA_3D_H
+
+#include "godot_collision_object_3d.h"
 
-#include "collision_object_3d_sw.h"
 #include "core/templates/self_list.h"
 #include "servers/physics_server_3d.h"
 
-class Space3DSW;
-class Body3DSW;
-class SoftBody3DSW;
-class Constraint3DSW;
-
-class Area3DSW : public CollisionObject3DSW {
-	PhysicsServer3D::AreaSpaceOverrideMode space_override_mode;
-	real_t gravity;
-	Vector3 gravity_vector;
-	bool gravity_is_point;
-	real_t gravity_distance_scale;
-	real_t point_attenuation;
-	real_t linear_damp;
-	real_t angular_damp;
+class GodotSpace3D;
+class GodotBody3D;
+class GodotSoftBody3D;
+class GodotConstraint3D;
+
+class GodotArea3D : public GodotCollisionObject3D {
+	PhysicsServer3D::AreaSpaceOverrideMode gravity_override_mode = PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED;
+	PhysicsServer3D::AreaSpaceOverrideMode linear_damping_override_mode = PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED;
+	PhysicsServer3D::AreaSpaceOverrideMode angular_damping_override_mode = PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED;
+
+	real_t gravity = 9.80665;
+	Vector3 gravity_vector = Vector3(0, -1, 0);
+	bool gravity_is_point = false;
+	real_t gravity_distance_scale = 0.0;
+	real_t point_attenuation = 1.0;
+	real_t linear_damp = 0.1;
+	real_t angular_damp = 0.1;
 	real_t wind_force_magnitude = 0.0;
 	real_t wind_attenuation_factor = 0.0;
 	Vector3 wind_source;
 	Vector3 wind_direction;
-	int priority;
-	bool monitorable;
-
-	ObjectID monitor_callback_id;
-	StringName monitor_callback_method;
+	int priority = 0;
+	bool monitorable = false;
 
-	ObjectID area_monitor_callback_id;
-	StringName area_monitor_callback_method;
+	Callable monitor_callback;
+	Callable area_monitor_callback;
 
-	SelfList<Area3DSW> monitor_query_list;
-	SelfList<Area3DSW> moved_list;
+	SelfList<GodotArea3D> monitor_query_list;
+	SelfList<GodotArea3D> moved_list;
 
 	struct BodyKey {
 		RID rid;
 		ObjectID instance_id;
-		uint32_t body_shape;
-		uint32_t area_shape;
+		uint32_t body_shape = 0;
+		uint32_t area_shape = 0;
 
 		_FORCE_INLINE_ bool operator<(const BodyKey &p_key) const {
 			if (rid == p_key.rid) {
@@ -84,49 +85,47 @@ class Area3DSW : public CollisionObject3DSW {
 		}
 
 		_FORCE_INLINE_ BodyKey() {}
-		BodyKey(SoftBody3DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape);
-		BodyKey(Body3DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape);
-		BodyKey(Area3DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape);
+		BodyKey(GodotSoftBody3D *p_body, uint32_t p_body_shape, uint32_t p_area_shape);
+		BodyKey(GodotBody3D *p_body, uint32_t p_body_shape, uint32_t p_area_shape);
+		BodyKey(GodotArea3D *p_body, uint32_t p_body_shape, uint32_t p_area_shape);
 	};
 
 	struct BodyState {
-		int state;
+		int state = 0;
 		_FORCE_INLINE_ void inc() { state++; }
 		_FORCE_INLINE_ void dec() { state--; }
-		_FORCE_INLINE_ BodyState() { state = 0; }
 	};
 
 	Map<BodyKey, BodyState> monitored_soft_bodies;
 	Map<BodyKey, BodyState> monitored_bodies;
 	Map<BodyKey, BodyState> monitored_areas;
 
-	Set<Constraint3DSW *> constraints;
+	Set<GodotConstraint3D *> constraints;
 
 	virtual void _shapes_changed();
 	void _queue_monitor_update();
 
+	void _set_space_override_mode(PhysicsServer3D::AreaSpaceOverrideMode &r_mode, PhysicsServer3D::AreaSpaceOverrideMode p_new_mode);
+
 public:
-	void set_monitor_callback(ObjectID p_id, const StringName &p_method);
-	_FORCE_INLINE_ bool has_monitor_callback() const { return monitor_callback_id.is_valid(); }
+	void set_monitor_callback(const Callable &p_callback);
+	_FORCE_INLINE_ bool has_monitor_callback() const { return !monitor_callback.is_null(); }
 
-	void set_area_monitor_callback(ObjectID p_id, const StringName &p_method);
-	_FORCE_INLINE_ bool has_area_monitor_callback() const { return area_monitor_callback_id.is_valid(); }
+	void set_area_monitor_callback(const Callable &p_callback);
+	_FORCE_INLINE_ bool has_area_monitor_callback() const { return !area_monitor_callback.is_null(); }
 
-	_FORCE_INLINE_ void add_body_to_query(Body3DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape);
-	_FORCE_INLINE_ void remove_body_from_query(Body3DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape);
+	_FORCE_INLINE_ void add_body_to_query(GodotBody3D *p_body, uint32_t p_body_shape, uint32_t p_area_shape);
+	_FORCE_INLINE_ void remove_body_from_query(GodotBody3D *p_body, uint32_t p_body_shape, uint32_t p_area_shape);
 
-	_FORCE_INLINE_ void add_soft_body_to_query(SoftBody3DSW *p_soft_body, uint32_t p_soft_body_shape, uint32_t p_area_shape);
-	_FORCE_INLINE_ void remove_soft_body_from_query(SoftBody3DSW *p_soft_body, uint32_t p_soft_body_shape, uint32_t p_area_shape);
+	_FORCE_INLINE_ void add_soft_body_to_query(GodotSoftBody3D *p_soft_body, uint32_t p_soft_body_shape, uint32_t p_area_shape);
+	_FORCE_INLINE_ void remove_soft_body_from_query(GodotSoftBody3D *p_soft_body, uint32_t p_soft_body_shape, uint32_t p_area_shape);
 
-	_FORCE_INLINE_ void add_area_to_query(Area3DSW *p_area, uint32_t p_area_shape, uint32_t p_self_shape);
-	_FORCE_INLINE_ void remove_area_from_query(Area3DSW *p_area, uint32_t p_area_shape, uint32_t p_self_shape);
+	_FORCE_INLINE_ void add_area_to_query(GodotArea3D *p_area, uint32_t p_area_shape, uint32_t p_self_shape);
+	_FORCE_INLINE_ void remove_area_from_query(GodotArea3D *p_area, uint32_t p_area_shape, uint32_t p_self_shape);
 
 	void set_param(PhysicsServer3D::AreaParameter p_param, const Variant &p_value);
 	Variant get_param(PhysicsServer3D::AreaParameter p_param) const;
 
-	void set_space_override_mode(PhysicsServer3D::AreaSpaceOverrideMode p_mode);
-	PhysicsServer3D::AreaSpaceOverrideMode get_space_override_mode() const { return space_override_mode; }
-
 	_FORCE_INLINE_ void set_gravity(real_t p_gravity) { gravity = p_gravity; }
 	_FORCE_INLINE_ real_t get_gravity() const { return gravity; }
 
@@ -163,9 +162,9 @@ public:
 	_FORCE_INLINE_ void set_wind_direction(const Vector3 &p_wind_direction) { wind_direction = p_wind_direction; }
 	_FORCE_INLINE_ const Vector3 &get_wind_direction() const { return wind_direction; }
 
-	_FORCE_INLINE_ void add_constraint(Constraint3DSW *p_constraint) { constraints.insert(p_constraint); }
-	_FORCE_INLINE_ void remove_constraint(Constraint3DSW *p_constraint) { constraints.erase(p_constraint); }
-	_FORCE_INLINE_ const Set<Constraint3DSW *> &get_constraints() const { return constraints; }
+	_FORCE_INLINE_ void add_constraint(GodotConstraint3D *p_constraint) { constraints.insert(p_constraint); }
+	_FORCE_INLINE_ void remove_constraint(GodotConstraint3D *p_constraint) { constraints.erase(p_constraint); }
+	_FORCE_INLINE_ const Set<GodotConstraint3D *> &get_constraints() const { return constraints; }
 	_FORCE_INLINE_ void clear_constraints() { constraints.clear(); }
 
 	void set_monitorable(bool p_monitorable);
@@ -173,17 +172,17 @@ public:
 
 	void set_transform(const Transform3D &p_transform);
 
-	void set_space(Space3DSW *p_space);
+	void set_space(GodotSpace3D *p_space);
 
 	void call_queries();
 
 	void compute_gravity(const Vector3 &p_position, Vector3 &r_gravity) const;
 
-	Area3DSW();
-	~Area3DSW();
+	GodotArea3D();
+	~GodotArea3D();
 };
 
-void Area3DSW::add_soft_body_to_query(SoftBody3DSW *p_soft_body, uint32_t p_soft_body_shape, uint32_t p_area_shape) {
+void GodotArea3D::add_soft_body_to_query(GodotSoftBody3D *p_soft_body, uint32_t p_soft_body_shape, uint32_t p_area_shape) {
 	BodyKey bk(p_soft_body, p_soft_body_shape, p_area_shape);
 	monitored_soft_bodies[bk].inc();
 	if (!monitor_query_list.in_list()) {
@@ -191,7 +190,7 @@ void Area3DSW::add_soft_body_to_query(SoftBody3DSW *p_soft_body, uint32_t p_soft
 	}
 }
 
-void Area3DSW::remove_soft_body_from_query(SoftBody3DSW *p_soft_body, uint32_t p_soft_body_shape, uint32_t p_area_shape) {
+void GodotArea3D::remove_soft_body_from_query(GodotSoftBody3D *p_soft_body, uint32_t p_soft_body_shape, uint32_t p_area_shape) {
 	BodyKey bk(p_soft_body, p_soft_body_shape, p_area_shape);
 	monitored_soft_bodies[bk].dec();
 	if (!monitor_query_list.in_list()) {
@@ -199,7 +198,7 @@ void Area3DSW::remove_soft_body_from_query(SoftBody3DSW *p_soft_body, uint32_t p
 	}
 }
 
-void Area3DSW::add_body_to_query(Body3DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape) {
+void GodotArea3D::add_body_to_query(GodotBody3D *p_body, uint32_t p_body_shape, uint32_t p_area_shape) {
 	BodyKey bk(p_body, p_body_shape, p_area_shape);
 	monitored_bodies[bk].inc();
 	if (!monitor_query_list.in_list()) {
@@ -207,7 +206,7 @@ void Area3DSW::add_body_to_query(Body3DSW *p_body, uint32_t p_body_shape, uint32
 	}
 }
 
-void Area3DSW::remove_body_from_query(Body3DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape) {
+void GodotArea3D::remove_body_from_query(GodotBody3D *p_body, uint32_t p_body_shape, uint32_t p_area_shape) {
 	BodyKey bk(p_body, p_body_shape, p_area_shape);
 	monitored_bodies[bk].dec();
 	if (!monitor_query_list.in_list()) {
@@ -215,7 +214,7 @@ void Area3DSW::remove_body_from_query(Body3DSW *p_body, uint32_t p_body_shape, u
 	}
 }
 
-void Area3DSW::add_area_to_query(Area3DSW *p_area, uint32_t p_area_shape, uint32_t p_self_shape) {
+void GodotArea3D::add_area_to_query(GodotArea3D *p_area, uint32_t p_area_shape, uint32_t p_self_shape) {
 	BodyKey bk(p_area, p_area_shape, p_self_shape);
 	monitored_areas[bk].inc();
 	if (!monitor_query_list.in_list()) {
@@ -223,7 +222,7 @@ void Area3DSW::add_area_to_query(Area3DSW *p_area, uint32_t p_area_shape, uint32
 	}
 }
 
-void Area3DSW::remove_area_from_query(Area3DSW *p_area, uint32_t p_area_shape, uint32_t p_self_shape) {
+void GodotArea3D::remove_area_from_query(GodotArea3D *p_area, uint32_t p_area_shape, uint32_t p_self_shape) {
 	BodyKey bk(p_area, p_area_shape, p_self_shape);
 	monitored_areas[bk].dec();
 	if (!monitor_query_list.in_list()) {
@@ -232,15 +231,15 @@ void Area3DSW::remove_area_from_query(Area3DSW *p_area, uint32_t p_area_shape, u
 }
 
 struct AreaCMP {
-	Area3DSW *area;
-	int refCount;
+	GodotArea3D *area = nullptr;
+	int refCount = 0;
 	_FORCE_INLINE_ bool operator==(const AreaCMP &p_cmp) const { return area->get_self() == p_cmp.area->get_self(); }
 	_FORCE_INLINE_ bool operator<(const AreaCMP &p_cmp) const { return area->get_priority() < p_cmp.area->get_priority(); }
 	_FORCE_INLINE_ AreaCMP() {}
-	_FORCE_INLINE_ AreaCMP(Area3DSW *p_area) {
+	_FORCE_INLINE_ AreaCMP(GodotArea3D *p_area) {
 		area = p_area;
 		refCount = 1;
 	}
 };
 
-#endif // AREA__SW_H
+#endif // GODOT_AREA_3D_H
diff --git a/servers/physics_3d/area_pair_3d_sw.cpp b/servers/physics_3d/godot_area_pair_3d.cpp
index bf4f0035b4..58188565e3 100644
--- a/servers/physics_3d/area_pair_3d_sw.cpp
+++ b/servers/physics_3d/godot_area_pair_3d.cpp
@@ -1,5 +1,5 @@
 /*************************************************************************/
-/*  area_pair_3d_sw.cpp                                                  */
+/*  godot_area_pair_3d.cpp                                               */
 /*************************************************************************/
 /*                       This file is part of:                           */
 /*                           GODOT ENGINE                                */
@@ -28,20 +28,29 @@
 /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
 /*************************************************************************/
 
-#include "area_pair_3d_sw.h"
-#include "collision_solver_3d_sw.h"
+#include "godot_area_pair_3d.h"
 
-bool AreaPair3DSW::setup(real_t p_step) {
+#include "godot_collision_solver_3d.h"
+
+bool GodotAreaPair3D::setup(real_t p_step) {
 	bool result = false;
-	if (area->collides_with(body) && CollisionSolver3DSW::solve_static(body->get_shape(body_shape), body->get_transform() * body->get_shape_transform(body_shape), area->get_shape(area_shape), area->get_transform() * area->get_shape_transform(area_shape), nullptr, this)) {
+	if (area->collides_with(body) && GodotCollisionSolver3D::solve_static(body->get_shape(body_shape), body->get_transform() * body->get_shape_transform(body_shape), area->get_shape(area_shape), area->get_transform() * area->get_shape_transform(area_shape), nullptr, this)) {
 		result = true;
 	}
 
 	process_collision = false;
+	has_space_override = false;
 	if (result != colliding) {
-		if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) {
-			process_collision = true;
-		} else if (area->has_monitor_callback()) {
+		if ((int)area->get_param(PhysicsServer3D::AREA_PARAM_GRAVITY_OVERRIDE_MODE) != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) {
+			has_space_override = true;
+		} else if ((int)area->get_param(PhysicsServer3D::AREA_PARAM_LINEAR_DAMP_OVERRIDE_MODE) != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) {
+			has_space_override = true;
+		} else if ((int)area->get_param(PhysicsServer3D::AREA_PARAM_ANGULAR_DAMP_OVERRIDE_MODE) != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) {
+			has_space_override = true;
+		}
+		process_collision = has_space_override;
+
+		if (area->has_monitor_callback()) {
 			process_collision = true;
 		}
 
@@ -51,13 +60,13 @@ bool AreaPair3DSW::setup(real_t p_step) {
 	return process_collision;
 }
 
-bool AreaPair3DSW::pre_solve(real_t p_step) {
+bool GodotAreaPair3D::pre_solve(real_t p_step) {
 	if (!process_collision) {
 		return false;
 	}
 
 	if (colliding) {
-		if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) {
+		if (has_space_override) {
 			body->add_area(area);
 		}
 
@@ -65,7 +74,7 @@ bool AreaPair3DSW::pre_solve(real_t p_step) {
 			area->add_body_to_query(body, body_shape, area_shape);
 		}
 	} else {
-		if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) {
+		if (has_space_override) {
 			body->remove_area(area);
 		}
 
@@ -77,11 +86,11 @@ bool AreaPair3DSW::pre_solve(real_t p_step) {
 	return false; // Never do any post solving.
 }
 
-void AreaPair3DSW::solve(real_t p_step) {
+void GodotAreaPair3D::solve(real_t p_step) {
 	// Nothing to do.
 }
 
-AreaPair3DSW::AreaPair3DSW(Body3DSW *p_body, int p_body_shape, Area3DSW *p_area, int p_area_shape) {
+GodotAreaPair3D::GodotAreaPair3D(GodotBody3D *p_body, int p_body_shape, GodotArea3D *p_area, int p_area_shape) {
 	body = p_body;
 	area = p_area;
 	body_shape = p_body_shape;
@@ -93,9 +102,9 @@ AreaPair3DSW::AreaPair3DSW(Body3DSW *p_body, int p_body_shape, Area3DSW *p_area,
 	}
 }
 
-AreaPair3DSW::~AreaPair3DSW() {
+GodotAreaPair3D::~GodotAreaPair3D() {
 	if (colliding) {
-		if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) {
+		if (has_space_override) {
 			body->remove_area(area);
 		}
 		if (area->has_monitor_callback()) {
@@ -108,10 +117,10 @@ AreaPair3DSW::~AreaPair3DSW() {
 
 ////////////////////////////////////////////////////
 
-bool Area2Pair3DSW::setup(real_t p_step) {
+bool GodotArea2Pair3D::setup(real_t p_step) {
 	bool result_a = area_a->collides_with(area_b);
 	bool result_b = area_b->collides_with(area_a);
-	if ((result_a || result_b) && !CollisionSolver3DSW::solve_static(area_a->get_shape(shape_a), area_a->get_transform() * area_a->get_shape_transform(shape_a), area_b->get_shape(shape_b), area_b->get_transform() * area_b->get_shape_transform(shape_b), nullptr, this)) {
+	if ((result_a || result_b) && !GodotCollisionSolver3D::solve_static(area_a->get_shape(shape_a), area_a->get_transform() * area_a->get_shape_transform(shape_a), area_b->get_shape(shape_b), area_b->get_transform() * area_b->get_shape_transform(shape_b), nullptr, this)) {
 		result_a = false;
 		result_b = false;
 	}
@@ -120,7 +129,7 @@ bool Area2Pair3DSW::setup(real_t p_step) {
 
 	process_collision_a = false;
 	if (result_a != colliding_a) {
-		if (area_a->has_area_monitor_callback() && area_b->is_monitorable()) {
+		if (area_a->has_area_monitor_callback() && area_b_monitorable) {
 			process_collision_a = true;
 			process_collision = true;
 		}
@@ -129,7 +138,7 @@ bool Area2Pair3DSW::setup(real_t p_step) {
 
 	process_collision_b = false;
 	if (result_b != colliding_b) {
-		if (area_b->has_area_monitor_callback() && area_a->is_monitorable()) {
+		if (area_b->has_area_monitor_callback() && area_a_monitorable) {
 			process_collision_b = true;
 			process_collision = true;
 		}
@@ -139,7 +148,7 @@ bool Area2Pair3DSW::setup(real_t p_step) {
 	return process_collision;
 }
 
-bool Area2Pair3DSW::pre_solve(real_t p_step) {
+bool GodotArea2Pair3D::pre_solve(real_t p_step) {
 	if (process_collision_a) {
 		if (colliding_a) {
 			area_a->add_area_to_query(area_b, shape_b, shape_a);
@@ -159,28 +168,30 @@ bool Area2Pair3DSW::pre_solve(real_t p_step) {
 	return false; // Never do any post solving.
 }
 
-void Area2Pair3DSW::solve(real_t p_step) {
+void GodotArea2Pair3D::solve(real_t p_step) {
 	// Nothing to do.
 }
 
-Area2Pair3DSW::Area2Pair3DSW(Area3DSW *p_area_a, int p_shape_a, Area3DSW *p_area_b, int p_shape_b) {
+GodotArea2Pair3D::GodotArea2Pair3D(GodotArea3D *p_area_a, int p_shape_a, GodotArea3D *p_area_b, int p_shape_b) {
 	area_a = p_area_a;
 	area_b = p_area_b;
 	shape_a = p_shape_a;
 	shape_b = p_shape_b;
+	area_a_monitorable = area_a->is_monitorable();
+	area_b_monitorable = area_b->is_monitorable();
 	area_a->add_constraint(this);
 	area_b->add_constraint(this);
 }
 
-Area2Pair3DSW::~Area2Pair3DSW() {
+GodotArea2Pair3D::~GodotArea2Pair3D() {
 	if (colliding_a) {
-		if (area_a->has_area_monitor_callback()) {
+		if (area_a->has_area_monitor_callback() && area_b_monitorable) {
 			area_a->remove_area_from_query(area_b, shape_b, shape_a);
 		}
 	}
 
 	if (colliding_b) {
-		if (area_b->has_area_monitor_callback()) {
+		if (area_b->has_area_monitor_callback() && area_a_monitorable) {
 			area_b->remove_area_from_query(area_a, shape_a, shape_b);
 		}
 	}
@@ -191,11 +202,11 @@ Area2Pair3DSW::~Area2Pair3DSW() {
 
 ////////////////////////////////////////////////////
 
-bool AreaSoftBodyPair3DSW::setup(real_t p_step) {
+bool GodotAreaSoftBodyPair3D::setup(real_t p_step) {
 	bool result = false;
 	if (
 			area->collides_with(soft_body) &&
-			CollisionSolver3DSW::solve_static(
+			GodotCollisionSolver3D::solve_static(
 					soft_body->get_shape(soft_body_shape),
 					soft_body->get_transform() * soft_body->get_shape_transform(soft_body_shape),
 					area->get_shape(area_shape),
@@ -206,10 +217,15 @@ bool AreaSoftBodyPair3DSW::setup(real_t p_step) {
 	}
 
 	process_collision = false;
+	has_space_override = false;
 	if (result != colliding) {
-		if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) {
-			process_collision = true;
-		} else if (area->has_monitor_callback()) {
+		if ((int)area->get_param(PhysicsServer3D::AREA_PARAM_GRAVITY_OVERRIDE_MODE) != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) {
+			has_space_override = true;
+		} else if (area->get_wind_force_magnitude() > CMP_EPSILON) {
+			has_space_override = true;
+		}
+
+		if (area->has_monitor_callback()) {
 			process_collision = true;
 		}
 
@@ -219,13 +235,13 @@ bool AreaSoftBodyPair3DSW::setup(real_t p_step) {
 	return process_collision;
 }
 
-bool AreaSoftBodyPair3DSW::pre_solve(real_t p_step) {
+bool GodotAreaSoftBodyPair3D::pre_solve(real_t p_step) {
 	if (!process_collision) {
 		return false;
 	}
 
 	if (colliding) {
-		if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) {
+		if (has_space_override) {
 			soft_body->add_area(area);
 		}
 
@@ -233,7 +249,7 @@ bool AreaSoftBodyPair3DSW::pre_solve(real_t p_step) {
 			area->add_soft_body_to_query(soft_body, soft_body_shape, area_shape);
 		}
 	} else {
-		if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) {
+		if (has_space_override) {
 			soft_body->remove_area(area);
 		}
 
@@ -245,11 +261,11 @@ bool AreaSoftBodyPair3DSW::pre_solve(real_t p_step) {
 	return false; // Never do any post solving.
 }
 
-void AreaSoftBodyPair3DSW::solve(real_t p_step) {
+void GodotAreaSoftBodyPair3D::solve(real_t p_step) {
 	// Nothing to do.
 }
 
-AreaSoftBodyPair3DSW::AreaSoftBodyPair3DSW(SoftBody3DSW *p_soft_body, int p_soft_body_shape, Area3DSW *p_area, int p_area_shape) {
+GodotAreaSoftBodyPair3D::GodotAreaSoftBodyPair3D(GodotSoftBody3D *p_soft_body, int p_soft_body_shape, GodotArea3D *p_area, int p_area_shape) {
 	soft_body = p_soft_body;
 	area = p_area;
 	soft_body_shape = p_soft_body_shape;
@@ -258,9 +274,9 @@ AreaSoftBodyPair3DSW::AreaSoftBodyPair3DSW(SoftBody3DSW *p_soft_body, int p_soft
 	area->add_constraint(this);
 }
 
-AreaSoftBodyPair3DSW::~AreaSoftBodyPair3DSW() {
+GodotAreaSoftBodyPair3D::~GodotAreaSoftBodyPair3D() {
 	if (colliding) {
-		if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) {
+		if (has_space_override) {
 			soft_body->remove_area(area);
 		}
 		if (area->has_monitor_callback()) {
diff --git a/servers/physics_3d/area_pair_3d_sw.h b/servers/physics_3d/godot_area_pair_3d.h
index 4572dcbb23..4237e7722e 100644
--- a/servers/physics_3d/area_pair_3d_sw.h
+++ b/servers/physics_3d/godot_area_pair_3d.h
@@ -1,5 +1,5 @@
 /*************************************************************************/
-/*  area_pair_3d_sw.h                                                    */
+/*  godot_area_pair_3d.h                                                 */
 /*************************************************************************/
 /*                       This file is part of:                           */
 /*                           GODOT ENGINE                                */
@@ -28,65 +28,69 @@
 /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
 /*************************************************************************/
 
-#ifndef AREA_PAIR_SW_H
-#define AREA_PAIR_SW_H
+#ifndef GODOT_AREA_PAIR_3D_H
+#define GODOT_AREA_PAIR_3D_H
 
-#include "area_3d_sw.h"
-#include "body_3d_sw.h"
-#include "constraint_3d_sw.h"
-#include "soft_body_3d_sw.h"
+#include "godot_area_3d.h"
+#include "godot_body_3d.h"
+#include "godot_constraint_3d.h"
+#include "godot_soft_body_3d.h"
 
-class AreaPair3DSW : public Constraint3DSW {
-	Body3DSW *body;
-	Area3DSW *area;
+class GodotAreaPair3D : public GodotConstraint3D {
+	GodotBody3D *body;
+	GodotArea3D *area;
 	int body_shape;
 	int area_shape;
 	bool colliding = false;
 	bool process_collision = false;
+	bool has_space_override = false;
 
 public:
 	virtual bool setup(real_t p_step) override;
 	virtual bool pre_solve(real_t p_step) override;
 	virtual void solve(real_t p_step) override;
 
-	AreaPair3DSW(Body3DSW *p_body, int p_body_shape, Area3DSW *p_area, int p_area_shape);
-	~AreaPair3DSW();
+	GodotAreaPair3D(GodotBody3D *p_body, int p_body_shape, GodotArea3D *p_area, int p_area_shape);
+	~GodotAreaPair3D();
 };
 
-class Area2Pair3DSW : public Constraint3DSW {
-	Area3DSW *area_a;
-	Area3DSW *area_b;
+class GodotArea2Pair3D : public GodotConstraint3D {
+	GodotArea3D *area_a;
+	GodotArea3D *area_b;
 	int shape_a;
 	int shape_b;
 	bool colliding_a = false;
 	bool colliding_b = false;
 	bool process_collision_a = false;
 	bool process_collision_b = false;
+	bool area_a_monitorable;
+	bool area_b_monitorable;
 
 public:
 	virtual bool setup(real_t p_step) override;
 	virtual bool pre_solve(real_t p_step) override;
 	virtual void solve(real_t p_step) override;
 
-	Area2Pair3DSW(Area3DSW *p_area_a, int p_shape_a, Area3DSW *p_area_b, int p_shape_b);
-	~Area2Pair3DSW();
+	GodotArea2Pair3D(GodotArea3D *p_area_a, int p_shape_a, GodotArea3D *p_area_b, int p_shape_b);
+	~GodotArea2Pair3D();
 };
 
-class AreaSoftBodyPair3DSW : public Constraint3DSW {
-	SoftBody3DSW *soft_body;
-	Area3DSW *area;
+class GodotAreaSoftBodyPair3D : public GodotConstraint3D {
+	GodotSoftBody3D *soft_body;
+	GodotArea3D *area;
 	int soft_body_shape;
 	int area_shape;
 	bool colliding = false;
 	bool process_collision = false;
+	bool has_space_override = false;
 
 public:
 	virtual bool setup(real_t p_step) override;
 	virtual bool pre_solve(real_t p_step) override;
 	virtual void solve(real_t p_step) override;
 
-	AreaSoftBodyPair3DSW(SoftBody3DSW *p_sof_body, int p_soft_body_shape, Area3DSW *p_area, int p_area_shape);
-	~AreaSoftBodyPair3DSW();
+	GodotAreaSoftBodyPair3D(GodotSoftBody3D *p_sof_body, int p_soft_body_shape, GodotArea3D *p_area, int p_area_shape);
+	~GodotAreaSoftBodyPair3D();
 };
 
-#endif // AREA_PAIR__SW_H
+#endif // GODOT_AREA_PAIR_3D_H
diff --git a/servers/physics_3d/body_3d_sw.cpp b/servers/physics_3d/godot_body_3d.cpp
index 7f62e4eb85..40d946655d 100644
--- a/servers/physics_3d/body_3d_sw.cpp
+++ b/servers/physics_3d/godot_body_3d.cpp
@@ -1,5 +1,5 @@
 /*************************************************************************/
-/*  body_3d_sw.cpp                                                       */
+/*  godot_body_3d.cpp                                                    */
 /*************************************************************************/
 /*                       This file is part of:                           */
 /*                           GODOT ENGINE                                */
@@ -28,23 +28,23 @@
 /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
 /*************************************************************************/
 
-#include "body_3d_sw.h"
+#include "godot_body_3d.h"
 
-#include "area_3d_sw.h"
-#include "body_direct_state_3d_sw.h"
-#include "space_3d_sw.h"
+#include "godot_area_3d.h"
+#include "godot_body_direct_state_3d.h"
+#include "godot_space_3d.h"
 
-void Body3DSW::_update_inertia() {
-	if (get_space() && !inertia_update_list.in_list()) {
-		get_space()->body_add_to_inertia_update_list(&inertia_update_list);
+void GodotBody3D::_mass_properties_changed() {
+	if (get_space() && !mass_properties_update_list.in_list() && (calculate_inertia || calculate_center_of_mass)) {
+		get_space()->body_add_to_mass_properties_update_list(&mass_properties_update_list);
 	}
 }
 
-void Body3DSW::_update_transform_dependant() {
+void GodotBody3D::_update_transform_dependent() {
 	center_of_mass = get_transform().basis.xform(center_of_mass_local);
 	principal_inertia_axes = get_transform().basis * principal_inertia_axes_local;
 
-	// update inertia tensor
+	// Update inertia tensor.
 	Basis tb = principal_inertia_axes;
 	Basis tbt = tb.transposed();
 	Basis diag;
@@ -52,74 +52,95 @@ void Body3DSW::_update_transform_dependant() {
 	_inv_inertia_tensor = tb * diag * tbt;
 }
 
-void Body3DSW::update_inertias() {
+void GodotBody3D::update_mass_properties() {
 	// Update shapes and motions.
 
 	switch (mode) {
 		case PhysicsServer3D::BODY_MODE_DYNAMIC: {
-			// Update tensor for all shapes, not the best way but should be somehow OK. (inspired from bullet)
 			real_t total_area = 0;
-
 			for (int i = 0; i < get_shape_count(); i++) {
+				if (is_shape_disabled(i)) {
+					continue;
+				}
+
 				total_area += get_shape_area(i);
 			}
 
-			// We have to recompute the center of mass.
-			center_of_mass_local.zero();
+			if (calculate_center_of_mass) {
+				// We have to recompute the center of mass.
+				center_of_mass_local.zero();
 
-			if (total_area != 0.0) {
-				for (int i = 0; i < get_shape_count(); i++) {
-					real_t area = get_shape_area(i);
+				if (total_area != 0.0) {
+					for (int i = 0; i < get_shape_count(); i++) {
+						if (is_shape_disabled(i)) {
+							continue;
+						}
 
-					real_t mass = area * this->mass / total_area;
+						real_t area = get_shape_area(i);
 
-					// NOTE: we assume that the shape origin is also its center of mass.
-					center_of_mass_local += mass * get_shape_transform(i).origin;
-				}
+						real_t mass = area * this->mass / total_area;
+
+						// NOTE: we assume that the shape origin is also its center of mass.
+						center_of_mass_local += mass * get_shape_transform(i).origin;
+					}
 
-				center_of_mass_local /= mass;
+					center_of_mass_local /= mass;
+				}
 			}
 
-			// Recompute the inertia tensor.
-			Basis inertia_tensor;
-			inertia_tensor.set_zero();
-			bool inertia_set = false;
+			if (calculate_inertia) {
+				// Recompute the inertia tensor.
+				Basis inertia_tensor;
+				inertia_tensor.set_zero();
+				bool inertia_set = false;
 
-			for (int i = 0; i < get_shape_count(); i++) {
-				if (is_shape_disabled(i)) {
-					continue;
-				}
+				for (int i = 0; i < get_shape_count(); i++) {
+					if (is_shape_disabled(i)) {
+						continue;
+					}
 
-				real_t area = get_shape_area(i);
-				if (area == 0.0) {
-					continue;
-				}
+					real_t area = get_shape_area(i);
+					if (area == 0.0) {
+						continue;
+					}
 
-				inertia_set = true;
+					inertia_set = true;
 
-				const Shape3DSW *shape = get_shape(i);
+					const GodotShape3D *shape = get_shape(i);
 
-				real_t mass = area * this->mass / total_area;
+					real_t mass = area * this->mass / total_area;
 
-				Basis shape_inertia_tensor = shape->get_moment_of_inertia(mass).to_diagonal_matrix();
-				Transform3D shape_transform = get_shape_transform(i);
-				Basis shape_basis = shape_transform.basis.orthonormalized();
+					Basis shape_inertia_tensor = Basis::from_scale(shape->get_moment_of_inertia(mass));
+					Transform3D shape_transform = get_shape_transform(i);
+					Basis shape_basis = shape_transform.basis.orthonormalized();
 
-				// NOTE: we don't take the scale of collision shapes into account when computing the inertia tensor!
-				shape_inertia_tensor = shape_basis * shape_inertia_tensor * shape_basis.transposed();
+					// NOTE: we don't take the scale of collision shapes into account when computing the inertia tensor!
+					shape_inertia_tensor = shape_basis * shape_inertia_tensor * shape_basis.transposed();
 
-				Vector3 shape_origin = shape_transform.origin - center_of_mass_local;
-				inertia_tensor += shape_inertia_tensor + (Basis() * shape_origin.dot(shape_origin) - shape_origin.outer(shape_origin)) * mass;
-			}
+					Vector3 shape_origin = shape_transform.origin - center_of_mass_local;
+					inertia_tensor += shape_inertia_tensor + (Basis() * shape_origin.dot(shape_origin) - shape_origin.outer(shape_origin)) * mass;
+				}
 
-			// Set the inertia to a valid value when there are no valid shapes.
-			if (!inertia_set) {
-				inertia_tensor.set_diagonal(Vector3(1.0, 1.0, 1.0));
-			}
+				// Set the inertia to a valid value when there are no valid shapes.
+				if (!inertia_set) {
+					inertia_tensor = Basis();
+				}
 
-			// Compute the principal axes of inertia.
-			principal_inertia_axes_local = inertia_tensor.diagonalize().transposed();
-			_inv_inertia = inertia_tensor.get_main_diagonal().inverse();
+				// Handle partial custom inertia.
+				if (inertia.x > 0.0) {
+					inertia_tensor[0][0] = inertia.x;
+				}
+				if (inertia.y > 0.0) {
+					inertia_tensor[1][1] = inertia.y;
+				}
+				if (inertia.z > 0.0) {
+					inertia_tensor[2][2] = inertia.z;
+				}
+
+				// Compute the principal axes of inertia.
+				principal_inertia_axes_local = inertia_tensor.diagonalize().transposed();
+				_inv_inertia = inertia_tensor.get_main_diagonal().inverse();
+			}
 
 			if (mass) {
 				_inv_mass = 1.0 / mass;
@@ -128,25 +149,28 @@ void Body3DSW::update_inertias() {
 			}
 
 		} break;
-
 		case PhysicsServer3D::BODY_MODE_KINEMATIC:
 		case PhysicsServer3D::BODY_MODE_STATIC: {
-			_inv_inertia_tensor.set_zero();
+			_inv_inertia = Vector3();
 			_inv_mass = 0;
 		} break;
-		case PhysicsServer3D::BODY_MODE_DYNAMIC_LOCKED: {
+		case PhysicsServer3D::BODY_MODE_DYNAMIC_LINEAR: {
 			_inv_inertia_tensor.set_zero();
 			_inv_mass = 1.0 / mass;
 
 		} break;
 	}
 
-	//_update_shapes();
+	_update_transform_dependent();
+}
 
-	_update_transform_dependant();
+void GodotBody3D::reset_mass_properties() {
+	calculate_inertia = true;
+	calculate_center_of_mass = true;
+	_mass_properties_changed();
 }
 
-void Body3DSW::set_active(bool p_active) {
+void GodotBody3D::set_active(bool p_active) {
 	if (active == p_active) {
 		return;
 	}
@@ -165,7 +189,7 @@ void Body3DSW::set_active(bool p_active) {
 	}
 }
 
-void Body3DSW::set_param(PhysicsServer3D::BodyParameter p_param, real_t p_value) {
+void GodotBody3D::set_param(PhysicsServer3D::BodyParameter p_param, const Variant &p_value) {
 	switch (p_param) {
 		case PhysicsServer3D::BODY_PARAM_BOUNCE: {
 			bounce = p_value;
@@ -174,14 +198,48 @@ void Body3DSW::set_param(PhysicsServer3D::BodyParameter p_param, real_t p_value)
 			friction = p_value;
 		} break;
 		case PhysicsServer3D::BODY_PARAM_MASS: {
-			ERR_FAIL_COND(p_value <= 0);
-			mass = p_value;
-			_update_inertia();
-
+			real_t mass_value = p_value;
+			ERR_FAIL_COND(mass_value <= 0);
+			mass = mass_value;
+			if (mode >= PhysicsServer3D::BODY_MODE_DYNAMIC) {
+				_mass_properties_changed();
+			}
+		} break;
+		case PhysicsServer3D::BODY_PARAM_INERTIA: {
+			inertia = p_value;
+			if ((inertia.x <= 0.0) || (inertia.y <= 0.0) || (inertia.z <= 0.0)) {
+				calculate_inertia = true;
+				if (mode == PhysicsServer3D::BODY_MODE_DYNAMIC) {
+					_mass_properties_changed();
+				}
+			} else {
+				calculate_inertia = false;
+				if (mode == PhysicsServer3D::BODY_MODE_DYNAMIC) {
+					principal_inertia_axes_local = Basis();
+					_inv_inertia = inertia.inverse();
+					_update_transform_dependent();
+				}
+			}
+		} break;
+		case PhysicsServer3D::BODY_PARAM_CENTER_OF_MASS: {
+			calculate_center_of_mass = false;
+			center_of_mass_local = p_value;
+			_update_transform_dependent();
 		} break;
 		case PhysicsServer3D::BODY_PARAM_GRAVITY_SCALE: {
+			if (Math::is_zero_approx(gravity_scale)) {
+				wakeup();
+			}
 			gravity_scale = p_value;
 		} break;
+		case PhysicsServer3D::BODY_PARAM_LINEAR_DAMP_MODE: {
+			int mode_value = p_value;
+			linear_damp_mode = (PhysicsServer3D::BodyDampMode)mode_value;
+		} break;
+		case PhysicsServer3D::BODY_PARAM_ANGULAR_DAMP_MODE: {
+			int mode_value = p_value;
+			angular_damp_mode = (PhysicsServer3D::BodyDampMode)mode_value;
+		} break;
 		case PhysicsServer3D::BODY_PARAM_LINEAR_DAMP: {
 			linear_damp = p_value;
 		} break;
@@ -193,7 +251,7 @@ void Body3DSW::set_param(PhysicsServer3D::BodyParameter p_param, real_t p_value)
 	}
 }
 
-real_t Body3DSW::get_param(PhysicsServer3D::BodyParameter p_param) const {
+Variant GodotBody3D::get_param(PhysicsServer3D::BodyParameter p_param) const {
 	switch (p_param) {
 		case PhysicsServer3D::BODY_PARAM_BOUNCE: {
 			return bounce;
@@ -204,9 +262,25 @@ real_t Body3DSW::get_param(PhysicsServer3D::BodyParameter p_param) const {
 		case PhysicsServer3D::BODY_PARAM_MASS: {
 			return mass;
 		} break;
+		case PhysicsServer3D::BODY_PARAM_INERTIA: {
+			if (mode == PhysicsServer3D::BODY_MODE_DYNAMIC) {
+				return _inv_inertia.inverse();
+			} else {
+				return Vector3();
+			}
+		} break;
+		case PhysicsServer3D::BODY_PARAM_CENTER_OF_MASS: {
+			return center_of_mass_local;
+		} break;
 		case PhysicsServer3D::BODY_PARAM_GRAVITY_SCALE: {
 			return gravity_scale;
 		} break;
+		case PhysicsServer3D::BODY_PARAM_LINEAR_DAMP_MODE: {
+			return linear_damp_mode;
+		}
+		case PhysicsServer3D::BODY_PARAM_ANGULAR_DAMP_MODE: {
+			return angular_damp_mode;
+		}
 		case PhysicsServer3D::BODY_PARAM_LINEAR_DAMP: {
 			return linear_damp;
 		} break;
@@ -221,56 +295,60 @@ real_t Body3DSW::get_param(PhysicsServer3D::BodyParameter p_param) const {
 	return 0;
 }
 
-void Body3DSW::set_mode(PhysicsServer3D::BodyMode p_mode) {
+void GodotBody3D::set_mode(PhysicsServer3D::BodyMode p_mode) {
 	PhysicsServer3D::BodyMode prev = mode;
 	mode = p_mode;
 
 	switch (p_mode) {
-		//CLEAR UP EVERYTHING IN CASE IT NOT WORKS!
 		case PhysicsServer3D::BODY_MODE_STATIC:
 		case PhysicsServer3D::BODY_MODE_KINEMATIC: {
 			_set_inv_transform(get_transform().affine_inverse());
 			_inv_mass = 0;
+			_inv_inertia = Vector3();
 			_set_static(p_mode == PhysicsServer3D::BODY_MODE_STATIC);
-			//set_active(p_mode==PhysicsServer3D::BODY_MODE_KINEMATIC);
 			set_active(p_mode == PhysicsServer3D::BODY_MODE_KINEMATIC && contacts.size());
 			linear_velocity = Vector3();
 			angular_velocity = Vector3();
 			if (mode == PhysicsServer3D::BODY_MODE_KINEMATIC && prev != mode) {
 				first_time_kinematic = true;
 			}
+			_update_transform_dependent();
 
 		} break;
 		case PhysicsServer3D::BODY_MODE_DYNAMIC: {
 			_inv_mass = mass > 0 ? (1.0 / mass) : 0;
+			if (!calculate_inertia) {
+				principal_inertia_axes_local = Basis();
+				_inv_inertia = inertia.inverse();
+				_update_transform_dependent();
+			}
+			_mass_properties_changed();
 			_set_static(false);
 			set_active(true);
 
 		} break;
-		case PhysicsServer3D::BODY_MODE_DYNAMIC_LOCKED: {
+		case PhysicsServer3D::BODY_MODE_DYNAMIC_LINEAR: {
 			_inv_mass = mass > 0 ? (1.0 / mass) : 0;
+			_inv_inertia = Vector3();
+			angular_velocity = Vector3();
+			_update_transform_dependent();
 			_set_static(false);
 			set_active(true);
-			angular_velocity = Vector3();
-		} break;
+		}
 	}
-
-	_update_inertia();
-	/*
-	if (get_space())
-		_update_queries();
-	*/
 }
 
-PhysicsServer3D::BodyMode Body3DSW::get_mode() const {
+PhysicsServer3D::BodyMode GodotBody3D::get_mode() const {
 	return mode;
 }
 
-void Body3DSW::_shapes_changed() {
-	_update_inertia();
+void GodotBody3D::_shapes_changed() {
+	_mass_properties_changed();
+	wakeup();
+	wakeup_neighbours();
 }
 
-void Body3DSW::set_state(PhysicsServer3D::BodyState p_state, const Variant &p_variant) {
+void GodotBody3D::set_state(PhysicsServer3D::BodyState p_state, const Variant &p_variant) {
 	switch (p_state) {
 		case PhysicsServer3D::BODY_STATE_TRANSFORM: {
 			if (mode == PhysicsServer3D::BODY_MODE_KINEMATIC) {
@@ -296,16 +374,19 @@ void Body3DSW::set_state(PhysicsServer3D::BodyState p_state, const Variant &p_va
 				}
 				_set_transform(t);
 				_set_inv_transform(get_transform().inverse());
+				_update_transform_dependent();
 			}
 			wakeup();
 
 		} break;
 		case PhysicsServer3D::BODY_STATE_LINEAR_VELOCITY: {
 			linear_velocity = p_variant;
+			constant_linear_velocity = linear_velocity;
 			wakeup();
 		} break;
 		case PhysicsServer3D::BODY_STATE_ANGULAR_VELOCITY: {
 			angular_velocity = p_variant;
+			constant_angular_velocity = angular_velocity;
 			wakeup();
 
 		} break;
@@ -326,7 +407,7 @@ void Body3DSW::set_state(PhysicsServer3D::BodyState p_state, const Variant &p_va
 		} break;
 		case PhysicsServer3D::BODY_STATE_CAN_SLEEP: {
 			can_sleep = p_variant;
-			if (mode == PhysicsServer3D::BODY_MODE_DYNAMIC && !active && !can_sleep) {
+			if (mode >= PhysicsServer3D::BODY_MODE_DYNAMIC && !active && !can_sleep) {
 				set_active(true);
 			}
 
@@ -334,7 +415,7 @@ void Body3DSW::set_state(PhysicsServer3D::BodyState p_state, const Variant &p_va
 	}
 }
 
-Variant Body3DSW::get_state(PhysicsServer3D::BodyState p_state) const {
+Variant GodotBody3D::get_state(PhysicsServer3D::BodyState p_state) const {
 	switch (p_state) {
 		case PhysicsServer3D::BODY_STATE_TRANSFORM: {
 			return get_transform();
@@ -356,10 +437,10 @@ Variant Body3DSW::get_state(PhysicsServer3D::BodyState p_state) const {
 	return Variant();
 }
 
-void Body3DSW::set_space(Space3DSW *p_space) {
+void GodotBody3D::set_space(GodotSpace3D *p_space) {
 	if (get_space()) {
-		if (inertia_update_list.in_list()) {
-			get_space()->body_remove_from_inertia_update_list(&inertia_update_list);
+		if (mass_properties_update_list.in_list()) {
+			get_space()->body_remove_from_mass_properties_update_list(&mass_properties_update_list);
 		}
 		if (active_list.in_list()) {
 			get_space()->body_remove_from_active_list(&active_list);
@@ -372,25 +453,14 @@ void Body3DSW::set_space(Space3DSW *p_space) {
 	_set_space(p_space);
 
 	if (get_space()) {
-		_update_inertia();
+		_mass_properties_changed();
 		if (active) {
 			get_space()->body_add_to_active_list(&active_list);
 		}
 	}
-
-	first_integration = true;
 }
 
-void Body3DSW::_compute_area_gravity_and_damping(const Area3DSW *p_area) {
-	Vector3 area_gravity;
-	p_area->compute_gravity(get_transform().get_origin(), area_gravity);
-	gravity += area_gravity;
-
-	area_linear_damp += p_area->get_linear_damp();
-	area_angular_damp += p_area->get_angular_damp();
-}
-
-void Body3DSW::set_axis_lock(PhysicsServer3D::BodyAxis p_axis, bool lock) {
+void GodotBody3D::set_axis_lock(PhysicsServer3D::BodyAxis p_axis, bool lock) {
 	if (lock) {
 		locked_axis |= p_axis;
 	} else {
@@ -398,73 +468,141 @@ void Body3DSW::set_axis_lock(PhysicsServer3D::BodyAxis p_axis, bool lock) {
 	}
 }
 
-bool Body3DSW::is_axis_locked(PhysicsServer3D::BodyAxis p_axis) const {
+bool GodotBody3D::is_axis_locked(PhysicsServer3D::BodyAxis p_axis) const {
 	return locked_axis & p_axis;
 }
 
-void Body3DSW::integrate_forces(real_t p_step) {
+void GodotBody3D::integrate_forces(real_t p_step) {
 	if (mode == PhysicsServer3D::BODY_MODE_STATIC) {
 		return;
 	}
 
-	Area3DSW *def_area = get_space()->get_default_area();
-	// AreaSW *damp_area = def_area;
-
-	ERR_FAIL_COND(!def_area);
+	ERR_FAIL_COND(!get_space());
 
 	int ac = areas.size();
+
+	bool gravity_done = false;
+	bool linear_damp_done = false;
+	bool angular_damp_done = false;
+
 	bool stopped = false;
+
 	gravity = Vector3(0, 0, 0);
-	area_linear_damp = 0;
-	area_angular_damp = 0;
+
+	total_linear_damp = 0.0;
+	total_angular_damp = 0.0;
+
+	// Combine gravity and damping from overlapping areas in priority order.
 	if (ac) {
 		areas.sort();
 		const AreaCMP *aa = &areas[0];
-		// damp_area = aa[ac-1].area;
 		for (int i = ac - 1; i >= 0 && !stopped; i--) {
-			PhysicsServer3D::AreaSpaceOverrideMode mode = aa[i].area->get_space_override_mode();
-			switch (mode) {
-				case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE:
-				case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: {
-					_compute_area_gravity_and_damping(aa[i].area);
-					stopped = mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE;
-				} break;
-				case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE:
-				case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE_COMBINE: {
-					gravity = Vector3(0, 0, 0);
-					area_angular_damp = 0;
-					area_linear_damp = 0;
-					_compute_area_gravity_and_damping(aa[i].area);
-					stopped = mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE;
-				} break;
-				default: {
+			if (!gravity_done) {
+				PhysicsServer3D::AreaSpaceOverrideMode area_gravity_mode = (PhysicsServer3D::AreaSpaceOverrideMode)(int)aa[i].area->get_param(PhysicsServer3D::AREA_PARAM_GRAVITY_OVERRIDE_MODE);
+				if (area_gravity_mode != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) {
+					Vector3 area_gravity;
+					aa[i].area->compute_gravity(get_transform().get_origin(), area_gravity);
+					switch (area_gravity_mode) {
+						case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE:
+						case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: {
+							gravity += area_gravity;
+							gravity_done = area_gravity_mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE;
+						} break;
+						case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE:
+						case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE_COMBINE: {
+							gravity = area_gravity;
+							gravity_done = area_gravity_mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE;
+						} break;
+						default: {
+						}
+					}
+				}
+			}
+			if (!linear_damp_done) {
+				PhysicsServer3D::AreaSpaceOverrideMode area_linear_damp_mode = (PhysicsServer3D::AreaSpaceOverrideMode)(int)aa[i].area->get_param(PhysicsServer3D::AREA_PARAM_LINEAR_DAMP_OVERRIDE_MODE);
+				if (area_linear_damp_mode != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) {
+					real_t area_linear_damp = aa[i].area->get_linear_damp();
+					switch (area_linear_damp_mode) {
+						case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE:
+						case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: {
+							total_linear_damp += area_linear_damp;
+							linear_damp_done = area_linear_damp_mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE;
+						} break;
+						case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE:
+						case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE_COMBINE: {
+							total_linear_damp = area_linear_damp;
+							linear_damp_done = area_linear_damp_mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE;
+						} break;
+						default: {
+						}
+					}
+				}
+			}
+			if (!angular_damp_done) {
+				PhysicsServer3D::AreaSpaceOverrideMode area_angular_damp_mode = (PhysicsServer3D::AreaSpaceOverrideMode)(int)aa[i].area->get_param(PhysicsServer3D::AREA_PARAM_ANGULAR_DAMP_OVERRIDE_MODE);
+				if (area_angular_damp_mode != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) {
+					real_t area_angular_damp = aa[i].area->get_angular_damp();
+					switch (area_angular_damp_mode) {
+						case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE:
+						case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: {
+							total_angular_damp += area_angular_damp;
+							angular_damp_done = area_angular_damp_mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE;
+						} break;
+						case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE:
+						case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE_COMBINE: {
+							total_angular_damp = area_angular_damp;
+							angular_damp_done = area_angular_damp_mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE;
+						} break;
+						default: {
+						}
+					}
 				}
 			}
+			stopped = gravity_done && linear_damp_done && angular_damp_done;
 		}
 	}
 
+	// Add default gravity and damping from space area.
 	if (!stopped) {
-		_compute_area_gravity_and_damping(def_area);
-	}
+		GodotArea3D *default_area = get_space()->get_default_area();
+		ERR_FAIL_COND(!default_area);
 
-	gravity *= gravity_scale;
+		if (!gravity_done) {
+			Vector3 default_gravity;
+			default_area->compute_gravity(get_transform().get_origin(), default_gravity);
+			gravity += default_gravity;
+		}
+
+		if (!linear_damp_done) {
+			total_linear_damp += default_area->get_linear_damp();
+		}
 
-	// If less than 0, override dampenings with that of the Body
-	if (angular_damp >= 0) {
-		area_angular_damp = angular_damp;
+		if (!angular_damp_done) {
+			total_angular_damp += default_area->get_angular_damp();
+		}
+	}
+
+	// Override linear damping with body's value.
+	switch (linear_damp_mode) {
+		case PhysicsServer3D::BODY_DAMP_MODE_COMBINE: {
+			total_linear_damp += linear_damp;
+		} break;
+		case PhysicsServer3D::BODY_DAMP_MODE_REPLACE: {
+			total_linear_damp = linear_damp;
+		} break;
 	}
-	/*
-	else
-		area_angular_damp=damp_area->get_angular_damp();
-	*/
 
-	if (linear_damp >= 0) {
-		area_linear_damp = linear_damp;
+	// Override angular damping with body's value.
+	switch (angular_damp_mode) {
+		case PhysicsServer3D::BODY_DAMP_MODE_COMBINE: {
+			total_angular_damp += angular_damp;
+		} break;
+		case PhysicsServer3D::BODY_DAMP_MODE_REPLACE: {
+			total_angular_damp = angular_damp;
+		} break;
 	}
-	/*
-	else
-		area_linear_damp=damp_area->get_linear_damp();
-	*/
+
+	gravity *= gravity_scale;
 
 	Vector3 motion;
 	bool do_motion = false;
@@ -473,7 +611,7 @@ void Body3DSW::integrate_forces(real_t p_step) {
 		//compute motion, angular and etc. velocities from prev transform
 		motion = new_transform.origin - get_transform().origin;
 		do_motion = true;
-		linear_velocity = motion / p_step;
+		linear_velocity = constant_linear_velocity + motion / p_step;
 
 		//compute a FAKE angular velocity, not so easy
 		Basis rot = new_transform.basis.orthonormalized() * get_transform().basis.orthonormalized().transposed();
@@ -482,22 +620,22 @@ void Body3DSW::integrate_forces(real_t p_step) {
 
 		rot.get_axis_angle(axis, angle);
 		axis.normalize();
-		angular_velocity = axis * (angle / p_step);
+		angular_velocity = constant_angular_velocity + axis * (angle / p_step);
 	} else {
-		if (!omit_force_integration && !first_integration) {
+		if (!omit_force_integration) {
 			//overridden by direct state query
 
 			Vector3 force = gravity * mass;
 			force += applied_force;
 			Vector3 torque = applied_torque;
 
-			real_t damp = 1.0 - p_step * area_linear_damp;
+			real_t damp = 1.0 - p_step * total_linear_damp;
 
 			if (damp < 0) { // reached zero in the given time
 				damp = 0;
 			}
 
-			real_t angular_damp = 1.0 - p_step * area_angular_damp;
+			real_t angular_damp = 1.0 - p_step * total_angular_damp;
 
 			if (angular_damp < 0) { // reached zero in the given time
 				angular_damp = 0;
@@ -518,9 +656,6 @@ void Body3DSW::integrate_forces(real_t p_step) {
 
 	applied_force = Vector3();
 	applied_torque = Vector3();
-	first_integration = false;
-
-	//motion=linear_velocity*p_step;
 
 	biased_angular_velocity = Vector3();
 	biased_linear_velocity = Vector3();
@@ -529,11 +664,10 @@ void Body3DSW::integrate_forces(real_t p_step) {
 		_update_shapes_with_motion(motion);
 	}
 
-	def_area = nullptr; // clear the area, so it is set in the next frame
 	contact_count = 0;
 }
 
-void Body3DSW::integrate_velocities(real_t p_step) {
+void GodotBody3D::integrate_velocities(real_t p_step) {
 	if (mode == PhysicsServer3D::BODY_MODE_STATIC) {
 		return;
 	}
@@ -594,53 +728,21 @@ void Body3DSW::integrate_velocities(real_t p_step) {
 	_set_transform(transform);
 	_set_inv_transform(get_transform().inverse());
 
-	_update_transform_dependant();
-}
-
-/*
-void BodySW::simulate_motion(const Transform3D& p_xform,real_t p_step) {
-	Transform3D inv_xform = p_xform.affine_inverse();
-	if (!get_space()) {
-		_set_transform(p_xform);
-		_set_inv_transform(inv_xform);
-
-		return;
-	}
-
-	//compute a FAKE linear velocity - this is easy
-
-	linear_velocity=(p_xform.origin - get_transform().origin)/p_step;
-
-	//compute a FAKE angular velocity, not so easy
-	Basis rot=get_transform().basis.orthonormalized().transposed() * p_xform.basis.orthonormalized();
-	Vector3 axis;
-	real_t angle;
-
-	rot.get_axis_angle(axis,angle);
-	axis.normalize();
-	angular_velocity=axis.normalized() * (angle/p_step);
-	linear_velocity = (p_xform.origin - get_transform().origin)/p_step;
-
-	if (!direct_state_query_list.in_list())// - callalways, so lv and av are cleared && (state_query || direct_state_query))
-		get_space()->body_add_to_state_query_list(&direct_state_query_list);
-	simulated_motion=true;
-	_set_transform(p_xform);
+	_update_transform_dependent();
 }
 
-*/
-
-void Body3DSW::wakeup_neighbours() {
-	for (Map<Constraint3DSW *, int>::Element *E = constraint_map.front(); E; E = E->next()) {
-		const Constraint3DSW *c = E->key();
-		Body3DSW **n = c->get_body_ptr();
+void GodotBody3D::wakeup_neighbours() {
+	for (const KeyValue<GodotConstraint3D *, int> &E : constraint_map) {
+		const GodotConstraint3D *c = E.key;
+		GodotBody3D **n = c->get_body_ptr();
 		int bc = c->get_body_count();
 
 		for (int i = 0; i < bc; i++) {
-			if (i == E->get()) {
+			if (i == E.value) {
 				continue;
 			}
-			Body3DSW *b = n[i];
-			if (b->mode != PhysicsServer3D::BODY_MODE_DYNAMIC) {
+			GodotBody3D *b = n[i];
+			if (b->mode < PhysicsServer3D::BODY_MODE_DYNAMIC) {
 				continue;
 			}
 
@@ -651,7 +753,7 @@ void Body3DSW::wakeup_neighbours() {
 	}
 }
 
-void Body3DSW::call_queries() {
+void GodotBody3D::call_queries() {
 	if (fi_callback_data) {
 		if (!fi_callback_data->callable.get_object()) {
 			set_force_integration_callback(Callable());
@@ -671,7 +773,7 @@ void Body3DSW::call_queries() {
 	}
 }
 
-bool Body3DSW::sleep_test(real_t p_step) {
+bool GodotBody3D::sleep_test(real_t p_step) {
 	if (mode == PhysicsServer3D::BODY_MODE_STATIC || mode == PhysicsServer3D::BODY_MODE_KINEMATIC) {
 		return true;
 	} else if (!can_sleep) {
@@ -688,12 +790,12 @@ bool Body3DSW::sleep_test(real_t p_step) {
 	}
 }
 
-void Body3DSW::set_state_sync_callback(void *p_instance, PhysicsServer3D::BodyStateCallback p_callback) {
+void GodotBody3D::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) {
+void GodotBody3D::set_force_integration_callback(const Callable &p_callable, const Variant &p_udata) {
 	if (p_callable.get_object()) {
 		if (!fi_callback_data) {
 			fi_callback_data = memnew(ForceIntegrationCallbackData);
@@ -706,46 +808,23 @@ void Body3DSW::set_force_integration_callback(const Callable &p_callable, const
 	}
 }
 
-PhysicsDirectBodyState3DSW *Body3DSW::get_direct_state() {
+GodotPhysicsDirectBodyState3D *GodotBody3D::get_direct_state() {
 	if (!direct_state) {
-		direct_state = memnew(PhysicsDirectBodyState3DSW);
+		direct_state = memnew(GodotPhysicsDirectBodyState3D);
 		direct_state->body = this;
 	}
 	return direct_state;
 }
 
-Body3DSW::Body3DSW() :
-		CollisionObject3DSW(TYPE_BODY),
+GodotBody3D::GodotBody3D() :
+		GodotCollisionObject3D(TYPE_BODY),
 		active_list(this),
-		inertia_update_list(this),
+		mass_properties_update_list(this),
 		direct_state_query_list(this) {
-	mode = PhysicsServer3D::BODY_MODE_DYNAMIC;
-	active = true;
-
-	mass = 1;
-	_inv_mass = 1;
-	bounce = 0;
-	friction = 1;
-	omit_force_integration = false;
-	//applied_torque=0;
-	island_step = 0;
-	first_time_kinematic = false;
-	first_integration = false;
 	_set_static(false);
-
-	contact_count = 0;
-	gravity_scale = 1.0;
-	linear_damp = -1;
-	angular_damp = -1;
-	area_angular_damp = 0;
-	area_linear_damp = 0;
-
-	still_time = 0;
-	continuous_cd = false;
-	can_sleep = true;
 }
 
-Body3DSW::~Body3DSW() {
+GodotBody3D::~GodotBody3D() {
 	if (fi_callback_data) {
 		memdelete(fi_callback_data);
 	}
diff --git a/servers/physics_3d/body_3d_sw.h b/servers/physics_3d/godot_body_3d.h
index f58f40652b..bba9ec6c3a 100644
--- a/servers/physics_3d/body_3d_sw.h
+++ b/servers/physics_3d/godot_body_3d.h
@@ -1,5 +1,5 @@
 /*************************************************************************/
-/*  body_3d_sw.h                                                         */
+/*  godot_body_3d.h                                                      */
 /*************************************************************************/
 /*                       This file is part of:                           */
 /*                           GODOT ENGINE                                */
@@ -28,35 +28,47 @@
 /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
 /*************************************************************************/
 
-#ifndef BODY_3D_SW_H
-#define BODY_3D_SW_H
+#ifndef GODOT_BODY_3D_H
+#define GODOT_BODY_3D_H
+
+#include "godot_area_3d.h"
+#include "godot_collision_object_3d.h"
 
-#include "area_3d_sw.h"
-#include "collision_object_3d_sw.h"
 #include "core/templates/vset.h"
 
-class Constraint3DSW;
-class PhysicsDirectBodyState3DSW;
+class GodotConstraint3D;
+class GodotPhysicsDirectBodyState3D;
 
-class Body3DSW : public CollisionObject3DSW {
-	PhysicsServer3D::BodyMode mode;
+class GodotBody3D : public GodotCollisionObject3D {
+	PhysicsServer3D::BodyMode mode = PhysicsServer3D::BODY_MODE_DYNAMIC;
 
 	Vector3 linear_velocity;
 	Vector3 angular_velocity;
 
+	Vector3 constant_linear_velocity;
+	Vector3 constant_angular_velocity;
+
 	Vector3 biased_linear_velocity;
 	Vector3 biased_angular_velocity;
-	real_t mass;
-	real_t bounce;
-	real_t friction;
+	real_t mass = 1.0;
+	real_t bounce = 0.0;
+	real_t friction = 1.0;
+	Vector3 inertia;
+
+	PhysicsServer3D::BodyDampMode linear_damp_mode = PhysicsServer3D::BODY_DAMP_MODE_COMBINE;
+	PhysicsServer3D::BodyDampMode angular_damp_mode = PhysicsServer3D::BODY_DAMP_MODE_COMBINE;
+
+	real_t linear_damp = 0.0;
+	real_t angular_damp = 0.0;
 
-	real_t linear_damp;
-	real_t angular_damp;
-	real_t gravity_scale;
+	real_t total_linear_damp = 0.0;
+	real_t total_angular_damp = 0.0;
+
+	real_t gravity_scale = 1.0;
 
 	uint16_t locked_axis = 0;
 
-	real_t _inv_mass;
+	real_t _inv_mass = 1.0;
 	Vector3 _inv_inertia; // Relative to the principal axes of inertia
 
 	// Relative to the local frame of reference
@@ -68,51 +80,50 @@ class Body3DSW : public CollisionObject3DSW {
 	Basis principal_inertia_axes;
 	Vector3 center_of_mass;
 
+	bool calculate_inertia = true;
+	bool calculate_center_of_mass = true;
+
 	Vector3 gravity;
 
-	real_t still_time;
+	real_t still_time = 0.0;
 
 	Vector3 applied_force;
 	Vector3 applied_torque;
 
-	real_t area_angular_damp;
-	real_t area_linear_damp;
-
-	SelfList<Body3DSW> active_list;
-	SelfList<Body3DSW> inertia_update_list;
-	SelfList<Body3DSW> direct_state_query_list;
+	SelfList<GodotBody3D> active_list;
+	SelfList<GodotBody3D> mass_properties_update_list;
+	SelfList<GodotBody3D> direct_state_query_list;
 
 	VSet<RID> exceptions;
-	bool omit_force_integration;
-	bool active;
+	bool omit_force_integration = false;
+	bool active = true;
 
-	bool first_integration;
+	bool continuous_cd = false;
+	bool can_sleep = true;
+	bool first_time_kinematic = false;
 
-	bool continuous_cd;
-	bool can_sleep;
-	bool first_time_kinematic;
-	void _update_inertia();
+	void _mass_properties_changed();
 	virtual void _shapes_changed();
 	Transform3D new_transform;
 
-	Map<Constraint3DSW *, int> constraint_map;
+	Map<GodotConstraint3D *, int> constraint_map;
 
 	Vector<AreaCMP> areas;
 
 	struct Contact {
 		Vector3 local_pos;
 		Vector3 local_normal;
-		real_t depth;
-		int local_shape;
+		real_t depth = 0.0;
+		int local_shape = 0;
 		Vector3 collider_pos;
-		int collider_shape;
+		int collider_shape = 0;
 		ObjectID collider_instance_id;
 		RID collider;
 		Vector3 collider_velocity_at_pos;
 	};
 
 	Vector<Contact> contacts; //no contacts by default
-	int contact_count;
+	int contact_count = 0;
 
 	void *body_state_callback_instance = nullptr;
 	PhysicsServer3D::BodyStateCallback body_state_callback = nullptr;
@@ -124,23 +135,21 @@ class Body3DSW : public CollisionObject3DSW {
 
 	ForceIntegrationCallbackData *fi_callback_data = nullptr;
 
-	PhysicsDirectBodyState3DSW *direct_state = nullptr;
-
-	uint64_t island_step;
+	GodotPhysicsDirectBodyState3D *direct_state = nullptr;
 
-	_FORCE_INLINE_ void _compute_area_gravity_and_damping(const Area3DSW *p_area);
+	uint64_t island_step = 0;
 
-	_FORCE_INLINE_ void _update_transform_dependant();
+	void _update_transform_dependent();
 
-	friend class PhysicsDirectBodyState3DSW; // i give up, too many functions to expose
+	friend class GodotPhysicsDirectBodyState3D; // 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();
+	GodotPhysicsDirectBodyState3D *get_direct_state();
 
-	_FORCE_INLINE_ void add_area(Area3DSW *p_area) {
+	_FORCE_INLINE_ void add_area(GodotArea3D *p_area) {
 		int index = areas.find(AreaCMP(p_area));
 		if (index > -1) {
 			areas.write[index].refCount += 1;
@@ -149,12 +158,12 @@ public:
 		}
 	}
 
-	_FORCE_INLINE_ void remove_area(Area3DSW *p_area) {
+	_FORCE_INLINE_ void remove_area(GodotArea3D *p_area) {
 		int index = areas.find(AreaCMP(p_area));
 		if (index > -1) {
 			areas.write[index].refCount -= 1;
 			if (areas[index].refCount < 1) {
-				areas.remove(index);
+				areas.remove_at(index);
 			}
 		}
 	}
@@ -179,9 +188,9 @@ public:
 	_FORCE_INLINE_ uint64_t get_island_step() const { return island_step; }
 	_FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step = p_step; }
 
-	_FORCE_INLINE_ void add_constraint(Constraint3DSW *p_constraint, int p_pos) { constraint_map[p_constraint] = p_pos; }
-	_FORCE_INLINE_ void remove_constraint(Constraint3DSW *p_constraint) { constraint_map.erase(p_constraint); }
-	const Map<Constraint3DSW *, int> &get_constraint_map() const { return constraint_map; }
+	_FORCE_INLINE_ void add_constraint(GodotConstraint3D *p_constraint, int p_pos) { constraint_map[p_constraint] = p_pos; }
+	_FORCE_INLINE_ void remove_constraint(GodotConstraint3D *p_constraint) { constraint_map.erase(p_constraint); }
+	const Map<GodotConstraint3D *, int> &get_constraint_map() const { return constraint_map; }
 	_FORCE_INLINE_ void clear_constraint_map() { constraint_map.clear(); }
 
 	_FORCE_INLINE_ void set_omit_force_integration(bool p_omit_force_integration) { omit_force_integration = p_omit_force_integration; }
@@ -189,6 +198,7 @@ public:
 
 	_FORCE_INLINE_ Basis get_principal_inertia_axes() const { return principal_inertia_axes; }
 	_FORCE_INLINE_ Vector3 get_center_of_mass() const { return center_of_mass; }
+	_FORCE_INLINE_ Vector3 get_center_of_mass_local() const { return center_of_mass_local; }
 	_FORCE_INLINE_ Vector3 xform_local_to_principal(const Vector3 &p_pos) const { return principal_inertia_axes_local.xform(p_pos - center_of_mass_local); }
 
 	_FORCE_INLINE_ void set_linear_velocity(const Vector3 &p_velocity) { linear_velocity = p_velocity; }
@@ -251,8 +261,8 @@ public:
 		set_active(true);
 	}
 
-	void set_param(PhysicsServer3D::BodyParameter p_param, real_t);
-	real_t get_param(PhysicsServer3D::BodyParameter p_param) const;
+	void set_param(PhysicsServer3D::BodyParameter p_param, const Variant &p_value);
+	Variant get_param(PhysicsServer3D::BodyParameter p_param) const;
 
 	void set_mode(PhysicsServer3D::BodyMode p_mode);
 	PhysicsServer3D::BodyMode get_mode() const;
@@ -269,15 +279,15 @@ public:
 	_FORCE_INLINE_ void set_continuous_collision_detection(bool p_enable) { continuous_cd = p_enable; }
 	_FORCE_INLINE_ bool is_continuous_collision_detection_enabled() const { return continuous_cd; }
 
-	void set_space(Space3DSW *p_space);
+	void set_space(GodotSpace3D *p_space);
 
-	void update_inertias();
+	void update_mass_properties();
+	void reset_mass_properties();
 
 	_FORCE_INLINE_ real_t get_inv_mass() const { return _inv_mass; }
 	_FORCE_INLINE_ const Vector3 &get_inv_inertia() const { return _inv_inertia; }
 	_FORCE_INLINE_ const Basis &get_inv_inertia_tensor() const { return _inv_inertia_tensor; }
 	_FORCE_INLINE_ real_t get_friction() const { return friction; }
-	_FORCE_INLINE_ const Vector3 &get_gravity() const { return gravity; }
 	_FORCE_INLINE_ real_t get_bounce() const { return bounce; }
 
 	void set_axis_lock(PhysicsServer3D::BodyAxis p_axis, bool lock);
@@ -310,13 +320,13 @@ public:
 
 	bool sleep_test(real_t p_step);
 
-	Body3DSW();
-	~Body3DSW();
+	GodotBody3D();
+	~GodotBody3D();
 };
 
 //add contact inline
 
-void Body3DSW::add_contact(const Vector3 &p_local_pos, const Vector3 &p_local_normal, real_t p_depth, int p_local_shape, const Vector3 &p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID &p_collider, const Vector3 &p_collider_velocity_at_pos) {
+void GodotBody3D::add_contact(const Vector3 &p_local_pos, const Vector3 &p_local_normal, real_t p_depth, int p_local_shape, const Vector3 &p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID &p_collider, const Vector3 &p_collider_velocity_at_pos) {
 	int c_max = contacts.size();
 
 	if (c_max == 0) {
@@ -358,4 +368,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;
 }
 
-#endif // BODY_3D_SW_H
+#endif // GODOT_BODY_3D_H
diff --git a/servers/physics_3d/body_direct_state_3d_sw.cpp b/servers/physics_3d/godot_body_direct_state_3d.cpp
index d197dd288d..a929cab6f9 100644
--- a/servers/physics_3d/body_direct_state_3d_sw.cpp
+++ b/servers/physics_3d/godot_body_direct_state_3d.cpp
@@ -1,5 +1,5 @@
 /*************************************************************************/
-/*  body_direct_state_3d_sw.cpp                                          */
+/*  godot_body_direct_state_3d.cpp                                       */
 /*************************************************************************/
 /*                       This file is part of:                           */
 /*                           GODOT ENGINE                                */
@@ -28,155 +28,167 @@
 /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
 /*************************************************************************/
 
-#include "body_direct_state_3d_sw.h"
+#include "godot_body_direct_state_3d.h"
 
-#include "body_3d_sw.h"
-#include "space_3d_sw.h"
+#include "godot_body_3d.h"
+#include "godot_space_3d.h"
 
-Vector3 PhysicsDirectBodyState3DSW::get_total_gravity() const {
+Vector3 GodotPhysicsDirectBodyState3D::get_total_gravity() const {
 	return body->gravity;
 }
 
-real_t PhysicsDirectBodyState3DSW::get_total_angular_damp() const {
-	return body->area_angular_damp;
+real_t GodotPhysicsDirectBodyState3D::get_total_angular_damp() const {
+	return body->total_angular_damp;
 }
 
-real_t PhysicsDirectBodyState3DSW::get_total_linear_damp() const {
-	return body->area_linear_damp;
+real_t GodotPhysicsDirectBodyState3D::get_total_linear_damp() const {
+	return body->total_linear_damp;
 }
 
-Vector3 PhysicsDirectBodyState3DSW::get_center_of_mass() const {
+Vector3 GodotPhysicsDirectBodyState3D::get_center_of_mass() const {
 	return body->get_center_of_mass();
 }
 
-Basis PhysicsDirectBodyState3DSW::get_principal_inertia_axes() const {
+Vector3 GodotPhysicsDirectBodyState3D::get_center_of_mass_local() const {
+	return body->get_center_of_mass_local();
+}
+
+Basis GodotPhysicsDirectBodyState3D::get_principal_inertia_axes() const {
 	return body->get_principal_inertia_axes();
 }
 
-real_t PhysicsDirectBodyState3DSW::get_inverse_mass() const {
+real_t GodotPhysicsDirectBodyState3D::get_inverse_mass() const {
 	return body->get_inv_mass();
 }
 
-Vector3 PhysicsDirectBodyState3DSW::get_inverse_inertia() const {
+Vector3 GodotPhysicsDirectBodyState3D::get_inverse_inertia() const {
 	return body->get_inv_inertia();
 }
 
-Basis PhysicsDirectBodyState3DSW::get_inverse_inertia_tensor() const {
+Basis GodotPhysicsDirectBodyState3D::get_inverse_inertia_tensor() const {
 	return body->get_inv_inertia_tensor();
 }
 
-void PhysicsDirectBodyState3DSW::set_linear_velocity(const Vector3 &p_velocity) {
+void GodotPhysicsDirectBodyState3D::set_linear_velocity(const Vector3 &p_velocity) {
+	body->wakeup();
 	body->set_linear_velocity(p_velocity);
 }
 
-Vector3 PhysicsDirectBodyState3DSW::get_linear_velocity() const {
+Vector3 GodotPhysicsDirectBodyState3D::get_linear_velocity() const {
 	return body->get_linear_velocity();
 }
 
-void PhysicsDirectBodyState3DSW::set_angular_velocity(const Vector3 &p_velocity) {
+void GodotPhysicsDirectBodyState3D::set_angular_velocity(const Vector3 &p_velocity) {
+	body->wakeup();
 	body->set_angular_velocity(p_velocity);
 }
 
-Vector3 PhysicsDirectBodyState3DSW::get_angular_velocity() const {
+Vector3 GodotPhysicsDirectBodyState3D::get_angular_velocity() const {
 	return body->get_angular_velocity();
 }
 
-void PhysicsDirectBodyState3DSW::set_transform(const Transform3D &p_transform) {
+void GodotPhysicsDirectBodyState3D::set_transform(const Transform3D &p_transform) {
 	body->set_state(PhysicsServer3D::BODY_STATE_TRANSFORM, p_transform);
 }
 
-Transform3D PhysicsDirectBodyState3DSW::get_transform() const {
+Transform3D GodotPhysicsDirectBodyState3D::get_transform() const {
 	return body->get_transform();
 }
 
-Vector3 PhysicsDirectBodyState3DSW::get_velocity_at_local_position(const Vector3 &p_position) const {
+Vector3 GodotPhysicsDirectBodyState3D::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) {
+void GodotPhysicsDirectBodyState3D::add_central_force(const Vector3 &p_force) {
+	body->wakeup();
 	body->add_central_force(p_force);
 }
 
-void PhysicsDirectBodyState3DSW::add_force(const Vector3 &p_force, const Vector3 &p_position) {
+void GodotPhysicsDirectBodyState3D::add_force(const Vector3 &p_force, const Vector3 &p_position) {
+	body->wakeup();
 	body->add_force(p_force, p_position);
 }
 
-void PhysicsDirectBodyState3DSW::add_torque(const Vector3 &p_torque) {
+void GodotPhysicsDirectBodyState3D::add_torque(const Vector3 &p_torque) {
+	body->wakeup();
 	body->add_torque(p_torque);
 }
 
-void PhysicsDirectBodyState3DSW::apply_central_impulse(const Vector3 &p_impulse) {
+void GodotPhysicsDirectBodyState3D::apply_central_impulse(const Vector3 &p_impulse) {
+	body->wakeup();
 	body->apply_central_impulse(p_impulse);
 }
 
-void PhysicsDirectBodyState3DSW::apply_impulse(const Vector3 &p_impulse, const Vector3 &p_position) {
+void GodotPhysicsDirectBodyState3D::apply_impulse(const Vector3 &p_impulse, const Vector3 &p_position) {
+	body->wakeup();
 	body->apply_impulse(p_impulse, p_position);
 }
 
-void PhysicsDirectBodyState3DSW::apply_torque_impulse(const Vector3 &p_impulse) {
+void GodotPhysicsDirectBodyState3D::apply_torque_impulse(const Vector3 &p_impulse) {
+	body->wakeup();
 	body->apply_torque_impulse(p_impulse);
 }
 
-void PhysicsDirectBodyState3DSW::set_sleep_state(bool p_sleep) {
+void GodotPhysicsDirectBodyState3D::set_sleep_state(bool p_sleep) {
 	body->set_active(!p_sleep);
 }
 
-bool PhysicsDirectBodyState3DSW::is_sleeping() const {
+bool GodotPhysicsDirectBodyState3D::is_sleeping() const {
 	return !body->is_active();
 }
 
-int PhysicsDirectBodyState3DSW::get_contact_count() const {
+int GodotPhysicsDirectBodyState3D::get_contact_count() const {
 	return body->contact_count;
 }
 
-Vector3 PhysicsDirectBodyState3DSW::get_contact_local_position(int p_contact_idx) const {
+Vector3 GodotPhysicsDirectBodyState3D::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 {
+Vector3 GodotPhysicsDirectBodyState3D::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 {
+real_t GodotPhysicsDirectBodyState3D::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 {
+int GodotPhysicsDirectBodyState3D::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 {
+RID GodotPhysicsDirectBodyState3D::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 {
+Vector3 GodotPhysicsDirectBodyState3D::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 {
+ObjectID GodotPhysicsDirectBodyState3D::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 {
+int GodotPhysicsDirectBodyState3D::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 {
+Vector3 GodotPhysicsDirectBodyState3D::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() {
+PhysicsDirectSpaceState3D *GodotPhysicsDirectBodyState3D::get_space_state() {
 	return body->get_space()->get_direct_state();
 }
 
-real_t PhysicsDirectBodyState3DSW::get_step() const {
+real_t GodotPhysicsDirectBodyState3D::get_step() const {
 	return body->get_space()->get_last_step();
 }
diff --git a/servers/physics_3d/body_direct_state_3d_sw.h b/servers/physics_3d/godot_body_direct_state_3d.h
index 5132376715..35fd1543b0 100644
--- a/servers/physics_3d/body_direct_state_3d_sw.h
+++ b/servers/physics_3d/godot_body_direct_state_3d.h
@@ -1,5 +1,5 @@
 /*************************************************************************/
-/*  body_direct_state_3d_sw.h                                            */
+/*  godot_body_direct_state_3d.h                                         */
 /*************************************************************************/
 /*                       This file is part of:                           */
 /*                           GODOT ENGINE                                */
@@ -28,24 +28,25 @@
 /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
 /*************************************************************************/
 
-#ifndef BODY_DIRECT_STATE_3D_SW_H
-#define BODY_DIRECT_STATE_3D_SW_H
+#ifndef GODOT_BODY_DIRECT_STATE_3D_H
+#define GODOT_BODY_DIRECT_STATE_3D_H
 
 #include "servers/physics_server_3d.h"
 
-class Body3DSW;
+class GodotBody3D;
 
-class PhysicsDirectBodyState3DSW : public PhysicsDirectBodyState3D {
-	GDCLASS(PhysicsDirectBodyState3DSW, PhysicsDirectBodyState3D);
+class GodotPhysicsDirectBodyState3D : public PhysicsDirectBodyState3D {
+	GDCLASS(GodotPhysicsDirectBodyState3D, PhysicsDirectBodyState3D);
 
 public:
-	Body3DSW *body = nullptr;
+	GodotBody3D *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 Vector3 get_center_of_mass_local() const override;
 	virtual Basis get_principal_inertia_axes() const override;
 
 	virtual real_t get_inverse_mass() const override;
@@ -91,4 +92,4 @@ public:
 	virtual real_t get_step() const override;
 };
 
-#endif // BODY_DIRECT_STATE_3D_SW_H
+#endif // GODOT_BODY_DIRECT_STATE_3D_H
diff --git a/servers/physics_3d/body_pair_3d_sw.cpp b/servers/physics_3d/godot_body_pair_3d.cpp
index c27a2ecced..f0002870ae 100644
--- a/servers/physics_3d/body_pair_3d_sw.cpp
+++ b/servers/physics_3d/godot_body_pair_3d.cpp
@@ -1,5 +1,5 @@
 /*************************************************************************/
-/*  body_pair_3d_sw.cpp                                                  */
+/*  godot_body_pair_3d.cpp                                               */
 /*************************************************************************/
 /*                       This file is part of:                           */
 /*                           GODOT ENGINE                                */
@@ -28,11 +28,12 @@
 /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
 /*************************************************************************/
 
-#include "body_pair_3d_sw.h"
+#include "godot_body_pair_3d.h"
+
+#include "godot_collision_solver_3d.h"
+#include "godot_space_3d.h"
 
-#include "collision_solver_3d_sw.h"
 #include "core/os/os.h"
-#include "space_3d_sw.h"
 
 /*
 #define NO_ACCUMULATE_IMPULSES
@@ -49,12 +50,12 @@
 #define MIN_VELOCITY 0.0001
 #define MAX_BIAS_ROTATION (Math_PI / 8)
 
-void BodyPair3DSW::_contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata) {
-	BodyPair3DSW *pair = (BodyPair3DSW *)p_userdata;
+void GodotBodyPair3D::_contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata) {
+	GodotBodyPair3D *pair = (GodotBodyPair3D *)p_userdata;
 	pair->contact_added_callback(p_point_A, p_index_A, p_point_B, p_index_B);
 }
 
-void BodyPair3DSW::contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B) {
+void GodotBodyPair3D::contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B) {
 	// check if we already have the contact
 
 	//Vector3 local_A = A->get_inv_transform().xform(p_point_A);
@@ -135,7 +136,7 @@ void BodyPair3DSW::contact_added_callback(const Vector3 &p_point_A, int p_index_
 	}
 }
 
-void BodyPair3DSW::validate_contacts() {
+void GodotBodyPair3D::validate_contacts() {
 	//make sure to erase contacts that are no longer valid
 
 	real_t contact_max_separation = space->get_contact_max_separation();
@@ -161,7 +162,7 @@ void BodyPair3DSW::validate_contacts() {
 	}
 }
 
-bool BodyPair3DSW::_test_ccd(real_t p_step, Body3DSW *p_A, int p_shape_A, const Transform3D &p_xform_A, Body3DSW *p_B, int p_shape_B, const Transform3D &p_xform_B) {
+bool GodotBodyPair3D::_test_ccd(real_t p_step, GodotBody3D *p_A, int p_shape_A, const Transform3D &p_xform_A, GodotBody3D *p_B, int p_shape_B, const Transform3D &p_xform_B) {
 	Vector3 motion = p_A->get_linear_velocity() * p_step;
 	real_t mlen = motion.length();
 	if (mlen < CMP_EPSILON) {
@@ -190,7 +191,7 @@ bool BodyPair3DSW::_test_ccd(real_t p_step, Body3DSW *p_A, int p_shape_A, const
 	Vector3 local_to = from_inv.xform(to);
 
 	Vector3 rpos, rnorm;
-	if (!p_B->get_shape(p_shape_B)->intersect_segment(local_from, local_to, rpos, rnorm)) {
+	if (!p_B->get_shape(p_shape_B)->intersect_segment(local_from, local_to, rpos, rnorm, true)) {
 		return false;
 	}
 
@@ -203,15 +204,15 @@ bool BodyPair3DSW::_test_ccd(real_t p_step, Body3DSW *p_A, int p_shape_A, const
 	return true;
 }
 
-real_t combine_bounce(Body3DSW *A, Body3DSW *B) {
+real_t combine_bounce(GodotBody3D *A, GodotBody3D *B) {
 	return CLAMP(A->get_bounce() + B->get_bounce(), 0, 1);
 }
 
-real_t combine_friction(Body3DSW *A, Body3DSW *B) {
+real_t combine_friction(GodotBody3D *A, GodotBody3D *B) {
 	return ABS(MIN(A->get_friction(), B->get_friction()));
 }
 
-bool BodyPair3DSW::setup(real_t p_step) {
+bool GodotBodyPair3D::setup(real_t p_step) {
 	if (!A->interacts_with(B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self())) {
 		collided = false;
 		return false;
@@ -242,10 +243,10 @@ bool BodyPair3DSW::setup(real_t p_step) {
 	xform_Bu.origin -= offset_A;
 	Transform3D xform_B = xform_Bu * B->get_shape_transform(shape_B);
 
-	Shape3DSW *shape_A_ptr = A->get_shape(shape_A);
-	Shape3DSW *shape_B_ptr = B->get_shape(shape_B);
+	GodotShape3D *shape_A_ptr = A->get_shape(shape_A);
+	GodotShape3D *shape_B_ptr = B->get_shape(shape_B);
 
-	collided = CollisionSolver3DSW::solve_static(shape_A_ptr, xform_A, shape_B_ptr, xform_B, _contact_added_callback, this, &sep_axis);
+	collided = GodotCollisionSolver3D::solve_static(shape_A_ptr, xform_A, shape_B_ptr, xform_B, _contact_added_callback, this, &sep_axis);
 
 	if (!collided) {
 		//test ccd (currently just a raycast)
@@ -264,7 +265,7 @@ bool BodyPair3DSW::setup(real_t p_step) {
 	return true;
 }
 
-bool BodyPair3DSW::pre_solve(real_t p_step) {
+bool GodotBodyPair3D::pre_solve(real_t p_step) {
 	if (!collided) {
 		return false;
 	}
@@ -273,8 +274,8 @@ bool BodyPair3DSW::pre_solve(real_t p_step) {
 
 	real_t bias = (real_t)0.3;
 
-	Shape3DSW *shape_A_ptr = A->get_shape(shape_A);
-	Shape3DSW *shape_B_ptr = B->get_shape(shape_B);
+	GodotShape3D *shape_A_ptr = A->get_shape(shape_A);
+	GodotShape3D *shape_B_ptr = B->get_shape(shape_B);
 
 	if (shape_A_ptr->get_custom_bias() || shape_B_ptr->get_custom_bias()) {
 		if (shape_A_ptr->get_custom_bias() == 0) {
@@ -380,7 +381,7 @@ bool BodyPair3DSW::pre_solve(real_t p_step) {
 	return do_process;
 }
 
-void BodyPair3DSW::solve(real_t p_step) {
+void GodotBodyPair3D::solve(real_t p_step) {
 	if (!collided) {
 		return;
 	}
@@ -494,8 +495,7 @@ void BodyPair3DSW::solve(real_t p_step) {
 			Vector3 temp1 = inv_inertia_tensor_A.xform(c.rA.cross(tv));
 			Vector3 temp2 = inv_inertia_tensor_B.xform(c.rB.cross(tv));
 
-			real_t t = -tvl /
-					   (inv_mass_A + inv_mass_B + tv.dot(temp1.cross(c.rA) + temp2.cross(c.rB)));
+			real_t t = -tvl / (inv_mass_A + inv_mass_B + tv.dot(temp1.cross(c.rA) + temp2.cross(c.rB)));
 
 			Vector3 jt = t * tv;
 
@@ -523,8 +523,8 @@ void BodyPair3DSW::solve(real_t p_step) {
 	}
 }
 
-BodyPair3DSW::BodyPair3DSW(Body3DSW *p_A, int p_shape_A, Body3DSW *p_B, int p_shape_B) :
-		BodyContact3DSW(_arr, 2) {
+GodotBodyPair3D::GodotBodyPair3D(GodotBody3D *p_A, int p_shape_A, GodotBody3D *p_B, int p_shape_B) :
+		GodotBodyContact3D(_arr, 2) {
 	A = p_A;
 	B = p_B;
 	shape_A = p_shape_A;
@@ -534,17 +534,17 @@ BodyPair3DSW::BodyPair3DSW(Body3DSW *p_A, int p_shape_A, Body3DSW *p_B, int p_sh
 	B->add_constraint(this, 1);
 }
 
-BodyPair3DSW::~BodyPair3DSW() {
+GodotBodyPair3D::~GodotBodyPair3D() {
 	A->remove_constraint(this);
 	B->remove_constraint(this);
 }
 
-void BodySoftBodyPair3DSW::_contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata) {
-	BodySoftBodyPair3DSW *pair = (BodySoftBodyPair3DSW *)p_userdata;
+void GodotBodySoftBodyPair3D::_contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata) {
+	GodotBodySoftBodyPair3D *pair = (GodotBodySoftBodyPair3D *)p_userdata;
 	pair->contact_added_callback(p_point_A, p_index_A, p_point_B, p_index_B);
 }
 
-void BodySoftBodyPair3DSW::contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B) {
+void GodotBodySoftBodyPair3D::contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B) {
 	Vector3 local_A = body->get_inv_transform().xform(p_point_A);
 	Vector3 local_B = p_point_B - soft_body->get_node_position(p_index_B);
 
@@ -582,7 +582,7 @@ void BodySoftBodyPair3DSW::contact_added_callback(const Vector3 &p_point_A, int
 	contacts.push_back(contact);
 }
 
-void BodySoftBodyPair3DSW::validate_contacts() {
+void GodotBodySoftBodyPair3D::validate_contacts() {
 	// Make sure to erase contacts that are no longer valid.
 	const Transform3D &transform_A = body->get_transform();
 
@@ -612,7 +612,7 @@ void BodySoftBodyPair3DSW::validate_contacts() {
 	contacts.resize(contact_count);
 }
 
-bool BodySoftBodyPair3DSW::setup(real_t p_step) {
+bool GodotBodySoftBodyPair3D::setup(real_t p_step) {
 	if (!body->interacts_with(soft_body) || body->has_exception(soft_body->get_self()) || soft_body->has_exception(body->get_self())) {
 		collided = false;
 		return false;
@@ -638,15 +638,15 @@ bool BodySoftBodyPair3DSW::setup(real_t p_step) {
 
 	validate_contacts();
 
-	Shape3DSW *shape_A_ptr = body->get_shape(body_shape);
-	Shape3DSW *shape_B_ptr = soft_body->get_shape(0);
+	GodotShape3D *shape_A_ptr = body->get_shape(body_shape);
+	GodotShape3D *shape_B_ptr = soft_body->get_shape(0);
 
-	collided = CollisionSolver3DSW::solve_static(shape_A_ptr, xform_A, shape_B_ptr, xform_B, _contact_added_callback, this, &sep_axis);
+	collided = GodotCollisionSolver3D::solve_static(shape_A_ptr, xform_A, shape_B_ptr, xform_B, _contact_added_callback, this, &sep_axis);
 
 	return collided;
 }
 
-bool BodySoftBodyPair3DSW::pre_solve(real_t p_step) {
+bool GodotBodySoftBodyPair3D::pre_solve(real_t p_step) {
 	if (!collided) {
 		return false;
 	}
@@ -655,7 +655,7 @@ bool BodySoftBodyPair3DSW::pre_solve(real_t p_step) {
 
 	real_t bias = (real_t)0.3;
 
-	Shape3DSW *shape_A_ptr = body->get_shape(body_shape);
+	GodotShape3D *shape_A_ptr = body->get_shape(body_shape);
 
 	if (shape_A_ptr->get_custom_bias()) {
 		bias = shape_A_ptr->get_custom_bias();
@@ -753,7 +753,7 @@ bool BodySoftBodyPair3DSW::pre_solve(real_t p_step) {
 	return do_process;
 }
 
-void BodySoftBodyPair3DSW::solve(real_t p_step) {
+void GodotBodySoftBodyPair3D::solve(real_t p_step) {
 	if (!collided) {
 		return;
 	}
@@ -862,8 +862,7 @@ void BodySoftBodyPair3DSW::solve(real_t p_step) {
 
 			Vector3 temp1 = body_inv_inertia_tensor.xform(c.rA.cross(tv));
 
-			real_t t = -tvl /
-					   (body_inv_mass + node_inv_mass + tv.dot(temp1.cross(c.rA)));
+			real_t t = -tvl / (body_inv_mass + node_inv_mass + tv.dot(temp1.cross(c.rA)));
 
 			Vector3 jt = t * tv;
 
@@ -891,8 +890,8 @@ void BodySoftBodyPair3DSW::solve(real_t p_step) {
 	}
 }
 
-BodySoftBodyPair3DSW::BodySoftBodyPair3DSW(Body3DSW *p_A, int p_shape_A, SoftBody3DSW *p_B) :
-		BodyContact3DSW(&body, 1) {
+GodotBodySoftBodyPair3D::GodotBodySoftBodyPair3D(GodotBody3D *p_A, int p_shape_A, GodotSoftBody3D *p_B) :
+		GodotBodyContact3D(&body, 1) {
 	body = p_A;
 	soft_body = p_B;
 	body_shape = p_shape_A;
@@ -901,7 +900,7 @@ BodySoftBodyPair3DSW::BodySoftBodyPair3DSW(Body3DSW *p_A, int p_shape_A, SoftBod
 	soft_body->add_constraint(this);
 }
 
-BodySoftBodyPair3DSW::~BodySoftBodyPair3DSW() {
+GodotBodySoftBodyPair3D::~GodotBodySoftBodyPair3D() {
 	body->remove_constraint(this);
 	soft_body->remove_constraint(this);
 }
diff --git a/servers/physics_3d/body_pair_3d_sw.h b/servers/physics_3d/godot_body_pair_3d.h
index 19d6a46880..c0a2424e05 100644
--- a/servers/physics_3d/body_pair_3d_sw.h
+++ b/servers/physics_3d/godot_body_pair_3d.h
@@ -1,5 +1,5 @@
 /*************************************************************************/
-/*  body_pair_3d_sw.h                                                    */
+/*  godot_body_pair_3d.h                                                 */
 /*************************************************************************/
 /*                       This file is part of:                           */
 /*                           GODOT ENGINE                                */
@@ -28,56 +28,57 @@
 /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
 /*************************************************************************/
 
-#ifndef BODY_PAIR_3D_SW_H
-#define BODY_PAIR_3D_SW_H
+#ifndef GODOT_BODY_PAIR_3D_H
+#define GODOT_BODY_PAIR_3D_H
+
+#include "godot_body_3d.h"
+#include "godot_constraint_3d.h"
+#include "godot_soft_body_3d.h"
 
-#include "body_3d_sw.h"
-#include "constraint_3d_sw.h"
 #include "core/templates/local_vector.h"
-#include "soft_body_3d_sw.h"
 
-class BodyContact3DSW : public Constraint3DSW {
+class GodotBodyContact3D : public GodotConstraint3D {
 protected:
 	struct Contact {
 		Vector3 position;
 		Vector3 normal;
-		int index_A, index_B;
+		int index_A = 0, index_B = 0;
 		Vector3 local_A, local_B;
-		real_t acc_normal_impulse; // accumulated normal impulse (Pn)
+		real_t acc_normal_impulse = 0.0; // accumulated normal impulse (Pn)
 		Vector3 acc_tangent_impulse; // accumulated tangent impulse (Pt)
-		real_t acc_bias_impulse; // accumulated normal impulse for position bias (Pnb)
-		real_t acc_bias_impulse_center_of_mass; // accumulated normal impulse for position bias applied to com
-		real_t mass_normal;
-		real_t bias;
-		real_t bounce;
-
-		real_t depth;
-		bool active;
+		real_t acc_bias_impulse = 0.0; // accumulated normal impulse for position bias (Pnb)
+		real_t acc_bias_impulse_center_of_mass = 0.0; // accumulated normal impulse for position bias applied to com
+		real_t mass_normal = 0.0;
+		real_t bias = 0.0;
+		real_t bounce = 0.0;
+
+		real_t depth = 0.0;
+		bool active = false;
 		Vector3 rA, rB; // Offset in world orientation with respect to center of mass
 	};
 
 	Vector3 sep_axis;
 	bool collided = false;
 
-	Space3DSW *space = nullptr;
+	GodotSpace3D *space = nullptr;
 
-	BodyContact3DSW(Body3DSW **p_body_ptr = nullptr, int p_body_count = 0) :
-			Constraint3DSW(p_body_ptr, p_body_count) {
+	GodotBodyContact3D(GodotBody3D **p_body_ptr = nullptr, int p_body_count = 0) :
+			GodotConstraint3D(p_body_ptr, p_body_count) {
 	}
 };
 
-class BodyPair3DSW : public BodyContact3DSW {
+class GodotBodyPair3D : public GodotBodyContact3D {
 	enum {
 		MAX_CONTACTS = 4
 	};
 
 	union {
 		struct {
-			Body3DSW *A;
-			Body3DSW *B;
+			GodotBody3D *A;
+			GodotBody3D *B;
 		};
 
-		Body3DSW *_arr[2] = { nullptr, nullptr };
+		GodotBody3D *_arr[2] = { nullptr, nullptr };
 	};
 
 	int shape_A = 0;
@@ -98,20 +99,20 @@ class BodyPair3DSW : public BodyContact3DSW {
 	void contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B);
 
 	void validate_contacts();
-	bool _test_ccd(real_t p_step, Body3DSW *p_A, int p_shape_A, const Transform3D &p_xform_A, Body3DSW *p_B, int p_shape_B, const Transform3D &p_xform_B);
+	bool _test_ccd(real_t p_step, GodotBody3D *p_A, int p_shape_A, const Transform3D &p_xform_A, GodotBody3D *p_B, int p_shape_B, const Transform3D &p_xform_B);
 
 public:
 	virtual bool setup(real_t p_step) override;
 	virtual bool pre_solve(real_t p_step) override;
 	virtual void solve(real_t p_step) override;
 
-	BodyPair3DSW(Body3DSW *p_A, int p_shape_A, Body3DSW *p_B, int p_shape_B);
-	~BodyPair3DSW();
+	GodotBodyPair3D(GodotBody3D *p_A, int p_shape_A, GodotBody3D *p_B, int p_shape_B);
+	~GodotBodyPair3D();
 };
 
-class BodySoftBodyPair3DSW : public BodyContact3DSW {
-	Body3DSW *body = nullptr;
-	SoftBody3DSW *soft_body = nullptr;
+class GodotBodySoftBodyPair3D : public GodotBodyContact3D {
+	GodotBody3D *body = nullptr;
+	GodotSoftBody3D *soft_body = nullptr;
 
 	int body_shape = 0;
 
@@ -133,11 +134,11 @@ public:
 	virtual bool pre_solve(real_t p_step) override;
 	virtual void solve(real_t p_step) override;
 
-	virtual SoftBody3DSW *get_soft_body_ptr(int p_index) const override { return soft_body; }
+	virtual GodotSoftBody3D *get_soft_body_ptr(int p_index) const override { return soft_body; }
 	virtual int get_soft_body_count() const override { return 1; }
 
-	BodySoftBodyPair3DSW(Body3DSW *p_A, int p_shape_A, SoftBody3DSW *p_B);
-	~BodySoftBodyPair3DSW();
+	GodotBodySoftBodyPair3D(GodotBody3D *p_A, int p_shape_A, GodotSoftBody3D *p_B);
+	~GodotBodySoftBodyPair3D();
 };
 
-#endif // BODY_PAIR_3D_SW_H
+#endif // GODOT_BODY_PAIR_3D_H
diff --git a/servers/physics_3d/broad_phase_3d_sw.cpp b/servers/physics_3d/godot_broad_phase_3d.cpp
index 8aa64034ec..db51dfb2b6 100644
--- a/servers/physics_3d/broad_phase_3d_sw.cpp
+++ b/servers/physics_3d/godot_broad_phase_3d.cpp
@@ -1,5 +1,5 @@
 /*************************************************************************/
-/*  broad_phase_3d_sw.cpp                                                */
+/*  godot_broad_phase_3d.cpp                                             */
 /*************************************************************************/
 /*                       This file is part of:                           */
 /*                           GODOT ENGINE                                */
@@ -28,9 +28,9 @@
 /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
 /*************************************************************************/
 
-#include "broad_phase_3d_sw.h"
+#include "godot_broad_phase_3d.h"
 
-BroadPhase3DSW::CreateFunction BroadPhase3DSW::create_func = nullptr;
+GodotBroadPhase3D::CreateFunction GodotBroadPhase3D::create_func = nullptr;
 
-BroadPhase3DSW::~BroadPhase3DSW() {
+GodotBroadPhase3D::~GodotBroadPhase3D() {
 }
diff --git a/servers/physics_3d/broad_phase_3d_sw.h b/servers/physics_3d/godot_broad_phase_3d.h
index 98313cb216..65423f293c 100644
--- a/servers/physics_3d/broad_phase_3d_sw.h
+++ b/servers/physics_3d/godot_broad_phase_3d.h
@@ -1,5 +1,5 @@
 /*************************************************************************/
-/*  broad_phase_3d_sw.h                                                  */
+/*  godot_broad_phase_3d.h                                               */
 /*************************************************************************/
 /*                       This file is part of:                           */
 /*                           GODOT ENGINE                                */
@@ -28,45 +28,45 @@
 /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
 /*************************************************************************/
 
-#ifndef BROAD_PHASE_SW_H
-#define BROAD_PHASE_SW_H
+#ifndef GODOT_BROAD_PHASE_3D_H
+#define GODOT_BROAD_PHASE_3D_H
 
 #include "core/math/aabb.h"
 #include "core/math/math_funcs.h"
 
-class CollisionObject3DSW;
+class GodotCollisionObject3D;
 
-class BroadPhase3DSW {
+class GodotBroadPhase3D {
 public:
-	typedef BroadPhase3DSW *(*CreateFunction)();
+	typedef GodotBroadPhase3D *(*CreateFunction)();
 
 	static CreateFunction create_func;
 
 	typedef uint32_t ID;
 
-	typedef void *(*PairCallback)(CollisionObject3DSW *A, int p_subindex_A, CollisionObject3DSW *B, int p_subindex_B, void *p_userdata);
-	typedef void (*UnpairCallback)(CollisionObject3DSW *A, int p_subindex_A, CollisionObject3DSW *B, int p_subindex_B, void *p_data, void *p_userdata);
+	typedef void *(*PairCallback)(GodotCollisionObject3D *A, int p_subindex_A, GodotCollisionObject3D *B, int p_subindex_B, void *p_userdata);
+	typedef void (*UnpairCallback)(GodotCollisionObject3D *A, int p_subindex_A, GodotCollisionObject3D *B, int p_subindex_B, void *p_data, void *p_userdata);
 
 	// 0 is an invalid ID
-	virtual ID create(CollisionObject3DSW *p_object_, int p_subindex = 0, const AABB &p_aabb = AABB(), bool p_static = false) = 0;
+	virtual ID create(GodotCollisionObject3D *p_object_, int p_subindex = 0, const AABB &p_aabb = AABB(), bool p_static = false) = 0;
 	virtual void move(ID p_id, const AABB &p_aabb) = 0;
 	virtual void set_static(ID p_id, bool p_static) = 0;
 	virtual void remove(ID p_id) = 0;
 
-	virtual CollisionObject3DSW *get_object(ID p_id) const = 0;
+	virtual GodotCollisionObject3D *get_object(ID p_id) const = 0;
 	virtual bool is_static(ID p_id) const = 0;
 	virtual int get_subindex(ID p_id) const = 0;
 
-	virtual int cull_point(const Vector3 &p_point, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices = nullptr) = 0;
-	virtual int cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices = nullptr) = 0;
-	virtual int cull_aabb(const AABB &p_aabb, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices = nullptr) = 0;
+	virtual int cull_point(const Vector3 &p_point, GodotCollisionObject3D **p_results, int p_max_results, int *p_result_indices = nullptr) = 0;
+	virtual int cull_segment(const Vector3 &p_from, const Vector3 &p_to, GodotCollisionObject3D **p_results, int p_max_results, int *p_result_indices = nullptr) = 0;
+	virtual int cull_aabb(const AABB &p_aabb, GodotCollisionObject3D **p_results, int p_max_results, int *p_result_indices = nullptr) = 0;
 
 	virtual void set_pair_callback(PairCallback p_pair_callback, void *p_userdata) = 0;
 	virtual void set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) = 0;
 
 	virtual void update() = 0;
 
-	virtual ~BroadPhase3DSW();
+	virtual ~GodotBroadPhase3D();
 };
 
-#endif // BROAD_PHASE__SW_H
+#endif // GODOT_BROAD_PHASE_3D_H
diff --git a/servers/physics_3d/broad_phase_3d_bvh.cpp b/servers/physics_3d/godot_broad_phase_3d_bvh.cpp
index f9f64f786d..0f2061a1ea 100644
--- a/servers/physics_3d/broad_phase_3d_bvh.cpp
+++ b/servers/physics_3d/godot_broad_phase_3d_bvh.cpp
@@ -1,5 +1,5 @@
 /*************************************************************************/
-/*  broad_phase_3d_bvh.cpp                                               */
+/*  godot_broad_phase_3d_bvh.cpp                                         */
 /*************************************************************************/
 /*                       This file is part of:                           */
 /*                           GODOT ENGINE                                */
@@ -28,55 +28,56 @@
 /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
 /*************************************************************************/
 
-#include "broad_phase_3d_bvh.h"
-#include "collision_object_3d_sw.h"
+#include "godot_broad_phase_3d_bvh.h"
 
-BroadPhase3DBVH::ID BroadPhase3DBVH::create(CollisionObject3DSW *p_object, int p_subindex, const AABB &p_aabb, bool p_static) {
+#include "godot_collision_object_3d.h"
+
+GodotBroadPhase3DBVH::ID GodotBroadPhase3DBVH::create(GodotCollisionObject3D *p_object, int p_subindex, const AABB &p_aabb, bool p_static) {
 	ID oid = bvh.create(p_object, true, p_aabb, p_subindex, !p_static, 1 << p_object->get_type(), p_static ? 0 : 0xFFFFF); // Pair everything, don't care?
 	return oid + 1;
 }
 
-void BroadPhase3DBVH::move(ID p_id, const AABB &p_aabb) {
+void GodotBroadPhase3DBVH::move(ID p_id, const AABB &p_aabb) {
 	bvh.move(p_id - 1, p_aabb);
 }
 
-void BroadPhase3DBVH::set_static(ID p_id, bool p_static) {
-	CollisionObject3DSW *it = bvh.get(p_id - 1);
+void GodotBroadPhase3DBVH::set_static(ID p_id, bool p_static) {
+	GodotCollisionObject3D *it = bvh.get(p_id - 1);
 	bvh.set_pairable(p_id - 1, !p_static, 1 << it->get_type(), p_static ? 0 : 0xFFFFF, false); // Pair everything, don't care?
 }
 
-void BroadPhase3DBVH::remove(ID p_id) {
+void GodotBroadPhase3DBVH::remove(ID p_id) {
 	bvh.erase(p_id - 1);
 }
 
-CollisionObject3DSW *BroadPhase3DBVH::get_object(ID p_id) const {
-	CollisionObject3DSW *it = bvh.get(p_id - 1);
+GodotCollisionObject3D *GodotBroadPhase3DBVH::get_object(ID p_id) const {
+	GodotCollisionObject3D *it = bvh.get(p_id - 1);
 	ERR_FAIL_COND_V(!it, nullptr);
 	return it;
 }
 
-bool BroadPhase3DBVH::is_static(ID p_id) const {
+bool GodotBroadPhase3DBVH::is_static(ID p_id) const {
 	return !bvh.is_pairable(p_id - 1);
 }
 
-int BroadPhase3DBVH::get_subindex(ID p_id) const {
+int GodotBroadPhase3DBVH::get_subindex(ID p_id) const {
 	return bvh.get_subindex(p_id - 1);
 }
 
-int BroadPhase3DBVH::cull_point(const Vector3 &p_point, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices) {
+int GodotBroadPhase3DBVH::cull_point(const Vector3 &p_point, GodotCollisionObject3D **p_results, int p_max_results, int *p_result_indices) {
 	return bvh.cull_point(p_point, p_results, p_max_results, p_result_indices);
 }
 
-int BroadPhase3DBVH::cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices) {
+int GodotBroadPhase3DBVH::cull_segment(const Vector3 &p_from, const Vector3 &p_to, GodotCollisionObject3D **p_results, int p_max_results, int *p_result_indices) {
 	return bvh.cull_segment(p_from, p_to, p_results, p_max_results, p_result_indices);
 }
 
-int BroadPhase3DBVH::cull_aabb(const AABB &p_aabb, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices) {
+int GodotBroadPhase3DBVH::cull_aabb(const AABB &p_aabb, GodotCollisionObject3D **p_results, int p_max_results, int *p_result_indices) {
 	return bvh.cull_aabb(p_aabb, p_results, p_max_results, p_result_indices);
 }
 
-void *BroadPhase3DBVH::_pair_callback(void *self, uint32_t p_A, CollisionObject3DSW *p_object_A, int subindex_A, uint32_t p_B, CollisionObject3DSW *p_object_B, int subindex_B) {
-	BroadPhase3DBVH *bpo = (BroadPhase3DBVH *)(self);
+void *GodotBroadPhase3DBVH::_pair_callback(void *self, uint32_t p_A, GodotCollisionObject3D *p_object_A, int subindex_A, uint32_t p_B, GodotCollisionObject3D *p_object_B, int subindex_B) {
+	GodotBroadPhase3DBVH *bpo = (GodotBroadPhase3DBVH *)(self);
 	if (!bpo->pair_callback) {
 		return nullptr;
 	}
@@ -84,8 +85,8 @@ void *BroadPhase3DBVH::_pair_callback(void *self, uint32_t p_A, CollisionObject3
 	return bpo->pair_callback(p_object_A, subindex_A, p_object_B, subindex_B, bpo->pair_userdata);
 }
 
-void BroadPhase3DBVH::_unpair_callback(void *self, uint32_t p_A, CollisionObject3DSW *p_object_A, int subindex_A, uint32_t p_B, CollisionObject3DSW *p_object_B, int subindex_B, void *pairdata) {
-	BroadPhase3DBVH *bpo = (BroadPhase3DBVH *)(self);
+void GodotBroadPhase3DBVH::_unpair_callback(void *self, uint32_t p_A, GodotCollisionObject3D *p_object_A, int subindex_A, uint32_t p_B, GodotCollisionObject3D *p_object_B, int subindex_B, void *pairdata) {
+	GodotBroadPhase3DBVH *bpo = (GodotBroadPhase3DBVH *)(self);
 	if (!bpo->unpair_callback) {
 		return;
 	}
@@ -93,28 +94,25 @@ void BroadPhase3DBVH::_unpair_callback(void *self, uint32_t p_A, CollisionObject
 	bpo->unpair_callback(p_object_A, subindex_A, p_object_B, subindex_B, pairdata, bpo->unpair_userdata);
 }
 
-void BroadPhase3DBVH::set_pair_callback(PairCallback p_pair_callback, void *p_userdata) {
+void GodotBroadPhase3DBVH::set_pair_callback(PairCallback p_pair_callback, void *p_userdata) {
 	pair_callback = p_pair_callback;
 	pair_userdata = p_userdata;
 }
 
-void BroadPhase3DBVH::set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) {
+void GodotBroadPhase3DBVH::set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) {
 	unpair_callback = p_unpair_callback;
 	unpair_userdata = p_userdata;
 }
 
-void BroadPhase3DBVH::update() {
+void GodotBroadPhase3DBVH::update() {
 	bvh.update();
 }
 
-BroadPhase3DSW *BroadPhase3DBVH::_create() {
-	return memnew(BroadPhase3DBVH);
+GodotBroadPhase3D *GodotBroadPhase3DBVH::_create() {
+	return memnew(GodotBroadPhase3DBVH);
 }
 
-BroadPhase3DBVH::BroadPhase3DBVH() {
+GodotBroadPhase3DBVH::GodotBroadPhase3DBVH() {
 	bvh.set_pair_callback(_pair_callback, this);
 	bvh.set_unpair_callback(_unpair_callback, this);
-	pair_callback = nullptr;
-	pair_userdata = nullptr;
-	unpair_userdata = nullptr;
 }
diff --git a/servers/physics_3d/broad_phase_3d_bvh.h b/servers/physics_3d/godot_broad_phase_3d_bvh.h
index 30b8b7f2aa..61127e52c1 100644
--- a/servers/physics_3d/broad_phase_3d_bvh.h
+++ b/servers/physics_3d/godot_broad_phase_3d_bvh.h
@@ -1,5 +1,5 @@
 /*************************************************************************/
-/*  broad_phase_3d_bvh.h                                                 */
+/*  godot_broad_phase_3d_bvh.h                                           */
 /*************************************************************************/
 /*                       This file is part of:                           */
 /*                           GODOT ENGINE                                */
@@ -28,45 +28,46 @@
 /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
 /*************************************************************************/
 
-#ifndef BROAD_PHASE_3D_BVH_H
-#define BROAD_PHASE_3D_BVH_H
+#ifndef GODOT_BROAD_PHASE_3D_BVH_H
+#define GODOT_BROAD_PHASE_3D_BVH_H
+
+#include "godot_broad_phase_3d.h"
 
-#include "broad_phase_3d_sw.h"
 #include "core/math/bvh.h"
 
-class BroadPhase3DBVH : public BroadPhase3DSW {
-	BVH_Manager<CollisionObject3DSW, true, 128> bvh;
+class GodotBroadPhase3DBVH : public GodotBroadPhase3D {
+	BVH_Manager<GodotCollisionObject3D, true, 128> bvh;
 
-	static void *_pair_callback(void *, uint32_t, CollisionObject3DSW *, int, uint32_t, CollisionObject3DSW *, int);
-	static void _unpair_callback(void *, uint32_t, CollisionObject3DSW *, int, uint32_t, CollisionObject3DSW *, int, void *);
+	static void *_pair_callback(void *, uint32_t, GodotCollisionObject3D *, int, uint32_t, GodotCollisionObject3D *, int);
+	static void _unpair_callback(void *, uint32_t, GodotCollisionObject3D *, int, uint32_t, GodotCollisionObject3D *, int, void *);
 
-	PairCallback pair_callback;
-	void *pair_userdata;
-	UnpairCallback unpair_callback;
-	void *unpair_userdata;
+	PairCallback pair_callback = nullptr;
+	void *pair_userdata = nullptr;
+	UnpairCallback unpair_callback = nullptr;
+	void *unpair_userdata = nullptr;
 
 public:
 	// 0 is an invalid ID
-	virtual ID create(CollisionObject3DSW *p_object, int p_subindex = 0, const AABB &p_aabb = AABB(), bool p_static = false);
+	virtual ID create(GodotCollisionObject3D *p_object, int p_subindex = 0, const AABB &p_aabb = AABB(), bool p_static = false);
 	virtual void move(ID p_id, const AABB &p_aabb);
 	virtual void set_static(ID p_id, bool p_static);
 	virtual void remove(ID p_id);
 
-	virtual CollisionObject3DSW *get_object(ID p_id) const;
+	virtual GodotCollisionObject3D *get_object(ID p_id) const;
 	virtual bool is_static(ID p_id) const;
 	virtual int get_subindex(ID p_id) const;
 
-	virtual int cull_point(const Vector3 &p_point, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices = nullptr);
-	virtual int cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices = nullptr);
-	virtual int cull_aabb(const AABB &p_aabb, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices = nullptr);
+	virtual int cull_point(const Vector3 &p_point, GodotCollisionObject3D **p_results, int p_max_results, int *p_result_indices = nullptr);
+	virtual int cull_segment(const Vector3 &p_from, const Vector3 &p_to, GodotCollisionObject3D **p_results, int p_max_results, int *p_result_indices = nullptr);
+	virtual int cull_aabb(const AABB &p_aabb, GodotCollisionObject3D **p_results, int p_max_results, int *p_result_indices = nullptr);
 
 	virtual void set_pair_callback(PairCallback p_pair_callback, void *p_userdata);
 	virtual void set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata);
 
 	virtual void update();
 
-	static BroadPhase3DSW *_create();
-	BroadPhase3DBVH();
+	static GodotBroadPhase3D *_create();
+	GodotBroadPhase3DBVH();
 };
 
-#endif // BROAD_PHASE_3D_BVH_H
+#endif // GODOT_BROAD_PHASE_3D_BVH_H
diff --git a/servers/physics_3d/collision_object_3d_sw.cpp b/servers/physics_3d/godot_collision_object_3d.cpp
index 24c7d7b85c..421291011b 100644
--- a/servers/physics_3d/collision_object_3d_sw.cpp
+++ b/servers/physics_3d/godot_collision_object_3d.cpp
@@ -1,5 +1,5 @@
 /*************************************************************************/
-/*  collision_object_3d_sw.cpp                                           */
+/*  godot_collision_object_3d.cpp                                        */
 /*************************************************************************/
 /*                       This file is part of:                           */
 /*                           GODOT ENGINE                                */
@@ -28,11 +28,12 @@
 /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
 /*************************************************************************/
 
-#include "collision_object_3d_sw.h"
-#include "servers/physics_3d/physics_server_3d_sw.h"
-#include "space_3d_sw.h"
+#include "godot_collision_object_3d.h"
 
-void CollisionObject3DSW::add_shape(Shape3DSW *p_shape, const Transform3D &p_transform, bool p_disabled) {
+#include "godot_physics_server_3d.h"
+#include "godot_space_3d.h"
+
+void GodotCollisionObject3D::add_shape(GodotShape3D *p_shape, const Transform3D &p_transform, bool p_disabled) {
 	Shape s;
 	s.shape = p_shape;
 	s.xform = p_transform;
@@ -43,35 +44,35 @@ void CollisionObject3DSW::add_shape(Shape3DSW *p_shape, const Transform3D &p_tra
 	p_shape->add_owner(this);
 
 	if (!pending_shape_update_list.in_list()) {
-		PhysicsServer3DSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list);
+		GodotPhysicsServer3D::godot_singleton->pending_shape_update_list.add(&pending_shape_update_list);
 	}
 }
 
-void CollisionObject3DSW::set_shape(int p_index, Shape3DSW *p_shape) {
+void GodotCollisionObject3D::set_shape(int p_index, GodotShape3D *p_shape) {
 	ERR_FAIL_INDEX(p_index, shapes.size());
 	shapes[p_index].shape->remove_owner(this);
 	shapes.write[p_index].shape = p_shape;
 
 	p_shape->add_owner(this);
 	if (!pending_shape_update_list.in_list()) {
-		PhysicsServer3DSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list);
+		GodotPhysicsServer3D::godot_singleton->pending_shape_update_list.add(&pending_shape_update_list);
 	}
 }
 
-void CollisionObject3DSW::set_shape_transform(int p_index, const Transform3D &p_transform) {
+void GodotCollisionObject3D::set_shape_transform(int p_index, const Transform3D &p_transform) {
 	ERR_FAIL_INDEX(p_index, shapes.size());
 
 	shapes.write[p_index].xform = p_transform;
 	shapes.write[p_index].xform_inv = p_transform.affine_inverse();
 	if (!pending_shape_update_list.in_list()) {
-		PhysicsServer3DSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list);
+		GodotPhysicsServer3D::godot_singleton->pending_shape_update_list.add(&pending_shape_update_list);
 	}
 }
 
-void CollisionObject3DSW::set_shape_disabled(int p_idx, bool p_disabled) {
+void GodotCollisionObject3D::set_shape_disabled(int p_idx, bool p_disabled) {
 	ERR_FAIL_INDEX(p_idx, shapes.size());
 
-	CollisionObject3DSW::Shape &shape = shapes.write[p_idx];
+	GodotCollisionObject3D::Shape &shape = shapes.write[p_idx];
 	if (shape.disabled == p_disabled) {
 		return;
 	}
@@ -86,16 +87,16 @@ void CollisionObject3DSW::set_shape_disabled(int p_idx, bool p_disabled) {
 		space->get_broadphase()->remove(shape.bpid);
 		shape.bpid = 0;
 		if (!pending_shape_update_list.in_list()) {
-			PhysicsServer3DSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list);
+			GodotPhysicsServer3D::godot_singleton->pending_shape_update_list.add(&pending_shape_update_list);
 		}
 	} else if (!p_disabled && shape.bpid == 0) {
 		if (!pending_shape_update_list.in_list()) {
-			PhysicsServer3DSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list);
+			GodotPhysicsServer3D::godot_singleton->pending_shape_update_list.add(&pending_shape_update_list);
 		}
 	}
 }
 
-void CollisionObject3DSW::remove_shape(Shape3DSW *p_shape) {
+void GodotCollisionObject3D::remove_shape(GodotShape3D *p_shape) {
 	//remove a shape, all the times it appears
 	for (int i = 0; i < shapes.size(); i++) {
 		if (shapes[i].shape == p_shape) {
@@ -105,7 +106,7 @@ void CollisionObject3DSW::remove_shape(Shape3DSW *p_shape) {
 	}
 }
 
-void CollisionObject3DSW::remove_shape(int p_index) {
+void GodotCollisionObject3D::remove_shape(int p_index) {
 	//remove anything from shape to be erased to end, so subindices don't change
 	ERR_FAIL_INDEX(p_index, shapes.size());
 	for (int i = p_index; i < shapes.size(); i++) {
@@ -117,14 +118,14 @@ void CollisionObject3DSW::remove_shape(int p_index) {
 		shapes.write[i].bpid = 0;
 	}
 	shapes[p_index].shape->remove_owner(this);
-	shapes.remove(p_index);
+	shapes.remove_at(p_index);
 
 	if (!pending_shape_update_list.in_list()) {
-		PhysicsServer3DSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list);
+		GodotPhysicsServer3D::godot_singleton->pending_shape_update_list.add(&pending_shape_update_list);
 	}
 }
 
-void CollisionObject3DSW::_set_static(bool p_static) {
+void GodotCollisionObject3D::_set_static(bool p_static) {
 	if (_static == p_static) {
 		return;
 	}
@@ -141,7 +142,7 @@ void CollisionObject3DSW::_set_static(bool p_static) {
 	}
 }
 
-void CollisionObject3DSW::_unregister_shapes() {
+void GodotCollisionObject3D::_unregister_shapes() {
 	for (int i = 0; i < shapes.size(); i++) {
 		Shape &s = shapes.write[i];
 		if (s.bpid > 0) {
@@ -151,7 +152,7 @@ void CollisionObject3DSW::_unregister_shapes() {
 	}
 }
 
-void CollisionObject3DSW::_update_shapes() {
+void GodotCollisionObject3D::_update_shapes() {
 	if (!space) {
 		return;
 	}
@@ -170,7 +171,7 @@ void CollisionObject3DSW::_update_shapes() {
 		s.aabb_cache = shape_aabb;
 
 		Vector3 scale = xform.get_basis().get_scale();
-		s.area_cache = s.shape->get_area() * scale.x * scale.y * scale.z;
+		s.area_cache = s.shape->get_volume() * scale.x * scale.y * scale.z;
 
 		if (s.bpid == 0) {
 			s.bpid = space->get_broadphase()->create(this, i, shape_aabb, _static);
@@ -181,7 +182,7 @@ void CollisionObject3DSW::_update_shapes() {
 	}
 }
 
-void CollisionObject3DSW::_update_shapes_with_motion(const Vector3 &p_motion) {
+void GodotCollisionObject3D::_update_shapes_with_motion(const Vector3 &p_motion) {
 	if (!space) {
 		return;
 	}
@@ -208,7 +209,7 @@ void CollisionObject3DSW::_update_shapes_with_motion(const Vector3 &p_motion) {
 	}
 }
 
-void CollisionObject3DSW::_set_space(Space3DSW *p_space) {
+void GodotCollisionObject3D::_set_space(GodotSpace3D *p_space) {
 	if (space) {
 		space->remove_object(this);
 
@@ -229,18 +230,12 @@ void CollisionObject3DSW::_set_space(Space3DSW *p_space) {
 	}
 }
 
-void CollisionObject3DSW::_shape_changed() {
+void GodotCollisionObject3D::_shape_changed() {
 	_update_shapes();
 	_shapes_changed();
 }
 
-CollisionObject3DSW::CollisionObject3DSW(Type p_type) :
+GodotCollisionObject3D::GodotCollisionObject3D(Type p_type) :
 		pending_shape_update_list(this) {
-	_static = true;
 	type = p_type;
-	space = nullptr;
-
-	collision_layer = 1;
-	collision_mask = 1;
-	ray_pickable = true;
 }
diff --git a/servers/physics_3d/collision_object_3d_sw.h b/servers/physics_3d/godot_collision_object_3d.h
index 6ffab54645..43558034e0 100644
--- a/servers/physics_3d/collision_object_3d_sw.h
+++ b/servers/physics_3d/godot_collision_object_3d.h
@@ -1,5 +1,5 @@
 /*************************************************************************/
-/*  collision_object_3d_sw.h                                             */
+/*  godot_collision_object_3d.h                                          */
 /*************************************************************************/
 /*                       This file is part of:                           */
 /*                           GODOT ENGINE                                */
@@ -28,13 +28,14 @@
 /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
 /*************************************************************************/
 
-#ifndef COLLISION_OBJECT_SW_H
-#define COLLISION_OBJECT_SW_H
+#ifndef GODOT_COLLISION_OBJECT_3D_H
+#define GODOT_COLLISION_OBJECT_3D_H
+
+#include "godot_broad_phase_3d.h"
+#include "godot_shape_3d.h"
 
-#include "broad_phase_3d_sw.h"
 #include "core/templates/self_list.h"
 #include "servers/physics_server_3d.h"
-#include "shape_3d_sw.h"
 
 #ifdef DEBUG_ENABLED
 #define MAX_OBJECT_DISTANCE 3.1622776601683791e+18
@@ -42,9 +43,9 @@
 #define MAX_OBJECT_DISTANCE_X2 (MAX_OBJECT_DISTANCE * MAX_OBJECT_DISTANCE)
 #endif
 
-class Space3DSW;
+class GodotSpace3D;
 
-class CollisionObject3DSW : public ShapeOwner3DSW {
+class GodotCollisionObject3D : public GodotShapeOwner3D {
 public:
 	enum Type {
 		TYPE_AREA,
@@ -56,28 +57,26 @@ private:
 	Type type;
 	RID self;
 	ObjectID instance_id;
-	uint32_t collision_layer;
-	uint32_t collision_mask;
+	uint32_t collision_layer = 1;
+	uint32_t collision_mask = 1;
 
 	struct Shape {
 		Transform3D xform;
 		Transform3D xform_inv;
-		BroadPhase3DSW::ID bpid;
+		GodotBroadPhase3D::ID bpid;
 		AABB aabb_cache; //for rayqueries
-		real_t area_cache;
-		Shape3DSW *shape;
-		bool disabled;
-
-		Shape() { disabled = false; }
+		real_t area_cache = 0.0;
+		GodotShape3D *shape = nullptr;
+		bool disabled = false;
 	};
 
 	Vector<Shape> shapes;
-	Space3DSW *space;
+	GodotSpace3D *space = nullptr;
 	Transform3D transform;
 	Transform3D inv_transform;
-	bool _static;
+	bool _static = true;
 
-	SelfList<CollisionObject3DSW> pending_shape_update_list;
+	SelfList<GodotCollisionObject3D> pending_shape_update_list;
 
 	void _update_shapes();
 
@@ -100,11 +99,11 @@ protected:
 	void _set_static(bool p_static);
 
 	virtual void _shapes_changed() = 0;
-	void _set_space(Space3DSW *p_space);
+	void _set_space(GodotSpace3D *p_space);
 
-	bool ray_pickable;
+	bool ray_pickable = true;
 
-	CollisionObject3DSW(Type p_type);
+	GodotCollisionObject3D(Type p_type);
 
 public:
 	_FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; }
@@ -116,11 +115,11 @@ public:
 	void _shape_changed();
 
 	_FORCE_INLINE_ Type get_type() const { return type; }
-	void add_shape(Shape3DSW *p_shape, const Transform3D &p_transform = Transform3D(), bool p_disabled = false);
-	void set_shape(int p_index, Shape3DSW *p_shape);
+	void add_shape(GodotShape3D *p_shape, const Transform3D &p_transform = Transform3D(), bool p_disabled = false);
+	void set_shape(int p_index, GodotShape3D *p_shape);
 	void set_shape_transform(int p_index, const Transform3D &p_transform);
 	_FORCE_INLINE_ int get_shape_count() const { return shapes.size(); }
-	_FORCE_INLINE_ Shape3DSW *get_shape(int p_index) const {
+	_FORCE_INLINE_ GodotShape3D *get_shape(int p_index) const {
 		CRASH_BAD_INDEX(p_index, shapes.size());
 		return shapes[p_index].shape;
 	}
@@ -143,7 +142,7 @@ public:
 
 	_FORCE_INLINE_ const Transform3D &get_transform() const { return transform; }
 	_FORCE_INLINE_ const Transform3D &get_inv_transform() const { return inv_transform; }
-	_FORCE_INLINE_ Space3DSW *get_space() const { return space; }
+	_FORCE_INLINE_ GodotSpace3D *get_space() const { return space; }
 
 	_FORCE_INLINE_ void set_ray_pickable(bool p_enable) { ray_pickable = p_enable; }
 	_FORCE_INLINE_ bool is_ray_pickable() const { return ray_pickable; }
@@ -166,22 +165,22 @@ public:
 	}
 	_FORCE_INLINE_ uint32_t get_collision_mask() const { return collision_mask; }
 
-	_FORCE_INLINE_ bool collides_with(CollisionObject3DSW *p_other) const {
+	_FORCE_INLINE_ bool collides_with(GodotCollisionObject3D *p_other) const {
 		return p_other->collision_layer & collision_mask;
 	}
 
-	_FORCE_INLINE_ bool interacts_with(CollisionObject3DSW *p_other) const {
+	_FORCE_INLINE_ bool interacts_with(GodotCollisionObject3D *p_other) const {
 		return collision_layer & p_other->collision_mask || p_other->collision_layer & collision_mask;
 	}
 
-	void remove_shape(Shape3DSW *p_shape);
+	void remove_shape(GodotShape3D *p_shape);
 	void remove_shape(int p_index);
 
-	virtual void set_space(Space3DSW *p_space) = 0;
+	virtual void set_space(GodotSpace3D *p_space) = 0;
 
 	_FORCE_INLINE_ bool is_static() const { return _static; }
 
-	virtual ~CollisionObject3DSW() {}
+	virtual ~GodotCollisionObject3D() {}
 };
 
-#endif // COLLISION_OBJECT_SW_H
+#endif // GODOT_COLLISION_OBJECT_3D_H
diff --git a/servers/physics_3d/collision_solver_3d_sw.cpp b/servers/physics_3d/godot_collision_solver_3d.cpp
index 4a4a8164d3..540b16c6e3 100644
--- a/servers/physics_3d/collision_solver_3d_sw.cpp
+++ b/servers/physics_3d/godot_collision_solver_3d.cpp
@@ -1,5 +1,5 @@
 /*************************************************************************/
-/*  collision_solver_3d_sw.cpp                                           */
+/*  godot_collision_solver_3d.cpp                                        */
 /*************************************************************************/
 /*                       This file is part of:                           */
 /*                           GODOT ENGINE                                */
@@ -28,29 +28,29 @@
 /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
 /*************************************************************************/
 
-#include "collision_solver_3d_sw.h"
-#include "collision_solver_3d_sat.h"
-#include "soft_body_3d_sw.h"
+#include "godot_collision_solver_3d.h"
+#include "godot_collision_solver_3d_sat.h"
+#include "godot_soft_body_3d.h"
 
 #include "gjk_epa.h"
 
 #define collision_solver sat_calculate_penetration
 //#define collision_solver gjk_epa_calculate_penetration
 
-bool CollisionSolver3DSW::solve_static_plane(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) {
-	const PlaneShape3DSW *plane = static_cast<const PlaneShape3DSW *>(p_shape_A);
-	if (p_shape_B->get_type() == PhysicsServer3D::SHAPE_PLANE) {
+bool GodotCollisionSolver3D::solve_static_world_boundary(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) {
+	const GodotWorldBoundaryShape3D *world_boundary = static_cast<const GodotWorldBoundaryShape3D *>(p_shape_A);
+	if (p_shape_B->get_type() == PhysicsServer3D::SHAPE_WORLD_BOUNDARY) {
 		return false;
 	}
-	Plane p = p_transform_A.xform(plane->get_plane());
+	Plane p = p_transform_A.xform(world_boundary->get_plane());
 
 	static const int max_supports = 16;
 	Vector3 supports[max_supports];
 	int support_count;
-	Shape3DSW::FeatureType support_type;
+	GodotShape3D::FeatureType support_type;
 	p_shape_B->get_supports(p_transform_B.basis.xform_inv(-p.normal).normalized(), max_supports, supports, support_count, support_type);
 
-	if (support_type == Shape3DSW::FEATURE_CIRCLE) {
+	if (support_type == GodotShape3D::FEATURE_CIRCLE) {
 		ERR_FAIL_COND_V(support_count != 3, false);
 
 		Vector3 circle_pos = supports[0];
@@ -89,8 +89,8 @@ bool CollisionSolver3DSW::solve_static_plane(const Shape3DSW *p_shape_A, const T
 	return found;
 }
 
-bool CollisionSolver3DSW::solve_separation_ray(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin) {
-	const SeparationRayShape3DSW *ray = static_cast<const SeparationRayShape3DSW *>(p_shape_A);
+bool GodotCollisionSolver3D::solve_separation_ray(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin) {
+	const GodotSeparationRayShape3D *ray = static_cast<const GodotSeparationRayShape3D *>(p_shape_A);
 
 	Vector3 from = p_transform_A.origin;
 	Vector3 to = from + p_transform_A.basis.get_axis(2) * (ray->get_length() + p_margin);
@@ -102,7 +102,7 @@ bool CollisionSolver3DSW::solve_separation_ray(const Shape3DSW *p_shape_A, const
 	to = ai.xform(to);
 
 	Vector3 p, n;
-	if (!p_shape_B->intersect_segment(from, to, p, n)) {
+	if (!p_shape_B->intersect_segment(from, to, p, n, true)) {
 		return false;
 	}
 
@@ -134,13 +134,13 @@ bool CollisionSolver3DSW::solve_separation_ray(const Shape3DSW *p_shape_A, const
 
 struct _SoftBodyContactCollisionInfo {
 	int node_index = 0;
-	CollisionSolver3DSW::CallbackResult result_callback = nullptr;
+	GodotCollisionSolver3D::CallbackResult result_callback = nullptr;
 	void *userdata = nullptr;
 	bool swap_result = false;
 	int contact_count = 0;
 };
 
-void CollisionSolver3DSW::soft_body_contact_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata) {
+void GodotCollisionSolver3D::soft_body_contact_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata) {
 	_SoftBodyContactCollisionInfo &cinfo = *(_SoftBodyContactCollisionInfo *)(p_userdata);
 
 	++cinfo.contact_count;
@@ -157,9 +157,9 @@ void CollisionSolver3DSW::soft_body_contact_callback(const Vector3 &p_point_A, i
 }
 
 struct _SoftBodyQueryInfo {
-	SoftBody3DSW *soft_body = nullptr;
-	const Shape3DSW *shape_A = nullptr;
-	const Shape3DSW *shape_B = nullptr;
+	GodotSoftBody3D *soft_body = nullptr;
+	const GodotShape3D *shape_A = nullptr;
+	const GodotShape3D *shape_B = nullptr;
 	Transform3D transform_A;
 	Transform3D node_transform;
 	_SoftBodyContactCollisionInfo contact_info;
@@ -169,7 +169,7 @@ struct _SoftBodyQueryInfo {
 #endif
 };
 
-bool CollisionSolver3DSW::soft_body_query_callback(uint32_t p_node_index, void *p_userdata) {
+bool GodotCollisionSolver3D::soft_body_query_callback(uint32_t p_node_index, void *p_userdata) {
 	_SoftBodyQueryInfo &query_cinfo = *(_SoftBodyQueryInfo *)(p_userdata);
 
 	Vector3 node_position = query_cinfo.soft_body->get_node_position(p_node_index);
@@ -188,7 +188,7 @@ bool CollisionSolver3DSW::soft_body_query_callback(uint32_t p_node_index, void *
 	return (collided && !query_cinfo.contact_info.result_callback);
 }
 
-bool CollisionSolver3DSW::soft_body_concave_callback(void *p_userdata, Shape3DSW *p_convex) {
+bool GodotCollisionSolver3D::soft_body_concave_callback(void *p_userdata, GodotShape3D *p_convex) {
 	_SoftBodyQueryInfo &query_cinfo = *(_SoftBodyQueryInfo *)(p_userdata);
 
 	query_cinfo.shape_A = p_convex;
@@ -220,15 +220,15 @@ bool CollisionSolver3DSW::soft_body_concave_callback(void *p_userdata, Shape3DSW
 	return (collided && !query_cinfo.contact_info.result_callback);
 }
 
-bool CollisionSolver3DSW::solve_soft_body(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) {
-	const SoftBodyShape3DSW *soft_body_shape_B = static_cast<const SoftBodyShape3DSW *>(p_shape_B);
+bool GodotCollisionSolver3D::solve_soft_body(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) {
+	const GodotSoftBodyShape3D *soft_body_shape_B = static_cast<const GodotSoftBodyShape3D *>(p_shape_B);
 
-	SoftBody3DSW *soft_body = soft_body_shape_B->get_soft_body();
+	GodotSoftBody3D *soft_body = soft_body_shape_B->get_soft_body();
 	const Transform3D &world_to_local = soft_body->get_inv_transform();
 
 	const real_t collision_margin = soft_body->get_collision_margin();
 
-	SphereShape3DSW sphere_shape;
+	GodotSphereShape3D sphere_shape;
 	sphere_shape.set_data(collision_margin);
 
 	_SoftBodyQueryInfo query_cinfo;
@@ -243,7 +243,7 @@ bool CollisionSolver3DSW::solve_soft_body(const Shape3DSW *p_shape_A, const Tran
 
 	if (p_shape_A->is_concave()) {
 		// In case of concave shape, query convex shapes first.
-		const ConcaveShape3DSW *concave_shape_A = static_cast<const ConcaveShape3DSW *>(p_shape_A);
+		const GodotConcaveShape3D *concave_shape_A = static_cast<const GodotConcaveShape3D *>(p_shape_A);
 
 		AABB soft_body_aabb = soft_body->get_bounds();
 		soft_body_aabb.grow_by(collision_margin);
@@ -264,7 +264,7 @@ bool CollisionSolver3DSW::solve_soft_body(const Shape3DSW *p_shape_A, const Tran
 			local_aabb.size[i] = smax - smin;
 		}
 
-		concave_shape_A->cull(local_aabb, soft_body_concave_callback, &query_cinfo);
+		concave_shape_A->cull(local_aabb, soft_body_concave_callback, &query_cinfo, true);
 	} else {
 		AABB shape_aabb = p_transform_A.xform(p_shape_A->get_aabb());
 		shape_aabb.grow_by(collision_margin);
@@ -277,9 +277,9 @@ bool CollisionSolver3DSW::solve_soft_body(const Shape3DSW *p_shape_A, const Tran
 
 struct _ConcaveCollisionInfo {
 	const Transform3D *transform_A;
-	const Shape3DSW *shape_A;
+	const GodotShape3D *shape_A;
 	const Transform3D *transform_B;
-	CollisionSolver3DSW::CallbackResult result_callback;
+	GodotCollisionSolver3D::CallbackResult result_callback;
 	void *userdata;
 	bool swap_result;
 	bool collided;
@@ -291,7 +291,7 @@ struct _ConcaveCollisionInfo {
 	Vector3 close_A, close_B;
 };
 
-bool CollisionSolver3DSW::concave_callback(void *p_userdata, Shape3DSW *p_convex) {
+bool GodotCollisionSolver3D::concave_callback(void *p_userdata, GodotShape3D *p_convex) {
 	_ConcaveCollisionInfo &cinfo = *(_ConcaveCollisionInfo *)(p_userdata);
 	cinfo.aabb_tests++;
 
@@ -307,8 +307,8 @@ bool CollisionSolver3DSW::concave_callback(void *p_userdata, Shape3DSW *p_convex
 	return !cinfo.result_callback;
 }
 
-bool CollisionSolver3DSW::solve_concave(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin_A, real_t p_margin_B) {
-	const ConcaveShape3DSW *concave_B = static_cast<const ConcaveShape3DSW *>(p_shape_B);
+bool GodotCollisionSolver3D::solve_concave(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin_A, real_t p_margin_B) {
+	const GodotConcaveShape3D *concave_B = static_cast<const GodotConcaveShape3D *>(p_shape_B);
 
 	_ConcaveCollisionInfo cinfo;
 	cinfo.transform_A = &p_transform_A;
@@ -346,12 +346,12 @@ bool CollisionSolver3DSW::solve_concave(const Shape3DSW *p_shape_A, const Transf
 		local_aabb.size[i] = smax - smin;
 	}
 
-	concave_B->cull(local_aabb, concave_callback, &cinfo);
+	concave_B->cull(local_aabb, concave_callback, &cinfo, false);
 
 	return cinfo.collided;
 }
 
-bool CollisionSolver3DSW::solve_static(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, Vector3 *r_sep_axis, real_t p_margin_A, real_t p_margin_B) {
+bool GodotCollisionSolver3D::solve_static(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, Vector3 *r_sep_axis, real_t p_margin_A, real_t p_margin_B) {
 	PhysicsServer3D::ShapeType type_A = p_shape_A->get_type();
 	PhysicsServer3D::ShapeType type_B = p_shape_B->get_type();
 	bool concave_A = p_shape_A->is_concave();
@@ -365,8 +365,8 @@ bool CollisionSolver3DSW::solve_static(const Shape3DSW *p_shape_A, const Transfo
 		swap = true;
 	}
 
-	if (type_A == PhysicsServer3D::SHAPE_PLANE) {
-		if (type_B == PhysicsServer3D::SHAPE_PLANE) {
+	if (type_A == PhysicsServer3D::SHAPE_WORLD_BOUNDARY) {
+		if (type_B == PhysicsServer3D::SHAPE_WORLD_BOUNDARY) {
 			return false;
 		}
 		if (type_B == PhysicsServer3D::SHAPE_SEPARATION_RAY) {
@@ -377,9 +377,9 @@ bool CollisionSolver3DSW::solve_static(const Shape3DSW *p_shape_A, const Transfo
 		}
 
 		if (swap) {
-			return solve_static_plane(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true);
+			return solve_static_world_boundary(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true);
 		} else {
-			return solve_static_plane(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false);
+			return solve_static_world_boundary(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false);
 		}
 
 	} else if (type_A == PhysicsServer3D::SHAPE_SEPARATION_RAY) {
@@ -421,7 +421,7 @@ bool CollisionSolver3DSW::solve_static(const Shape3DSW *p_shape_A, const Transfo
 	}
 }
 
-bool CollisionSolver3DSW::concave_distance_callback(void *p_userdata, Shape3DSW *p_convex) {
+bool GodotCollisionSolver3D::concave_distance_callback(void *p_userdata, GodotShape3D *p_convex) {
 	_ConcaveCollisionInfo &cinfo = *(_ConcaveCollisionInfo *)(p_userdata);
 	cinfo.aabb_tests++;
 
@@ -443,21 +443,21 @@ bool CollisionSolver3DSW::concave_distance_callback(void *p_userdata, Shape3DSW
 	return false;
 }
 
-bool CollisionSolver3DSW::solve_distance_plane(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B) {
-	const PlaneShape3DSW *plane = static_cast<const PlaneShape3DSW *>(p_shape_A);
-	if (p_shape_B->get_type() == PhysicsServer3D::SHAPE_PLANE) {
+bool GodotCollisionSolver3D::solve_distance_world_boundary(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B) {
+	const GodotWorldBoundaryShape3D *world_boundary = static_cast<const GodotWorldBoundaryShape3D *>(p_shape_A);
+	if (p_shape_B->get_type() == PhysicsServer3D::SHAPE_WORLD_BOUNDARY) {
 		return false;
 	}
-	Plane p = p_transform_A.xform(plane->get_plane());
+	Plane p = p_transform_A.xform(world_boundary->get_plane());
 
 	static const int max_supports = 16;
 	Vector3 supports[max_supports];
 	int support_count;
-	Shape3DSW::FeatureType support_type;
+	GodotShape3D::FeatureType support_type;
 
 	p_shape_B->get_supports(p_transform_B.basis.xform_inv(-p.normal).normalized(), max_supports, supports, support_count, support_type);
 
-	if (support_type == Shape3DSW::FEATURE_CIRCLE) {
+	if (support_type == GodotShape3D::FEATURE_CIRCLE) {
 		ERR_FAIL_COND_V(support_count != 3, false);
 
 		Vector3 circle_pos = supports[0];
@@ -495,14 +495,14 @@ bool CollisionSolver3DSW::solve_distance_plane(const Shape3DSW *p_shape_A, const
 	return collided;
 }
 
-bool CollisionSolver3DSW::solve_distance(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B, const AABB &p_concave_hint, Vector3 *r_sep_axis) {
+bool GodotCollisionSolver3D::solve_distance(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B, const AABB &p_concave_hint, Vector3 *r_sep_axis) {
 	if (p_shape_A->is_concave()) {
 		return false;
 	}
 
-	if (p_shape_B->get_type() == PhysicsServer3D::SHAPE_PLANE) {
+	if (p_shape_B->get_type() == PhysicsServer3D::SHAPE_WORLD_BOUNDARY) {
 		Vector3 a, b;
-		bool col = solve_distance_plane(p_shape_B, p_transform_B, p_shape_A, p_transform_A, a, b);
+		bool col = solve_distance_world_boundary(p_shape_B, p_transform_B, p_shape_A, p_transform_A, a, b);
 		r_point_A = b;
 		r_point_B = a;
 		return !col;
@@ -512,7 +512,7 @@ bool CollisionSolver3DSW::solve_distance(const Shape3DSW *p_shape_A, const Trans
 			return false;
 		}
 
-		const ConcaveShape3DSW *concave_B = static_cast<const ConcaveShape3DSW *>(p_shape_B);
+		const GodotConcaveShape3D *concave_B = static_cast<const GodotConcaveShape3D *>(p_shape_B);
 
 		_ConcaveCollisionInfo cinfo;
 		cinfo.transform_A = &p_transform_A;
@@ -547,7 +547,7 @@ bool CollisionSolver3DSW::solve_distance(const Shape3DSW *p_shape_A, const Trans
 			real_t smin, smax;
 
 			if (use_cc_hint) {
-				cc_hint_aabb.project_range_in_plane(Plane(axis, 0), smin, smax);
+				cc_hint_aabb.project_range_in_plane(Plane(axis), smin, smax);
 			} else {
 				p_shape_A->project_range(axis, rel_transform, smin, smax);
 			}
@@ -559,7 +559,7 @@ bool CollisionSolver3DSW::solve_distance(const Shape3DSW *p_shape_A, const Trans
 			local_aabb.size[i] = smax - smin;
 		}
 
-		concave_B->cull(local_aabb, concave_distance_callback, &cinfo);
+		concave_B->cull(local_aabb, concave_distance_callback, &cinfo, false);
 		if (!cinfo.collided) {
 			r_point_A = cinfo.close_A;
 			r_point_B = cinfo.close_B;
diff --git a/servers/physics_3d/collision_solver_3d_sw.h b/servers/physics_3d/godot_collision_solver_3d.h
index c13614ab3e..133635ca7e 100644
--- a/servers/physics_3d/collision_solver_3d_sw.h
+++ b/servers/physics_3d/godot_collision_solver_3d.h
@@ -1,5 +1,5 @@
 /*************************************************************************/
-/*  collision_solver_3d_sw.h                                             */
+/*  godot_collision_solver_3d.h                                          */
 /*************************************************************************/
 /*                       This file is part of:                           */
 /*                           GODOT ENGINE                                */
@@ -28,30 +28,30 @@
 /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
 /*************************************************************************/
 
-#ifndef COLLISION_SOLVER_SW_H
-#define COLLISION_SOLVER_SW_H
+#ifndef GODOT_COLLISION_SOLVER_3D_H
+#define GODOT_COLLISION_SOLVER_3D_H
 
-#include "shape_3d_sw.h"
+#include "godot_shape_3d.h"
 
-class CollisionSolver3DSW {
+class GodotCollisionSolver3D {
 public:
 	typedef void (*CallbackResult)(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata);
 
 private:
 	static bool soft_body_query_callback(uint32_t p_node_index, void *p_userdata);
 	static void soft_body_contact_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata);
-	static bool soft_body_concave_callback(void *p_userdata, Shape3DSW *p_convex);
-	static bool concave_callback(void *p_userdata, Shape3DSW *p_convex);
-	static bool solve_static_plane(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result);
-	static bool solve_separation_ray(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin = 0);
-	static bool solve_soft_body(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result);
-	static bool solve_concave(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin_A = 0, real_t p_margin_B = 0);
-	static bool concave_distance_callback(void *p_userdata, Shape3DSW *p_convex);
-	static bool solve_distance_plane(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B);
+	static bool soft_body_concave_callback(void *p_userdata, GodotShape3D *p_convex);
+	static bool concave_callback(void *p_userdata, GodotShape3D *p_convex);
+	static bool solve_static_world_boundary(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result);
+	static bool solve_separation_ray(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin = 0);
+	static bool solve_soft_body(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result);
+	static bool solve_concave(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin_A = 0, real_t p_margin_B = 0);
+	static bool concave_distance_callback(void *p_userdata, GodotShape3D *p_convex);
+	static bool solve_distance_world_boundary(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B);
 
 public:
-	static bool solve_static(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, Vector3 *r_sep_axis = nullptr, real_t p_margin_A = 0, real_t p_margin_B = 0);
-	static bool solve_distance(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B, const AABB &p_concave_hint, Vector3 *r_sep_axis = nullptr);
+	static bool solve_static(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, Vector3 *r_sep_axis = nullptr, real_t p_margin_A = 0, real_t p_margin_B = 0);
+	static bool solve_distance(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B, const AABB &p_concave_hint, Vector3 *r_sep_axis = nullptr);
 };
 
-#endif // COLLISION_SOLVER__SW_H
+#endif // GODOT_COLLISION_SOLVER_3D_H
diff --git a/servers/physics_3d/collision_solver_3d_sat.cpp b/servers/physics_3d/godot_collision_solver_3d_sat.cpp
index de81348b4e..4faa07b6c9 100644
--- a/servers/physics_3d/collision_solver_3d_sat.cpp
+++ b/servers/physics_3d/godot_collision_solver_3d_sat.cpp
@@ -1,5 +1,5 @@
 /*************************************************************************/
-/*  collision_solver_3d_sat.cpp                                          */
+/*  godot_collision_solver_3d_sat.cpp                                    */
 /*************************************************************************/
 /*                       This file is part of:                           */
 /*                           GODOT ENGINE                                */
@@ -28,13 +28,16 @@
 /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
 /*************************************************************************/
 
-#include "collision_solver_3d_sat.h"
-#include "core/math/geometry_3d.h"
+#include "godot_collision_solver_3d_sat.h"
 
 #include "gjk_epa.h"
 
+#include "core/math/geometry_3d.h"
+
 #define fallback_collision_solver gjk_epa_calculate_penetration
 
+#define _BACKFACE_NORMAL_THRESHOLD -0.0002
+
 // Cylinder SAT analytic methods and face-circle contact points for cylinder-trimesh and cylinder-box collision are based on ODE colliders.
 
 /*
@@ -65,12 +68,12 @@
  *************************************************************************/
 
 struct _CollectorCallback {
-	CollisionSolver3DSW::CallbackResult callback;
-	void *userdata;
-	bool swap;
-	bool collided;
+	GodotCollisionSolver3D::CallbackResult callback;
+	void *userdata = nullptr;
+	bool swap = false;
+	bool collided = false;
 	Vector3 normal;
-	Vector3 *prev_axis;
+	Vector3 *prev_axis = nullptr;
 
 	_FORCE_INLINE_ void call(const Vector3 &p_point_A, const Vector3 &p_point_B) {
 		if (swap) {
@@ -183,7 +186,7 @@ static void _generate_contacts_edge_circle(const Vector3 *p_points_A, int p_poin
 	real_t circle_B_radius = circle_B_line_1.length();
 	Vector3 circle_B_normal = circle_B_line_1.cross(circle_B_line_2).normalized();
 
-	Plane circle_plane(circle_B_pos, circle_B_normal);
+	Plane circle_plane(circle_B_normal, circle_B_pos);
 
 	static const int max_clip = 2;
 	Vector3 contact_points[max_clip];
@@ -299,7 +302,7 @@ static void _generate_contacts_face_face(const Vector3 *p_points_A, int p_point_
 		Vector3 clip_normal = (edge0_B - edge1_B).cross(plane_B.normal).normalized();
 		// make a clip plane
 
-		Plane clip(edge0_B, clip_normal);
+		Plane clip(clip_normal, edge0_B);
 		// avoid double clip if A is edge
 		int dst_idx = 0;
 		bool edge = clipbuf_len == 2;
@@ -385,7 +388,7 @@ static void _generate_contacts_face_circle(const Vector3 *p_points_A, int p_poin
 	// Clip face with circle plane.
 	Vector3 circle_B_normal = circle_B_line_1.cross(circle_B_line_2).normalized();
 
-	Plane circle_plane(circle_B_pos, circle_B_normal);
+	Plane circle_plane(circle_B_normal, circle_B_pos);
 
 	static const int max_clip = 32;
 	Vector3 contact_points[max_clip];
@@ -522,7 +525,7 @@ static void _generate_contacts_circle_circle(const Vector3 *p_points_A, int p_po
 		}
 	}
 
-	Plane circle_B_plane(circle_B_pos, circle_B_normal);
+	Plane circle_B_plane(circle_B_normal, circle_B_pos);
 
 	// Generate contact points.
 	for (int i = 0; i < num_points; i++) {
@@ -539,7 +542,7 @@ static void _generate_contacts_circle_circle(const Vector3 *p_points_A, int p_po
 	}
 }
 
-static void _generate_contacts_from_supports(const Vector3 *p_points_A, int p_point_count_A, Shape3DSW::FeatureType p_feature_type_A, const Vector3 *p_points_B, int p_point_count_B, Shape3DSW::FeatureType p_feature_type_B, _CollectorCallback *p_callback) {
+static void _generate_contacts_from_supports(const Vector3 *p_points_A, int p_point_count_A, GodotShape3D::FeatureType p_feature_type_A, const Vector3 *p_points_B, int p_point_count_B, GodotShape3D::FeatureType p_feature_type_B, _CollectorCallback *p_callback) {
 #ifdef DEBUG_ENABLED
 	ERR_FAIL_COND(p_point_count_A < 1);
 	ERR_FAIL_COND(p_point_count_B < 1);
@@ -606,18 +609,19 @@ static void _generate_contacts_from_supports(const Vector3 *p_points_A, int p_po
 
 template <class ShapeA, class ShapeB, bool withMargin = false>
 class SeparatorAxisTest {
-	const ShapeA *shape_A;
-	const ShapeB *shape_B;
-	const Transform3D *transform_A;
-	const Transform3D *transform_B;
-	real_t best_depth;
-	Vector3 best_axis;
-	_CollectorCallback *callback;
-	real_t margin_A;
-	real_t margin_B;
+	const ShapeA *shape_A = nullptr;
+	const ShapeB *shape_B = nullptr;
+	const Transform3D *transform_A = nullptr;
+	const Transform3D *transform_B = nullptr;
+	real_t best_depth = 1e15;
+	_CollectorCallback *callback = nullptr;
+	real_t margin_A = 0.0;
+	real_t margin_B = 0.0;
 	Vector3 separator_axis;
 
 public:
+	Vector3 best_axis;
+
 	_FORCE_INLINE_ bool test_previous_axis() {
 		if (callback && callback->prev_axis && *callback->prev_axis != Vector3()) {
 			return test_axis(*callback->prev_axis);
@@ -626,7 +630,7 @@ public:
 		}
 	}
 
-	_FORCE_INLINE_ bool test_axis(const Vector3 &p_axis, bool p_directional = false) {
+	_FORCE_INLINE_ bool test_axis(const Vector3 &p_axis) {
 		Vector3 axis = p_axis;
 
 		if (axis.is_equal_approx(Vector3())) {
@@ -660,12 +664,7 @@ public:
 		//use the smallest depth
 
 		if (min_B < 0.0) { // could be +0.0, we don't want it to become -0.0
-			if (p_directional) {
-				min_B = max_B;
-				axis = -axis;
-			} else {
-				min_B = -min_B;
-			}
+			min_B = -min_B;
 		}
 
 		if (max_B < min_B) {
@@ -713,7 +712,7 @@ public:
 
 		Vector3 supports_A[max_supports];
 		int support_count_A;
-		Shape3DSW::FeatureType support_type_A;
+		GodotShape3D::FeatureType support_type_A;
 		shape_A->get_supports(transform_A->basis.xform_inv(-best_axis).normalized(), max_supports, supports_A, support_count_A, support_type_A);
 		for (int i = 0; i < support_count_A; i++) {
 			supports_A[i] = transform_A->xform(supports_A[i]);
@@ -727,7 +726,7 @@ public:
 
 		Vector3 supports_B[max_supports];
 		int support_count_B;
-		Shape3DSW::FeatureType support_type_B;
+		GodotShape3D::FeatureType support_type_B;
 		shape_B->get_supports(transform_B->basis.xform_inv(best_axis).normalized(), max_supports, supports_B, support_count_B, support_type_B);
 		for (int i = 0; i < support_count_B; i++) {
 			supports_B[i] = transform_B->xform(supports_B[i]);
@@ -749,7 +748,6 @@ public:
 	}
 
 	_FORCE_INLINE_ SeparatorAxisTest(const ShapeA *p_shape_A, const Transform3D &p_transform_A, const ShapeB *p_shape_B, const Transform3D &p_transform_B, _CollectorCallback *p_callback, real_t p_margin_A = 0, real_t p_margin_B = 0) {
-		best_depth = 1e15;
 		shape_A = p_shape_A;
 		shape_B = p_shape_B;
 		transform_A = &p_transform_A;
@@ -762,14 +760,14 @@ public:
 
 /****** SAT TESTS *******/
 
-typedef void (*CollisionFunc)(const Shape3DSW *, const Transform3D &, const Shape3DSW *, const Transform3D &, _CollectorCallback *p_callback, real_t, real_t);
+typedef void (*CollisionFunc)(const GodotShape3D *, const Transform3D &, const GodotShape3D *, const Transform3D &, _CollectorCallback *p_callback, real_t, real_t);
 
 template <bool withMargin>
-static void _collision_sphere_sphere(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
-	const SphereShape3DSW *sphere_A = static_cast<const SphereShape3DSW *>(p_a);
-	const SphereShape3DSW *sphere_B = static_cast<const SphereShape3DSW *>(p_b);
+static void _collision_sphere_sphere(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
+	const GodotSphereShape3D *sphere_A = static_cast<const GodotSphereShape3D *>(p_a);
+	const GodotSphereShape3D *sphere_B = static_cast<const GodotSphereShape3D *>(p_b);
 
-	SeparatorAxisTest<SphereShape3DSW, SphereShape3DSW, withMargin> separator(sphere_A, p_transform_a, sphere_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
+	SeparatorAxisTest<GodotSphereShape3D, GodotSphereShape3D, withMargin> separator(sphere_A, p_transform_a, sphere_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
 
 	// previous axis
 
@@ -785,11 +783,11 @@ static void _collision_sphere_sphere(const Shape3DSW *p_a, const Transform3D &p_
 }
 
 template <bool withMargin>
-static void _collision_sphere_box(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
-	const SphereShape3DSW *sphere_A = static_cast<const SphereShape3DSW *>(p_a);
-	const BoxShape3DSW *box_B = static_cast<const BoxShape3DSW *>(p_b);
+static void _collision_sphere_box(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
+	const GodotSphereShape3D *sphere_A = static_cast<const GodotSphereShape3D *>(p_a);
+	const GodotBoxShape3D *box_B = static_cast<const GodotBoxShape3D *>(p_b);
 
-	SeparatorAxisTest<SphereShape3DSW, BoxShape3DSW, withMargin> separator(sphere_A, p_transform_a, box_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
+	SeparatorAxisTest<GodotSphereShape3D, GodotBoxShape3D, withMargin> separator(sphere_A, p_transform_a, box_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
 
 	if (!separator.test_previous_axis()) {
 		return;
@@ -836,11 +834,11 @@ static void _collision_sphere_box(const Shape3DSW *p_a, const Transform3D &p_tra
 }
 
 template <bool withMargin>
-static void _collision_sphere_capsule(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
-	const SphereShape3DSW *sphere_A = static_cast<const SphereShape3DSW *>(p_a);
-	const CapsuleShape3DSW *capsule_B = static_cast<const CapsuleShape3DSW *>(p_b);
+static void _collision_sphere_capsule(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
+	const GodotSphereShape3D *sphere_A = static_cast<const GodotSphereShape3D *>(p_a);
+	const GodotCapsuleShape3D *capsule_B = static_cast<const GodotCapsuleShape3D *>(p_b);
 
-	SeparatorAxisTest<SphereShape3DSW, CapsuleShape3DSW, withMargin> separator(sphere_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
+	SeparatorAxisTest<GodotSphereShape3D, GodotCapsuleShape3D, withMargin> separator(sphere_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
 
 	if (!separator.test_previous_axis()) {
 		return;
@@ -878,11 +876,11 @@ static void _collision_sphere_capsule(const Shape3DSW *p_a, const Transform3D &p
 }
 
 template <bool withMargin>
-static void _collision_sphere_cylinder(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
-	const SphereShape3DSW *sphere_A = static_cast<const SphereShape3DSW *>(p_a);
-	const CylinderShape3DSW *cylinder_B = static_cast<const CylinderShape3DSW *>(p_b);
+static void _collision_sphere_cylinder(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
+	const GodotSphereShape3D *sphere_A = static_cast<const GodotSphereShape3D *>(p_a);
+	const GodotCylinderShape3D *cylinder_B = static_cast<const GodotCylinderShape3D *>(p_b);
 
-	SeparatorAxisTest<SphereShape3DSW, CylinderShape3DSW, withMargin> separator(sphere_A, p_transform_a, cylinder_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
+	SeparatorAxisTest<GodotSphereShape3D, GodotCylinderShape3D, withMargin> separator(sphere_A, p_transform_a, cylinder_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
 
 	if (!separator.test_previous_axis()) {
 		return;
@@ -937,11 +935,11 @@ static void _collision_sphere_cylinder(const Shape3DSW *p_a, const Transform3D &
 }
 
 template <bool withMargin>
-static void _collision_sphere_convex_polygon(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
-	const SphereShape3DSW *sphere_A = static_cast<const SphereShape3DSW *>(p_a);
-	const ConvexPolygonShape3DSW *convex_polygon_B = static_cast<const ConvexPolygonShape3DSW *>(p_b);
+static void _collision_sphere_convex_polygon(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
+	const GodotSphereShape3D *sphere_A = static_cast<const GodotSphereShape3D *>(p_a);
+	const GodotConvexPolygonShape3D *convex_polygon_B = static_cast<const GodotConvexPolygonShape3D *>(p_b);
 
-	SeparatorAxisTest<SphereShape3DSW, ConvexPolygonShape3DSW, withMargin> separator(sphere_A, p_transform_a, convex_polygon_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
+	SeparatorAxisTest<GodotSphereShape3D, GodotConvexPolygonShape3D, withMargin> separator(sphere_A, p_transform_a, convex_polygon_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
 
 	if (!separator.test_previous_axis()) {
 		return;
@@ -1000,11 +998,11 @@ static void _collision_sphere_convex_polygon(const Shape3DSW *p_a, const Transfo
 }
 
 template <bool withMargin>
-static void _collision_sphere_face(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
-	const SphereShape3DSW *sphere_A = static_cast<const SphereShape3DSW *>(p_a);
-	const FaceShape3DSW *face_B = static_cast<const FaceShape3DSW *>(p_b);
+static void _collision_sphere_face(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
+	const GodotSphereShape3D *sphere_A = static_cast<const GodotSphereShape3D *>(p_a);
+	const GodotFaceShape3D *face_B = static_cast<const GodotFaceShape3D *>(p_b);
 
-	SeparatorAxisTest<SphereShape3DSW, FaceShape3DSW, withMargin> separator(sphere_A, p_transform_a, face_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
+	SeparatorAxisTest<GodotSphereShape3D, GodotFaceShape3D, withMargin> separator(sphere_A, p_transform_a, face_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
 
 	Vector3 vertex[3] = {
 		p_transform_b.xform(face_B->vertex[0]),
@@ -1014,7 +1012,7 @@ static void _collision_sphere_face(const Shape3DSW *p_a, const Transform3D &p_tr
 
 	Vector3 normal = (vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized();
 
-	if (!separator.test_axis(normal, !face_B->backface_collision)) {
+	if (!separator.test_axis(normal)) {
 		return;
 	}
 
@@ -1041,15 +1039,26 @@ static void _collision_sphere_face(const Shape3DSW *p_a, const Transform3D &p_tr
 		}
 	}
 
+	if (!face_B->backface_collision) {
+		if (separator.best_axis.dot(normal) < _BACKFACE_NORMAL_THRESHOLD) {
+			if (face_B->invert_backface_collision) {
+				separator.best_axis = separator.best_axis.bounce(normal);
+			} else {
+				// Just ignore backface collision.
+				return;
+			}
+		}
+	}
+
 	separator.generate_contacts();
 }
 
 template <bool withMargin>
-static void _collision_box_box(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
-	const BoxShape3DSW *box_A = static_cast<const BoxShape3DSW *>(p_a);
-	const BoxShape3DSW *box_B = static_cast<const BoxShape3DSW *>(p_b);
+static void _collision_box_box(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
+	const GodotBoxShape3D *box_A = static_cast<const GodotBoxShape3D *>(p_a);
+	const GodotBoxShape3D *box_B = static_cast<const GodotBoxShape3D *>(p_b);
 
-	SeparatorAxisTest<BoxShape3DSW, BoxShape3DSW, withMargin> separator(box_A, p_transform_a, box_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
+	SeparatorAxisTest<GodotBoxShape3D, GodotBoxShape3D, withMargin> separator(box_A, p_transform_a, box_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
 
 	if (!separator.test_previous_axis()) {
 		return;
@@ -1143,11 +1152,11 @@ static void _collision_box_box(const Shape3DSW *p_a, const Transform3D &p_transf
 }
 
 template <bool withMargin>
-static void _collision_box_capsule(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
-	const BoxShape3DSW *box_A = static_cast<const BoxShape3DSW *>(p_a);
-	const CapsuleShape3DSW *capsule_B = static_cast<const CapsuleShape3DSW *>(p_b);
+static void _collision_box_capsule(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
+	const GodotBoxShape3D *box_A = static_cast<const GodotBoxShape3D *>(p_a);
+	const GodotCapsuleShape3D *capsule_B = static_cast<const GodotCapsuleShape3D *>(p_b);
 
-	SeparatorAxisTest<BoxShape3DSW, CapsuleShape3DSW, withMargin> separator(box_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
+	SeparatorAxisTest<GodotBoxShape3D, GodotCapsuleShape3D, withMargin> separator(box_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
 
 	if (!separator.test_previous_axis()) {
 		return;
@@ -1195,7 +1204,7 @@ static void _collision_box_capsule(const Shape3DSW *p_a, const Transform3D &p_tr
 				}
 
 				//Vector3 axis = (point - cyl_axis * cyl_axis.dot(point)).normalized();
-				Vector3 axis = Plane(cyl_axis, 0).project(point).normalized();
+				Vector3 axis = Plane(cyl_axis).project(point).normalized();
 
 				if (!separator.test_axis(axis)) {
 					return;
@@ -1241,11 +1250,11 @@ static void _collision_box_capsule(const Shape3DSW *p_a, const Transform3D &p_tr
 }
 
 template <bool withMargin>
-static void _collision_box_cylinder(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
-	const BoxShape3DSW *box_A = static_cast<const BoxShape3DSW *>(p_a);
-	const CylinderShape3DSW *cylinder_B = static_cast<const CylinderShape3DSW *>(p_b);
+static void _collision_box_cylinder(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
+	const GodotBoxShape3D *box_A = static_cast<const GodotBoxShape3D *>(p_a);
+	const GodotCylinderShape3D *cylinder_B = static_cast<const GodotCylinderShape3D *>(p_b);
 
-	SeparatorAxisTest<BoxShape3DSW, CylinderShape3DSW, withMargin> separator(box_A, p_transform_a, cylinder_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
+	SeparatorAxisTest<GodotBoxShape3D, GodotCylinderShape3D, withMargin> separator(box_A, p_transform_a, cylinder_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
 
 	if (!separator.test_previous_axis()) {
 		return;
@@ -1304,7 +1313,7 @@ static void _collision_box_cylinder(const Shape3DSW *p_a, const Transform3D &p_t
 	// Points of A, cylinder lateral surface.
 	for (int i = 0; i < 8; i++) {
 		const Vector3 &point = vertices_A[i];
-		Vector3 axis = Plane(cyl_axis, 0).project(point).normalized();
+		Vector3 axis = Plane(cyl_axis).project(point).normalized();
 
 		if (!separator.test_axis(axis)) {
 			return;
@@ -1354,11 +1363,11 @@ static void _collision_box_cylinder(const Shape3DSW *p_a, const Transform3D &p_t
 }
 
 template <bool withMargin>
-static void _collision_box_convex_polygon(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
-	const BoxShape3DSW *box_A = static_cast<const BoxShape3DSW *>(p_a);
-	const ConvexPolygonShape3DSW *convex_polygon_B = static_cast<const ConvexPolygonShape3DSW *>(p_b);
+static void _collision_box_convex_polygon(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
+	const GodotBoxShape3D *box_A = static_cast<const GodotBoxShape3D *>(p_a);
+	const GodotConvexPolygonShape3D *convex_polygon_B = static_cast<const GodotConvexPolygonShape3D *>(p_b);
 
-	SeparatorAxisTest<BoxShape3DSW, ConvexPolygonShape3DSW, withMargin> separator(box_A, p_transform_a, convex_polygon_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
+	SeparatorAxisTest<GodotBoxShape3D, GodotConvexPolygonShape3D, withMargin> separator(box_A, p_transform_a, convex_polygon_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
 
 	if (!separator.test_previous_axis()) {
 		return;
@@ -1472,11 +1481,11 @@ static void _collision_box_convex_polygon(const Shape3DSW *p_a, const Transform3
 }
 
 template <bool withMargin>
-static void _collision_box_face(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
-	const BoxShape3DSW *box_A = static_cast<const BoxShape3DSW *>(p_a);
-	const FaceShape3DSW *face_B = static_cast<const FaceShape3DSW *>(p_b);
+static void _collision_box_face(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
+	const GodotBoxShape3D *box_A = static_cast<const GodotBoxShape3D *>(p_a);
+	const GodotFaceShape3D *face_B = static_cast<const GodotFaceShape3D *>(p_b);
 
-	SeparatorAxisTest<BoxShape3DSW, FaceShape3DSW, withMargin> separator(box_A, p_transform_a, face_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
+	SeparatorAxisTest<GodotBoxShape3D, GodotFaceShape3D, withMargin> separator(box_A, p_transform_a, face_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
 
 	Vector3 vertex[3] = {
 		p_transform_b.xform(face_B->vertex[0]),
@@ -1486,7 +1495,7 @@ static void _collision_box_face(const Shape3DSW *p_a, const Transform3D &p_trans
 
 	Vector3 normal = (vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized();
 
-	if (!separator.test_axis(normal, !face_B->backface_collision)) {
+	if (!separator.test_axis(normal)) {
 		return;
 	}
 
@@ -1591,15 +1600,26 @@ static void _collision_box_face(const Shape3DSW *p_a, const Transform3D &p_trans
 		}
 	}
 
+	if (!face_B->backface_collision) {
+		if (separator.best_axis.dot(normal) < _BACKFACE_NORMAL_THRESHOLD) {
+			if (face_B->invert_backface_collision) {
+				separator.best_axis = separator.best_axis.bounce(normal);
+			} else {
+				// Just ignore backface collision.
+				return;
+			}
+		}
+	}
+
 	separator.generate_contacts();
 }
 
 template <bool withMargin>
-static void _collision_capsule_capsule(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
-	const CapsuleShape3DSW *capsule_A = static_cast<const CapsuleShape3DSW *>(p_a);
-	const CapsuleShape3DSW *capsule_B = static_cast<const CapsuleShape3DSW *>(p_b);
+static void _collision_capsule_capsule(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
+	const GodotCapsuleShape3D *capsule_A = static_cast<const GodotCapsuleShape3D *>(p_a);
+	const GodotCapsuleShape3D *capsule_B = static_cast<const GodotCapsuleShape3D *>(p_b);
 
-	SeparatorAxisTest<CapsuleShape3DSW, CapsuleShape3DSW, withMargin> separator(capsule_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
+	SeparatorAxisTest<GodotCapsuleShape3D, GodotCapsuleShape3D, withMargin> separator(capsule_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
 
 	if (!separator.test_previous_axis()) {
 		return;
@@ -1659,11 +1679,11 @@ static void _collision_capsule_capsule(const Shape3DSW *p_a, const Transform3D &
 }
 
 template <bool withMargin>
-static void _collision_capsule_cylinder(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
-	const CapsuleShape3DSW *capsule_A = static_cast<const CapsuleShape3DSW *>(p_a);
-	const CylinderShape3DSW *cylinder_B = static_cast<const CylinderShape3DSW *>(p_b);
+static void _collision_capsule_cylinder(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
+	const GodotCapsuleShape3D *capsule_A = static_cast<const GodotCapsuleShape3D *>(p_a);
+	const GodotCylinderShape3D *cylinder_B = static_cast<const GodotCylinderShape3D *>(p_b);
 
-	SeparatorAxisTest<CapsuleShape3DSW, CylinderShape3DSW, withMargin> separator(capsule_A, p_transform_a, cylinder_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
+	SeparatorAxisTest<GodotCapsuleShape3D, GodotCylinderShape3D, withMargin> separator(capsule_A, p_transform_a, cylinder_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
 
 	if (!separator.test_previous_axis()) {
 		return;
@@ -1710,7 +1730,7 @@ static void _collision_capsule_cylinder(const Shape3DSW *p_a, const Transform3D
 		return;
 	}
 
-	CollisionSolver3DSW::CallbackResult callback = SeparatorAxisTest<CapsuleShape3DSW, CylinderShape3DSW, withMargin>::test_contact_points;
+	GodotCollisionSolver3D::CallbackResult callback = SeparatorAxisTest<GodotCapsuleShape3D, GodotCylinderShape3D, withMargin>::test_contact_points;
 
 	// Fallback to generic algorithm to find the best separating axis.
 	if (!fallback_collision_solver(p_a, p_transform_a, p_b, p_transform_b, callback, &separator, false, p_margin_a, p_margin_b)) {
@@ -1721,11 +1741,11 @@ static void _collision_capsule_cylinder(const Shape3DSW *p_a, const Transform3D
 }
 
 template <bool withMargin>
-static void _collision_capsule_convex_polygon(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
-	const CapsuleShape3DSW *capsule_A = static_cast<const CapsuleShape3DSW *>(p_a);
-	const ConvexPolygonShape3DSW *convex_polygon_B = static_cast<const ConvexPolygonShape3DSW *>(p_b);
+static void _collision_capsule_convex_polygon(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
+	const GodotCapsuleShape3D *capsule_A = static_cast<const GodotCapsuleShape3D *>(p_a);
+	const GodotConvexPolygonShape3D *convex_polygon_B = static_cast<const GodotConvexPolygonShape3D *>(p_b);
 
-	SeparatorAxisTest<CapsuleShape3DSW, ConvexPolygonShape3DSW, withMargin> separator(capsule_A, p_transform_a, convex_polygon_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
+	SeparatorAxisTest<GodotCapsuleShape3D, GodotConvexPolygonShape3D, withMargin> separator(capsule_A, p_transform_a, convex_polygon_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
 
 	if (!separator.test_previous_axis()) {
 		return;
@@ -1788,11 +1808,11 @@ static void _collision_capsule_convex_polygon(const Shape3DSW *p_a, const Transf
 }
 
 template <bool withMargin>
-static void _collision_capsule_face(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
-	const CapsuleShape3DSW *capsule_A = static_cast<const CapsuleShape3DSW *>(p_a);
-	const FaceShape3DSW *face_B = static_cast<const FaceShape3DSW *>(p_b);
+static void _collision_capsule_face(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
+	const GodotCapsuleShape3D *capsule_A = static_cast<const GodotCapsuleShape3D *>(p_a);
+	const GodotFaceShape3D *face_B = static_cast<const GodotFaceShape3D *>(p_b);
 
-	SeparatorAxisTest<CapsuleShape3DSW, FaceShape3DSW, withMargin> separator(capsule_A, p_transform_a, face_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
+	SeparatorAxisTest<GodotCapsuleShape3D, GodotFaceShape3D, withMargin> separator(capsule_A, p_transform_a, face_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
 
 	Vector3 vertex[3] = {
 		p_transform_b.xform(face_B->vertex[0]),
@@ -1802,7 +1822,7 @@ static void _collision_capsule_face(const Shape3DSW *p_a, const Transform3D &p_t
 
 	Vector3 normal = (vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized();
 
-	if (!separator.test_axis(normal, !face_B->backface_collision)) {
+	if (!separator.test_axis(normal)) {
 		return;
 	}
 
@@ -1858,15 +1878,26 @@ static void _collision_capsule_face(const Shape3DSW *p_a, const Transform3D &p_t
 		}
 	}
 
+	if (!face_B->backface_collision) {
+		if (separator.best_axis.dot(normal) < _BACKFACE_NORMAL_THRESHOLD) {
+			if (face_B->invert_backface_collision) {
+				separator.best_axis = separator.best_axis.bounce(normal);
+			} else {
+				// Just ignore backface collision.
+				return;
+			}
+		}
+	}
+
 	separator.generate_contacts();
 }
 
 template <bool withMargin>
-static void _collision_cylinder_cylinder(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
-	const CylinderShape3DSW *cylinder_A = static_cast<const CylinderShape3DSW *>(p_a);
-	const CylinderShape3DSW *cylinder_B = static_cast<const CylinderShape3DSW *>(p_b);
+static void _collision_cylinder_cylinder(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
+	const GodotCylinderShape3D *cylinder_A = static_cast<const GodotCylinderShape3D *>(p_a);
+	const GodotCylinderShape3D *cylinder_B = static_cast<const GodotCylinderShape3D *>(p_b);
 
-	SeparatorAxisTest<CylinderShape3DSW, CylinderShape3DSW, withMargin> separator(cylinder_A, p_transform_a, cylinder_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
+	SeparatorAxisTest<GodotCylinderShape3D, GodotCylinderShape3D, withMargin> separator(cylinder_A, p_transform_a, cylinder_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
 
 	Vector3 cylinder_A_axis = p_transform_a.basis.get_axis(1);
 	Vector3 cylinder_B_axis = p_transform_b.basis.get_axis(1);
@@ -1905,7 +1936,7 @@ static void _collision_cylinder_cylinder(const Shape3DSW *p_a, const Transform3D
 		return;
 	}
 
-	CollisionSolver3DSW::CallbackResult callback = SeparatorAxisTest<CylinderShape3DSW, CylinderShape3DSW, withMargin>::test_contact_points;
+	GodotCollisionSolver3D::CallbackResult callback = SeparatorAxisTest<GodotCylinderShape3D, GodotCylinderShape3D, withMargin>::test_contact_points;
 
 	// Fallback to generic algorithm to find the best separating axis.
 	if (!fallback_collision_solver(p_a, p_transform_a, p_b, p_transform_b, callback, &separator, false, p_margin_a, p_margin_b)) {
@@ -1916,13 +1947,13 @@ static void _collision_cylinder_cylinder(const Shape3DSW *p_a, const Transform3D
 }
 
 template <bool withMargin>
-static void _collision_cylinder_convex_polygon(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
-	const CylinderShape3DSW *cylinder_A = static_cast<const CylinderShape3DSW *>(p_a);
-	const ConvexPolygonShape3DSW *convex_polygon_B = static_cast<const ConvexPolygonShape3DSW *>(p_b);
+static void _collision_cylinder_convex_polygon(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
+	const GodotCylinderShape3D *cylinder_A = static_cast<const GodotCylinderShape3D *>(p_a);
+	const GodotConvexPolygonShape3D *convex_polygon_B = static_cast<const GodotConvexPolygonShape3D *>(p_b);
 
-	SeparatorAxisTest<CylinderShape3DSW, ConvexPolygonShape3DSW, withMargin> separator(cylinder_A, p_transform_a, convex_polygon_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
+	SeparatorAxisTest<GodotCylinderShape3D, GodotConvexPolygonShape3D, withMargin> separator(cylinder_A, p_transform_a, convex_polygon_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
 
-	CollisionSolver3DSW::CallbackResult callback = SeparatorAxisTest<CylinderShape3DSW, ConvexPolygonShape3DSW, withMargin>::test_contact_points;
+	GodotCollisionSolver3D::CallbackResult callback = SeparatorAxisTest<GodotCylinderShape3D, GodotConvexPolygonShape3D, withMargin>::test_contact_points;
 
 	// Fallback to generic algorithm to find the best separating axis.
 	if (!fallback_collision_solver(p_a, p_transform_a, p_b, p_transform_b, callback, &separator, false, p_margin_a, p_margin_b)) {
@@ -1933,11 +1964,11 @@ static void _collision_cylinder_convex_polygon(const Shape3DSW *p_a, const Trans
 }
 
 template <bool withMargin>
-static void _collision_cylinder_face(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
-	const CylinderShape3DSW *cylinder_A = static_cast<const CylinderShape3DSW *>(p_a);
-	const FaceShape3DSW *face_B = static_cast<const FaceShape3DSW *>(p_b);
+static void _collision_cylinder_face(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
+	const GodotCylinderShape3D *cylinder_A = static_cast<const GodotCylinderShape3D *>(p_a);
+	const GodotFaceShape3D *face_B = static_cast<const GodotFaceShape3D *>(p_b);
 
-	SeparatorAxisTest<CylinderShape3DSW, FaceShape3DSW, withMargin> separator(cylinder_A, p_transform_a, face_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
+	SeparatorAxisTest<GodotCylinderShape3D, GodotFaceShape3D, withMargin> separator(cylinder_A, p_transform_a, face_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
 
 	if (!separator.test_previous_axis()) {
 		return;
@@ -1952,7 +1983,7 @@ static void _collision_cylinder_face(const Shape3DSW *p_a, const Transform3D &p_
 	Vector3 normal = (vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized();
 
 	// Face B normal.
-	if (!separator.test_axis(normal, !face_B->backface_collision)) {
+	if (!separator.test_axis(normal)) {
 		return;
 	}
 
@@ -1986,7 +2017,7 @@ static void _collision_cylinder_face(const Shape3DSW *p_a, const Transform3D &p_
 	// Points of B, cylinder lateral surface.
 	for (int i = 0; i < 3; i++) {
 		const Vector3 &point = vertex[i];
-		Vector3 axis = Plane(cyl_axis, 0).project(point).normalized();
+		Vector3 axis = Plane(cyl_axis).project(point).normalized();
 		if (axis.dot(normal) < 0.0) {
 			axis *= -1.0;
 		}
@@ -2034,15 +2065,26 @@ static void _collision_cylinder_face(const Shape3DSW *p_a, const Transform3D &p_
 		}
 	}
 
+	if (!face_B->backface_collision) {
+		if (separator.best_axis.dot(normal) < _BACKFACE_NORMAL_THRESHOLD) {
+			if (face_B->invert_backface_collision) {
+				separator.best_axis = separator.best_axis.bounce(normal);
+			} else {
+				// Just ignore backface collision.
+				return;
+			}
+		}
+	}
+
 	separator.generate_contacts();
 }
 
 template <bool withMargin>
-static void _collision_convex_polygon_convex_polygon(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
-	const ConvexPolygonShape3DSW *convex_polygon_A = static_cast<const ConvexPolygonShape3DSW *>(p_a);
-	const ConvexPolygonShape3DSW *convex_polygon_B = static_cast<const ConvexPolygonShape3DSW *>(p_b);
+static void _collision_convex_polygon_convex_polygon(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
+	const GodotConvexPolygonShape3D *convex_polygon_A = static_cast<const GodotConvexPolygonShape3D *>(p_a);
+	const GodotConvexPolygonShape3D *convex_polygon_B = static_cast<const GodotConvexPolygonShape3D *>(p_b);
 
-	SeparatorAxisTest<ConvexPolygonShape3DSW, ConvexPolygonShape3DSW, withMargin> separator(convex_polygon_A, p_transform_a, convex_polygon_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
+	SeparatorAxisTest<GodotConvexPolygonShape3D, GodotConvexPolygonShape3D, withMargin> separator(convex_polygon_A, p_transform_a, convex_polygon_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
 
 	if (!separator.test_previous_axis()) {
 		return;
@@ -2151,11 +2193,11 @@ static void _collision_convex_polygon_convex_polygon(const Shape3DSW *p_a, const
 }
 
 template <bool withMargin>
-static void _collision_convex_polygon_face(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
-	const ConvexPolygonShape3DSW *convex_polygon_A = static_cast<const ConvexPolygonShape3DSW *>(p_a);
-	const FaceShape3DSW *face_B = static_cast<const FaceShape3DSW *>(p_b);
+static void _collision_convex_polygon_face(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
+	const GodotConvexPolygonShape3D *convex_polygon_A = static_cast<const GodotConvexPolygonShape3D *>(p_a);
+	const GodotFaceShape3D *face_B = static_cast<const GodotFaceShape3D *>(p_b);
 
-	SeparatorAxisTest<ConvexPolygonShape3DSW, FaceShape3DSW, withMargin> separator(convex_polygon_A, p_transform_a, face_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
+	SeparatorAxisTest<GodotConvexPolygonShape3D, GodotFaceShape3D, withMargin> separator(convex_polygon_A, p_transform_a, face_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
 
 	const Geometry3D::MeshData &mesh = convex_polygon_A->get_mesh();
 
@@ -2174,7 +2216,7 @@ static void _collision_convex_polygon_face(const Shape3DSW *p_a, const Transform
 
 	Vector3 normal = (vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized();
 
-	if (!separator.test_axis(normal, !face_B->backface_collision)) {
+	if (!separator.test_axis(normal)) {
 		return;
 	}
 
@@ -2266,19 +2308,30 @@ static void _collision_convex_polygon_face(const Shape3DSW *p_a, const Transform
 		}
 	}
 
+	if (!face_B->backface_collision) {
+		if (separator.best_axis.dot(normal) < _BACKFACE_NORMAL_THRESHOLD) {
+			if (face_B->invert_backface_collision) {
+				separator.best_axis = separator.best_axis.bounce(normal);
+			} else {
+				// Just ignore backface collision.
+				return;
+			}
+		}
+	}
+
 	separator.generate_contacts();
 }
 
-bool sat_calculate_penetration(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CollisionSolver3DSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap, Vector3 *r_prev_axis, real_t p_margin_a, real_t p_margin_b) {
+bool sat_calculate_penetration(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, GodotCollisionSolver3D::CallbackResult p_result_callback, void *p_userdata, bool p_swap, Vector3 *r_prev_axis, real_t p_margin_a, real_t p_margin_b) {
 	PhysicsServer3D::ShapeType type_A = p_shape_A->get_type();
 
-	ERR_FAIL_COND_V(type_A == PhysicsServer3D::SHAPE_PLANE, false);
+	ERR_FAIL_COND_V(type_A == PhysicsServer3D::SHAPE_WORLD_BOUNDARY, false);
 	ERR_FAIL_COND_V(type_A == PhysicsServer3D::SHAPE_SEPARATION_RAY, false);
 	ERR_FAIL_COND_V(p_shape_A->is_concave(), false);
 
 	PhysicsServer3D::ShapeType type_B = p_shape_B->get_type();
 
-	ERR_FAIL_COND_V(type_B == PhysicsServer3D::SHAPE_PLANE, false);
+	ERR_FAIL_COND_V(type_B == PhysicsServer3D::SHAPE_WORLD_BOUNDARY, false);
 	ERR_FAIL_COND_V(type_B == PhysicsServer3D::SHAPE_SEPARATION_RAY, false);
 	ERR_FAIL_COND_V(p_shape_B->is_concave(), false);
 
@@ -2367,8 +2420,8 @@ bool sat_calculate_penetration(const Shape3DSW *p_shape_A, const Transform3D &p_
 	callback.collided = false;
 	callback.prev_axis = r_prev_axis;
 
-	const Shape3DSW *A = p_shape_A;
-	const Shape3DSW *B = p_shape_B;
+	const GodotShape3D *A = p_shape_A;
+	const GodotShape3D *B = p_shape_B;
 	const Transform3D *transform_A = &p_transform_A;
 	const Transform3D *transform_B = &p_transform_B;
 	real_t margin_A = p_margin_a;
diff --git a/servers/physics_3d/collision_solver_3d_sat.h b/servers/physics_3d/godot_collision_solver_3d_sat.h
index e50da7b101..069a701cba 100644
--- a/servers/physics_3d/collision_solver_3d_sat.h
+++ b/servers/physics_3d/godot_collision_solver_3d_sat.h
@@ -1,5 +1,5 @@
 /*************************************************************************/
-/*  collision_solver_3d_sat.h                                            */
+/*  godot_collision_solver_3d_sat.h                                      */
 /*************************************************************************/
 /*                       This file is part of:                           */
 /*                           GODOT ENGINE                                */
@@ -28,11 +28,11 @@
 /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
 /*************************************************************************/
 
-#ifndef COLLISION_SOLVER_SAT_H
-#define COLLISION_SOLVER_SAT_H
+#ifndef GODOT_COLLISION_SOLVER_SAT_H
+#define GODOT_COLLISION_SOLVER_SAT_H
 
-#include "collision_solver_3d_sw.h"
+#include "godot_collision_solver_3d.h"
 
-bool sat_calculate_penetration(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CollisionSolver3DSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap = false, Vector3 *r_prev_axis = nullptr, real_t p_margin_a = 0, real_t p_margin_b = 0);
+bool sat_calculate_penetration(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, GodotCollisionSolver3D::CallbackResult p_result_callback, void *p_userdata, bool p_swap = false, Vector3 *r_prev_axis = nullptr, real_t p_margin_a = 0, real_t p_margin_b = 0);
 
-#endif // COLLISION_SOLVER_SAT_H
+#endif // GODOT_COLLISION_SOLVER_SAT_H
diff --git a/servers/physics_3d/constraint_3d_sw.h b/servers/physics_3d/godot_constraint_3d.h
index 7b44726ef5..840c81716c 100644
--- a/servers/physics_3d/constraint_3d_sw.h
+++ b/servers/physics_3d/godot_constraint_3d.h
@@ -1,5 +1,5 @@
 /*************************************************************************/
-/*  constraint_3d_sw.h                                                   */
+/*  godot_constraint_3d.h                                                */
 /*************************************************************************/
 /*                       This file is part of:                           */
 /*                           GODOT ENGINE                                */
@@ -28,14 +28,14 @@
 /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
 /*************************************************************************/
 
-#ifndef CONSTRAINT_SW_H
-#define CONSTRAINT_SW_H
+#ifndef GODOT_CONSTRAINT_3D_H
+#define GODOT_CONSTRAINT_3D_H
 
-class Body3DSW;
-class SoftBody3DSW;
+class GodotBody3D;
+class GodotSoftBody3D;
 
-class Constraint3DSW {
-	Body3DSW **_body_ptr;
+class GodotConstraint3D {
+	GodotBody3D **_body_ptr;
 	int _body_count;
 	uint64_t island_step;
 	int priority;
@@ -44,7 +44,7 @@ class Constraint3DSW {
 	RID self;
 
 protected:
-	Constraint3DSW(Body3DSW **p_body_ptr = nullptr, int p_body_count = 0) {
+	GodotConstraint3D(GodotBody3D **p_body_ptr = nullptr, int p_body_count = 0) {
 		_body_ptr = p_body_ptr;
 		_body_count = p_body_count;
 		island_step = 0;
@@ -59,10 +59,10 @@ public:
 	_FORCE_INLINE_ uint64_t get_island_step() const { return island_step; }
 	_FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step = p_step; }
 
-	_FORCE_INLINE_ Body3DSW **get_body_ptr() const { return _body_ptr; }
+	_FORCE_INLINE_ GodotBody3D **get_body_ptr() const { return _body_ptr; }
 	_FORCE_INLINE_ int get_body_count() const { return _body_count; }
 
-	virtual SoftBody3DSW *get_soft_body_ptr(int p_index) const { return nullptr; }
+	virtual GodotSoftBody3D *get_soft_body_ptr(int p_index) const { return nullptr; }
 	virtual int get_soft_body_count() const { return 0; }
 
 	_FORCE_INLINE_ void set_priority(int p_priority) { priority = p_priority; }
@@ -75,7 +75,7 @@ public:
 	virtual bool pre_solve(real_t p_step) = 0;
 	virtual void solve(real_t p_step) = 0;
 
-	virtual ~Constraint3DSW() {}
+	virtual ~GodotConstraint3D() {}
 };
 
-#endif // CONSTRAINT__SW_H
+#endif // GODOT_CONSTRAINT_3D_H
diff --git a/servers/physics_3d/joints_3d_sw.h b/servers/physics_3d/godot_joint_3d.h
index e2514674ea..4086bb53e1 100644
--- a/servers/physics_3d/joints_3d_sw.h
+++ b/servers/physics_3d/godot_joint_3d.h
@@ -1,5 +1,5 @@
 /*************************************************************************/
-/*  joints_3d_sw.h                                                       */
+/*  godot_joint_3d.h                                                     */
 /*************************************************************************/
 /*                       This file is part of:                           */
 /*                           GODOT ENGINE                                */
@@ -28,13 +28,13 @@
 /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
 /*************************************************************************/
 
-#ifndef JOINTS_SW_H
-#define JOINTS_SW_H
+#ifndef GODOT_JOINT_3D_H
+#define GODOT_JOINT_3D_H
 
-#include "body_3d_sw.h"
-#include "constraint_3d_sw.h"
+#include "godot_body_3d.h"
+#include "godot_constraint_3d.h"
 
-class Joint3DSW : public Constraint3DSW {
+class GodotJoint3D : public GodotConstraint3D {
 protected:
 	bool dynamic_A = false;
 	bool dynamic_B = false;
@@ -44,20 +44,20 @@ public:
 	virtual bool pre_solve(real_t p_step) override { return true; }
 	virtual void solve(real_t p_step) override {}
 
-	void copy_settings_from(Joint3DSW *p_joint) {
+	void copy_settings_from(GodotJoint3D *p_joint) {
 		set_self(p_joint->get_self());
 		set_priority(p_joint->get_priority());
 		disable_collisions_between_bodies(p_joint->is_disabled_collisions_between_bodies());
 	}
 
 	virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_TYPE_MAX; }
-	_FORCE_INLINE_ Joint3DSW(Body3DSW **p_body_ptr = nullptr, int p_body_count = 0) :
-			Constraint3DSW(p_body_ptr, p_body_count) {
+	_FORCE_INLINE_ GodotJoint3D(GodotBody3D **p_body_ptr = nullptr, int p_body_count = 0) :
+			GodotConstraint3D(p_body_ptr, p_body_count) {
 	}
 
-	virtual ~Joint3DSW() {
+	virtual ~GodotJoint3D() {
 		for (int i = 0; i < get_body_count(); i++) {
-			Body3DSW *body = get_body_ptr()[i];
+			GodotBody3D *body = get_body_ptr()[i];
 			if (body) {
 				body->remove_constraint(this);
 			}
@@ -65,4 +65,4 @@ public:
 	}
 };
 
-#endif // JOINTS_SW_H
+#endif // GODOT_JOINT_3D_H
diff --git a/servers/physics_3d/godot_physics_server_3d.cpp b/servers/physics_3d/godot_physics_server_3d.cpp
new file mode 100644
index 0000000000..9acae62382
--- /dev/null
+++ b/servers/physics_3d/godot_physics_server_3d.cpp
@@ -0,0 +1,1740 @@
+/*************************************************************************/
+/*  godot_physics_server_3d.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 "godot_physics_server_3d.h"
+
+#include "godot_body_direct_state_3d.h"
+#include "godot_broad_phase_3d_bvh.h"
+#include "joints/godot_cone_twist_joint_3d.h"
+#include "joints/godot_generic_6dof_joint_3d.h"
+#include "joints/godot_hinge_joint_3d.h"
+#include "joints/godot_pin_joint_3d.h"
+#include "joints/godot_slider_joint_3d.h"
+
+#include "core/debugger/engine_debugger.h"
+#include "core/os/os.h"
+
+#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 GodotPhysicsServer3D::world_boundary_shape_create() {
+	GodotShape3D *shape = memnew(GodotWorldBoundaryShape3D);
+	RID rid = shape_owner.make_rid(shape);
+	shape->set_self(rid);
+	return rid;
+}
+RID GodotPhysicsServer3D::separation_ray_shape_create() {
+	GodotShape3D *shape = memnew(GodotSeparationRayShape3D);
+	RID rid = shape_owner.make_rid(shape);
+	shape->set_self(rid);
+	return rid;
+}
+RID GodotPhysicsServer3D::sphere_shape_create() {
+	GodotShape3D *shape = memnew(GodotSphereShape3D);
+	RID rid = shape_owner.make_rid(shape);
+	shape->set_self(rid);
+	return rid;
+}
+RID GodotPhysicsServer3D::box_shape_create() {
+	GodotShape3D *shape = memnew(GodotBoxShape3D);
+	RID rid = shape_owner.make_rid(shape);
+	shape->set_self(rid);
+	return rid;
+}
+RID GodotPhysicsServer3D::capsule_shape_create() {
+	GodotShape3D *shape = memnew(GodotCapsuleShape3D);
+	RID rid = shape_owner.make_rid(shape);
+	shape->set_self(rid);
+	return rid;
+}
+RID GodotPhysicsServer3D::cylinder_shape_create() {
+	GodotShape3D *shape = memnew(GodotCylinderShape3D);
+	RID rid = shape_owner.make_rid(shape);
+	shape->set_self(rid);
+	return rid;
+}
+RID GodotPhysicsServer3D::convex_polygon_shape_create() {
+	GodotShape3D *shape = memnew(GodotConvexPolygonShape3D);
+	RID rid = shape_owner.make_rid(shape);
+	shape->set_self(rid);
+	return rid;
+}
+RID GodotPhysicsServer3D::concave_polygon_shape_create() {
+	GodotShape3D *shape = memnew(GodotConcavePolygonShape3D);
+	RID rid = shape_owner.make_rid(shape);
+	shape->set_self(rid);
+	return rid;
+}
+RID GodotPhysicsServer3D::heightmap_shape_create() {
+	GodotShape3D *shape = memnew(GodotHeightMapShape3D);
+	RID rid = shape_owner.make_rid(shape);
+	shape->set_self(rid);
+	return rid;
+}
+RID GodotPhysicsServer3D::custom_shape_create() {
+	ERR_FAIL_V(RID());
+}
+
+void GodotPhysicsServer3D::shape_set_data(RID p_shape, const Variant &p_data) {
+	GodotShape3D *shape = shape_owner.get_or_null(p_shape);
+	ERR_FAIL_COND(!shape);
+	shape->set_data(p_data);
+};
+
+void GodotPhysicsServer3D::shape_set_custom_solver_bias(RID p_shape, real_t p_bias) {
+	GodotShape3D *shape = shape_owner.get_or_null(p_shape);
+	ERR_FAIL_COND(!shape);
+	shape->set_custom_bias(p_bias);
+}
+
+PhysicsServer3D::ShapeType GodotPhysicsServer3D::shape_get_type(RID p_shape) const {
+	const GodotShape3D *shape = shape_owner.get_or_null(p_shape);
+	ERR_FAIL_COND_V(!shape, SHAPE_CUSTOM);
+	return shape->get_type();
+};
+
+Variant GodotPhysicsServer3D::shape_get_data(RID p_shape) const {
+	const GodotShape3D *shape = shape_owner.get_or_null(p_shape);
+	ERR_FAIL_COND_V(!shape, Variant());
+	ERR_FAIL_COND_V(!shape->is_configured(), Variant());
+	return shape->get_data();
+};
+
+void GodotPhysicsServer3D::shape_set_margin(RID p_shape, real_t p_margin) {
+}
+
+real_t GodotPhysicsServer3D::shape_get_margin(RID p_shape) const {
+	return 0.0;
+}
+
+real_t GodotPhysicsServer3D::shape_get_custom_solver_bias(RID p_shape) const {
+	const GodotShape3D *shape = shape_owner.get_or_null(p_shape);
+	ERR_FAIL_COND_V(!shape, 0);
+	return shape->get_custom_bias();
+}
+
+RID GodotPhysicsServer3D::space_create() {
+	GodotSpace3D *space = memnew(GodotSpace3D);
+	RID id = space_owner.make_rid(space);
+	space->set_self(id);
+	RID area_id = area_create();
+	GodotArea3D *area = area_owner.get_or_null(area_id);
+	ERR_FAIL_COND_V(!area, RID());
+	space->set_default_area(area);
+	area->set_space(space);
+	area->set_priority(-1);
+	RID sgb = body_create();
+	body_set_space(sgb, id);
+	body_set_mode(sgb, BODY_MODE_STATIC);
+	space->set_static_global_body(sgb);
+
+	return id;
+};
+
+void GodotPhysicsServer3D::space_set_active(RID p_space, bool p_active) {
+	GodotSpace3D *space = space_owner.get_or_null(p_space);
+	ERR_FAIL_COND(!space);
+	if (p_active) {
+		active_spaces.insert(space);
+	} else {
+		active_spaces.erase(space);
+	}
+}
+
+bool GodotPhysicsServer3D::space_is_active(RID p_space) const {
+	const GodotSpace3D *space = space_owner.get_or_null(p_space);
+	ERR_FAIL_COND_V(!space, false);
+
+	return active_spaces.has(space);
+}
+
+void GodotPhysicsServer3D::space_set_param(RID p_space, SpaceParameter p_param, real_t p_value) {
+	GodotSpace3D *space = space_owner.get_or_null(p_space);
+	ERR_FAIL_COND(!space);
+
+	space->set_param(p_param, p_value);
+}
+
+real_t GodotPhysicsServer3D::space_get_param(RID p_space, SpaceParameter p_param) const {
+	const GodotSpace3D *space = space_owner.get_or_null(p_space);
+	ERR_FAIL_COND_V(!space, 0);
+	return space->get_param(p_param);
+}
+
+PhysicsDirectSpaceState3D *GodotPhysicsServer3D::space_get_direct_state(RID p_space) {
+	GodotSpace3D *space = space_owner.get_or_null(p_space);
+	ERR_FAIL_COND_V(!space, nullptr);
+	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();
+}
+
+void GodotPhysicsServer3D::space_set_debug_contacts(RID p_space, int p_max_contacts) {
+	GodotSpace3D *space = space_owner.get_or_null(p_space);
+	ERR_FAIL_COND(!space);
+	space->set_debug_contacts(p_max_contacts);
+}
+
+Vector<Vector3> GodotPhysicsServer3D::space_get_contacts(RID p_space) const {
+	GodotSpace3D *space = space_owner.get_or_null(p_space);
+	ERR_FAIL_COND_V(!space, Vector<Vector3>());
+	return space->get_debug_contacts();
+}
+
+int GodotPhysicsServer3D::space_get_contact_count(RID p_space) const {
+	GodotSpace3D *space = space_owner.get_or_null(p_space);
+	ERR_FAIL_COND_V(!space, 0);
+	return space->get_debug_contact_count();
+}
+
+RID GodotPhysicsServer3D::area_create() {
+	GodotArea3D *area = memnew(GodotArea3D);
+	RID rid = area_owner.make_rid(area);
+	area->set_self(rid);
+	return rid;
+}
+
+void GodotPhysicsServer3D::area_set_space(RID p_area, RID p_space) {
+	GodotArea3D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_COND(!area);
+
+	GodotSpace3D *space = nullptr;
+	if (p_space.is_valid()) {
+		space = space_owner.get_or_null(p_space);
+		ERR_FAIL_COND(!space);
+	}
+
+	if (area->get_space() == space) {
+		return; //pointless
+	}
+
+	area->clear_constraints();
+	area->set_space(space);
+}
+
+RID GodotPhysicsServer3D::area_get_space(RID p_area) const {
+	GodotArea3D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_COND_V(!area, RID());
+
+	GodotSpace3D *space = area->get_space();
+	if (!space) {
+		return RID();
+	}
+	return space->get_self();
+}
+
+void GodotPhysicsServer3D::area_add_shape(RID p_area, RID p_shape, const Transform3D &p_transform, bool p_disabled) {
+	GodotArea3D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_COND(!area);
+
+	GodotShape3D *shape = shape_owner.get_or_null(p_shape);
+	ERR_FAIL_COND(!shape);
+
+	area->add_shape(shape, p_transform, p_disabled);
+}
+
+void GodotPhysicsServer3D::area_set_shape(RID p_area, int p_shape_idx, RID p_shape) {
+	GodotArea3D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_COND(!area);
+
+	GodotShape3D *shape = shape_owner.get_or_null(p_shape);
+	ERR_FAIL_COND(!shape);
+	ERR_FAIL_COND(!shape->is_configured());
+
+	area->set_shape(p_shape_idx, shape);
+}
+
+void GodotPhysicsServer3D::area_set_shape_transform(RID p_area, int p_shape_idx, const Transform3D &p_transform) {
+	GodotArea3D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_COND(!area);
+
+	area->set_shape_transform(p_shape_idx, p_transform);
+}
+
+int GodotPhysicsServer3D::area_get_shape_count(RID p_area) const {
+	GodotArea3D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_COND_V(!area, -1);
+
+	return area->get_shape_count();
+}
+
+RID GodotPhysicsServer3D::area_get_shape(RID p_area, int p_shape_idx) const {
+	GodotArea3D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_COND_V(!area, RID());
+
+	GodotShape3D *shape = area->get_shape(p_shape_idx);
+	ERR_FAIL_COND_V(!shape, RID());
+
+	return shape->get_self();
+}
+
+Transform3D GodotPhysicsServer3D::area_get_shape_transform(RID p_area, int p_shape_idx) const {
+	GodotArea3D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_COND_V(!area, Transform3D());
+
+	return area->get_shape_transform(p_shape_idx);
+}
+
+void GodotPhysicsServer3D::area_remove_shape(RID p_area, int p_shape_idx) {
+	GodotArea3D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_COND(!area);
+
+	area->remove_shape(p_shape_idx);
+}
+
+void GodotPhysicsServer3D::area_clear_shapes(RID p_area) {
+	GodotArea3D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_COND(!area);
+
+	while (area->get_shape_count()) {
+		area->remove_shape(0);
+	}
+}
+
+void GodotPhysicsServer3D::area_set_shape_disabled(RID p_area, int p_shape_idx, bool p_disabled) {
+	GodotArea3D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_COND(!area);
+	ERR_FAIL_INDEX(p_shape_idx, area->get_shape_count());
+	FLUSH_QUERY_CHECK(area);
+	area->set_shape_disabled(p_shape_idx, p_disabled);
+}
+
+void GodotPhysicsServer3D::area_attach_object_instance_id(RID p_area, ObjectID p_id) {
+	if (space_owner.owns(p_area)) {
+		GodotSpace3D *space = space_owner.get_or_null(p_area);
+		p_area = space->get_default_area()->get_self();
+	}
+	GodotArea3D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_COND(!area);
+	area->set_instance_id(p_id);
+}
+
+ObjectID GodotPhysicsServer3D::area_get_object_instance_id(RID p_area) const {
+	if (space_owner.owns(p_area)) {
+		GodotSpace3D *space = space_owner.get_or_null(p_area);
+		p_area = space->get_default_area()->get_self();
+	}
+	GodotArea3D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_COND_V(!area, ObjectID());
+	return area->get_instance_id();
+}
+
+void GodotPhysicsServer3D::area_set_param(RID p_area, AreaParameter p_param, const Variant &p_value) {
+	if (space_owner.owns(p_area)) {
+		GodotSpace3D *space = space_owner.get_or_null(p_area);
+		p_area = space->get_default_area()->get_self();
+	}
+	GodotArea3D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_COND(!area);
+	area->set_param(p_param, p_value);
+};
+
+void GodotPhysicsServer3D::area_set_transform(RID p_area, const Transform3D &p_transform) {
+	GodotArea3D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_COND(!area);
+	area->set_transform(p_transform);
+};
+
+Variant GodotPhysicsServer3D::area_get_param(RID p_area, AreaParameter p_param) const {
+	if (space_owner.owns(p_area)) {
+		GodotSpace3D *space = space_owner.get_or_null(p_area);
+		p_area = space->get_default_area()->get_self();
+	}
+	GodotArea3D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_COND_V(!area, Variant());
+
+	return area->get_param(p_param);
+};
+
+Transform3D GodotPhysicsServer3D::area_get_transform(RID p_area) const {
+	GodotArea3D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_COND_V(!area, Transform3D());
+
+	return area->get_transform();
+};
+
+void GodotPhysicsServer3D::area_set_collision_layer(RID p_area, uint32_t p_layer) {
+	GodotArea3D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_COND(!area);
+
+	area->set_collision_layer(p_layer);
+}
+
+void GodotPhysicsServer3D::area_set_collision_mask(RID p_area, uint32_t p_mask) {
+	GodotArea3D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_COND(!area);
+
+	area->set_collision_mask(p_mask);
+}
+
+void GodotPhysicsServer3D::area_set_monitorable(RID p_area, bool p_monitorable) {
+	GodotArea3D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_COND(!area);
+	FLUSH_QUERY_CHECK(area);
+
+	area->set_monitorable(p_monitorable);
+}
+
+void GodotPhysicsServer3D::area_set_monitor_callback(RID p_area, const Callable &p_callback) {
+	GodotArea3D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_COND(!area);
+
+	area->set_monitor_callback(p_callback.is_valid() ? p_callback : Callable());
+}
+
+void GodotPhysicsServer3D::area_set_ray_pickable(RID p_area, bool p_enable) {
+	GodotArea3D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_COND(!area);
+
+	area->set_ray_pickable(p_enable);
+}
+
+void GodotPhysicsServer3D::area_set_area_monitor_callback(RID p_area, const Callable &p_callback) {
+	GodotArea3D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_COND(!area);
+
+	area->set_area_monitor_callback(p_callback.is_valid() ? p_callback : Callable());
+}
+
+/* BODY API */
+
+RID GodotPhysicsServer3D::body_create() {
+	GodotBody3D *body = memnew(GodotBody3D);
+	RID rid = body_owner.make_rid(body);
+	body->set_self(rid);
+	return rid;
+};
+
+void GodotPhysicsServer3D::body_set_space(RID p_body, RID p_space) {
+	GodotBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_COND(!body);
+
+	GodotSpace3D *space = nullptr;
+	if (p_space.is_valid()) {
+		space = space_owner.get_or_null(p_space);
+		ERR_FAIL_COND(!space);
+	}
+
+	if (body->get_space() == space) {
+		return; //pointless
+	}
+
+	body->clear_constraint_map();
+	body->set_space(space);
+};
+
+RID GodotPhysicsServer3D::body_get_space(RID p_body) const {
+	GodotBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_COND_V(!body, RID());
+
+	GodotSpace3D *space = body->get_space();
+	if (!space) {
+		return RID();
+	}
+	return space->get_self();
+};
+
+void GodotPhysicsServer3D::body_set_mode(RID p_body, BodyMode p_mode) {
+	GodotBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->set_mode(p_mode);
+};
+
+PhysicsServer3D::BodyMode GodotPhysicsServer3D::body_get_mode(RID p_body) const {
+	GodotBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_COND_V(!body, BODY_MODE_STATIC);
+
+	return body->get_mode();
+};
+
+void GodotPhysicsServer3D::body_add_shape(RID p_body, RID p_shape, const Transform3D &p_transform, bool p_disabled) {
+	GodotBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_COND(!body);
+
+	GodotShape3D *shape = shape_owner.get_or_null(p_shape);
+	ERR_FAIL_COND(!shape);
+
+	body->add_shape(shape, p_transform, p_disabled);
+}
+
+void GodotPhysicsServer3D::body_set_shape(RID p_body, int p_shape_idx, RID p_shape) {
+	GodotBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_COND(!body);
+
+	GodotShape3D *shape = shape_owner.get_or_null(p_shape);
+	ERR_FAIL_COND(!shape);
+	ERR_FAIL_COND(!shape->is_configured());
+
+	body->set_shape(p_shape_idx, shape);
+}
+void GodotPhysicsServer3D::body_set_shape_transform(RID p_body, int p_shape_idx, const Transform3D &p_transform) {
+	GodotBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->set_shape_transform(p_shape_idx, p_transform);
+}
+
+int GodotPhysicsServer3D::body_get_shape_count(RID p_body) const {
+	GodotBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_COND_V(!body, -1);
+
+	return body->get_shape_count();
+}
+
+RID GodotPhysicsServer3D::body_get_shape(RID p_body, int p_shape_idx) const {
+	GodotBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_COND_V(!body, RID());
+
+	GodotShape3D *shape = body->get_shape(p_shape_idx);
+	ERR_FAIL_COND_V(!shape, RID());
+
+	return shape->get_self();
+}
+
+void GodotPhysicsServer3D::body_set_shape_disabled(RID p_body, int p_shape_idx, bool p_disabled) {
+	GodotBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_COND(!body);
+	ERR_FAIL_INDEX(p_shape_idx, body->get_shape_count());
+	FLUSH_QUERY_CHECK(body);
+
+	body->set_shape_disabled(p_shape_idx, p_disabled);
+}
+
+Transform3D GodotPhysicsServer3D::body_get_shape_transform(RID p_body, int p_shape_idx) const {
+	GodotBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_COND_V(!body, Transform3D());
+
+	return body->get_shape_transform(p_shape_idx);
+}
+
+void GodotPhysicsServer3D::body_remove_shape(RID p_body, int p_shape_idx) {
+	GodotBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->remove_shape(p_shape_idx);
+}
+
+void GodotPhysicsServer3D::body_clear_shapes(RID p_body) {
+	GodotBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_COND(!body);
+
+	while (body->get_shape_count()) {
+		body->remove_shape(0);
+	}
+}
+
+void GodotPhysicsServer3D::body_set_enable_continuous_collision_detection(RID p_body, bool p_enable) {
+	GodotBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->set_continuous_collision_detection(p_enable);
+}
+
+bool GodotPhysicsServer3D::body_is_continuous_collision_detection_enabled(RID p_body) const {
+	GodotBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_COND_V(!body, false);
+
+	return body->is_continuous_collision_detection_enabled();
+}
+
+void GodotPhysicsServer3D::body_set_collision_layer(RID p_body, uint32_t p_layer) {
+	GodotBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->set_collision_layer(p_layer);
+}
+
+uint32_t GodotPhysicsServer3D::body_get_collision_layer(RID p_body) const {
+	const GodotBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_COND_V(!body, 0);
+
+	return body->get_collision_layer();
+}
+
+void GodotPhysicsServer3D::body_set_collision_mask(RID p_body, uint32_t p_mask) {
+	GodotBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->set_collision_mask(p_mask);
+}
+
+uint32_t GodotPhysicsServer3D::body_get_collision_mask(RID p_body) const {
+	const GodotBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_COND_V(!body, 0);
+
+	return body->get_collision_mask();
+}
+
+void GodotPhysicsServer3D::body_attach_object_instance_id(RID p_body, ObjectID p_id) {
+	GodotBody3D *body = body_owner.get_or_null(p_body);
+	if (body) {
+		body->set_instance_id(p_id);
+		return;
+	}
+
+	GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body);
+	if (soft_body) {
+		soft_body->set_instance_id(p_id);
+		return;
+	}
+
+	ERR_FAIL_MSG("Invalid ID.");
+};
+
+ObjectID GodotPhysicsServer3D::body_get_object_instance_id(RID p_body) const {
+	GodotBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_COND_V(!body, ObjectID());
+
+	return body->get_instance_id();
+};
+
+void GodotPhysicsServer3D::body_set_user_flags(RID p_body, uint32_t p_flags) {
+	GodotBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_COND(!body);
+};
+
+uint32_t GodotPhysicsServer3D::body_get_user_flags(RID p_body) const {
+	GodotBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_COND_V(!body, 0);
+
+	return 0;
+};
+
+void GodotPhysicsServer3D::body_set_param(RID p_body, BodyParameter p_param, const Variant &p_value) {
+	GodotBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->set_param(p_param, p_value);
+};
+
+Variant GodotPhysicsServer3D::body_get_param(RID p_body, BodyParameter p_param) const {
+	GodotBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_COND_V(!body, 0);
+
+	return body->get_param(p_param);
+};
+
+void GodotPhysicsServer3D::body_reset_mass_properties(RID p_body) {
+	GodotBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_COND(!body);
+
+	return body->reset_mass_properties();
+}
+
+void GodotPhysicsServer3D::body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) {
+	GodotBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->set_state(p_state, p_variant);
+};
+
+Variant GodotPhysicsServer3D::body_get_state(RID p_body, BodyState p_state) const {
+	GodotBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_COND_V(!body, Variant());
+
+	return body->get_state(p_state);
+};
+
+void GodotPhysicsServer3D::body_set_applied_force(RID p_body, const Vector3 &p_force) {
+	GodotBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->set_applied_force(p_force);
+	body->wakeup();
+};
+
+Vector3 GodotPhysicsServer3D::body_get_applied_force(RID p_body) const {
+	GodotBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_COND_V(!body, Vector3());
+	return body->get_applied_force();
+};
+
+void GodotPhysicsServer3D::body_set_applied_torque(RID p_body, const Vector3 &p_torque) {
+	GodotBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->set_applied_torque(p_torque);
+	body->wakeup();
+};
+
+Vector3 GodotPhysicsServer3D::body_get_applied_torque(RID p_body) const {
+	GodotBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_COND_V(!body, Vector3());
+
+	return body->get_applied_torque();
+};
+
+void GodotPhysicsServer3D::body_add_central_force(RID p_body, const Vector3 &p_force) {
+	GodotBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->add_central_force(p_force);
+	body->wakeup();
+}
+
+void GodotPhysicsServer3D::body_add_force(RID p_body, const Vector3 &p_force, const Vector3 &p_position) {
+	GodotBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->add_force(p_force, p_position);
+	body->wakeup();
+};
+
+void GodotPhysicsServer3D::body_add_torque(RID p_body, const Vector3 &p_torque) {
+	GodotBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->add_torque(p_torque);
+	body->wakeup();
+};
+
+void GodotPhysicsServer3D::body_apply_central_impulse(RID p_body, const Vector3 &p_impulse) {
+	GodotBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_COND(!body);
+
+	_update_shapes();
+
+	body->apply_central_impulse(p_impulse);
+	body->wakeup();
+}
+
+void GodotPhysicsServer3D::body_apply_impulse(RID p_body, const Vector3 &p_impulse, const Vector3 &p_position) {
+	GodotBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_COND(!body);
+
+	_update_shapes();
+
+	body->apply_impulse(p_impulse, p_position);
+	body->wakeup();
+};
+
+void GodotPhysicsServer3D::body_apply_torque_impulse(RID p_body, const Vector3 &p_impulse) {
+	GodotBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_COND(!body);
+
+	_update_shapes();
+
+	body->apply_torque_impulse(p_impulse);
+	body->wakeup();
+};
+
+void GodotPhysicsServer3D::body_set_axis_velocity(RID p_body, const Vector3 &p_axis_velocity) {
+	GodotBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_COND(!body);
+
+	_update_shapes();
+
+	Vector3 v = body->get_linear_velocity();
+	Vector3 axis = p_axis_velocity.normalized();
+	v -= axis * axis.dot(v);
+	v += p_axis_velocity;
+	body->set_linear_velocity(v);
+	body->wakeup();
+};
+
+void GodotPhysicsServer3D::body_set_axis_lock(RID p_body, BodyAxis p_axis, bool p_lock) {
+	GodotBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->set_axis_lock(p_axis, p_lock);
+	body->wakeup();
+}
+
+bool GodotPhysicsServer3D::body_is_axis_locked(RID p_body, BodyAxis p_axis) const {
+	const GodotBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_COND_V(!body, 0);
+	return body->is_axis_locked(p_axis);
+}
+
+void GodotPhysicsServer3D::body_add_collision_exception(RID p_body, RID p_body_b) {
+	GodotBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->add_exception(p_body_b);
+	body->wakeup();
+};
+
+void GodotPhysicsServer3D::body_remove_collision_exception(RID p_body, RID p_body_b) {
+	GodotBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->remove_exception(p_body_b);
+	body->wakeup();
+};
+
+void GodotPhysicsServer3D::body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions) {
+	GodotBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_COND(!body);
+
+	for (int i = 0; i < body->get_exceptions().size(); i++) {
+		p_exceptions->push_back(body->get_exceptions()[i]);
+	}
+};
+
+void GodotPhysicsServer3D::body_set_contacts_reported_depth_threshold(RID p_body, real_t p_threshold) {
+	GodotBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_COND(!body);
+};
+
+real_t GodotPhysicsServer3D::body_get_contacts_reported_depth_threshold(RID p_body) const {
+	GodotBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_COND_V(!body, 0);
+	return 0;
+};
+
+void GodotPhysicsServer3D::body_set_omit_force_integration(RID p_body, bool p_omit) {
+	GodotBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->set_omit_force_integration(p_omit);
+};
+
+bool GodotPhysicsServer3D::body_is_omitting_force_integration(RID p_body) const {
+	GodotBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_COND_V(!body, false);
+	return body->get_omit_force_integration();
+};
+
+void GodotPhysicsServer3D::body_set_max_contacts_reported(RID p_body, int p_contacts) {
+	GodotBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_COND(!body);
+	body->set_max_contacts_reported(p_contacts);
+}
+
+int GodotPhysicsServer3D::body_get_max_contacts_reported(RID p_body) const {
+	GodotBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_COND_V(!body, -1);
+	return body->get_max_contacts_reported();
+}
+
+void GodotPhysicsServer3D::body_set_state_sync_callback(RID p_body, void *p_instance, BodyStateCallback p_callback) {
+	GodotBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_COND(!body);
+	body->set_state_sync_callback(p_instance, p_callback);
+}
+
+void GodotPhysicsServer3D::body_set_force_integration_callback(RID p_body, const Callable &p_callable, const Variant &p_udata) {
+	GodotBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_COND(!body);
+	body->set_force_integration_callback(p_callable, p_udata);
+}
+
+void GodotPhysicsServer3D::body_set_ray_pickable(RID p_body, bool p_enable) {
+	GodotBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_COND(!body);
+	body->set_ray_pickable(p_enable);
+}
+
+bool GodotPhysicsServer3D::body_test_motion(RID p_body, const MotionParameters &p_parameters, MotionResult *r_result) {
+	GodotBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_COND_V(!body, false);
+	ERR_FAIL_COND_V(!body->get_space(), false);
+	ERR_FAIL_COND_V(body->get_space()->is_locked(), false);
+
+	_update_shapes();
+
+	return body->get_space()->test_body_motion(body, p_parameters, r_result);
+}
+
+PhysicsDirectBodyState3D *GodotPhysicsServer3D::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;
+	}
+
+	GodotBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL_V(body, nullptr);
+
+	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.");
+
+	return body->get_direct_state();
+}
+
+/* SOFT BODY */
+
+RID GodotPhysicsServer3D::soft_body_create() {
+	GodotSoftBody3D *soft_body = memnew(GodotSoftBody3D);
+	RID rid = soft_body_owner.make_rid(soft_body);
+	soft_body->set_self(rid);
+	return rid;
+}
+
+void GodotPhysicsServer3D::soft_body_update_rendering_server(RID p_body, RenderingServerHandler *p_rendering_server_handler) {
+	GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body);
+	ERR_FAIL_COND(!soft_body);
+
+	soft_body->update_rendering_server(p_rendering_server_handler);
+}
+
+void GodotPhysicsServer3D::soft_body_set_space(RID p_body, RID p_space) {
+	GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body);
+	ERR_FAIL_COND(!soft_body);
+
+	GodotSpace3D *space = nullptr;
+	if (p_space.is_valid()) {
+		space = space_owner.get_or_null(p_space);
+		ERR_FAIL_COND(!space);
+	}
+
+	if (soft_body->get_space() == space) {
+		return;
+	}
+
+	soft_body->set_space(space);
+}
+
+RID GodotPhysicsServer3D::soft_body_get_space(RID p_body) const {
+	GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body);
+	ERR_FAIL_COND_V(!soft_body, RID());
+
+	GodotSpace3D *space = soft_body->get_space();
+	if (!space) {
+		return RID();
+	}
+	return space->get_self();
+}
+
+void GodotPhysicsServer3D::soft_body_set_collision_layer(RID p_body, uint32_t p_layer) {
+	GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body);
+	ERR_FAIL_COND(!soft_body);
+
+	soft_body->set_collision_layer(p_layer);
+}
+
+uint32_t GodotPhysicsServer3D::soft_body_get_collision_layer(RID p_body) const {
+	GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body);
+	ERR_FAIL_COND_V(!soft_body, 0);
+
+	return soft_body->get_collision_layer();
+}
+
+void GodotPhysicsServer3D::soft_body_set_collision_mask(RID p_body, uint32_t p_mask) {
+	GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body);
+	ERR_FAIL_COND(!soft_body);
+
+	soft_body->set_collision_mask(p_mask);
+}
+
+uint32_t GodotPhysicsServer3D::soft_body_get_collision_mask(RID p_body) const {
+	GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body);
+	ERR_FAIL_COND_V(!soft_body, 0);
+
+	return soft_body->get_collision_mask();
+}
+
+void GodotPhysicsServer3D::soft_body_add_collision_exception(RID p_body, RID p_body_b) {
+	GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body);
+	ERR_FAIL_COND(!soft_body);
+
+	soft_body->add_exception(p_body_b);
+}
+
+void GodotPhysicsServer3D::soft_body_remove_collision_exception(RID p_body, RID p_body_b) {
+	GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body);
+	ERR_FAIL_COND(!soft_body);
+
+	soft_body->remove_exception(p_body_b);
+}
+
+void GodotPhysicsServer3D::soft_body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions) {
+	GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body);
+	ERR_FAIL_COND(!soft_body);
+
+	for (int i = 0; i < soft_body->get_exceptions().size(); i++) {
+		p_exceptions->push_back(soft_body->get_exceptions()[i]);
+	}
+}
+
+void GodotPhysicsServer3D::soft_body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) {
+	GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body);
+	ERR_FAIL_COND(!soft_body);
+
+	soft_body->set_state(p_state, p_variant);
+}
+
+Variant GodotPhysicsServer3D::soft_body_get_state(RID p_body, BodyState p_state) const {
+	GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body);
+	ERR_FAIL_COND_V(!soft_body, Variant());
+
+	return soft_body->get_state(p_state);
+}
+
+void GodotPhysicsServer3D::soft_body_set_transform(RID p_body, const Transform3D &p_transform) {
+	GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body);
+	ERR_FAIL_COND(!soft_body);
+
+	soft_body->set_state(BODY_STATE_TRANSFORM, p_transform);
+}
+
+void GodotPhysicsServer3D::soft_body_set_ray_pickable(RID p_body, bool p_enable) {
+	GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body);
+	ERR_FAIL_COND(!soft_body);
+
+	soft_body->set_ray_pickable(p_enable);
+}
+
+void GodotPhysicsServer3D::soft_body_set_simulation_precision(RID p_body, int p_simulation_precision) {
+	GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body);
+	ERR_FAIL_COND(!soft_body);
+
+	soft_body->set_iteration_count(p_simulation_precision);
+}
+
+int GodotPhysicsServer3D::soft_body_get_simulation_precision(RID p_body) const {
+	GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body);
+	ERR_FAIL_COND_V(!soft_body, 0.f);
+
+	return soft_body->get_iteration_count();
+}
+
+void GodotPhysicsServer3D::soft_body_set_total_mass(RID p_body, real_t p_total_mass) {
+	GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body);
+	ERR_FAIL_COND(!soft_body);
+
+	soft_body->set_total_mass(p_total_mass);
+}
+
+real_t GodotPhysicsServer3D::soft_body_get_total_mass(RID p_body) const {
+	GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body);
+	ERR_FAIL_COND_V(!soft_body, 0.f);
+
+	return soft_body->get_total_mass();
+}
+
+void GodotPhysicsServer3D::soft_body_set_linear_stiffness(RID p_body, real_t p_stiffness) {
+	GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body);
+	ERR_FAIL_COND(!soft_body);
+
+	soft_body->set_linear_stiffness(p_stiffness);
+}
+
+real_t GodotPhysicsServer3D::soft_body_get_linear_stiffness(RID p_body) const {
+	GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body);
+	ERR_FAIL_COND_V(!soft_body, 0.f);
+
+	return soft_body->get_linear_stiffness();
+}
+
+void GodotPhysicsServer3D::soft_body_set_pressure_coefficient(RID p_body, real_t p_pressure_coefficient) {
+	GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body);
+	ERR_FAIL_COND(!soft_body);
+
+	soft_body->set_pressure_coefficient(p_pressure_coefficient);
+}
+
+real_t GodotPhysicsServer3D::soft_body_get_pressure_coefficient(RID p_body) const {
+	GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body);
+	ERR_FAIL_COND_V(!soft_body, 0.f);
+
+	return soft_body->get_pressure_coefficient();
+}
+
+void GodotPhysicsServer3D::soft_body_set_damping_coefficient(RID p_body, real_t p_damping_coefficient) {
+	GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body);
+	ERR_FAIL_COND(!soft_body);
+
+	soft_body->set_damping_coefficient(p_damping_coefficient);
+}
+
+real_t GodotPhysicsServer3D::soft_body_get_damping_coefficient(RID p_body) const {
+	GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body);
+	ERR_FAIL_COND_V(!soft_body, 0.f);
+
+	return soft_body->get_damping_coefficient();
+}
+
+void GodotPhysicsServer3D::soft_body_set_drag_coefficient(RID p_body, real_t p_drag_coefficient) {
+	GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body);
+	ERR_FAIL_COND(!soft_body);
+
+	soft_body->set_drag_coefficient(p_drag_coefficient);
+}
+
+real_t GodotPhysicsServer3D::soft_body_get_drag_coefficient(RID p_body) const {
+	GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body);
+	ERR_FAIL_COND_V(!soft_body, 0.f);
+
+	return soft_body->get_drag_coefficient();
+}
+
+void GodotPhysicsServer3D::soft_body_set_mesh(RID p_body, RID p_mesh) {
+	GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body);
+	ERR_FAIL_COND(!soft_body);
+
+	soft_body->set_mesh(p_mesh);
+}
+
+AABB GodotPhysicsServer3D::soft_body_get_bounds(RID p_body) const {
+	GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body);
+	ERR_FAIL_COND_V(!soft_body, AABB());
+
+	return soft_body->get_bounds();
+}
+
+void GodotPhysicsServer3D::soft_body_move_point(RID p_body, int p_point_index, const Vector3 &p_global_position) {
+	GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body);
+	ERR_FAIL_COND(!soft_body);
+
+	soft_body->set_vertex_position(p_point_index, p_global_position);
+}
+
+Vector3 GodotPhysicsServer3D::soft_body_get_point_global_position(RID p_body, int p_point_index) const {
+	GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body);
+	ERR_FAIL_COND_V(!soft_body, Vector3());
+
+	return soft_body->get_vertex_position(p_point_index);
+}
+
+void GodotPhysicsServer3D::soft_body_remove_all_pinned_points(RID p_body) {
+	GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body);
+	ERR_FAIL_COND(!soft_body);
+
+	soft_body->unpin_all_vertices();
+}
+
+void GodotPhysicsServer3D::soft_body_pin_point(RID p_body, int p_point_index, bool p_pin) {
+	GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body);
+	ERR_FAIL_COND(!soft_body);
+
+	if (p_pin) {
+		soft_body->pin_vertex(p_point_index);
+	} else {
+		soft_body->unpin_vertex(p_point_index);
+	}
+}
+
+bool GodotPhysicsServer3D::soft_body_is_point_pinned(RID p_body, int p_point_index) const {
+	GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body);
+	ERR_FAIL_COND_V(!soft_body, false);
+
+	return soft_body->is_vertex_pinned(p_point_index);
+}
+
+/* JOINT API */
+
+RID GodotPhysicsServer3D::joint_create() {
+	GodotJoint3D *joint = memnew(GodotJoint3D);
+	RID rid = joint_owner.make_rid(joint);
+	joint->set_self(rid);
+	return rid;
+}
+
+void GodotPhysicsServer3D::joint_clear(RID p_joint) {
+	GodotJoint3D *joint = joint_owner.get_or_null(p_joint);
+	if (joint->get_type() != JOINT_TYPE_MAX) {
+		GodotJoint3D *empty_joint = memnew(GodotJoint3D);
+		empty_joint->copy_settings_from(joint);
+
+		joint_owner.replace(p_joint, empty_joint);
+		memdelete(joint);
+	}
+}
+
+void GodotPhysicsServer3D::joint_make_pin(RID p_joint, RID p_body_A, const Vector3 &p_local_A, RID p_body_B, const Vector3 &p_local_B) {
+	GodotBody3D *body_A = body_owner.get_or_null(p_body_A);
+	ERR_FAIL_COND(!body_A);
+
+	if (!p_body_B.is_valid()) {
+		ERR_FAIL_COND(!body_A->get_space());
+		p_body_B = body_A->get_space()->get_static_global_body();
+	}
+
+	GodotBody3D *body_B = body_owner.get_or_null(p_body_B);
+	ERR_FAIL_COND(!body_B);
+
+	ERR_FAIL_COND(body_A == body_B);
+
+	GodotJoint3D *prev_joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_COND(prev_joint == nullptr);
+
+	GodotJoint3D *joint = memnew(GodotPinJoint3D(body_A, p_local_A, body_B, p_local_B));
+
+	joint->copy_settings_from(prev_joint);
+	joint_owner.replace(p_joint, joint);
+	memdelete(prev_joint);
+}
+
+void GodotPhysicsServer3D::pin_joint_set_param(RID p_joint, PinJointParam p_param, real_t p_value) {
+	GodotJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_COND(!joint);
+	ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_PIN);
+	GodotPinJoint3D *pin_joint = static_cast<GodotPinJoint3D *>(joint);
+	pin_joint->set_param(p_param, p_value);
+}
+
+real_t GodotPhysicsServer3D::pin_joint_get_param(RID p_joint, PinJointParam p_param) const {
+	GodotJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_COND_V(!joint, 0);
+	ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_PIN, 0);
+	GodotPinJoint3D *pin_joint = static_cast<GodotPinJoint3D *>(joint);
+	return pin_joint->get_param(p_param);
+}
+
+void GodotPhysicsServer3D::pin_joint_set_local_a(RID p_joint, const Vector3 &p_A) {
+	GodotJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_COND(!joint);
+	ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_PIN);
+	GodotPinJoint3D *pin_joint = static_cast<GodotPinJoint3D *>(joint);
+	pin_joint->set_pos_a(p_A);
+}
+
+Vector3 GodotPhysicsServer3D::pin_joint_get_local_a(RID p_joint) const {
+	GodotJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_COND_V(!joint, Vector3());
+	ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_PIN, Vector3());
+	GodotPinJoint3D *pin_joint = static_cast<GodotPinJoint3D *>(joint);
+	return pin_joint->get_position_a();
+}
+
+void GodotPhysicsServer3D::pin_joint_set_local_b(RID p_joint, const Vector3 &p_B) {
+	GodotJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_COND(!joint);
+	ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_PIN);
+	GodotPinJoint3D *pin_joint = static_cast<GodotPinJoint3D *>(joint);
+	pin_joint->set_pos_b(p_B);
+}
+
+Vector3 GodotPhysicsServer3D::pin_joint_get_local_b(RID p_joint) const {
+	GodotJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_COND_V(!joint, Vector3());
+	ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_PIN, Vector3());
+	GodotPinJoint3D *pin_joint = static_cast<GodotPinJoint3D *>(joint);
+	return pin_joint->get_position_b();
+}
+
+void GodotPhysicsServer3D::joint_make_hinge(RID p_joint, RID p_body_A, const Transform3D &p_frame_A, RID p_body_B, const Transform3D &p_frame_B) {
+	GodotBody3D *body_A = body_owner.get_or_null(p_body_A);
+	ERR_FAIL_COND(!body_A);
+
+	if (!p_body_B.is_valid()) {
+		ERR_FAIL_COND(!body_A->get_space());
+		p_body_B = body_A->get_space()->get_static_global_body();
+	}
+
+	GodotBody3D *body_B = body_owner.get_or_null(p_body_B);
+	ERR_FAIL_COND(!body_B);
+
+	ERR_FAIL_COND(body_A == body_B);
+
+	GodotJoint3D *prev_joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_COND(prev_joint == nullptr);
+
+	GodotJoint3D *joint = memnew(GodotHingeJoint3D(body_A, body_B, p_frame_A, p_frame_B));
+
+	joint->copy_settings_from(prev_joint);
+	joint_owner.replace(p_joint, joint);
+	memdelete(prev_joint);
+}
+
+void GodotPhysicsServer3D::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) {
+	GodotBody3D *body_A = body_owner.get_or_null(p_body_A);
+	ERR_FAIL_COND(!body_A);
+
+	if (!p_body_B.is_valid()) {
+		ERR_FAIL_COND(!body_A->get_space());
+		p_body_B = body_A->get_space()->get_static_global_body();
+	}
+
+	GodotBody3D *body_B = body_owner.get_or_null(p_body_B);
+	ERR_FAIL_COND(!body_B);
+
+	ERR_FAIL_COND(body_A == body_B);
+
+	GodotJoint3D *prev_joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_COND(prev_joint == nullptr);
+
+	GodotJoint3D *joint = memnew(GodotHingeJoint3D(body_A, body_B, p_pivot_A, p_pivot_B, p_axis_A, p_axis_B));
+
+	joint->copy_settings_from(prev_joint);
+	joint_owner.replace(p_joint, joint);
+	memdelete(prev_joint);
+}
+
+void GodotPhysicsServer3D::hinge_joint_set_param(RID p_joint, HingeJointParam p_param, real_t p_value) {
+	GodotJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_COND(!joint);
+	ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_HINGE);
+	GodotHingeJoint3D *hinge_joint = static_cast<GodotHingeJoint3D *>(joint);
+	hinge_joint->set_param(p_param, p_value);
+}
+
+real_t GodotPhysicsServer3D::hinge_joint_get_param(RID p_joint, HingeJointParam p_param) const {
+	GodotJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_COND_V(!joint, 0);
+	ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_HINGE, 0);
+	GodotHingeJoint3D *hinge_joint = static_cast<GodotHingeJoint3D *>(joint);
+	return hinge_joint->get_param(p_param);
+}
+
+void GodotPhysicsServer3D::hinge_joint_set_flag(RID p_joint, HingeJointFlag p_flag, bool p_value) {
+	GodotJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_COND(!joint);
+	ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_HINGE);
+	GodotHingeJoint3D *hinge_joint = static_cast<GodotHingeJoint3D *>(joint);
+	hinge_joint->set_flag(p_flag, p_value);
+}
+
+bool GodotPhysicsServer3D::hinge_joint_get_flag(RID p_joint, HingeJointFlag p_flag) const {
+	GodotJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_COND_V(!joint, false);
+	ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_HINGE, false);
+	GodotHingeJoint3D *hinge_joint = static_cast<GodotHingeJoint3D *>(joint);
+	return hinge_joint->get_flag(p_flag);
+}
+
+void GodotPhysicsServer3D::joint_set_solver_priority(RID p_joint, int p_priority) {
+	GodotJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_COND(!joint);
+	joint->set_priority(p_priority);
+}
+
+int GodotPhysicsServer3D::joint_get_solver_priority(RID p_joint) const {
+	GodotJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_COND_V(!joint, 0);
+	return joint->get_priority();
+}
+
+void GodotPhysicsServer3D::joint_disable_collisions_between_bodies(RID p_joint, const bool p_disable) {
+	GodotJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_COND(!joint);
+
+	joint->disable_collisions_between_bodies(p_disable);
+
+	if (2 == joint->get_body_count()) {
+		GodotBody3D *body_a = *joint->get_body_ptr();
+		GodotBody3D *body_b = *(joint->get_body_ptr() + 1);
+
+		if (p_disable) {
+			body_add_collision_exception(body_a->get_self(), body_b->get_self());
+			body_add_collision_exception(body_b->get_self(), body_a->get_self());
+		} else {
+			body_remove_collision_exception(body_a->get_self(), body_b->get_self());
+			body_remove_collision_exception(body_b->get_self(), body_a->get_self());
+		}
+	}
+}
+
+bool GodotPhysicsServer3D::joint_is_disabled_collisions_between_bodies(RID p_joint) const {
+	GodotJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_COND_V(!joint, true);
+
+	return joint->is_disabled_collisions_between_bodies();
+}
+
+GodotPhysicsServer3D::JointType GodotPhysicsServer3D::joint_get_type(RID p_joint) const {
+	GodotJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_COND_V(!joint, JOINT_TYPE_PIN);
+	return joint->get_type();
+}
+
+void GodotPhysicsServer3D::joint_make_slider(RID p_joint, RID p_body_A, const Transform3D &p_local_frame_A, RID p_body_B, const Transform3D &p_local_frame_B) {
+	GodotBody3D *body_A = body_owner.get_or_null(p_body_A);
+	ERR_FAIL_COND(!body_A);
+
+	if (!p_body_B.is_valid()) {
+		ERR_FAIL_COND(!body_A->get_space());
+		p_body_B = body_A->get_space()->get_static_global_body();
+	}
+
+	GodotBody3D *body_B = body_owner.get_or_null(p_body_B);
+	ERR_FAIL_COND(!body_B);
+
+	ERR_FAIL_COND(body_A == body_B);
+
+	GodotJoint3D *prev_joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_COND(prev_joint == nullptr);
+
+	GodotJoint3D *joint = memnew(GodotSliderJoint3D(body_A, body_B, p_local_frame_A, p_local_frame_B));
+
+	joint->copy_settings_from(prev_joint);
+	joint_owner.replace(p_joint, joint);
+	memdelete(prev_joint);
+}
+
+void GodotPhysicsServer3D::slider_joint_set_param(RID p_joint, SliderJointParam p_param, real_t p_value) {
+	GodotJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_COND(!joint);
+	ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_SLIDER);
+	GodotSliderJoint3D *slider_joint = static_cast<GodotSliderJoint3D *>(joint);
+	slider_joint->set_param(p_param, p_value);
+}
+
+real_t GodotPhysicsServer3D::slider_joint_get_param(RID p_joint, SliderJointParam p_param) const {
+	GodotJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_COND_V(!joint, 0);
+	ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_CONE_TWIST, 0);
+	GodotSliderJoint3D *slider_joint = static_cast<GodotSliderJoint3D *>(joint);
+	return slider_joint->get_param(p_param);
+}
+
+void GodotPhysicsServer3D::joint_make_cone_twist(RID p_joint, RID p_body_A, const Transform3D &p_local_frame_A, RID p_body_B, const Transform3D &p_local_frame_B) {
+	GodotBody3D *body_A = body_owner.get_or_null(p_body_A);
+	ERR_FAIL_COND(!body_A);
+
+	if (!p_body_B.is_valid()) {
+		ERR_FAIL_COND(!body_A->get_space());
+		p_body_B = body_A->get_space()->get_static_global_body();
+	}
+
+	GodotBody3D *body_B = body_owner.get_or_null(p_body_B);
+	ERR_FAIL_COND(!body_B);
+
+	ERR_FAIL_COND(body_A == body_B);
+
+	GodotJoint3D *prev_joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_COND(prev_joint == nullptr);
+
+	GodotJoint3D *joint = memnew(GodotConeTwistJoint3D(body_A, body_B, p_local_frame_A, p_local_frame_B));
+
+	joint->copy_settings_from(prev_joint);
+	joint_owner.replace(p_joint, joint);
+	memdelete(prev_joint);
+}
+
+void GodotPhysicsServer3D::cone_twist_joint_set_param(RID p_joint, ConeTwistJointParam p_param, real_t p_value) {
+	GodotJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_COND(!joint);
+	ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_CONE_TWIST);
+	GodotConeTwistJoint3D *cone_twist_joint = static_cast<GodotConeTwistJoint3D *>(joint);
+	cone_twist_joint->set_param(p_param, p_value);
+}
+
+real_t GodotPhysicsServer3D::cone_twist_joint_get_param(RID p_joint, ConeTwistJointParam p_param) const {
+	GodotJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_COND_V(!joint, 0);
+	ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_CONE_TWIST, 0);
+	GodotConeTwistJoint3D *cone_twist_joint = static_cast<GodotConeTwistJoint3D *>(joint);
+	return cone_twist_joint->get_param(p_param);
+}
+
+void GodotPhysicsServer3D::joint_make_generic_6dof(RID p_joint, RID p_body_A, const Transform3D &p_local_frame_A, RID p_body_B, const Transform3D &p_local_frame_B) {
+	GodotBody3D *body_A = body_owner.get_or_null(p_body_A);
+	ERR_FAIL_COND(!body_A);
+
+	if (!p_body_B.is_valid()) {
+		ERR_FAIL_COND(!body_A->get_space());
+		p_body_B = body_A->get_space()->get_static_global_body();
+	}
+
+	GodotBody3D *body_B = body_owner.get_or_null(p_body_B);
+	ERR_FAIL_COND(!body_B);
+
+	ERR_FAIL_COND(body_A == body_B);
+
+	GodotJoint3D *prev_joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_COND(prev_joint == nullptr);
+
+	GodotJoint3D *joint = memnew(GodotGeneric6DOFJoint3D(body_A, body_B, p_local_frame_A, p_local_frame_B, true));
+
+	joint->copy_settings_from(prev_joint);
+	joint_owner.replace(p_joint, joint);
+	memdelete(prev_joint);
+}
+
+void GodotPhysicsServer3D::generic_6dof_joint_set_param(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisParam p_param, real_t p_value) {
+	GodotJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_COND(!joint);
+	ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_6DOF);
+	GodotGeneric6DOFJoint3D *generic_6dof_joint = static_cast<GodotGeneric6DOFJoint3D *>(joint);
+	generic_6dof_joint->set_param(p_axis, p_param, p_value);
+}
+
+real_t GodotPhysicsServer3D::generic_6dof_joint_get_param(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisParam p_param) const {
+	GodotJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_COND_V(!joint, 0);
+	ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_6DOF, 0);
+	GodotGeneric6DOFJoint3D *generic_6dof_joint = static_cast<GodotGeneric6DOFJoint3D *>(joint);
+	return generic_6dof_joint->get_param(p_axis, p_param);
+}
+
+void GodotPhysicsServer3D::generic_6dof_joint_set_flag(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisFlag p_flag, bool p_enable) {
+	GodotJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_COND(!joint);
+	ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_6DOF);
+	GodotGeneric6DOFJoint3D *generic_6dof_joint = static_cast<GodotGeneric6DOFJoint3D *>(joint);
+	generic_6dof_joint->set_flag(p_axis, p_flag, p_enable);
+}
+
+bool GodotPhysicsServer3D::generic_6dof_joint_get_flag(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisFlag p_flag) const {
+	GodotJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_COND_V(!joint, false);
+	ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_6DOF, false);
+	GodotGeneric6DOFJoint3D *generic_6dof_joint = static_cast<GodotGeneric6DOFJoint3D *>(joint);
+	return generic_6dof_joint->get_flag(p_axis, p_flag);
+}
+
+void GodotPhysicsServer3D::free(RID p_rid) {
+	_update_shapes(); //just in case
+
+	if (shape_owner.owns(p_rid)) {
+		GodotShape3D *shape = shape_owner.get_or_null(p_rid);
+
+		while (shape->get_owners().size()) {
+			GodotShapeOwner3D *so = shape->get_owners().front()->key();
+			so->remove_shape(shape);
+		}
+
+		shape_owner.free(p_rid);
+		memdelete(shape);
+	} else if (body_owner.owns(p_rid)) {
+		GodotBody3D *body = body_owner.get_or_null(p_rid);
+
+		/*
+		if (body->get_state_query())
+			_clear_query(body->get_state_query());
+
+		if (body->get_direct_state_query())
+			_clear_query(body->get_direct_state_query());
+		*/
+
+		body->set_space(nullptr);
+
+		while (body->get_shape_count()) {
+			body->remove_shape(0);
+		}
+
+		body_owner.free(p_rid);
+		memdelete(body);
+	} else if (soft_body_owner.owns(p_rid)) {
+		GodotSoftBody3D *soft_body = soft_body_owner.get_or_null(p_rid);
+
+		soft_body->set_space(nullptr);
+
+		soft_body_owner.free(p_rid);
+		memdelete(soft_body);
+	} else if (area_owner.owns(p_rid)) {
+		GodotArea3D *area = area_owner.get_or_null(p_rid);
+
+		/*
+		if (area->get_monitor_query())
+			_clear_query(area->get_monitor_query());
+		*/
+
+		area->set_space(nullptr);
+
+		while (area->get_shape_count()) {
+			area->remove_shape(0);
+		}
+
+		area_owner.free(p_rid);
+		memdelete(area);
+	} else if (space_owner.owns(p_rid)) {
+		GodotSpace3D *space = space_owner.get_or_null(p_rid);
+
+		while (space->get_objects().size()) {
+			GodotCollisionObject3D *co = (GodotCollisionObject3D *)space->get_objects().front()->get();
+			co->set_space(nullptr);
+		}
+
+		active_spaces.erase(space);
+		free(space->get_default_area()->get_self());
+		free(space->get_static_global_body());
+
+		space_owner.free(p_rid);
+		memdelete(space);
+	} else if (joint_owner.owns(p_rid)) {
+		GodotJoint3D *joint = joint_owner.get_or_null(p_rid);
+
+		joint_owner.free(p_rid);
+		memdelete(joint);
+
+	} else {
+		ERR_FAIL_MSG("Invalid ID.");
+	}
+};
+
+void GodotPhysicsServer3D::set_active(bool p_active) {
+	active = p_active;
+};
+
+void GodotPhysicsServer3D::set_collision_iterations(int p_iterations) {
+	iterations = p_iterations;
+};
+
+void GodotPhysicsServer3D::init() {
+	iterations = 8; // 8?
+	stepper = memnew(GodotStep3D);
+};
+
+void GodotPhysicsServer3D::step(real_t p_step) {
+#ifndef _3D_DISABLED
+
+	if (!active) {
+		return;
+	}
+
+	_update_shapes();
+
+	island_count = 0;
+	active_objects = 0;
+	collision_pairs = 0;
+	for (Set<const GodotSpace3D *>::Element *E = active_spaces.front(); E; E = E->next()) {
+		stepper->step((GodotSpace3D *)E->get(), p_step, iterations);
+		island_count += E->get()->get_island_count();
+		active_objects += E->get()->get_active_objects();
+		collision_pairs += E->get()->get_collision_pairs();
+	}
+#endif
+}
+
+void GodotPhysicsServer3D::sync() {
+	doing_sync = true;
+};
+
+void GodotPhysicsServer3D::flush_queries() {
+#ifndef _3D_DISABLED
+
+	if (!active) {
+		return;
+	}
+
+	flushing_queries = true;
+
+	uint64_t time_beg = OS::get_singleton()->get_ticks_usec();
+
+	for (Set<const GodotSpace3D *>::Element *E = active_spaces.front(); E; E = E->next()) {
+		GodotSpace3D *space = (GodotSpace3D *)E->get();
+		space->call_queries();
+	}
+
+	flushing_queries = false;
+
+	if (EngineDebugger::is_profiling("servers")) {
+		uint64_t total_time[GodotSpace3D::ELAPSED_TIME_MAX];
+		static const char *time_name[GodotSpace3D::ELAPSED_TIME_MAX] = {
+			"integrate_forces",
+			"generate_islands",
+			"setup_constraints",
+			"solve_constraints",
+			"integrate_velocities"
+		};
+
+		for (int i = 0; i < GodotSpace3D::ELAPSED_TIME_MAX; i++) {
+			total_time[i] = 0;
+		}
+
+		for (Set<const GodotSpace3D *>::Element *E = active_spaces.front(); E; E = E->next()) {
+			for (int i = 0; i < GodotSpace3D::ELAPSED_TIME_MAX; i++) {
+				total_time[i] += E->get()->get_elapsed_time(GodotSpace3D::ElapsedTime(i));
+			}
+		}
+
+		Array values;
+		values.resize(GodotSpace3D::ELAPSED_TIME_MAX * 2);
+		for (int i = 0; i < GodotSpace3D::ELAPSED_TIME_MAX; i++) {
+			values[i * 2 + 0] = time_name[i];
+			values[i * 2 + 1] = USEC_TO_SEC(total_time[i]);
+		}
+		values.push_back("flush_queries");
+		values.push_back(USEC_TO_SEC(OS::get_singleton()->get_ticks_usec() - time_beg));
+
+		values.push_front("physics");
+		EngineDebugger::profiler_add_frame_data("servers", values);
+	}
+#endif
+};
+
+void GodotPhysicsServer3D::end_sync() {
+	doing_sync = false;
+};
+
+void GodotPhysicsServer3D::finish() {
+	memdelete(stepper);
+};
+
+int GodotPhysicsServer3D::get_process_info(ProcessInfo p_info) {
+	switch (p_info) {
+		case INFO_ACTIVE_OBJECTS: {
+			return active_objects;
+		} break;
+		case INFO_COLLISION_PAIRS: {
+			return collision_pairs;
+		} break;
+		case INFO_ISLAND_COUNT: {
+			return island_count;
+		} break;
+	}
+
+	return 0;
+}
+
+void GodotPhysicsServer3D::_update_shapes() {
+	while (pending_shape_update_list.first()) {
+		pending_shape_update_list.first()->self()->_shape_changed();
+		pending_shape_update_list.remove(pending_shape_update_list.first());
+	}
+}
+
+void GodotPhysicsServer3D::_shape_col_cbk(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata) {
+	CollCbkData *cbk = (CollCbkData *)p_userdata;
+
+	if (cbk->max == 0) {
+		return;
+	}
+
+	if (cbk->amount == cbk->max) {
+		//find least deep
+		real_t min_depth = 1e20;
+		int min_depth_idx = 0;
+		for (int i = 0; i < cbk->amount; i++) {
+			real_t d = cbk->ptr[i * 2 + 0].distance_squared_to(cbk->ptr[i * 2 + 1]);
+			if (d < min_depth) {
+				min_depth = d;
+				min_depth_idx = i;
+			}
+		}
+
+		real_t d = p_point_A.distance_squared_to(p_point_B);
+		if (d < min_depth) {
+			return;
+		}
+		cbk->ptr[min_depth_idx * 2 + 0] = p_point_A;
+		cbk->ptr[min_depth_idx * 2 + 1] = p_point_B;
+
+	} else {
+		cbk->ptr[cbk->amount * 2 + 0] = p_point_A;
+		cbk->ptr[cbk->amount * 2 + 1] = p_point_B;
+		cbk->amount++;
+	}
+}
+
+GodotPhysicsServer3D *GodotPhysicsServer3D::godot_singleton = nullptr;
+GodotPhysicsServer3D::GodotPhysicsServer3D(bool p_using_threads) {
+	godot_singleton = this;
+	GodotBroadPhase3D::create_func = GodotBroadPhase3DBVH::_create;
+
+	using_threads = p_using_threads;
+};
diff --git a/servers/physics_3d/physics_server_3d_sw.h b/servers/physics_3d/godot_physics_server_3d.h
index f283f83112..f5c8e0f60d 100644
--- a/servers/physics_3d/physics_server_3d_sw.h
+++ b/servers/physics_3d/godot_physics_server_3d.h
@@ -1,5 +1,5 @@
 /*************************************************************************/
-/*  physics_server_3d_sw.h                                               */
+/*  godot_physics_server_3d.h                                            */
 /*************************************************************************/
 /*                       This file is part of:                           */
 /*                           GODOT ENGINE                                */
@@ -28,47 +28,48 @@
 /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
 /*************************************************************************/
 
-#ifndef PHYSICS_SERVER_SW
-#define PHYSICS_SERVER_SW
+#ifndef GODOT_PHYSICS_SERVER_3D_H
+#define GODOT_PHYSICS_SERVER_3D_H
+
+#include "godot_joint_3d.h"
+#include "godot_shape_3d.h"
+#include "godot_space_3d.h"
+#include "godot_step_3d.h"
 
 #include "core/templates/rid_owner.h"
-#include "joints_3d_sw.h"
 #include "servers/physics_server_3d.h"
-#include "shape_3d_sw.h"
-#include "space_3d_sw.h"
-#include "step_3d_sw.h"
 
-class PhysicsServer3DSW : public PhysicsServer3D {
-	GDCLASS(PhysicsServer3DSW, PhysicsServer3D);
+class GodotPhysicsServer3D : public PhysicsServer3D {
+	GDCLASS(GodotPhysicsServer3D, PhysicsServer3D);
 
-	friend class PhysicsDirectSpaceState3DSW;
-	bool active;
-	int iterations;
+	friend class GodotPhysicsDirectSpaceState3D;
+	bool active = true;
+	int iterations = 0;
 
-	int island_count;
-	int active_objects;
-	int collision_pairs;
+	int island_count = 0;
+	int active_objects = 0;
+	int collision_pairs = 0;
 
-	bool using_threads;
-	bool doing_sync;
-	bool flushing_queries;
+	bool using_threads = false;
+	bool doing_sync = false;
+	bool flushing_queries = false;
 
-	Step3DSW *stepper;
-	Set<const Space3DSW *> active_spaces;
+	GodotStep3D *stepper = nullptr;
+	Set<const GodotSpace3D *> active_spaces;
 
-	mutable RID_PtrOwner<Shape3DSW, true> shape_owner;
-	mutable RID_PtrOwner<Space3DSW, true> space_owner;
-	mutable RID_PtrOwner<Area3DSW, true> area_owner;
-	mutable RID_PtrOwner<Body3DSW, true> body_owner;
-	mutable RID_PtrOwner<SoftBody3DSW, true> soft_body_owner;
-	mutable RID_PtrOwner<Joint3DSW, true> joint_owner;
+	mutable RID_PtrOwner<GodotShape3D, true> shape_owner;
+	mutable RID_PtrOwner<GodotSpace3D, true> space_owner;
+	mutable RID_PtrOwner<GodotArea3D, true> area_owner;
+	mutable RID_PtrOwner<GodotBody3D, true> body_owner;
+	mutable RID_PtrOwner<GodotSoftBody3D, true> soft_body_owner;
+	mutable RID_PtrOwner<GodotJoint3D, true> joint_owner;
 
 	//void _clear_query(QuerySW *p_query);
-	friend class CollisionObject3DSW;
-	SelfList<CollisionObject3DSW>::List pending_shape_update_list;
+	friend class GodotCollisionObject3D;
+	SelfList<GodotCollisionObject3D>::List pending_shape_update_list;
 	void _update_shapes();
 
-	static PhysicsServer3DSW *singletonsw;
+	static GodotPhysicsServer3D *godot_singleton;
 
 public:
 	struct CollCbkData {
@@ -79,7 +80,7 @@ public:
 
 	static void _shape_col_cbk(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata);
 
-	virtual RID plane_shape_create() override;
+	virtual RID world_boundary_shape_create() override;
 	virtual RID separation_ray_shape_create() override;
 	virtual RID sphere_shape_create() override;
 	virtual RID box_shape_create() override;
@@ -121,9 +122,6 @@ public:
 
 	virtual RID area_create() override;
 
-	virtual void area_set_space_override_mode(RID p_area, AreaSpaceOverrideMode p_mode) override;
-	virtual AreaSpaceOverrideMode area_get_space_override_mode(RID p_area) const override;
-
 	virtual void area_set_space(RID p_area, RID p_space) override;
 	virtual RID area_get_space(RID p_area) const override;
 
@@ -156,8 +154,8 @@ public:
 
 	virtual void area_set_monitorable(RID p_area, bool p_monitorable) override;
 
-	virtual void area_set_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) override;
-	virtual void area_set_area_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) override;
+	virtual void area_set_monitor_callback(RID p_area, const Callable &p_callback) override;
+	virtual void area_set_area_monitor_callback(RID p_area, const Callable &p_callback) override;
 
 	/* BODY API */
 
@@ -198,8 +196,10 @@ public:
 	virtual void body_set_user_flags(RID p_body, uint32_t p_flags) override;
 	virtual uint32_t body_get_user_flags(RID p_body) const override;
 
-	virtual void body_set_param(RID p_body, BodyParameter p_param, real_t p_value) override;
-	virtual real_t body_get_param(RID p_body, BodyParameter p_param) const override;
+	virtual void body_set_param(RID p_body, BodyParameter p_param, const Variant &p_value) override;
+	virtual Variant body_get_param(RID p_body, BodyParameter p_param) const override;
+
+	virtual void body_reset_mass_properties(RID p_body) override;
 
 	virtual void body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) override;
 	virtual Variant body_get_state(RID p_body, BodyState p_state) const override;
@@ -240,7 +240,7 @@ public:
 
 	virtual void body_set_ray_pickable(RID p_body, bool p_enable) override;
 
-	virtual bool body_test_motion(RID p_body, const Transform3D &p_from, const Vector3 &p_motion, real_t p_margin = 0.001, MotionResult *r_result = nullptr, bool p_collide_separation_ray = false, const Set<RID> &p_exclude = Set<RID>()) override;
+	virtual bool body_test_motion(RID p_body, const MotionParameters &p_parameters, MotionResult *r_result = nullptr) override;
 
 	// this function only works on physics process, errors and returns null otherwise
 	virtual PhysicsDirectBodyState3D *body_get_direct_state(RID p_body) override;
@@ -289,7 +289,7 @@ public:
 	virtual void soft_body_set_drag_coefficient(RID p_body, real_t p_drag_coefficient) override;
 	virtual real_t soft_body_get_drag_coefficient(RID p_body) const override;
 
-	virtual void soft_body_set_mesh(RID p_body, const REF &p_mesh) override;
+	virtual void soft_body_set_mesh(RID p_body, RID p_mesh) override;
 
 	virtual AABB soft_body_get_bounds(RID p_body) const override;
 
@@ -370,8 +370,8 @@ public:
 
 	int get_process_info(ProcessInfo p_info) override;
 
-	PhysicsServer3DSW(bool p_using_threads = false);
-	~PhysicsServer3DSW() {}
+	GodotPhysicsServer3D(bool p_using_threads = false);
+	~GodotPhysicsServer3D() {}
 };
 
-#endif
+#endif // GODOT_PHYSICS_SERVER_3D_H
diff --git a/servers/physics_3d/shape_3d_sw.cpp b/servers/physics_3d/godot_shape_3d.cpp
index b81d3272c3..5364a9833d 100644
--- a/servers/physics_3d/shape_3d_sw.cpp
+++ b/servers/physics_3d/godot_shape_3d.cpp
@@ -1,5 +1,5 @@
 /*************************************************************************/
-/*  shape_3d_sw.cpp                                                      */
+/*  godot_shape_3d.cpp                                                   */
 /*************************************************************************/
 /*                       This file is part of:                           */
 /*                           GODOT ENGINE                                */
@@ -28,18 +28,18 @@
 /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
 /*************************************************************************/
 
-#include "shape_3d_sw.h"
+#include "godot_shape_3d.h"
 
 #include "core/io/image.h"
 #include "core/math/convex_hull.h"
 #include "core/math/geometry_3d.h"
 #include "core/templates/sort_array.h"
 
-// HeightMapShape3DSW is based on Bullet btHeightfieldTerrainShape.
+// GodotHeightMapShape3D is based on Bullet btHeightfieldTerrainShape.
 
 /*
 Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2009 Erwin Coumans  https://bulletphysics.org
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
 
 This software is provided 'as-is', without any express or implied warranty.
 In no event will the authors be held liable for any damages arising from the use of this software.
@@ -58,16 +58,16 @@ subject to the following restrictions:
 #define _CYLINDER_EDGE_IS_VALID_SUPPORT_THRESHOLD 0.002
 #define _CYLINDER_FACE_IS_VALID_SUPPORT_THRESHOLD 0.999
 
-void Shape3DSW::configure(const AABB &p_aabb) {
+void GodotShape3D::configure(const AABB &p_aabb) {
 	aabb = p_aabb;
 	configured = true;
-	for (Map<ShapeOwner3DSW *, int>::Element *E = owners.front(); E; E = E->next()) {
-		ShapeOwner3DSW *co = (ShapeOwner3DSW *)E->key();
+	for (const KeyValue<GodotShapeOwner3D *, int> &E : owners) {
+		GodotShapeOwner3D *co = (GodotShapeOwner3D *)E.key;
 		co->_shape_changed();
 	}
 }
 
-Vector3 Shape3DSW::get_support(const Vector3 &p_normal) const {
+Vector3 GodotShape3D::get_support(const Vector3 &p_normal) const {
 	Vector3 res;
 	int amnt;
 	FeatureType type;
@@ -75,8 +75,8 @@ Vector3 Shape3DSW::get_support(const Vector3 &p_normal) const {
 	return res;
 }
 
-void Shape3DSW::add_owner(ShapeOwner3DSW *p_owner) {
-	Map<ShapeOwner3DSW *, int>::Element *E = owners.find(p_owner);
+void GodotShape3D::add_owner(GodotShapeOwner3D *p_owner) {
+	Map<GodotShapeOwner3D *, int>::Element *E = owners.find(p_owner);
 	if (E) {
 		E->get()++;
 	} else {
@@ -84,8 +84,8 @@ void Shape3DSW::add_owner(ShapeOwner3DSW *p_owner) {
 	}
 }
 
-void Shape3DSW::remove_owner(ShapeOwner3DSW *p_owner) {
-	Map<ShapeOwner3DSW *, int>::Element *E = owners.find(p_owner);
+void GodotShape3D::remove_owner(GodotShapeOwner3D *p_owner) {
+	Map<GodotShapeOwner3D *, int>::Element *E = owners.find(p_owner);
 	ERR_FAIL_COND(!E);
 	E->get()--;
 	if (E->get() == 0) {
@@ -93,38 +93,33 @@ void Shape3DSW::remove_owner(ShapeOwner3DSW *p_owner) {
 	}
 }
 
-bool Shape3DSW::is_owner(ShapeOwner3DSW *p_owner) const {
+bool GodotShape3D::is_owner(GodotShapeOwner3D *p_owner) const {
 	return owners.has(p_owner);
 }
 
-const Map<ShapeOwner3DSW *, int> &Shape3DSW::get_owners() const {
+const Map<GodotShapeOwner3D *, int> &GodotShape3D::get_owners() const {
 	return owners;
 }
 
-Shape3DSW::Shape3DSW() {
-	custom_bias = 0;
-	configured = false;
-}
-
-Shape3DSW::~Shape3DSW() {
+GodotShape3D::~GodotShape3D() {
 	ERR_FAIL_COND(owners.size());
 }
 
-Plane PlaneShape3DSW::get_plane() const {
+Plane GodotWorldBoundaryShape3D::get_plane() const {
 	return plane;
 }
 
-void PlaneShape3DSW::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const {
+void GodotWorldBoundaryShape3D::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const {
 	// gibberish, a plane is infinity
 	r_min = -1e7;
 	r_max = 1e7;
 }
 
-Vector3 PlaneShape3DSW::get_support(const Vector3 &p_normal) const {
+Vector3 GodotWorldBoundaryShape3D::get_support(const Vector3 &p_normal) const {
 	return p_normal * 1e15;
 }
 
-bool PlaneShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const {
+bool GodotWorldBoundaryShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, bool p_hit_back_faces) const {
 	bool inters = plane.intersects_segment(p_begin, p_end, &r_result);
 	if (inters) {
 		r_normal = plane.normal;
@@ -132,11 +127,11 @@ bool PlaneShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_
 	return inters;
 }
 
-bool PlaneShape3DSW::intersect_point(const Vector3 &p_point) const {
+bool GodotWorldBoundaryShape3D::intersect_point(const Vector3 &p_point) const {
 	return plane.distance_to(p_point) < 0;
 }
 
-Vector3 PlaneShape3DSW::get_closest_point_to(const Vector3 &p_point) const {
+Vector3 GodotWorldBoundaryShape3D::get_closest_point_to(const Vector3 &p_point) const {
 	if (plane.is_point_over(p_point)) {
 		return plane.project(p_point);
 	} else {
@@ -144,43 +139,43 @@ Vector3 PlaneShape3DSW::get_closest_point_to(const Vector3 &p_point) const {
 	}
 }
 
-Vector3 PlaneShape3DSW::get_moment_of_inertia(real_t p_mass) const {
-	return Vector3(); //wtf
+Vector3 GodotWorldBoundaryShape3D::get_moment_of_inertia(real_t p_mass) const {
+	return Vector3(); // not applicable.
 }
 
-void PlaneShape3DSW::_setup(const Plane &p_plane) {
+void GodotWorldBoundaryShape3D::_setup(const Plane &p_plane) {
 	plane = p_plane;
 	configure(AABB(Vector3(-1e4, -1e4, -1e4), Vector3(1e4 * 2, 1e4 * 2, 1e4 * 2)));
 }
 
-void PlaneShape3DSW::set_data(const Variant &p_data) {
+void GodotWorldBoundaryShape3D::set_data(const Variant &p_data) {
 	_setup(p_data);
 }
 
-Variant PlaneShape3DSW::get_data() const {
+Variant GodotWorldBoundaryShape3D::get_data() const {
 	return plane;
 }
 
-PlaneShape3DSW::PlaneShape3DSW() {
+GodotWorldBoundaryShape3D::GodotWorldBoundaryShape3D() {
 }
 
 //
 
-real_t SeparationRayShape3DSW::get_length() const {
+real_t GodotSeparationRayShape3D::get_length() const {
 	return length;
 }
 
-bool SeparationRayShape3DSW::get_slide_on_slope() const {
+bool GodotSeparationRayShape3D::get_slide_on_slope() const {
 	return slide_on_slope;
 }
 
-void SeparationRayShape3DSW::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const {
+void GodotSeparationRayShape3D::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const {
 	// don't think this will be even used
 	r_min = 0;
 	r_max = 1;
 }
 
-Vector3 SeparationRayShape3DSW::get_support(const Vector3 &p_normal) const {
+Vector3 GodotSeparationRayShape3D::get_support(const Vector3 &p_normal) const {
 	if (p_normal.z > 0) {
 		return Vector3(0, 0, length);
 	} else {
@@ -188,7 +183,7 @@ Vector3 SeparationRayShape3DSW::get_support(const Vector3 &p_normal) const {
 	}
 }
 
-void SeparationRayShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const {
+void GodotSeparationRayShape3D::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const {
 	if (Math::abs(p_normal.z) < _EDGE_IS_VALID_SUPPORT_THRESHOLD) {
 		r_amount = 2;
 		r_type = FEATURE_EDGE;
@@ -205,15 +200,15 @@ void SeparationRayShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Ve
 	}
 }
 
-bool SeparationRayShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const {
+bool GodotSeparationRayShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, bool p_hit_back_faces) const {
 	return false; //simply not possible
 }
 
-bool SeparationRayShape3DSW::intersect_point(const Vector3 &p_point) const {
+bool GodotSeparationRayShape3D::intersect_point(const Vector3 &p_point) const {
 	return false; //simply not possible
 }
 
-Vector3 SeparationRayShape3DSW::get_closest_point_to(const Vector3 &p_point) const {
+Vector3 GodotSeparationRayShape3D::get_closest_point_to(const Vector3 &p_point) const {
 	Vector3 s[2] = {
 		Vector3(0, 0, 0),
 		Vector3(0, 0, length)
@@ -222,40 +217,37 @@ Vector3 SeparationRayShape3DSW::get_closest_point_to(const Vector3 &p_point) con
 	return Geometry3D::get_closest_point_to_segment(p_point, s);
 }
 
-Vector3 SeparationRayShape3DSW::get_moment_of_inertia(real_t p_mass) const {
+Vector3 GodotSeparationRayShape3D::get_moment_of_inertia(real_t p_mass) const {
 	return Vector3();
 }
 
-void SeparationRayShape3DSW::_setup(real_t p_length, bool p_slide_on_slope) {
+void GodotSeparationRayShape3D::_setup(real_t p_length, bool p_slide_on_slope) {
 	length = p_length;
 	slide_on_slope = p_slide_on_slope;
 	configure(AABB(Vector3(0, 0, 0), Vector3(0.1, 0.1, length)));
 }
 
-void SeparationRayShape3DSW::set_data(const Variant &p_data) {
+void GodotSeparationRayShape3D::set_data(const Variant &p_data) {
 	Dictionary d = p_data;
 	_setup(d["length"], d["slide_on_slope"]);
 }
 
-Variant SeparationRayShape3DSW::get_data() const {
+Variant GodotSeparationRayShape3D::get_data() const {
 	Dictionary d;
 	d["length"] = length;
 	d["slide_on_slope"] = slide_on_slope;
 	return d;
 }
 
-SeparationRayShape3DSW::SeparationRayShape3DSW() {
-	length = 1;
-	slide_on_slope = false;
-}
+GodotSeparationRayShape3D::GodotSeparationRayShape3D() {}
 
 /********** SPHERE *************/
 
-real_t SphereShape3DSW::get_radius() const {
+real_t GodotSphereShape3D::get_radius() const {
 	return radius;
 }
 
-void SphereShape3DSW::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const {
+void GodotSphereShape3D::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const {
 	real_t d = p_normal.dot(p_transform.origin);
 
 	// figure out scale at point
@@ -266,25 +258,25 @@ void SphereShape3DSW::project_range(const Vector3 &p_normal, const Transform3D &
 	r_max = d + (radius)*scale;
 }
 
-Vector3 SphereShape3DSW::get_support(const Vector3 &p_normal) const {
+Vector3 GodotSphereShape3D::get_support(const Vector3 &p_normal) const {
 	return p_normal * radius;
 }
 
-void SphereShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const {
+void GodotSphereShape3D::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const {
 	*r_supports = p_normal * radius;
 	r_amount = 1;
 	r_type = FEATURE_POINT;
 }
 
-bool SphereShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const {
+bool GodotSphereShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, bool p_hit_back_faces) const {
 	return Geometry3D::segment_intersects_sphere(p_begin, p_end, Vector3(), radius, &r_result, &r_normal);
 }
 
-bool SphereShape3DSW::intersect_point(const Vector3 &p_point) const {
+bool GodotSphereShape3D::intersect_point(const Vector3 &p_point) const {
 	return p_point.length() < radius;
 }
 
-Vector3 SphereShape3DSW::get_closest_point_to(const Vector3 &p_point) const {
+Vector3 GodotSphereShape3D::get_closest_point_to(const Vector3 &p_point) const {
 	Vector3 p = p_point;
 	real_t l = p.length();
 	if (l < radius) {
@@ -293,31 +285,29 @@ Vector3 SphereShape3DSW::get_closest_point_to(const Vector3 &p_point) const {
 	return (p / l) * radius;
 }
 
-Vector3 SphereShape3DSW::get_moment_of_inertia(real_t p_mass) const {
+Vector3 GodotSphereShape3D::get_moment_of_inertia(real_t p_mass) const {
 	real_t s = 0.4 * p_mass * radius * radius;
 	return Vector3(s, s, s);
 }
 
-void SphereShape3DSW::_setup(real_t p_radius) {
+void GodotSphereShape3D::_setup(real_t p_radius) {
 	radius = p_radius;
 	configure(AABB(Vector3(-radius, -radius, -radius), Vector3(radius * 2.0, radius * 2.0, radius * 2.0)));
 }
 
-void SphereShape3DSW::set_data(const Variant &p_data) {
+void GodotSphereShape3D::set_data(const Variant &p_data) {
 	_setup(p_data);
 }
 
-Variant SphereShape3DSW::get_data() const {
+Variant GodotSphereShape3D::get_data() const {
 	return radius;
 }
 
-SphereShape3DSW::SphereShape3DSW() {
-	radius = 0;
-}
+GodotSphereShape3D::GodotSphereShape3D() {}
 
 /********** BOX *************/
 
-void BoxShape3DSW::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const {
+void GodotBoxShape3D::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const {
 	// no matter the angle, the box is mirrored anyway
 	Vector3 local_normal = p_transform.basis.xform_inv(p_normal);
 
@@ -328,7 +318,7 @@ void BoxShape3DSW::project_range(const Vector3 &p_normal, const Transform3D &p_t
 	r_max = distance + length;
 }
 
-Vector3 BoxShape3DSW::get_support(const Vector3 &p_normal) const {
+Vector3 GodotBoxShape3D::get_support(const Vector3 &p_normal) const {
 	Vector3 point(
 			(p_normal.x < 0) ? -half_extents.x : half_extents.x,
 			(p_normal.y < 0) ? -half_extents.y : half_extents.y,
@@ -337,7 +327,7 @@ Vector3 BoxShape3DSW::get_support(const Vector3 &p_normal) const {
 	return point;
 }
 
-void BoxShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const {
+void GodotBoxShape3D::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const {
 	static const int next[3] = { 1, 2, 0 };
 	static const int next2[3] = { 2, 0, 1 };
 
@@ -420,17 +410,17 @@ void BoxShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_s
 	r_supports[0] = point;
 }
 
-bool BoxShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const {
+bool GodotBoxShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, bool p_hit_back_faces) const {
 	AABB aabb(-half_extents, half_extents * 2.0);
 
 	return aabb.intersects_segment(p_begin, p_end, &r_result, &r_normal);
 }
 
-bool BoxShape3DSW::intersect_point(const Vector3 &p_point) const {
+bool GodotBoxShape3D::intersect_point(const Vector3 &p_point) const {
 	return (Math::abs(p_point.x) < half_extents.x && Math::abs(p_point.y) < half_extents.y && Math::abs(p_point.z) < half_extents.z);
 }
 
-Vector3 BoxShape3DSW::get_closest_point_to(const Vector3 &p_point) const {
+Vector3 GodotBoxShape3D::get_closest_point_to(const Vector3 &p_point) const {
 	int outside = 0;
 	Vector3 min_point;
 
@@ -440,7 +430,7 @@ Vector3 BoxShape3DSW::get_closest_point_to(const Vector3 &p_point) const {
 			if (outside == 1) {
 				//use plane if only one side matches
 				Vector3 n;
-				n[i] = SGN(p_point[i]);
+				n[i] = SIGN(p_point[i]);
 
 				Plane p(n, half_extents[i]);
 				min_point = p.project(p_point);
@@ -480,7 +470,7 @@ Vector3 BoxShape3DSW::get_closest_point_to(const Vector3 &p_point) const {
 	return min_point;
 }
 
-Vector3 BoxShape3DSW::get_moment_of_inertia(real_t p_mass) const {
+Vector3 GodotBoxShape3D::get_moment_of_inertia(real_t p_mass) const {
 	real_t lx = half_extents.x;
 	real_t ly = half_extents.y;
 	real_t lz = half_extents.z;
@@ -488,26 +478,25 @@ Vector3 BoxShape3DSW::get_moment_of_inertia(real_t p_mass) const {
 	return Vector3((p_mass / 3.0) * (ly * ly + lz * lz), (p_mass / 3.0) * (lx * lx + lz * lz), (p_mass / 3.0) * (lx * lx + ly * ly));
 }
 
-void BoxShape3DSW::_setup(const Vector3 &p_half_extents) {
+void GodotBoxShape3D::_setup(const Vector3 &p_half_extents) {
 	half_extents = p_half_extents.abs();
 
 	configure(AABB(-half_extents, half_extents * 2));
 }
 
-void BoxShape3DSW::set_data(const Variant &p_data) {
+void GodotBoxShape3D::set_data(const Variant &p_data) {
 	_setup(p_data);
 }
 
-Variant BoxShape3DSW::get_data() const {
+Variant GodotBoxShape3D::get_data() const {
 	return half_extents;
 }
 
-BoxShape3DSW::BoxShape3DSW() {
-}
+GodotBoxShape3D::GodotBoxShape3D() {}
 
 /********** CAPSULE *************/
 
-void CapsuleShape3DSW::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const {
+void GodotCapsuleShape3D::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const {
 	Vector3 n = p_transform.basis.xform_inv(p_normal).normalized();
 	real_t h = height * 0.5 - radius;
 
@@ -518,7 +507,7 @@ void CapsuleShape3DSW::project_range(const Vector3 &p_normal, const Transform3D
 	r_min = p_normal.dot(p_transform.xform(-n));
 }
 
-Vector3 CapsuleShape3DSW::get_support(const Vector3 &p_normal) const {
+Vector3 GodotCapsuleShape3D::get_support(const Vector3 &p_normal) const {
 	Vector3 n = p_normal;
 
 	real_t h = height * 0.5 - radius;
@@ -528,7 +517,7 @@ Vector3 CapsuleShape3DSW::get_support(const Vector3 &p_normal) const {
 	return n;
 }
 
-void CapsuleShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const {
+void GodotCapsuleShape3D::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const {
 	Vector3 n = p_normal;
 
 	real_t d = n.y;
@@ -557,7 +546,7 @@ void CapsuleShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3
 	}
 }
 
-bool CapsuleShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const {
+bool GodotCapsuleShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, bool p_hit_back_faces) const {
 	Vector3 norm = (p_end - p_begin).normalized();
 	real_t min_d = 1e20;
 
@@ -613,7 +602,7 @@ bool CapsuleShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &
 	return collision;
 }
 
-bool CapsuleShape3DSW::intersect_point(const Vector3 &p_point) const {
+bool GodotCapsuleShape3D::intersect_point(const Vector3 &p_point) const {
 	if (Math::abs(p_point.y) < height * 0.5 - radius) {
 		return Vector3(p_point.x, 0, p_point.z).length() < radius;
 	} else {
@@ -623,7 +612,7 @@ bool CapsuleShape3DSW::intersect_point(const Vector3 &p_point) const {
 	}
 }
 
-Vector3 CapsuleShape3DSW::get_closest_point_to(const Vector3 &p_point) const {
+Vector3 GodotCapsuleShape3D::get_closest_point_to(const Vector3 &p_point) const {
 	Vector3 s[2] = {
 		Vector3(0, -height * 0.5 + radius, 0),
 		Vector3(0, height * 0.5 - radius, 0),
@@ -638,7 +627,7 @@ Vector3 CapsuleShape3DSW::get_closest_point_to(const Vector3 &p_point) const {
 	return p + (p_point - p).normalized() * radius;
 }
 
-Vector3 CapsuleShape3DSW::get_moment_of_inertia(real_t p_mass) const {
+Vector3 GodotCapsuleShape3D::get_moment_of_inertia(real_t p_mass) const {
 	// use bad AABB approximation
 	Vector3 extents = get_aabb().size * 0.5;
 
@@ -648,33 +637,31 @@ Vector3 CapsuleShape3DSW::get_moment_of_inertia(real_t p_mass) const {
 			(p_mass / 3.0) * (extents.x * extents.x + extents.y * extents.y));
 }
 
-void CapsuleShape3DSW::_setup(real_t p_height, real_t p_radius) {
+void GodotCapsuleShape3D::_setup(real_t p_height, real_t p_radius) {
 	height = p_height;
 	radius = p_radius;
 	configure(AABB(Vector3(-radius, -height * 0.5, -radius), Vector3(radius * 2, height, radius * 2)));
 }
 
-void CapsuleShape3DSW::set_data(const Variant &p_data) {
+void GodotCapsuleShape3D::set_data(const Variant &p_data) {
 	Dictionary d = p_data;
 	ERR_FAIL_COND(!d.has("radius"));
 	ERR_FAIL_COND(!d.has("height"));
 	_setup(d["height"], d["radius"]);
 }
 
-Variant CapsuleShape3DSW::get_data() const {
+Variant GodotCapsuleShape3D::get_data() const {
 	Dictionary d;
 	d["radius"] = radius;
 	d["height"] = height;
 	return d;
 }
 
-CapsuleShape3DSW::CapsuleShape3DSW() {
-	height = radius = 0;
-}
+GodotCapsuleShape3D::GodotCapsuleShape3D() {}
 
 /********** CYLINDER *************/
 
-void CylinderShape3DSW::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const {
+void GodotCylinderShape3D::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const {
 	Vector3 cylinder_axis = p_transform.basis.get_axis(1).normalized();
 	real_t axis_dot = cylinder_axis.dot(p_normal);
 
@@ -696,7 +683,7 @@ void CylinderShape3DSW::project_range(const Vector3 &p_normal, const Transform3D
 	r_max = distance + length;
 }
 
-Vector3 CylinderShape3DSW::get_support(const Vector3 &p_normal) const {
+Vector3 GodotCylinderShape3D::get_support(const Vector3 &p_normal) const {
 	Vector3 n = p_normal;
 	real_t h = (n.y > 0) ? height : -height;
 	real_t s = Math::sqrt(n.x * n.x + n.z * n.z);
@@ -714,7 +701,7 @@ Vector3 CylinderShape3DSW::get_support(const Vector3 &p_normal) const {
 	return n;
 }
 
-void CylinderShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const {
+void GodotCylinderShape3D::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const {
 	real_t d = p_normal.y;
 	if (Math::abs(d) > _CYLINDER_FACE_IS_VALID_SUPPORT_THRESHOLD) {
 		real_t h = (d > 0) ? height : -height;
@@ -774,23 +761,23 @@ void CylinderShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3
 	}
 }
 
-bool CylinderShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const {
+bool GodotCylinderShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, bool p_hit_back_faces) const {
 	return Geometry3D::segment_intersects_cylinder(p_begin, p_end, height, radius, &r_result, &r_normal, 1);
 }
 
-bool CylinderShape3DSW::intersect_point(const Vector3 &p_point) const {
+bool GodotCylinderShape3D::intersect_point(const Vector3 &p_point) const {
 	if (Math::abs(p_point.y) < height * 0.5) {
 		return Vector3(p_point.x, 0, p_point.z).length() < radius;
 	}
 	return false;
 }
 
-Vector3 CylinderShape3DSW::get_closest_point_to(const Vector3 &p_point) const {
+Vector3 GodotCylinderShape3D::get_closest_point_to(const Vector3 &p_point) const {
 	if (Math::absf(p_point.y) > height * 0.5) {
 		// Project point to top disk.
 		real_t dir = p_point.y > 0.0 ? 1.0 : -1.0;
 		Vector3 circle_pos(0.0, dir * height * 0.5, 0.0);
-		Plane circle_plane(circle_pos, Vector3(0.0, dir, 0.0));
+		Plane circle_plane(Vector3(0.0, dir, 0.0), circle_pos);
 		Vector3 proj_point = circle_plane.project(p_point);
 
 		// Clip position.
@@ -818,7 +805,7 @@ Vector3 CylinderShape3DSW::get_closest_point_to(const Vector3 &p_point) const {
 	}
 }
 
-Vector3 CylinderShape3DSW::get_moment_of_inertia(real_t p_mass) const {
+Vector3 GodotCylinderShape3D::get_moment_of_inertia(real_t p_mass) const {
 	// use bad AABB approximation
 	Vector3 extents = get_aabb().size * 0.5;
 
@@ -828,33 +815,31 @@ Vector3 CylinderShape3DSW::get_moment_of_inertia(real_t p_mass) const {
 			(p_mass / 3.0) * (extents.x * extents.x + extents.y * extents.y));
 }
 
-void CylinderShape3DSW::_setup(real_t p_height, real_t p_radius) {
+void GodotCylinderShape3D::_setup(real_t p_height, real_t p_radius) {
 	height = p_height;
 	radius = p_radius;
 	configure(AABB(Vector3(-radius, -height * 0.5, -radius), Vector3(radius * 2.0, height, radius * 2.0)));
 }
 
-void CylinderShape3DSW::set_data(const Variant &p_data) {
+void GodotCylinderShape3D::set_data(const Variant &p_data) {
 	Dictionary d = p_data;
 	ERR_FAIL_COND(!d.has("radius"));
 	ERR_FAIL_COND(!d.has("height"));
 	_setup(d["height"], d["radius"]);
 }
 
-Variant CylinderShape3DSW::get_data() const {
+Variant GodotCylinderShape3D::get_data() const {
 	Dictionary d;
 	d["radius"] = radius;
 	d["height"] = height;
 	return d;
 }
 
-CylinderShape3DSW::CylinderShape3DSW() {
-	height = radius = 0;
-}
+GodotCylinderShape3D::GodotCylinderShape3D() {}
 
 /********** CONVEX POLYGON *************/
 
-void ConvexPolygonShape3DSW::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const {
+void GodotConvexPolygonShape3D::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const {
 	int vertex_count = mesh.vertices.size();
 	if (vertex_count == 0) {
 		return;
@@ -874,7 +859,7 @@ void ConvexPolygonShape3DSW::project_range(const Vector3 &p_normal, const Transf
 	}
 }
 
-Vector3 ConvexPolygonShape3DSW::get_support(const Vector3 &p_normal) const {
+Vector3 GodotConvexPolygonShape3D::get_support(const Vector3 &p_normal) const {
 	Vector3 n = p_normal;
 
 	int vert_support_idx = -1;
@@ -899,7 +884,7 @@ Vector3 ConvexPolygonShape3DSW::get_support(const Vector3 &p_normal) const {
 	return vrts[vert_support_idx];
 }
 
-void ConvexPolygonShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const {
+void GodotConvexPolygonShape3D::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const {
 	const Geometry3D::MeshData::Face *faces = mesh.faces.ptr();
 	int fc = mesh.faces.size();
 
@@ -969,7 +954,7 @@ void ConvexPolygonShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Ve
 	r_type = FEATURE_POINT;
 }
 
-bool ConvexPolygonShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const {
+bool GodotConvexPolygonShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, bool p_hit_back_faces) const {
 	const Geometry3D::MeshData::Face *faces = mesh.faces.ptr();
 	int fc = mesh.faces.size();
 
@@ -1007,7 +992,7 @@ bool ConvexPolygonShape3DSW::intersect_segment(const Vector3 &p_begin, const Vec
 	return col;
 }
 
-bool ConvexPolygonShape3DSW::intersect_point(const Vector3 &p_point) const {
+bool GodotConvexPolygonShape3D::intersect_point(const Vector3 &p_point) const {
 	const Geometry3D::MeshData::Face *faces = mesh.faces.ptr();
 	int fc = mesh.faces.size();
 
@@ -1020,7 +1005,7 @@ bool ConvexPolygonShape3DSW::intersect_point(const Vector3 &p_point) const {
 	return true;
 }
 
-Vector3 ConvexPolygonShape3DSW::get_closest_point_to(const Vector3 &p_point) const {
+Vector3 GodotConvexPolygonShape3D::get_closest_point_to(const Vector3 &p_point) const {
 	const Geometry3D::MeshData::Face *faces = mesh.faces.ptr();
 	int fc = mesh.faces.size();
 	const Vector3 *vertices = mesh.vertices.ptr();
@@ -1040,7 +1025,7 @@ Vector3 ConvexPolygonShape3DSW::get_closest_point_to(const Vector3 &p_point) con
 			Vector3 a = vertices[indices[j]];
 			Vector3 b = vertices[indices[(j + 1) % ic]];
 			Vector3 n = (a - b).cross(faces[i].plane.normal).normalized();
-			if (Plane(a, n).is_point_over(p_point)) {
+			if (Plane(n, a).is_point_over(p_point)) {
 				is_inside = false;
 				break;
 			}
@@ -1078,7 +1063,7 @@ Vector3 ConvexPolygonShape3DSW::get_closest_point_to(const Vector3 &p_point) con
 	return min_point;
 }
 
-Vector3 ConvexPolygonShape3DSW::get_moment_of_inertia(real_t p_mass) const {
+Vector3 GodotConvexPolygonShape3D::get_moment_of_inertia(real_t p_mass) const {
 	// use bad AABB approximation
 	Vector3 extents = get_aabb().size * 0.5;
 
@@ -1088,7 +1073,7 @@ Vector3 ConvexPolygonShape3DSW::get_moment_of_inertia(real_t p_mass) const {
 			(p_mass / 3.0) * (extents.x * extents.x + extents.y * extents.y));
 }
 
-void ConvexPolygonShape3DSW::_setup(const Vector<Vector3> &p_vertices) {
+void GodotConvexPolygonShape3D::_setup(const Vector<Vector3> &p_vertices) {
 	Error err = ConvexHullComputer::convex_hull(p_vertices, mesh);
 	if (err != OK) {
 		ERR_PRINT("Failed to build convex hull");
@@ -1107,20 +1092,20 @@ void ConvexPolygonShape3DSW::_setup(const Vector<Vector3> &p_vertices) {
 	configure(_aabb);
 }
 
-void ConvexPolygonShape3DSW::set_data(const Variant &p_data) {
+void GodotConvexPolygonShape3D::set_data(const Variant &p_data) {
 	_setup(p_data);
 }
 
-Variant ConvexPolygonShape3DSW::get_data() const {
+Variant GodotConvexPolygonShape3D::get_data() const {
 	return mesh.vertices;
 }
 
-ConvexPolygonShape3DSW::ConvexPolygonShape3DSW() {
+GodotConvexPolygonShape3D::GodotConvexPolygonShape3D() {
 }
 
 /********** FACE POLYGON *************/
 
-void FaceShape3DSW::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const {
+void GodotFaceShape3D::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const {
 	for (int i = 0; i < 3; i++) {
 		Vector3 v = p_transform.xform(vertex[i]);
 		real_t d = p_normal.dot(v);
@@ -1135,7 +1120,7 @@ void FaceShape3DSW::project_range(const Vector3 &p_normal, const Transform3D &p_
 	}
 }
 
-Vector3 FaceShape3DSW::get_support(const Vector3 &p_normal) const {
+Vector3 GodotFaceShape3D::get_support(const Vector3 &p_normal) const {
 	int vert_support_idx = -1;
 	real_t support_max = 0;
 
@@ -1151,7 +1136,7 @@ Vector3 FaceShape3DSW::get_support(const Vector3 &p_normal) const {
 	return vertex[vert_support_idx];
 }
 
-void FaceShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const {
+void GodotFaceShape3D::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const {
 	Vector3 n = p_normal;
 
 	/** TEST FACE AS SUPPORT **/
@@ -1203,12 +1188,12 @@ void FaceShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_
 	r_supports[0] = vertex[vert_support_idx];
 }
 
-bool FaceShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const {
+bool GodotFaceShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, bool p_hit_back_faces) const {
 	bool c = Geometry3D::segment_intersects_triangle(p_begin, p_end, vertex[0], vertex[1], vertex[2], &r_result);
 	if (c) {
 		r_normal = Plane(vertex[0], vertex[1], vertex[2]).normal;
 		if (r_normal.dot(p_end - p_begin) > 0) {
-			if (backface_collision) {
+			if (backface_collision && p_hit_back_faces) {
 				r_normal = -r_normal;
 			} else {
 				c = false;
@@ -1219,23 +1204,23 @@ bool FaceShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_e
 	return c;
 }
 
-bool FaceShape3DSW::intersect_point(const Vector3 &p_point) const {
+bool GodotFaceShape3D::intersect_point(const Vector3 &p_point) const {
 	return false; //face is flat
 }
 
-Vector3 FaceShape3DSW::get_closest_point_to(const Vector3 &p_point) const {
+Vector3 GodotFaceShape3D::get_closest_point_to(const Vector3 &p_point) const {
 	return Face3(vertex[0], vertex[1], vertex[2]).get_closest_point_to(p_point);
 }
 
-Vector3 FaceShape3DSW::get_moment_of_inertia(real_t p_mass) const {
+Vector3 GodotFaceShape3D::get_moment_of_inertia(real_t p_mass) const {
 	return Vector3(); // Sorry, but i don't think anyone cares, FaceShape!
 }
 
-FaceShape3DSW::FaceShape3DSW() {
+GodotFaceShape3D::GodotFaceShape3D() {
 	configure(AABB());
 }
 
-Vector<Vector3> ConcavePolygonShape3DSW::get_faces() const {
+Vector<Vector3> GodotConcavePolygonShape3D::get_faces() const {
 	Vector<Vector3> rfaces;
 	rfaces.resize(faces.size() * 3);
 
@@ -1250,7 +1235,7 @@ Vector<Vector3> ConcavePolygonShape3DSW::get_faces() const {
 	return rfaces;
 }
 
-void ConcavePolygonShape3DSW::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const {
+void GodotConcavePolygonShape3D::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const {
 	int count = vertices.size();
 	if (count == 0) {
 		r_min = 0;
@@ -1271,7 +1256,7 @@ void ConcavePolygonShape3DSW::project_range(const Vector3 &p_normal, const Trans
 	}
 }
 
-Vector3 ConcavePolygonShape3DSW::get_support(const Vector3 &p_normal) const {
+Vector3 GodotConcavePolygonShape3D::get_support(const Vector3 &p_normal) const {
 	int count = vertices.size();
 	if (count == 0) {
 		return Vector3();
@@ -1296,7 +1281,7 @@ Vector3 ConcavePolygonShape3DSW::get_support(const Vector3 &p_normal) const {
 	return vptr[vert_support_idx];
 }
 
-void ConcavePolygonShape3DSW::_cull_segment(int p_idx, _SegmentCullParams *p_params) const {
+void GodotConcavePolygonShape3D::_cull_segment(int p_idx, _SegmentCullParams *p_params) const {
 	const BVH *bvh = &p_params->bvh[p_idx];
 
 	/*
@@ -1311,7 +1296,7 @@ void ConcavePolygonShape3DSW::_cull_segment(int p_idx, _SegmentCullParams *p_par
 
 	if (bvh->face_index >= 0) {
 		const Face *f = &p_params->faces[bvh->face_index];
-		FaceShape3DSW *face = p_params->face;
+		GodotFaceShape3D *face = p_params->face;
 		face->normal = f->normal;
 		face->vertex[0] = p_params->vertices[f->indices[0]];
 		face->vertex[1] = p_params->vertices[f->indices[1]];
@@ -1319,7 +1304,7 @@ void ConcavePolygonShape3DSW::_cull_segment(int p_idx, _SegmentCullParams *p_par
 
 		Vector3 res;
 		Vector3 normal;
-		if (face->intersect_segment(p_params->from, p_params->to, res, normal)) {
+		if (face->intersect_segment(p_params->from, p_params->to, res, normal, true)) {
 			real_t d = p_params->dir.dot(res) - p_params->dir.dot(p_params->from);
 			if ((d > 0) && (d < p_params->min_d)) {
 				p_params->min_d = d;
@@ -1338,7 +1323,7 @@ void ConcavePolygonShape3DSW::_cull_segment(int p_idx, _SegmentCullParams *p_par
 	}
 }
 
-bool ConcavePolygonShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const {
+bool GodotConcavePolygonShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, bool p_hit_back_faces) const {
 	if (faces.size() == 0) {
 		return false;
 	}
@@ -1348,8 +1333,8 @@ bool ConcavePolygonShape3DSW::intersect_segment(const Vector3 &p_begin, const Ve
 	const Vector3 *vr = vertices.ptr();
 	const BVH *br = bvh.ptr();
 
-	FaceShape3DSW face;
-	face.backface_collision = backface_collision;
+	GodotFaceShape3D face;
+	face.backface_collision = backface_collision && p_hit_back_faces;
 
 	_SegmentCullParams params;
 	params.from = p_begin;
@@ -1374,15 +1359,15 @@ bool ConcavePolygonShape3DSW::intersect_segment(const Vector3 &p_begin, const Ve
 	}
 }
 
-bool ConcavePolygonShape3DSW::intersect_point(const Vector3 &p_point) const {
+bool GodotConcavePolygonShape3D::intersect_point(const Vector3 &p_point) const {
 	return false; //face is flat
 }
 
-Vector3 ConcavePolygonShape3DSW::get_closest_point_to(const Vector3 &p_point) const {
+Vector3 GodotConcavePolygonShape3D::get_closest_point_to(const Vector3 &p_point) const {
 	return Vector3();
 }
 
-bool ConcavePolygonShape3DSW::_cull(int p_idx, _CullParams *p_params) const {
+bool GodotConcavePolygonShape3D::_cull(int p_idx, _CullParams *p_params) const {
 	const BVH *bvh = &p_params->bvh[p_idx];
 
 	if (!p_params->aabb.intersects(bvh->aabb)) {
@@ -1391,7 +1376,7 @@ bool ConcavePolygonShape3DSW::_cull(int p_idx, _CullParams *p_params) const {
 
 	if (bvh->face_index >= 0) {
 		const Face *f = &p_params->faces[bvh->face_index];
-		FaceShape3DSW *face = p_params->face;
+		GodotFaceShape3D *face = p_params->face;
 		face->normal = f->normal;
 		face->vertex[0] = p_params->vertices[f->indices[0]];
 		face->vertex[1] = p_params->vertices[f->indices[1]];
@@ -1416,7 +1401,7 @@ bool ConcavePolygonShape3DSW::_cull(int p_idx, _CullParams *p_params) const {
 	return false;
 }
 
-void ConcavePolygonShape3DSW::cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata) const {
+void GodotConcavePolygonShape3D::cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata, bool p_invert_backface_collision) const {
 	// make matrix local to concave
 	if (faces.size() == 0) {
 		return;
@@ -1429,8 +1414,9 @@ void ConcavePolygonShape3DSW::cull(const AABB &p_local_aabb, QueryCallback p_cal
 	const Vector3 *vr = vertices.ptr();
 	const BVH *br = bvh.ptr();
 
-	FaceShape3DSW face; // use this to send in the callback
+	GodotFaceShape3D face; // use this to send in the callback
 	face.backface_collision = backface_collision;
+	face.invert_backface_collision = p_invert_backface_collision;
 
 	_CullParams params;
 	params.aabb = local_aabb;
@@ -1445,7 +1431,7 @@ void ConcavePolygonShape3DSW::cull(const AABB &p_local_aabb, QueryCallback p_cal
 	_cull(0, &params);
 }
 
-Vector3 ConcavePolygonShape3DSW::get_moment_of_inertia(real_t p_mass) const {
+Vector3 GodotConcavePolygonShape3D::get_moment_of_inertia(real_t p_mass) const {
 	// use bad AABB approximation
 	Vector3 extents = get_aabb().size * 0.5;
 
@@ -1455,40 +1441,40 @@ Vector3 ConcavePolygonShape3DSW::get_moment_of_inertia(real_t p_mass) const {
 			(p_mass / 3.0) * (extents.x * extents.x + extents.y * extents.y));
 }
 
-struct _VolumeSW_BVH_Element {
+struct _Volume_BVH_Element {
 	AABB aabb;
 	Vector3 center;
 	int face_index;
 };
 
-struct _VolumeSW_BVH_CompareX {
-	_FORCE_INLINE_ bool operator()(const _VolumeSW_BVH_Element &a, const _VolumeSW_BVH_Element &b) const {
+struct _Volume_BVH_CompareX {
+	_FORCE_INLINE_ bool operator()(const _Volume_BVH_Element &a, const _Volume_BVH_Element &b) const {
 		return a.center.x < b.center.x;
 	}
 };
 
-struct _VolumeSW_BVH_CompareY {
-	_FORCE_INLINE_ bool operator()(const _VolumeSW_BVH_Element &a, const _VolumeSW_BVH_Element &b) const {
+struct _Volume_BVH_CompareY {
+	_FORCE_INLINE_ bool operator()(const _Volume_BVH_Element &a, const _Volume_BVH_Element &b) const {
 		return a.center.y < b.center.y;
 	}
 };
 
-struct _VolumeSW_BVH_CompareZ {
-	_FORCE_INLINE_ bool operator()(const _VolumeSW_BVH_Element &a, const _VolumeSW_BVH_Element &b) const {
+struct _Volume_BVH_CompareZ {
+	_FORCE_INLINE_ bool operator()(const _Volume_BVH_Element &a, const _Volume_BVH_Element &b) const {
 		return a.center.z < b.center.z;
 	}
 };
 
-struct _VolumeSW_BVH {
+struct _Volume_BVH {
 	AABB aabb;
-	_VolumeSW_BVH *left;
-	_VolumeSW_BVH *right;
+	_Volume_BVH *left;
+	_Volume_BVH *right;
 
 	int face_index;
 };
 
-_VolumeSW_BVH *_volume_sw_build_bvh(_VolumeSW_BVH_Element *p_elements, int p_size, int &count) {
-	_VolumeSW_BVH *bvh = memnew(_VolumeSW_BVH);
+_Volume_BVH *_volume_build_bvh(_Volume_BVH_Element *p_elements, int p_size, int &count) {
+	_Volume_BVH *bvh = memnew(_Volume_BVH);
 
 	if (p_size == 1) {
 		//leaf
@@ -1513,30 +1499,30 @@ _VolumeSW_BVH *_volume_sw_build_bvh(_VolumeSW_BVH_Element *p_elements, int p_siz
 	bvh->aabb = aabb;
 	switch (aabb.get_longest_axis_index()) {
 		case 0: {
-			SortArray<_VolumeSW_BVH_Element, _VolumeSW_BVH_CompareX> sort_x;
+			SortArray<_Volume_BVH_Element, _Volume_BVH_CompareX> sort_x;
 			sort_x.sort(p_elements, p_size);
 
 		} break;
 		case 1: {
-			SortArray<_VolumeSW_BVH_Element, _VolumeSW_BVH_CompareY> sort_y;
+			SortArray<_Volume_BVH_Element, _Volume_BVH_CompareY> sort_y;
 			sort_y.sort(p_elements, p_size);
 		} break;
 		case 2: {
-			SortArray<_VolumeSW_BVH_Element, _VolumeSW_BVH_CompareZ> sort_z;
+			SortArray<_Volume_BVH_Element, _Volume_BVH_CompareZ> sort_z;
 			sort_z.sort(p_elements, p_size);
 		} break;
 	}
 
 	int split = p_size / 2;
-	bvh->left = _volume_sw_build_bvh(p_elements, split, count);
-	bvh->right = _volume_sw_build_bvh(&p_elements[split], p_size - split, count);
+	bvh->left = _volume_build_bvh(p_elements, split, count);
+	bvh->right = _volume_build_bvh(&p_elements[split], p_size - split, count);
 
 	//printf("branch at %p - %i: %i\n",bvh,count,bvh->face_index);
 	count++;
 	return bvh;
 }
 
-void ConcavePolygonShape3DSW::_fill_bvh(_VolumeSW_BVH *p_bvh_tree, BVH *p_bvh_array, int &p_idx) {
+void GodotConcavePolygonShape3D::_fill_bvh(_Volume_BVH *p_bvh_tree, BVH *p_bvh_array, int &p_idx) {
 	int idx = p_idx;
 
 	p_bvh_array[idx].aabb = p_bvh_tree->aabb;
@@ -1562,7 +1548,7 @@ void ConcavePolygonShape3DSW::_fill_bvh(_VolumeSW_BVH *p_bvh_tree, BVH *p_bvh_ar
 	memdelete(p_bvh_tree);
 }
 
-void ConcavePolygonShape3DSW::_setup(const Vector<Vector3> &p_faces, bool p_backface_collision) {
+void GodotConcavePolygonShape3D::_setup(const Vector<Vector3> &p_faces, bool p_backface_collision) {
 	int src_face_count = p_faces.size();
 	if (src_face_count == 0) {
 		configure(AABB());
@@ -1573,10 +1559,10 @@ void ConcavePolygonShape3DSW::_setup(const Vector<Vector3> &p_faces, bool p_back
 
 	const Vector3 *facesr = p_faces.ptr();
 
-	Vector<_VolumeSW_BVH_Element> bvh_array;
+	Vector<_Volume_BVH_Element> bvh_array;
 	bvh_array.resize(src_face_count);
 
-	_VolumeSW_BVH_Element *bvh_arrayw = bvh_array.ptrw();
+	_Volume_BVH_Element *bvh_arrayw = bvh_array.ptrw();
 
 	faces.resize(src_face_count);
 	Face *facesw = faces.ptrw();
@@ -1591,7 +1577,7 @@ void ConcavePolygonShape3DSW::_setup(const Vector<Vector3> &p_faces, bool p_back
 		Face3 face(facesr[i * 3 + 0], facesr[i * 3 + 1], facesr[i * 3 + 2]);
 
 		bvh_arrayw[i].aabb = face.get_aabb();
-		bvh_arrayw[i].center = bvh_arrayw[i].aabb.position + bvh_arrayw[i].aabb.size * 0.5;
+		bvh_arrayw[i].center = bvh_arrayw[i].aabb.get_center();
 		bvh_arrayw[i].face_index = i;
 		facesw[i].indices[0] = i * 3 + 0;
 		facesw[i].indices[1] = i * 3 + 1;
@@ -1608,7 +1594,7 @@ void ConcavePolygonShape3DSW::_setup(const Vector<Vector3> &p_faces, bool p_back
 	}
 
 	int count = 0;
-	_VolumeSW_BVH *bvh_tree = _volume_sw_build_bvh(bvh_arrayw, src_face_count, count);
+	_Volume_BVH *bvh_tree = _volume_build_bvh(bvh_arrayw, src_face_count, count);
 
 	bvh.resize(count + 1);
 
@@ -1622,14 +1608,14 @@ void ConcavePolygonShape3DSW::_setup(const Vector<Vector3> &p_faces, bool p_back
 	configure(_aabb); // this type of shape has no margin
 }
 
-void ConcavePolygonShape3DSW::set_data(const Variant &p_data) {
+void GodotConcavePolygonShape3D::set_data(const Variant &p_data) {
 	Dictionary d = p_data;
 	ERR_FAIL_COND(!d.has("faces"));
 
 	_setup(d["faces"], d["backface_collision"]);
 }
 
-Variant ConcavePolygonShape3DSW::get_data() const {
+Variant GodotConcavePolygonShape3D::get_data() const {
 	Dictionary d;
 	d["faces"] = get_faces();
 	d["backface_collision"] = backface_collision;
@@ -1637,29 +1623,29 @@ Variant ConcavePolygonShape3DSW::get_data() const {
 	return d;
 }
 
-ConcavePolygonShape3DSW::ConcavePolygonShape3DSW() {
+GodotConcavePolygonShape3D::GodotConcavePolygonShape3D() {
 }
 
 /* HEIGHT MAP SHAPE */
 
-Vector<real_t> HeightMapShape3DSW::get_heights() const {
+Vector<real_t> GodotHeightMapShape3D::get_heights() const {
 	return heights;
 }
 
-int HeightMapShape3DSW::get_width() const {
+int GodotHeightMapShape3D::get_width() const {
 	return width;
 }
 
-int HeightMapShape3DSW::get_depth() const {
+int GodotHeightMapShape3D::get_depth() const {
 	return depth;
 }
 
-void HeightMapShape3DSW::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const {
+void GodotHeightMapShape3D::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const {
 	//not very useful, but not very used either
-	p_transform.xform(get_aabb()).project_range_in_plane(Plane(p_normal, 0), r_min, r_max);
+	p_transform.xform(get_aabb()).project_range_in_plane(Plane(p_normal), r_min, r_max);
 }
 
-Vector3 HeightMapShape3DSW::get_support(const Vector3 &p_normal) const {
+Vector3 GodotHeightMapShape3D::get_support(const Vector3 &p_normal) const {
 	//not very useful, but not very used either
 	return get_aabb().get_support(p_normal);
 }
@@ -1672,14 +1658,25 @@ struct _HeightmapSegmentCullParams {
 	Vector3 result;
 	Vector3 normal;
 
-	const HeightMapShape3DSW *heightmap = nullptr;
-	FaceShape3DSW *face = nullptr;
+	const GodotHeightMapShape3D *heightmap = nullptr;
+	GodotFaceShape3D *face = nullptr;
+};
+
+struct _HeightmapGridCullState {
+	real_t length = 0.0;
+	real_t length_flat = 0.0;
+
+	real_t dist = 0.0;
+	real_t prev_dist = 0.0;
+
+	int x = 0;
+	int z = 0;
 };
 
 _FORCE_INLINE_ bool _heightmap_face_cull_segment(_HeightmapSegmentCullParams &p_params) {
 	Vector3 res;
 	Vector3 normal;
-	if (p_params.face->intersect_segment(p_params.from, p_params.to, res, normal)) {
+	if (p_params.face->intersect_segment(p_params.from, p_params.to, res, normal, true)) {
 		p_params.result = res;
 		p_params.normal = normal;
 		return true;
@@ -1688,11 +1685,11 @@ _FORCE_INLINE_ bool _heightmap_face_cull_segment(_HeightmapSegmentCullParams &p_
 	return false;
 }
 
-_FORCE_INLINE_ bool _heightmap_cell_cull_segment(_HeightmapSegmentCullParams &p_params, int p_x, int p_z) {
+_FORCE_INLINE_ bool _heightmap_cell_cull_segment(_HeightmapSegmentCullParams &p_params, const _HeightmapGridCullState &p_state) {
 	// First triangle.
-	p_params.heightmap->_get_point(p_x, p_z, p_params.face->vertex[0]);
-	p_params.heightmap->_get_point(p_x + 1, p_z, p_params.face->vertex[1]);
-	p_params.heightmap->_get_point(p_x, p_z + 1, p_params.face->vertex[2]);
+	p_params.heightmap->_get_point(p_state.x, p_state.z, p_params.face->vertex[0]);
+	p_params.heightmap->_get_point(p_state.x + 1, p_state.z, p_params.face->vertex[1]);
+	p_params.heightmap->_get_point(p_state.x, p_state.z + 1, p_params.face->vertex[2]);
 	p_params.face->normal = Plane(p_params.face->vertex[0], p_params.face->vertex[1], p_params.face->vertex[2]).normal;
 	if (_heightmap_face_cull_segment(p_params)) {
 		return true;
@@ -1700,7 +1697,7 @@ _FORCE_INLINE_ bool _heightmap_cell_cull_segment(_HeightmapSegmentCullParams &p_
 
 	// Second triangle.
 	p_params.face->vertex[0] = p_params.face->vertex[1];
-	p_params.heightmap->_get_point(p_x + 1, p_z + 1, p_params.face->vertex[1]);
+	p_params.heightmap->_get_point(p_state.x + 1, p_state.z + 1, p_params.face->vertex[1]);
 	p_params.face->normal = Plane(p_params.face->vertex[0], p_params.face->vertex[1], p_params.face->vertex[2]).normal;
 	if (_heightmap_face_cull_segment(p_params)) {
 		return true;
@@ -1709,165 +1706,248 @@ _FORCE_INLINE_ bool _heightmap_cell_cull_segment(_HeightmapSegmentCullParams &p_
 	return false;
 }
 
-bool HeightMapShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal) const {
-	if (heights.is_empty()) {
+_FORCE_INLINE_ bool _heightmap_chunk_cull_segment(_HeightmapSegmentCullParams &p_params, const _HeightmapGridCullState &p_state) {
+	const GodotHeightMapShape3D::Range &chunk = p_params.heightmap->_get_bounds_chunk(p_state.x, p_state.z);
+
+	Vector3 enter_pos;
+	Vector3 exit_pos;
+
+	if (p_state.length_flat > CMP_EPSILON) {
+		real_t flat_to_3d = p_state.length / p_state.length_flat;
+		real_t enter_param = p_state.prev_dist * flat_to_3d;
+		real_t exit_param = p_state.dist * flat_to_3d;
+		enter_pos = p_params.from + p_params.dir * enter_param;
+		exit_pos = p_params.from + p_params.dir * exit_param;
+	} else {
+		// Consider the ray vertical.
+		// (though we shouldn't reach this often because there is an early check up-front)
+		enter_pos = p_params.from;
+		exit_pos = p_params.to;
+	}
+
+	// Transform positions to heightmap space.
+	enter_pos *= GodotHeightMapShape3D::BOUNDS_CHUNK_SIZE;
+	exit_pos *= GodotHeightMapShape3D::BOUNDS_CHUNK_SIZE;
+
+	// We did enter the flat projection of the AABB,
+	// but we have to check if we intersect it on the vertical axis.
+	if ((enter_pos.y > chunk.max) && (exit_pos.y > chunk.max)) {
+		return false;
+	}
+	if ((enter_pos.y < chunk.min) && (exit_pos.y < chunk.min)) {
 		return false;
 	}
 
-	Vector3 local_begin = p_begin + local_origin;
-	Vector3 local_end = p_end + local_origin;
+	return p_params.heightmap->_intersect_grid_segment(_heightmap_cell_cull_segment, enter_pos, exit_pos, p_params.heightmap->width, p_params.heightmap->depth, p_params.heightmap->local_origin, p_params.result, p_params.normal);
+}
 
-	FaceShape3DSW face;
+template <typename ProcessFunction>
+bool GodotHeightMapShape3D::_intersect_grid_segment(ProcessFunction &p_process, const Vector3 &p_begin, const Vector3 &p_end, int p_width, int p_depth, const Vector3 &offset, Vector3 &r_point, Vector3 &r_normal) const {
+	Vector3 delta = (p_end - p_begin);
+	real_t length = delta.length();
+
+	if (length < CMP_EPSILON) {
+		return false;
+	}
+
+	Vector3 local_begin = p_begin + offset;
+
+	GodotFaceShape3D face;
 	face.backface_collision = false;
 
 	_HeightmapSegmentCullParams params;
 	params.from = p_begin;
 	params.to = p_end;
-	params.dir = (p_end - p_begin).normalized();
+	params.dir = delta / length;
 	params.heightmap = this;
 	params.face = &face;
 
-	// Quantize the ray begin/end.
-	int begin_x = floor(local_begin.x);
-	int begin_z = floor(local_begin.z);
-	int end_x = floor(local_end.x);
-	int end_z = floor(local_end.z);
+	_HeightmapGridCullState state;
 
-	if ((begin_x == end_x) && (begin_z == end_z)) {
-		// Simple case for rays that don't traverse the grid horizontally.
-		// Just perform a test on the given cell.
-		int x = CLAMP(begin_x, 0, width - 2);
-		int z = CLAMP(begin_z, 0, depth - 2);
-		if (_heightmap_cell_cull_segment(params, x, z)) {
-			r_point = params.result;
-			r_normal = params.normal;
-			return true;
-		}
-	} else {
-		// Perform grid query from projected ray.
-		Vector2 ray_dir_proj(local_end.x - local_begin.x, local_end.z - local_begin.z);
-		real_t ray_dist_proj = ray_dir_proj.length();
+	// Perform grid query from projected ray.
+	Vector2 ray_dir_flat(delta.x, delta.z);
+	state.length = length;
+	state.length_flat = ray_dir_flat.length();
 
-		if (ray_dist_proj < CMP_EPSILON) {
-			ray_dir_proj = Vector2();
-		} else {
-			ray_dir_proj /= ray_dist_proj;
-		}
+	if (state.length_flat < CMP_EPSILON) {
+		ray_dir_flat = Vector2();
+	} else {
+		ray_dir_flat /= state.length_flat;
+	}
 
-		const int x_step = (ray_dir_proj.x > CMP_EPSILON) ? 1 : ((ray_dir_proj.x < -CMP_EPSILON) ? -1 : 0);
-		const int z_step = (ray_dir_proj.y > CMP_EPSILON) ? 1 : ((ray_dir_proj.y < -CMP_EPSILON) ? -1 : 0);
+	const int x_step = (ray_dir_flat.x > CMP_EPSILON) ? 1 : ((ray_dir_flat.x < -CMP_EPSILON) ? -1 : 0);
+	const int z_step = (ray_dir_flat.y > CMP_EPSILON) ? 1 : ((ray_dir_flat.y < -CMP_EPSILON) ? -1 : 0);
 
-		const real_t infinite = 1e20;
-		const real_t delta_x = (x_step != 0) ? 1.f / Math::abs(ray_dir_proj.x) : infinite;
-		const real_t delta_z = (z_step != 0) ? 1.f / Math::abs(ray_dir_proj.y) : infinite;
+	const real_t infinite = 1e20;
+	const real_t delta_x = (x_step != 0) ? 1.f / Math::abs(ray_dir_flat.x) : infinite;
+	const real_t delta_z = (z_step != 0) ? 1.f / Math::abs(ray_dir_flat.y) : infinite;
 
-		real_t cross_x; // At which value of `param` we will cross a x-axis lane?
-		real_t cross_z; // At which value of `param` we will cross a z-axis lane?
+	real_t cross_x; // At which value of `param` we will cross a x-axis lane?
+	real_t cross_z; // At which value of `param` we will cross a z-axis lane?
 
-		// X initialization.
-		if (x_step != 0) {
-			if (x_step == 1) {
-				cross_x = (ceil(local_begin.x) - local_begin.x) * delta_x;
-			} else {
-				cross_x = (local_begin.x - floor(local_begin.x)) * delta_x;
-			}
+	// X initialization.
+	if (x_step != 0) {
+		if (x_step == 1) {
+			cross_x = (Math::ceil(local_begin.x) - local_begin.x) * delta_x;
 		} else {
-			cross_x = infinite; // Will never cross on X.
+			cross_x = (local_begin.x - Math::floor(local_begin.x)) * delta_x;
 		}
+	} else {
+		cross_x = infinite; // Will never cross on X.
+	}
 
-		// Z initialization.
-		if (z_step != 0) {
-			if (z_step == 1) {
-				cross_z = (ceil(local_begin.z) - local_begin.z) * delta_z;
-			} else {
-				cross_z = (local_begin.z - floor(local_begin.z)) * delta_z;
-			}
+	// Z initialization.
+	if (z_step != 0) {
+		if (z_step == 1) {
+			cross_z = (Math::ceil(local_begin.z) - local_begin.z) * delta_z;
 		} else {
-			cross_z = infinite; // Will never cross on Z.
+			cross_z = (local_begin.z - Math::floor(local_begin.z)) * delta_z;
 		}
+	} else {
+		cross_z = infinite; // Will never cross on Z.
+	}
 
-		int x = floor(local_begin.x);
-		int z = floor(local_begin.z);
+	int x = Math::floor(local_begin.x);
+	int z = Math::floor(local_begin.z);
 
-		// Workaround cases where the ray starts at an integer position.
-		if (Math::is_zero_approx(cross_x)) {
-			cross_x += delta_x;
-			// If going backwards, we should ignore the position we would get by the above flooring,
-			// because the ray is not heading in that direction.
-			if (x_step == -1) {
-				x -= 1;
-			}
+	// Workaround cases where the ray starts at an integer position.
+	if (Math::is_zero_approx(cross_x)) {
+		cross_x += delta_x;
+		// If going backwards, we should ignore the position we would get by the above flooring,
+		// because the ray is not heading in that direction.
+		if (x_step == -1) {
+			x -= 1;
 		}
+	}
 
-		if (Math::is_zero_approx(cross_z)) {
-			cross_z += delta_z;
-			if (z_step == -1) {
-				z -= 1;
-			}
+	if (Math::is_zero_approx(cross_z)) {
+		cross_z += delta_z;
+		if (z_step == -1) {
+			z -= 1;
 		}
+	}
+
+	// Start inside the grid.
+	int x_start = MAX(MIN(x, p_width - 2), 0);
+	int z_start = MAX(MIN(z, p_depth - 2), 0);
 
-		// Start inside the grid.
-		int x_start = CLAMP(x, 0, width - 2);
-		int z_start = CLAMP(z, 0, depth - 2);
+	// Adjust initial cross values.
+	cross_x += delta_x * x_step * (x_start - x);
+	cross_z += delta_z * z_step * (z_start - z);
 
-		// Adjust initial cross values.
-		cross_x += delta_x * x_step * (x_start - x);
-		cross_z += delta_z * z_step * (z_start - z);
+	x = x_start;
+	z = z_start;
 
-		x = x_start;
-		z = z_start;
+	while (true) {
+		state.prev_dist = state.dist;
+		state.x = x;
+		state.z = z;
 
-		if (_heightmap_cell_cull_segment(params, x, z)) {
+		if (cross_x < cross_z) {
+			// X lane.
+			x += x_step;
+			// Assign before advancing the param,
+			// to be in sync with the initialization step.
+			state.dist = cross_x;
+			cross_x += delta_x;
+		} else {
+			// Z lane.
+			z += z_step;
+			state.dist = cross_z;
+			cross_z += delta_z;
+		}
+
+		if (state.dist > state.length_flat) {
+			state.dist = state.length_flat;
+			if (p_process(params, state)) {
+				r_point = params.result;
+				r_normal = params.normal;
+				return true;
+			}
+			break;
+		}
+
+		if (p_process(params, state)) {
 			r_point = params.result;
 			r_normal = params.normal;
 			return true;
 		}
 
-		real_t dist = 0.0;
-		while (true) {
-			if (cross_x < cross_z) {
-				// X lane.
-				x += x_step;
-				// Assign before advancing the param,
-				// to be in sync with the initialization step.
-				dist = cross_x;
-				cross_x += delta_x;
-			} else {
-				// Z lane.
-				z += z_step;
-				dist = cross_z;
-				cross_z += delta_z;
-			}
+		// Stop when outside the grid.
+		if ((x < 0) || (z < 0) || (x >= p_width - 1) || (z >= p_depth - 1)) {
+			break;
+		}
+	}
 
-			// Stop when outside the grid.
-			if ((x < 0) || (z < 0) || (x >= width - 1) || (z >= depth - 1)) {
-				break;
-			}
+	return false;
+}
 
-			if (_heightmap_cell_cull_segment(params, x, z)) {
-				r_point = params.result;
-				r_normal = params.normal;
-				return true;
-			}
+bool GodotHeightMapShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal, bool p_hit_back_faces) const {
+	if (heights.is_empty()) {
+		return false;
+	}
 
-			if (dist > ray_dist_proj) {
-				break;
-			}
+	Vector3 local_begin = p_begin + local_origin;
+	Vector3 local_end = p_end + local_origin;
+
+	// Quantize the ray begin/end.
+	int begin_x = Math::floor(local_begin.x);
+	int begin_z = Math::floor(local_begin.z);
+	int end_x = Math::floor(local_end.x);
+	int end_z = Math::floor(local_end.z);
+
+	if ((begin_x == end_x) && (begin_z == end_z)) {
+		// Simple case for rays that don't traverse the grid horizontally.
+		// Just perform a test on the given cell.
+		GodotFaceShape3D face;
+		face.backface_collision = p_hit_back_faces;
+
+		_HeightmapSegmentCullParams params;
+		params.from = p_begin;
+		params.to = p_end;
+		params.dir = (p_end - p_begin).normalized();
+
+		params.heightmap = this;
+		params.face = &face;
+
+		_HeightmapGridCullState state;
+		state.x = MAX(MIN(begin_x, width - 2), 0);
+		state.z = MAX(MIN(begin_z, depth - 2), 0);
+		if (_heightmap_cell_cull_segment(params, state)) {
+			r_point = params.result;
+			r_normal = params.normal;
+			return true;
+		}
+	} else if (bounds_grid.is_empty()) {
+		// Process all cells intersecting the flat projection of the ray.
+		return _intersect_grid_segment(_heightmap_cell_cull_segment, p_begin, p_end, width, depth, local_origin, r_point, r_normal);
+	} else {
+		Vector3 ray_diff = (p_end - p_begin);
+		real_t length_flat_sqr = ray_diff.x * ray_diff.x + ray_diff.z * ray_diff.z;
+		if (length_flat_sqr < BOUNDS_CHUNK_SIZE * BOUNDS_CHUNK_SIZE) {
+			// Don't use chunks, the ray is too short in the plane.
+			return _intersect_grid_segment(_heightmap_cell_cull_segment, p_begin, p_end, width, depth, local_origin, r_point, r_normal);
+		} else {
+			// The ray is long, run raycast on a higher-level grid.
+			Vector3 bounds_from = p_begin / BOUNDS_CHUNK_SIZE;
+			Vector3 bounds_to = p_end / BOUNDS_CHUNK_SIZE;
+			Vector3 bounds_offset = local_origin / BOUNDS_CHUNK_SIZE;
+			return _intersect_grid_segment(_heightmap_chunk_cull_segment, bounds_from, bounds_to, bounds_grid_width, bounds_grid_depth, bounds_offset, r_point, r_normal);
 		}
 	}
 
 	return false;
 }
 
-bool HeightMapShape3DSW::intersect_point(const Vector3 &p_point) const {
+bool GodotHeightMapShape3D::intersect_point(const Vector3 &p_point) const {
 	return false;
 }
 
-Vector3 HeightMapShape3DSW::get_closest_point_to(const Vector3 &p_point) const {
+Vector3 GodotHeightMapShape3D::get_closest_point_to(const Vector3 &p_point) const {
 	return Vector3();
 }
 
-void HeightMapShape3DSW::_get_cell(const Vector3 &p_point, int &r_x, int &r_y, int &r_z) const {
+void GodotHeightMapShape3D::_get_cell(const Vector3 &p_point, int &r_x, int &r_y, int &r_z) const {
 	const AABB &aabb = get_aabb();
 
 	Vector3 pos_local = aabb.position + local_origin;
@@ -1882,7 +1962,7 @@ void HeightMapShape3DSW::_get_cell(const Vector3 &p_point, int &r_x, int &r_y, i
 	r_z = (clamped_point.z < 0.0) ? (clamped_point.z - 0.5) : (clamped_point.z + 0.5);
 }
 
-void HeightMapShape3DSW::cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata) const {
+void GodotHeightMapShape3D::cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata, bool p_invert_backface_collision) const {
 	if (heights.is_empty()) {
 		return;
 	}
@@ -1908,8 +1988,9 @@ void HeightMapShape3DSW::cull(const AABB &p_local_aabb, QueryCallback p_callback
 	int start_z = MAX(0, aabb_min[2]);
 	int end_z = MIN(depth - 1, aabb_max[2]);
 
-	FaceShape3DSW face;
-	face.backface_collision = true;
+	GodotFaceShape3D face;
+	face.backface_collision = !p_invert_backface_collision;
+	face.invert_backface_collision = p_invert_backface_collision;
 
 	for (int z = start_z; z < end_z; z++) {
 		for (int x = start_x; x < end_x; x++) {
@@ -1917,7 +1998,7 @@ void HeightMapShape3DSW::cull(const AABB &p_local_aabb, QueryCallback p_callback
 			_get_point(x, z, face.vertex[0]);
 			_get_point(x + 1, z, face.vertex[1]);
 			_get_point(x, z + 1, face.vertex[2]);
-			face.normal = Plane(face.vertex[0], face.vertex[2], face.vertex[1]).normal;
+			face.normal = Plane(face.vertex[0], face.vertex[1], face.vertex[2]).normal;
 			if (p_callback(p_userdata, &face)) {
 				return;
 			}
@@ -1925,7 +2006,7 @@ void HeightMapShape3DSW::cull(const AABB &p_local_aabb, QueryCallback p_callback
 			// Second triangle.
 			face.vertex[0] = face.vertex[1];
 			_get_point(x + 1, z + 1, face.vertex[1]);
-			face.normal = Plane(face.vertex[0], face.vertex[2], face.vertex[1]).normal;
+			face.normal = Plane(face.vertex[0], face.vertex[1], face.vertex[2]).normal;
 			if (p_callback(p_userdata, &face)) {
 				return;
 			}
@@ -1933,7 +2014,7 @@ void HeightMapShape3DSW::cull(const AABB &p_local_aabb, QueryCallback p_callback
 	}
 }
 
-Vector3 HeightMapShape3DSW::get_moment_of_inertia(real_t p_mass) const {
+Vector3 GodotHeightMapShape3D::get_moment_of_inertia(real_t p_mass) const {
 	// use bad AABB approximation
 	Vector3 extents = get_aabb().size * 0.5;
 
@@ -1943,7 +2024,76 @@ Vector3 HeightMapShape3DSW::get_moment_of_inertia(real_t p_mass) const {
 			(p_mass / 3.0) * (extents.x * extents.x + extents.y * extents.y));
 }
 
-void HeightMapShape3DSW::_setup(const Vector<real_t> &p_heights, int p_width, int p_depth, real_t p_min_height, real_t p_max_height) {
+void GodotHeightMapShape3D::_build_accelerator() {
+	bounds_grid.clear();
+
+	bounds_grid_width = width / BOUNDS_CHUNK_SIZE;
+	bounds_grid_depth = depth / BOUNDS_CHUNK_SIZE;
+
+	if (width % BOUNDS_CHUNK_SIZE > 0) {
+		++bounds_grid_width; // In case terrain size isn't dividable by chunk size.
+	}
+
+	if (depth % BOUNDS_CHUNK_SIZE > 0) {
+		++bounds_grid_depth;
+	}
+
+	uint32_t bound_grid_size = (uint32_t)(bounds_grid_width * bounds_grid_depth);
+
+	if (bound_grid_size < 2) {
+		// Grid is empty or just one chunk.
+		return;
+	}
+
+	bounds_grid.resize(bound_grid_size);
+
+	// Compute min and max height for all chunks.
+	for (int cz = 0; cz < bounds_grid_depth; ++cz) {
+		int z0 = cz * BOUNDS_CHUNK_SIZE;
+
+		for (int cx = 0; cx < bounds_grid_width; ++cx) {
+			int x0 = cx * BOUNDS_CHUNK_SIZE;
+
+			Range r;
+
+			r.min = _get_height(x0, z0);
+			r.max = r.min;
+
+			// Compute min and max height for this chunk.
+			// We have to include one extra cell to account for neighbors.
+			// Here is why:
+			// Say we have a flat terrain, and a plateau that fits a chunk perfectly.
+			//
+			//   Left        Right
+			// 0---0---0---1---1---1
+			// |   |   |   |   |   |
+			// 0---0---0---1---1---1
+			// |   |   |   |   |   |
+			// 0---0---0---1---1---1
+			//           x
+			//
+			// If the AABB for the Left chunk did not share vertices with the Right,
+			// then we would fail collision tests at x due to a gap.
+			//
+			int z_max = MIN(z0 + BOUNDS_CHUNK_SIZE + 1, depth);
+			int x_max = MIN(x0 + BOUNDS_CHUNK_SIZE + 1, width);
+			for (int z = z0; z < z_max; ++z) {
+				for (int x = x0; x < x_max; ++x) {
+					real_t height = _get_height(x, z);
+					if (height < r.min) {
+						r.min = height;
+					} else if (height > r.max) {
+						r.max = height;
+					}
+				}
+			}
+
+			bounds_grid[cx + cz * bounds_grid_width] = r;
+		}
+	}
+}
+
+void GodotHeightMapShape3D::_setup(const Vector<real_t> &p_heights, int p_width, int p_depth, real_t p_min_height, real_t p_max_height) {
 	heights = p_heights;
 	width = p_width;
 	depth = p_depth;
@@ -1959,10 +2109,12 @@ void HeightMapShape3DSW::_setup(const Vector<real_t> &p_heights, int p_width, in
 
 	aabb.position -= local_origin;
 
+	_build_accelerator();
+
 	configure(aabb);
 }
 
-void HeightMapShape3DSW::set_data(const Variant &p_data) {
+void GodotHeightMapShape3D::set_data(const Variant &p_data) {
 	ERR_FAIL_COND(p_data.get_type() != Variant::DICTIONARY);
 
 	Dictionary d = p_data;
@@ -2017,7 +2169,7 @@ void HeightMapShape3DSW::set_data(const Variant &p_data) {
 	} else {
 		int heights_size = heights.size();
 		for (int i = 0; i < heights_size; ++i) {
-			float h = heights[i];
+			real_t h = heights[i];
 			if (h < min_height) {
 				min_height = h;
 			} else if (h > max_height) {
@@ -2034,7 +2186,7 @@ void HeightMapShape3DSW::set_data(const Variant &p_data) {
 	_setup(heights_buffer, width, depth, min_height, max_height);
 }
 
-Variant HeightMapShape3DSW::get_data() const {
+Variant GodotHeightMapShape3D::get_data() const {
 	Dictionary d;
 	d["width"] = width;
 	d["depth"] = depth;
@@ -2048,5 +2200,5 @@ Variant HeightMapShape3DSW::get_data() const {
 	return d;
 }
 
-HeightMapShape3DSW::HeightMapShape3DSW() {
+GodotHeightMapShape3D::GodotHeightMapShape3D() {
 }
diff --git a/servers/physics_3d/shape_3d_sw.h b/servers/physics_3d/godot_shape_3d.h
index b05f65f268..43319510d4 100644
--- a/servers/physics_3d/shape_3d_sw.h
+++ b/servers/physics_3d/godot_shape_3d.h
@@ -1,5 +1,5 @@
 /*************************************************************************/
-/*  shape_3d_sw.h                                                        */
+/*  godot_shape_3d.h                                                     */
 /*************************************************************************/
 /*                       This file is part of:                           */
 /*                           GODOT ENGINE                                */
@@ -28,29 +28,30 @@
 /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
 /*************************************************************************/
 
-#ifndef SHAPE_SW_H
-#define SHAPE_SW_H
+#ifndef GODOT_SHAPE_3D_H
+#define GODOT_SHAPE_3D_H
 
 #include "core/math/geometry_3d.h"
+#include "core/templates/local_vector.h"
 #include "servers/physics_server_3d.h"
 
-class Shape3DSW;
+class GodotShape3D;
 
-class ShapeOwner3DSW {
+class GodotShapeOwner3D {
 public:
 	virtual void _shape_changed() = 0;
-	virtual void remove_shape(Shape3DSW *p_shape) = 0;
+	virtual void remove_shape(GodotShape3D *p_shape) = 0;
 
-	virtual ~ShapeOwner3DSW() {}
+	virtual ~GodotShapeOwner3D() {}
 };
 
-class Shape3DSW {
+class GodotShape3D {
 	RID self;
 	AABB aabb;
-	bool configured;
-	real_t custom_bias;
+	bool configured = false;
+	real_t custom_bias = 0.0;
 
-	Map<ShapeOwner3DSW *, int> owners;
+	Map<GodotShapeOwner3D *, int> owners;
 
 protected:
 	void configure(const AABB &p_aabb);
@@ -63,7 +64,7 @@ public:
 		FEATURE_CIRCLE,
 	};
 
-	virtual real_t get_area() const { return aabb.get_area(); }
+	virtual real_t get_volume() const { return aabb.get_volume(); }
 
 	_FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; }
 	_FORCE_INLINE_ RID get_self() const { return self; }
@@ -79,7 +80,7 @@ public:
 	virtual Vector3 get_support(const Vector3 &p_normal) const;
 	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const = 0;
 	virtual Vector3 get_closest_point_to(const Vector3 &p_point) const = 0;
-	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal) const = 0;
+	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal, bool p_hit_back_faces) const = 0;
 	virtual bool intersect_point(const Vector3 &p_point) const = 0;
 	virtual Vector3 get_moment_of_inertia(real_t p_mass) const = 0;
 
@@ -89,29 +90,29 @@ public:
 	_FORCE_INLINE_ void set_custom_bias(real_t p_bias) { custom_bias = p_bias; }
 	_FORCE_INLINE_ real_t get_custom_bias() const { return custom_bias; }
 
-	void add_owner(ShapeOwner3DSW *p_owner);
-	void remove_owner(ShapeOwner3DSW *p_owner);
-	bool is_owner(ShapeOwner3DSW *p_owner) const;
-	const Map<ShapeOwner3DSW *, int> &get_owners() const;
+	void add_owner(GodotShapeOwner3D *p_owner);
+	void remove_owner(GodotShapeOwner3D *p_owner);
+	bool is_owner(GodotShapeOwner3D *p_owner) const;
+	const Map<GodotShapeOwner3D *, int> &get_owners() const;
 
-	Shape3DSW();
-	virtual ~Shape3DSW();
+	GodotShape3D() {}
+	virtual ~GodotShape3D();
 };
 
-class ConcaveShape3DSW : public Shape3DSW {
+class GodotConcaveShape3D : public GodotShape3D {
 public:
 	virtual bool is_concave() const override { return true; }
 	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override { r_amount = 0; }
 
 	// Returns true to stop the query.
-	typedef bool (*QueryCallback)(void *p_userdata, Shape3DSW *p_convex);
+	typedef bool (*QueryCallback)(void *p_userdata, GodotShape3D *p_convex);
 
-	virtual void cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata) const = 0;
+	virtual void cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata, bool p_invert_backface_collision) const = 0;
 
-	ConcaveShape3DSW() {}
+	GodotConcaveShape3D() {}
 };
 
-class PlaneShape3DSW : public Shape3DSW {
+class GodotWorldBoundaryShape3D : public GodotShape3D {
 	Plane plane;
 
 	void _setup(const Plane &p_plane);
@@ -119,13 +120,13 @@ class PlaneShape3DSW : public Shape3DSW {
 public:
 	Plane get_plane() const;
 
-	virtual real_t get_area() const override { return INFINITY; }
-	virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_PLANE; }
+	virtual real_t get_volume() const override { return INFINITY; }
+	virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_WORLD_BOUNDARY; }
 	virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override;
 	virtual Vector3 get_support(const Vector3 &p_normal) const override;
 	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override { r_amount = 0; }
 
-	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const override;
+	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, bool p_hit_back_faces) const override;
 	virtual bool intersect_point(const Vector3 &p_point) const override;
 	virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override;
 	virtual Vector3 get_moment_of_inertia(real_t p_mass) const override;
@@ -133,12 +134,12 @@ public:
 	virtual void set_data(const Variant &p_data) override;
 	virtual Variant get_data() const override;
 
-	PlaneShape3DSW();
+	GodotWorldBoundaryShape3D();
 };
 
-class SeparationRayShape3DSW : public Shape3DSW {
-	real_t length;
-	bool slide_on_slope;
+class GodotSeparationRayShape3D : public GodotShape3D {
+	real_t length = 1.0;
+	bool slide_on_slope = false;
 
 	void _setup(real_t p_length, bool p_slide_on_slope);
 
@@ -146,13 +147,13 @@ public:
 	real_t get_length() const;
 	bool get_slide_on_slope() const;
 
-	virtual real_t get_area() const override { return 0.0; }
+	virtual real_t get_volume() const override { return 0.0; }
 	virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_SEPARATION_RAY; }
 	virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override;
 	virtual Vector3 get_support(const Vector3 &p_normal) const override;
 	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override;
 
-	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const override;
+	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, bool p_hit_back_faces) const override;
 	virtual bool intersect_point(const Vector3 &p_point) const override;
 	virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override;
 
@@ -161,25 +162,25 @@ public:
 	virtual void set_data(const Variant &p_data) override;
 	virtual Variant get_data() const override;
 
-	SeparationRayShape3DSW();
+	GodotSeparationRayShape3D();
 };
 
-class SphereShape3DSW : public Shape3DSW {
-	real_t radius;
+class GodotSphereShape3D : public GodotShape3D {
+	real_t radius = 0.0;
 
 	void _setup(real_t p_radius);
 
 public:
 	real_t get_radius() const;
 
-	virtual real_t get_area() const override { return 4.0 / 3.0 * Math_PI * radius * radius * radius; }
+	virtual real_t get_volume() const override { return 4.0 / 3.0 * Math_PI * radius * radius * radius; }
 
 	virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_SPHERE; }
 
 	virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override;
 	virtual Vector3 get_support(const Vector3 &p_normal) const override;
 	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override;
-	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const override;
+	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, bool p_hit_back_faces) const override;
 	virtual bool intersect_point(const Vector3 &p_point) const override;
 	virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override;
 
@@ -188,23 +189,23 @@ public:
 	virtual void set_data(const Variant &p_data) override;
 	virtual Variant get_data() const override;
 
-	SphereShape3DSW();
+	GodotSphereShape3D();
 };
 
-class BoxShape3DSW : public Shape3DSW {
+class GodotBoxShape3D : public GodotShape3D {
 	Vector3 half_extents;
 	void _setup(const Vector3 &p_half_extents);
 
 public:
 	_FORCE_INLINE_ Vector3 get_half_extents() const { return half_extents; }
-	virtual real_t get_area() const override { return 8 * half_extents.x * half_extents.y * half_extents.z; }
+	virtual real_t get_volume() const override { return 8 * half_extents.x * half_extents.y * half_extents.z; }
 
 	virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_BOX; }
 
 	virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override;
 	virtual Vector3 get_support(const Vector3 &p_normal) const override;
 	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override;
-	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const override;
+	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, bool p_hit_back_faces) const override;
 	virtual bool intersect_point(const Vector3 &p_point) const override;
 	virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override;
 
@@ -213,12 +214,12 @@ public:
 	virtual void set_data(const Variant &p_data) override;
 	virtual Variant get_data() const override;
 
-	BoxShape3DSW();
+	GodotBoxShape3D();
 };
 
-class CapsuleShape3DSW : public Shape3DSW {
-	real_t height;
-	real_t radius;
+class GodotCapsuleShape3D : public GodotShape3D {
+	real_t height = 0.0;
+	real_t radius = 0.0;
 
 	void _setup(real_t p_height, real_t p_radius);
 
@@ -226,14 +227,14 @@ public:
 	_FORCE_INLINE_ real_t get_height() const { return height; }
 	_FORCE_INLINE_ real_t get_radius() const { return radius; }
 
-	virtual real_t get_area() const override { return 4.0 / 3.0 * Math_PI * radius * radius * radius + (height - radius * 2.0) * Math_PI * radius * radius; }
+	virtual real_t get_volume() const override { return 4.0 / 3.0 * Math_PI * radius * radius * radius + (height - radius * 2.0) * Math_PI * radius * radius; }
 
 	virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_CAPSULE; }
 
 	virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override;
 	virtual Vector3 get_support(const Vector3 &p_normal) const override;
 	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override;
-	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const override;
+	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, bool p_hit_back_faces) const override;
 	virtual bool intersect_point(const Vector3 &p_point) const override;
 	virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override;
 
@@ -242,12 +243,12 @@ public:
 	virtual void set_data(const Variant &p_data) override;
 	virtual Variant get_data() const override;
 
-	CapsuleShape3DSW();
+	GodotCapsuleShape3D();
 };
 
-class CylinderShape3DSW : public Shape3DSW {
-	real_t height;
-	real_t radius;
+class GodotCylinderShape3D : public GodotShape3D {
+	real_t height = 0.0;
+	real_t radius = 0.0;
 
 	void _setup(real_t p_height, real_t p_radius);
 
@@ -255,14 +256,14 @@ public:
 	_FORCE_INLINE_ real_t get_height() const { return height; }
 	_FORCE_INLINE_ real_t get_radius() const { return radius; }
 
-	virtual real_t get_area() const override { return 4.0 / 3.0 * Math_PI * radius * radius * radius + height * Math_PI * radius * radius; }
+	virtual real_t get_volume() const override { return height * Math_PI * radius * radius; }
 
 	virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_CYLINDER; }
 
 	virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override;
 	virtual Vector3 get_support(const Vector3 &p_normal) const override;
 	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override;
-	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const override;
+	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, bool p_hit_back_faces) const override;
 	virtual bool intersect_point(const Vector3 &p_point) const override;
 	virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override;
 
@@ -271,10 +272,10 @@ public:
 	virtual void set_data(const Variant &p_data) override;
 	virtual Variant get_data() const override;
 
-	CylinderShape3DSW();
+	GodotCylinderShape3D();
 };
 
-struct ConvexPolygonShape3DSW : public Shape3DSW {
+struct GodotConvexPolygonShape3D : public GodotShape3D {
 	Geometry3D::MeshData mesh;
 
 	void _setup(const Vector<Vector3> &p_vertices);
@@ -287,7 +288,7 @@ public:
 	virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override;
 	virtual Vector3 get_support(const Vector3 &p_normal) const override;
 	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override;
-	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const override;
+	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, bool p_hit_back_faces) const override;
 	virtual bool intersect_point(const Vector3 &p_point) const override;
 	virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override;
 
@@ -296,18 +297,18 @@ public:
 	virtual void set_data(const Variant &p_data) override;
 	virtual Variant get_data() const override;
 
-	ConvexPolygonShape3DSW();
+	GodotConvexPolygonShape3D();
 };
 
-struct _VolumeSW_BVH;
-struct FaceShape3DSW;
+struct _Volume_BVH;
+struct GodotFaceShape3D;
 
-struct ConcavePolygonShape3DSW : public ConcaveShape3DSW {
+struct GodotConcavePolygonShape3D : public GodotConcaveShape3D {
 	// always a trimesh
 
 	struct Face {
 		Vector3 normal;
-		int indices[3];
+		int indices[3] = {};
 	};
 
 	Vector<Face> faces;
@@ -315,10 +316,10 @@ struct ConcavePolygonShape3DSW : public ConcaveShape3DSW {
 
 	struct BVH {
 		AABB aabb;
-		int left;
-		int right;
+		int left = 0;
+		int right = 0;
 
-		int face_index;
+		int face_index = 0;
 	};
 
 	Vector<BVH> bvh;
@@ -330,7 +331,7 @@ struct ConcavePolygonShape3DSW : public ConcaveShape3DSW {
 		const Face *faces = nullptr;
 		const Vector3 *vertices = nullptr;
 		const BVH *bvh = nullptr;
-		FaceShape3DSW *face = nullptr;
+		GodotFaceShape3D *face = nullptr;
 	};
 
 	struct _SegmentCullParams {
@@ -340,7 +341,7 @@ struct ConcavePolygonShape3DSW : public ConcaveShape3DSW {
 		const Face *faces = nullptr;
 		const Vector3 *vertices = nullptr;
 		const BVH *bvh = nullptr;
-		FaceShape3DSW *face = nullptr;
+		GodotFaceShape3D *face = nullptr;
 
 		Vector3 result;
 		Vector3 normal;
@@ -353,7 +354,7 @@ struct ConcavePolygonShape3DSW : public ConcaveShape3DSW {
 	void _cull_segment(int p_idx, _SegmentCullParams *p_params) const;
 	bool _cull(int p_idx, _CullParams *p_params) const;
 
-	void _fill_bvh(_VolumeSW_BVH *p_bvh_tree, BVH *p_bvh_array, int &p_idx);
+	void _fill_bvh(_Volume_BVH *p_bvh_tree, BVH *p_bvh_array, int &p_idx);
 
 	void _setup(const Vector<Vector3> &p_faces, bool p_backface_collision);
 
@@ -365,26 +366,41 @@ public:
 	virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override;
 	virtual Vector3 get_support(const Vector3 &p_normal) const override;
 
-	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const override;
+	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, bool p_hit_back_faces) const override;
 	virtual bool intersect_point(const Vector3 &p_point) const override;
 	virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override;
 
-	virtual void cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata) const override;
+	virtual void cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata, bool p_invert_backface_collision) const override;
 
 	virtual Vector3 get_moment_of_inertia(real_t p_mass) const override;
 
 	virtual void set_data(const Variant &p_data) override;
 	virtual Variant get_data() const override;
 
-	ConcavePolygonShape3DSW();
+	GodotConcavePolygonShape3D();
 };
 
-struct HeightMapShape3DSW : public ConcaveShape3DSW {
+struct GodotHeightMapShape3D : public GodotConcaveShape3D {
 	Vector<real_t> heights;
 	int width = 0;
 	int depth = 0;
 	Vector3 local_origin;
 
+	// Accelerator.
+	struct Range {
+		real_t min = 0.0;
+		real_t max = 0.0;
+	};
+	LocalVector<Range> bounds_grid;
+	int bounds_grid_width = 0;
+	int bounds_grid_depth = 0;
+
+	static const int BOUNDS_CHUNK_SIZE = 16;
+
+	_FORCE_INLINE_ const Range &_get_bounds_chunk(int p_x, int p_z) const {
+		return bounds_grid[(p_z * bounds_grid_width) + p_x];
+	}
+
 	_FORCE_INLINE_ real_t _get_height(int p_x, int p_z) const {
 		return heights[(p_z * width) + p_x];
 	}
@@ -397,6 +413,11 @@ struct HeightMapShape3DSW : public ConcaveShape3DSW {
 
 	void _get_cell(const Vector3 &p_point, int &r_x, int &r_y, int &r_z) const;
 
+	void _build_accelerator();
+
+	template <typename ProcessFunction>
+	bool _intersect_grid_segment(ProcessFunction &p_process, const Vector3 &p_begin, const Vector3 &p_end, int p_width, int p_depth, const Vector3 &offset, Vector3 &r_point, Vector3 &r_normal) const;
+
 	void _setup(const Vector<real_t> &p_heights, int p_width, int p_depth, real_t p_min_height, real_t p_max_height);
 
 public:
@@ -408,25 +429,26 @@ public:
 
 	virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override;
 	virtual Vector3 get_support(const Vector3 &p_normal) const override;
-	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal) const override;
+	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal, bool p_hit_back_faces) const override;
 	virtual bool intersect_point(const Vector3 &p_point) const override;
 
 	virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override;
-	virtual void cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata) const override;
+	virtual void cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata, bool p_invert_backface_collision) const override;
 
 	virtual Vector3 get_moment_of_inertia(real_t p_mass) const override;
 
 	virtual void set_data(const Variant &p_data) override;
 	virtual Variant get_data() const override;
 
-	HeightMapShape3DSW();
+	GodotHeightMapShape3D();
 };
 
 //used internally
-struct FaceShape3DSW : public Shape3DSW {
+struct GodotFaceShape3D : public GodotShape3D {
 	Vector3 normal; //cache
 	Vector3 vertex[3];
 	bool backface_collision = false;
+	bool invert_backface_collision = false;
 
 	virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_CONCAVE_POLYGON; }
 
@@ -435,7 +457,7 @@ struct FaceShape3DSW : public Shape3DSW {
 	virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override;
 	virtual Vector3 get_support(const Vector3 &p_normal) const override;
 	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override;
-	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const override;
+	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, bool p_hit_back_faces) const override;
 	virtual bool intersect_point(const Vector3 &p_point) const override;
 	virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override;
 
@@ -444,11 +466,11 @@ struct FaceShape3DSW : public Shape3DSW {
 	virtual void set_data(const Variant &p_data) override {}
 	virtual Variant get_data() const override { return Variant(); }
 
-	FaceShape3DSW();
+	GodotFaceShape3D();
 };
 
-struct MotionShape3DSW : public Shape3DSW {
-	Shape3DSW *shape;
+struct GodotMotionShape3D : public GodotShape3D {
+	GodotShape3D *shape = nullptr;
 	Vector3 motion;
 
 	virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_CONVEX_POLYGON; }
@@ -474,7 +496,7 @@ struct MotionShape3DSW : public Shape3DSW {
 	}
 
 	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override { r_amount = 0; }
-	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const override { return false; }
+	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, bool p_hit_back_faces) const override { return false; }
 	virtual bool intersect_point(const Vector3 &p_point) const override { return false; }
 	virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override { return p_point; }
 
@@ -483,7 +505,7 @@ struct MotionShape3DSW : public Shape3DSW {
 	virtual void set_data(const Variant &p_data) override {}
 	virtual Variant get_data() const override { return Variant(); }
 
-	MotionShape3DSW() { configure(AABB()); }
+	GodotMotionShape3D() { configure(AABB()); }
 };
 
-#endif // SHAPE_SW_H
+#endif // GODOT_SHAPE_3D_H
diff --git a/servers/physics_3d/soft_body_3d_sw.cpp b/servers/physics_3d/godot_soft_body_3d.cpp
index d7e13867bf..43d4433302 100644
--- a/servers/physics_3d/soft_body_3d_sw.cpp
+++ b/servers/physics_3d/godot_soft_body_3d.cpp
@@ -1,5 +1,5 @@
 /*************************************************************************/
-/*  soft_body_3d_sw.cpp                                                  */
+/*  godot_soft_body_3d.cpp                                               */
 /*************************************************************************/
 /*                       This file is part of:                           */
 /*                           GODOT ENGINE                                */
@@ -28,17 +28,19 @@
 /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
 /*************************************************************************/
 
-#include "soft_body_3d_sw.h"
-#include "space_3d_sw.h"
+#include "godot_soft_body_3d.h"
+
+#include "godot_space_3d.h"
 
 #include "core/math/geometry_3d.h"
 #include "core/templates/map.h"
+#include "servers/rendering_server.h"
 
 // Based on Bullet soft body.
 
 /*
 Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans  https://bulletphysics.org
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
 
 This software is provided 'as-is', without any express or implied warranty.
 In no event will the authors be held liable for any damages arising from the use of this software.
@@ -52,16 +54,16 @@ subject to the following restrictions:
 */
 ///btSoftBody implementation by Nathanael Presson
 
-SoftBody3DSW::SoftBody3DSW() :
-		CollisionObject3DSW(TYPE_SOFT_BODY),
+GodotSoftBody3D::GodotSoftBody3D() :
+		GodotCollisionObject3D(TYPE_SOFT_BODY),
 		active_list(this) {
 	_set_static(false);
 }
 
-void SoftBody3DSW::_shapes_changed() {
+void GodotSoftBody3D::_shapes_changed() {
 }
 
-void SoftBody3DSW::set_state(PhysicsServer3D::BodyState p_state, const Variant &p_variant) {
+void GodotSoftBody3D::set_state(PhysicsServer3D::BodyState p_state, const Variant &p_variant) {
 	switch (p_state) {
 		case PhysicsServer3D::BODY_STATE_TRANSFORM: {
 			_set_transform(p_variant);
@@ -86,7 +88,7 @@ void SoftBody3DSW::set_state(PhysicsServer3D::BodyState p_state, const Variant &
 	}
 }
 
-Variant SoftBody3DSW::get_state(PhysicsServer3D::BodyState p_state) const {
+Variant GodotSoftBody3D::get_state(PhysicsServer3D::BodyState p_state) const {
 	switch (p_state) {
 		case PhysicsServer3D::BODY_STATE_TRANSFORM: {
 			return get_transform();
@@ -109,7 +111,7 @@ Variant SoftBody3DSW::get_state(PhysicsServer3D::BodyState p_state) const {
 	return Variant();
 }
 
-void SoftBody3DSW::set_space(Space3DSW *p_space) {
+void GodotSoftBody3D::set_space(GodotSpace3D *p_space) {
 	if (get_space()) {
 		get_space()->soft_body_remove_from_active_list(&active_list);
 
@@ -127,7 +129,7 @@ void SoftBody3DSW::set_space(Space3DSW *p_space) {
 	}
 }
 
-void SoftBody3DSW::set_mesh(const Ref<Mesh> &p_mesh) {
+void GodotSoftBody3D::set_mesh(RID p_mesh) {
 	destroy();
 
 	soft_mesh = p_mesh;
@@ -136,17 +138,16 @@ void SoftBody3DSW::set_mesh(const Ref<Mesh> &p_mesh) {
 		return;
 	}
 
-	Array arrays = soft_mesh->surface_get_arrays(0);
-	ERR_FAIL_COND(!(soft_mesh->surface_get_format(0) & RS::ARRAY_FORMAT_INDEX));
+	Array arrays = RenderingServer::get_singleton()->mesh_surface_get_arrays(soft_mesh, 0);
+	ERR_FAIL_COND(arrays.is_empty());
 
-	bool success = create_from_trimesh(arrays[RS::ARRAY_INDEX], arrays[RS::ARRAY_VERTEX]);
+	bool success = create_from_trimesh(arrays[RenderingServer::ARRAY_INDEX], arrays[RenderingServer::ARRAY_VERTEX]);
 	if (!success) {
 		destroy();
-		soft_mesh = Ref<Mesh>();
 	}
 }
 
-void SoftBody3DSW::update_rendering_server(RenderingServerHandler *p_rendering_server_handler) {
+void GodotSoftBody3D::update_rendering_server(RenderingServerHandler *p_rendering_server_handler) {
 	if (soft_mesh.is_null()) {
 		return;
 	}
@@ -165,7 +166,7 @@ void SoftBody3DSW::update_rendering_server(RenderingServerHandler *p_rendering_s
 	p_rendering_server_handler->set_aabb(bounds);
 }
 
-void SoftBody3DSW::update_normals_and_centroids() {
+void GodotSoftBody3D::update_normals_and_centroids() {
 	uint32_t i, ni;
 
 	for (i = 0, ni = nodes.size(); i < ni; ++i) {
@@ -192,7 +193,7 @@ void SoftBody3DSW::update_normals_and_centroids() {
 	}
 }
 
-void SoftBody3DSW::update_bounds() {
+void GodotSoftBody3D::update_bounds() {
 	AABB prev_bounds = bounds;
 	prev_bounds.grow_by(collision_margin);
 
@@ -224,13 +225,13 @@ void SoftBody3DSW::update_bounds() {
 	}
 }
 
-void SoftBody3DSW::update_constants() {
+void GodotSoftBody3D::update_constants() {
 	reset_link_rest_lengths();
 	update_link_constants();
 	update_area();
 }
 
-void SoftBody3DSW::update_area() {
+void GodotSoftBody3D::update_area() {
 	int i, ni;
 
 	// Face area.
@@ -249,8 +250,10 @@ void SoftBody3DSW::update_area() {
 
 	// Node area.
 	LocalVector<int> counts;
-	counts.resize(nodes.size());
-	memset(counts.ptr(), 0, counts.size() * sizeof(int));
+	if (nodes.size() > 0) {
+		counts.resize(nodes.size());
+		memset(counts.ptr(), 0, counts.size() * sizeof(int));
+	}
 
 	for (i = 0, ni = nodes.size(); i < ni; ++i) {
 		nodes[i].area = 0.0;
@@ -274,7 +277,7 @@ void SoftBody3DSW::update_area() {
 	}
 }
 
-void SoftBody3DSW::reset_link_rest_lengths() {
+void GodotSoftBody3D::reset_link_rest_lengths() {
 	for (uint32_t i = 0, ni = links.size(); i < ni; ++i) {
 		Link &link = links[i];
 		link.rl = (link.n[0]->x - link.n[1]->x).length();
@@ -282,7 +285,7 @@ void SoftBody3DSW::reset_link_rest_lengths() {
 	}
 }
 
-void SoftBody3DSW::update_link_constants() {
+void GodotSoftBody3D::update_link_constants() {
 	real_t inv_linear_stiffness = 1.0 / linear_stiffness;
 	for (uint32_t i = 0, ni = links.size(); i < ni; ++i) {
 		Link &link = links[i];
@@ -290,7 +293,7 @@ void SoftBody3DSW::update_link_constants() {
 	}
 }
 
-void SoftBody3DSW::apply_nodes_transform(const Transform3D &p_transform) {
+void GodotSoftBody3D::apply_nodes_transform(const Transform3D &p_transform) {
 	if (soft_mesh.is_null()) {
 		return;
 	}
@@ -316,24 +319,28 @@ void SoftBody3DSW::apply_nodes_transform(const Transform3D &p_transform) {
 	update_constants();
 }
 
-Vector3 SoftBody3DSW::get_vertex_position(int p_index) const {
+Vector3 GodotSoftBody3D::get_vertex_position(int p_index) const {
+	ERR_FAIL_COND_V(p_index < 0, Vector3());
+
 	if (soft_mesh.is_null()) {
 		return Vector3();
 	}
 
-	ERR_FAIL_INDEX_V(p_index, (int)map_visual_to_physics.size(), Vector3());
+	ERR_FAIL_COND_V(p_index >= (int)map_visual_to_physics.size(), Vector3());
 	uint32_t node_index = map_visual_to_physics[p_index];
 
 	ERR_FAIL_COND_V(node_index >= nodes.size(), Vector3());
 	return nodes[node_index].x;
 }
 
-void SoftBody3DSW::set_vertex_position(int p_index, const Vector3 &p_position) {
+void GodotSoftBody3D::set_vertex_position(int p_index, const Vector3 &p_position) {
+	ERR_FAIL_COND(p_index < 0);
+
 	if (soft_mesh.is_null()) {
 		return;
 	}
 
-	ERR_FAIL_INDEX(p_index, (int)map_visual_to_physics.size());
+	ERR_FAIL_COND(p_index >= (int)map_visual_to_physics.size());
 	uint32_t node_index = map_visual_to_physics[p_index];
 
 	ERR_FAIL_COND(node_index >= nodes.size());
@@ -342,7 +349,9 @@ void SoftBody3DSW::set_vertex_position(int p_index, const Vector3 &p_position) {
 	node.x = p_position;
 }
 
-void SoftBody3DSW::pin_vertex(int p_index) {
+void GodotSoftBody3D::pin_vertex(int p_index) {
+	ERR_FAIL_COND(p_index < 0);
+
 	if (is_vertex_pinned(p_index)) {
 		return;
 	}
@@ -350,7 +359,7 @@ void SoftBody3DSW::pin_vertex(int p_index) {
 	pinned_vertices.push_back(p_index);
 
 	if (!soft_mesh.is_null()) {
-		ERR_FAIL_INDEX(p_index, (int)map_visual_to_physics.size());
+		ERR_FAIL_COND(p_index >= (int)map_visual_to_physics.size());
 		uint32_t node_index = map_visual_to_physics[p_index];
 
 		ERR_FAIL_COND(node_index >= nodes.size());
@@ -359,14 +368,16 @@ void SoftBody3DSW::pin_vertex(int p_index) {
 	}
 }
 
-void SoftBody3DSW::unpin_vertex(int p_index) {
+void GodotSoftBody3D::unpin_vertex(int p_index) {
+	ERR_FAIL_COND(p_index < 0);
+
 	uint32_t pinned_count = pinned_vertices.size();
 	for (uint32_t i = 0; i < pinned_count; ++i) {
 		if (p_index == pinned_vertices[i]) {
-			pinned_vertices.remove(i);
+			pinned_vertices.remove_at(i);
 
 			if (!soft_mesh.is_null()) {
-				ERR_FAIL_INDEX(p_index, (int)map_visual_to_physics.size());
+				ERR_FAIL_COND(p_index >= (int)map_visual_to_physics.size());
 				uint32_t node_index = map_visual_to_physics[p_index];
 
 				ERR_FAIL_COND(node_index >= nodes.size());
@@ -381,15 +392,15 @@ void SoftBody3DSW::unpin_vertex(int p_index) {
 	}
 }
 
-void SoftBody3DSW::unpin_all_vertices() {
+void GodotSoftBody3D::unpin_all_vertices() {
 	if (!soft_mesh.is_null()) {
 		real_t inv_node_mass = nodes.size() * inv_total_mass;
 		uint32_t pinned_count = pinned_vertices.size();
 		for (uint32_t i = 0; i < pinned_count; ++i) {
-			uint32_t vertex_index = pinned_vertices[i];
+			int pinned_vertex = pinned_vertices[i];
 
-			ERR_CONTINUE(vertex_index >= map_visual_to_physics.size());
-			uint32_t node_index = map_visual_to_physics[vertex_index];
+			ERR_CONTINUE(pinned_vertex >= (int)map_visual_to_physics.size());
+			uint32_t node_index = map_visual_to_physics[pinned_vertex];
 
 			ERR_CONTINUE(node_index >= nodes.size());
 			Node &node = nodes[node_index];
@@ -400,7 +411,9 @@ void SoftBody3DSW::unpin_all_vertices() {
 	pinned_vertices.clear();
 }
 
-bool SoftBody3DSW::is_vertex_pinned(int p_index) const {
+bool GodotSoftBody3D::is_vertex_pinned(int p_index) const {
+	ERR_FAIL_COND_V(p_index < 0, false);
+
 	uint32_t pinned_count = pinned_vertices.size();
 	for (uint32_t i = 0; i < pinned_count; ++i) {
 		if (p_index == pinned_vertices[i]) {
@@ -411,47 +424,47 @@ bool SoftBody3DSW::is_vertex_pinned(int p_index) const {
 	return false;
 }
 
-uint32_t SoftBody3DSW::get_node_count() const {
+uint32_t GodotSoftBody3D::get_node_count() const {
 	return nodes.size();
 }
 
-real_t SoftBody3DSW::get_node_inv_mass(uint32_t p_node_index) const {
+real_t GodotSoftBody3D::get_node_inv_mass(uint32_t p_node_index) const {
 	ERR_FAIL_COND_V(p_node_index >= nodes.size(), 0.0);
 	return nodes[p_node_index].im;
 }
 
-Vector3 SoftBody3DSW::get_node_position(uint32_t p_node_index) const {
+Vector3 GodotSoftBody3D::get_node_position(uint32_t p_node_index) const {
 	ERR_FAIL_COND_V(p_node_index >= nodes.size(), Vector3());
 	return nodes[p_node_index].x;
 }
 
-Vector3 SoftBody3DSW::get_node_velocity(uint32_t p_node_index) const {
+Vector3 GodotSoftBody3D::get_node_velocity(uint32_t p_node_index) const {
 	ERR_FAIL_COND_V(p_node_index >= nodes.size(), Vector3());
 	return nodes[p_node_index].v;
 }
 
-Vector3 SoftBody3DSW::get_node_biased_velocity(uint32_t p_node_index) const {
+Vector3 GodotSoftBody3D::get_node_biased_velocity(uint32_t p_node_index) const {
 	ERR_FAIL_COND_V(p_node_index >= nodes.size(), Vector3());
 	return nodes[p_node_index].bv;
 }
 
-void SoftBody3DSW::apply_node_impulse(uint32_t p_node_index, const Vector3 &p_impulse) {
+void GodotSoftBody3D::apply_node_impulse(uint32_t p_node_index, const Vector3 &p_impulse) {
 	ERR_FAIL_COND(p_node_index >= nodes.size());
 	Node &node = nodes[p_node_index];
 	node.v += p_impulse * node.im;
 }
 
-void SoftBody3DSW::apply_node_bias_impulse(uint32_t p_node_index, const Vector3 &p_impulse) {
+void GodotSoftBody3D::apply_node_bias_impulse(uint32_t p_node_index, const Vector3 &p_impulse) {
 	ERR_FAIL_COND(p_node_index >= nodes.size());
 	Node &node = nodes[p_node_index];
 	node.bv += p_impulse * node.im;
 }
 
-uint32_t SoftBody3DSW::get_face_count() const {
+uint32_t GodotSoftBody3D::get_face_count() const {
 	return faces.size();
 }
 
-void SoftBody3DSW::get_face_points(uint32_t p_face_index, Vector3 &r_point_1, Vector3 &r_point_2, Vector3 &r_point_3) const {
+void GodotSoftBody3D::get_face_points(uint32_t p_face_index, Vector3 &r_point_1, Vector3 &r_point_2, Vector3 &r_point_3) const {
 	ERR_FAIL_COND(p_face_index >= faces.size());
 	const Face &face = faces[p_face_index];
 	r_point_1 = face.n[0]->x;
@@ -459,12 +472,15 @@ void SoftBody3DSW::get_face_points(uint32_t p_face_index, Vector3 &r_point_1, Ve
 	r_point_3 = face.n[2]->x;
 }
 
-Vector3 SoftBody3DSW::get_face_normal(uint32_t p_face_index) const {
+Vector3 GodotSoftBody3D::get_face_normal(uint32_t p_face_index) const {
 	ERR_FAIL_COND_V(p_face_index >= faces.size(), Vector3());
 	return faces[p_face_index].normal;
 }
 
-bool SoftBody3DSW::create_from_trimesh(const Vector<int> &p_indices, const Vector<Vector3> &p_vertices) {
+bool GodotSoftBody3D::create_from_trimesh(const Vector<int> &p_indices, const Vector<Vector3> &p_vertices) {
+	ERR_FAIL_COND_V(p_indices.is_empty(), false);
+	ERR_FAIL_COND_V(p_vertices.is_empty(), false);
+
 	uint32_t node_count = 0;
 	LocalVector<Vector3> vertices;
 	const int visual_vertex_count(p_vertices.size());
@@ -581,7 +597,7 @@ bool SoftBody3DSW::create_from_trimesh(const Vector<int> &p_indices, const Vecto
 	return true;
 }
 
-void SoftBody3DSW::generate_bending_constraints(int p_distance) {
+void GodotSoftBody3D::generate_bending_constraints(int p_distance) {
 	uint32_t i, j;
 
 	if (p_distance > 1) {
@@ -694,13 +710,15 @@ void SoftBody3DSW::generate_bending_constraints(int p_distance) {
 // A small structure to track lists of dependent link calculations.
 class LinkDeps {
 public:
-	int value; // A link calculation that is dependent on this one
-			// Positive values = "input A" while negative values = "input B"
-	LinkDeps *next; // Next dependence in the list
+	// A link calculation that is dependent on this one.
+	// Positive values = "input A" while negative values = "input B".
+	int value;
+	// Next dependence in the list.
+	LinkDeps *next;
 };
 typedef LinkDeps *LinkDepsPtr;
 
-void SoftBody3DSW::reoptimize_link_order() {
+void GodotSoftBody3D::reoptimize_link_order() {
 	const int reop_not_dependent = -1;
 	const int reop_node_complete = -2;
 
@@ -809,7 +827,7 @@ void SoftBody3DSW::reoptimize_link_order() {
 	memdelete_arr(link_buffer);
 }
 
-void SoftBody3DSW::append_link(uint32_t p_node1, uint32_t p_node2) {
+void GodotSoftBody3D::append_link(uint32_t p_node1, uint32_t p_node2) {
 	if (p_node1 == p_node2) {
 		return;
 	}
@@ -825,7 +843,7 @@ void SoftBody3DSW::append_link(uint32_t p_node1, uint32_t p_node2) {
 	links.push_back(link);
 }
 
-void SoftBody3DSW::append_face(uint32_t p_node1, uint32_t p_node2, uint32_t p_node3) {
+void GodotSoftBody3D::append_face(uint32_t p_node1, uint32_t p_node2, uint32_t p_node3) {
 	if (p_node1 == p_node2) {
 		return;
 	}
@@ -850,11 +868,11 @@ void SoftBody3DSW::append_face(uint32_t p_node1, uint32_t p_node2, uint32_t p_no
 	faces.push_back(face);
 }
 
-void SoftBody3DSW::set_iteration_count(int p_val) {
+void GodotSoftBody3D::set_iteration_count(int p_val) {
 	iteration_count = p_val;
 }
 
-void SoftBody3DSW::set_total_mass(real_t p_val) {
+void GodotSoftBody3D::set_total_mass(real_t p_val) {
 	ERR_FAIL_COND(p_val < 0.0);
 
 	inv_total_mass = 1.0 / p_val;
@@ -870,27 +888,27 @@ void SoftBody3DSW::set_total_mass(real_t p_val) {
 	update_constants();
 }
 
-void SoftBody3DSW::set_collision_margin(real_t p_val) {
+void GodotSoftBody3D::set_collision_margin(real_t p_val) {
 	collision_margin = p_val;
 }
 
-void SoftBody3DSW::set_linear_stiffness(real_t p_val) {
+void GodotSoftBody3D::set_linear_stiffness(real_t p_val) {
 	linear_stiffness = p_val;
 }
 
-void SoftBody3DSW::set_pressure_coefficient(real_t p_val) {
+void GodotSoftBody3D::set_pressure_coefficient(real_t p_val) {
 	pressure_coefficient = p_val;
 }
 
-void SoftBody3DSW::set_damping_coefficient(real_t p_val) {
+void GodotSoftBody3D::set_damping_coefficient(real_t p_val) {
 	damping_coefficient = p_val;
 }
 
-void SoftBody3DSW::set_drag_coefficient(real_t p_val) {
+void GodotSoftBody3D::set_drag_coefficient(real_t p_val) {
 	drag_coefficient = p_val;
 }
 
-void SoftBody3DSW::add_velocity(const Vector3 &p_velocity) {
+void GodotSoftBody3D::add_velocity(const Vector3 &p_velocity) {
 	for (uint32_t i = 0, ni = nodes.size(); i < ni; ++i) {
 		Node &node = nodes[i];
 		if (node.im > 0) {
@@ -899,9 +917,7 @@ void SoftBody3DSW::add_velocity(const Vector3 &p_velocity) {
 	}
 }
 
-void SoftBody3DSW::apply_forces(bool p_has_wind_forces) {
-	int ac = areas.size();
-
+void GodotSoftBody3D::apply_forces(const LocalVector<GodotArea3D *> &p_wind_areas) {
 	if (nodes.is_empty()) {
 		return;
 	}
@@ -914,7 +930,6 @@ void SoftBody3DSW::apply_forces(bool p_has_wind_forces) {
 
 	// Iterate over faces (try not to iterate elsewhere if possible).
 	for (i = 0, ni = faces.size(); i < ni; ++i) {
-		bool stopped = false;
 		const Face &face = faces[i];
 
 		Vector3 wind_force(0, 0, 0);
@@ -923,24 +938,10 @@ void SoftBody3DSW::apply_forces(bool p_has_wind_forces) {
 		volume += vec3_dot(face.n[0]->x - org, vec3_cross(face.n[1]->x - org, face.n[2]->x - org));
 
 		// Compute nodal forces from area winds.
-		if (ac && p_has_wind_forces) {
-			const AreaCMP *aa = &areas[0];
-			for (j = ac - 1; j >= 0 && !stopped; j--) {
-				PhysicsServer3D::AreaSpaceOverrideMode mode = aa[j].area->get_space_override_mode();
-				switch (mode) {
-					case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE:
-					case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: {
-						wind_force += _compute_area_windforce(aa[j].area, &face);
-						stopped = mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE;
-					} break;
-					case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE:
-					case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE_COMBINE: {
-						wind_force = _compute_area_windforce(aa[j].area, &face);
-						stopped = mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE;
-					} break;
-					default: {
-					}
-				}
+		int wind_area_count = p_wind_areas.size();
+		if (wind_area_count > 0) {
+			for (j = 0; j < wind_area_count; j++) {
+				wind_force += _compute_area_windforce(p_wind_areas[j], &face);
 			}
 
 			for (j = 0; j < 3; j++) {
@@ -963,13 +964,7 @@ void SoftBody3DSW::apply_forces(bool p_has_wind_forces) {
 	}
 }
 
-void SoftBody3DSW::_compute_area_gravity(const Area3DSW *p_area) {
-	Vector3 area_gravity;
-	p_area->compute_gravity(get_transform().get_origin(), area_gravity);
-	gravity += area_gravity;
-}
-
-Vector3 SoftBody3DSW::_compute_area_windforce(const Area3DSW *p_area, const Face *p_face) {
+Vector3 GodotSoftBody3D::_compute_area_windforce(const GodotArea3D *p_area, const Face *p_face) {
 	real_t wfm = p_area->get_wind_force_magnitude();
 	real_t waf = p_area->get_wind_attenuation_factor();
 	const Vector3 &wd = p_area->get_wind_direction();
@@ -981,49 +976,64 @@ Vector3 SoftBody3DSW::_compute_area_windforce(const Area3DSW *p_area, const Face
 	return nodal_force_magnitude * p_face->normal;
 }
 
-void SoftBody3DSW::predict_motion(real_t p_delta) {
+void GodotSoftBody3D::predict_motion(real_t p_delta) {
 	const real_t inv_delta = 1.0 / p_delta;
 
 	ERR_FAIL_COND(!get_space());
 
-	Area3DSW *def_area = get_space()->get_default_area();
-	ERR_FAIL_COND(!def_area);
-	gravity = def_area->get_gravity_vector() * def_area->get_gravity();
+	bool gravity_done = false;
+	Vector3 gravity;
 
-	int ac = areas.size();
-	bool stopped = false;
-	bool has_wind_forces = false;
+	LocalVector<GodotArea3D *> wind_areas;
 
+	int ac = areas.size();
 	if (ac) {
 		areas.sort();
 		const AreaCMP *aa = &areas[0];
-		for (int i = ac - 1; i >= 0 && !stopped; i--) {
-			// Avoids unnecessary loop in apply_forces().
-			has_wind_forces = has_wind_forces || aa[i].area->get_wind_force_magnitude() > CMP_EPSILON;
-
-			PhysicsServer3D::AreaSpaceOverrideMode mode = aa[i].area->get_space_override_mode();
-			switch (mode) {
-				case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE:
-				case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: {
-					_compute_area_gravity(aa[i].area);
-					stopped = mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE;
-				} break;
-				case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE:
-				case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE_COMBINE: {
-					gravity = Vector3(0, 0, 0);
-					_compute_area_gravity(aa[i].area);
-					stopped = mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE;
-				} break;
-				default: {
+		for (int i = ac - 1; i >= 0; i--) {
+			if (!gravity_done) {
+				PhysicsServer3D::AreaSpaceOverrideMode area_gravity_mode = (PhysicsServer3D::AreaSpaceOverrideMode)(int)aa[i].area->get_param(PhysicsServer3D::AREA_PARAM_GRAVITY_OVERRIDE_MODE);
+				if (area_gravity_mode != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) {
+					Vector3 area_gravity;
+					aa[i].area->compute_gravity(get_transform().get_origin(), area_gravity);
+					switch (area_gravity_mode) {
+						case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE:
+						case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: {
+							gravity += area_gravity;
+							gravity_done = area_gravity_mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE;
+						} break;
+						case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE:
+						case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE_COMBINE: {
+							gravity = Vector3(0, 0, 0);
+							gravity = area_gravity;
+							gravity_done = area_gravity_mode == PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE;
+						} break;
+						default: {
+						}
+					}
 				}
 			}
+
+			if (aa[i].area->get_wind_force_magnitude() > CMP_EPSILON) {
+				wind_areas.push_back(aa[i].area);
+			}
 		}
 	}
 
+	// Add default gravity and damping from space area.
+	if (!gravity_done) {
+		GodotArea3D *default_area = get_space()->get_default_area();
+		ERR_FAIL_COND(!default_area);
+
+		Vector3 default_gravity;
+		default_area->compute_gravity(get_transform().get_origin(), default_gravity);
+		gravity += default_gravity;
+	}
+
 	// Apply forces.
 	add_velocity(gravity * p_delta);
-	if (pressure_coefficient > CMP_EPSILON || has_wind_forces) {
-		apply_forces(has_wind_forces);
+	if (pressure_coefficient > CMP_EPSILON || !wind_areas.is_empty()) {
+		apply_forces(wind_areas);
 	}
 
 	// Avoid soft body from 'exploding' so use some upper threshold of maximum motion
@@ -1069,7 +1079,7 @@ void SoftBody3DSW::predict_motion(real_t p_delta) {
 	face_tree.optimize_incremental(1);
 }
 
-void SoftBody3DSW::solve_constraints(real_t p_delta) {
+void GodotSoftBody3D::solve_constraints(real_t p_delta) {
 	const real_t inv_delta = 1.0 / p_delta;
 
 	uint32_t i, ni;
@@ -1106,7 +1116,7 @@ void SoftBody3DSW::solve_constraints(real_t p_delta) {
 	update_normals_and_centroids();
 }
 
-void SoftBody3DSW::solve_links(real_t kst, real_t ti) {
+void GodotSoftBody3D::solve_links(real_t kst, real_t ti) {
 	for (uint32_t i = 0, ni = links.size(); i < ni; ++i) {
 		Link &link = links[i];
 		if (link.c0 > 0) {
@@ -1124,16 +1134,16 @@ void SoftBody3DSW::solve_links(real_t kst, real_t ti) {
 }
 
 struct AABBQueryResult {
-	const SoftBody3DSW *soft_body = nullptr;
+	const GodotSoftBody3D *soft_body = nullptr;
 	void *userdata = nullptr;
-	SoftBody3DSW::QueryResultCallback result_callback = nullptr;
+	GodotSoftBody3D::QueryResultCallback result_callback = nullptr;
 
 	_FORCE_INLINE_ bool operator()(void *p_data) {
 		return result_callback(soft_body->get_node_index(p_data), userdata);
 	};
 };
 
-void SoftBody3DSW::query_aabb(const AABB &p_aabb, SoftBody3DSW::QueryResultCallback p_result_callback, void *p_userdata) {
+void GodotSoftBody3D::query_aabb(const AABB &p_aabb, GodotSoftBody3D::QueryResultCallback p_result_callback, void *p_userdata) {
 	AABBQueryResult query_result;
 	query_result.soft_body = this;
 	query_result.result_callback = p_result_callback;
@@ -1143,16 +1153,16 @@ void SoftBody3DSW::query_aabb(const AABB &p_aabb, SoftBody3DSW::QueryResultCallb
 }
 
 struct RayQueryResult {
-	const SoftBody3DSW *soft_body = nullptr;
+	const GodotSoftBody3D *soft_body = nullptr;
 	void *userdata = nullptr;
-	SoftBody3DSW::QueryResultCallback result_callback = nullptr;
+	GodotSoftBody3D::QueryResultCallback result_callback = nullptr;
 
 	_FORCE_INLINE_ bool operator()(void *p_data) {
 		return result_callback(soft_body->get_face_index(p_data), userdata);
 	};
 };
 
-void SoftBody3DSW::query_ray(const Vector3 &p_from, const Vector3 &p_to, SoftBody3DSW::QueryResultCallback p_result_callback, void *p_userdata) {
+void GodotSoftBody3D::query_ray(const Vector3 &p_from, const Vector3 &p_to, GodotSoftBody3D::QueryResultCallback p_result_callback, void *p_userdata) {
 	if (face_tree.is_empty()) {
 		initialize_face_tree();
 	}
@@ -1165,7 +1175,7 @@ void SoftBody3DSW::query_ray(const Vector3 &p_from, const Vector3 &p_to, SoftBod
 	face_tree.ray_query(p_from, p_to, query_result);
 }
 
-void SoftBody3DSW::initialize_face_tree() {
+void GodotSoftBody3D::initialize_face_tree() {
 	face_tree.clear();
 	for (uint32_t i = 0; i < faces.size(); ++i) {
 		Face &face = faces[i];
@@ -1182,7 +1192,7 @@ void SoftBody3DSW::initialize_face_tree() {
 	}
 }
 
-void SoftBody3DSW::update_face_tree(real_t p_delta) {
+void GodotSoftBody3D::update_face_tree(real_t p_delta) {
 	for (uint32_t i = 0; i < faces.size(); ++i) {
 		const Face &face = faces[i];
 
@@ -1206,25 +1216,27 @@ void SoftBody3DSW::update_face_tree(real_t p_delta) {
 	}
 }
 
-void SoftBody3DSW::initialize_shape(bool p_force_move) {
+void GodotSoftBody3D::initialize_shape(bool p_force_move) {
 	if (get_shape_count() == 0) {
-		SoftBodyShape3DSW *soft_body_shape = memnew(SoftBodyShape3DSW(this));
+		GodotSoftBodyShape3D *soft_body_shape = memnew(GodotSoftBodyShape3D(this));
 		add_shape(soft_body_shape);
 	} else if (p_force_move) {
-		SoftBodyShape3DSW *soft_body_shape = static_cast<SoftBodyShape3DSW *>(get_shape(0));
+		GodotSoftBodyShape3D *soft_body_shape = static_cast<GodotSoftBodyShape3D *>(get_shape(0));
 		soft_body_shape->update_bounds();
 	}
 }
 
-void SoftBody3DSW::deinitialize_shape() {
+void GodotSoftBody3D::deinitialize_shape() {
 	if (get_shape_count() > 0) {
-		Shape3DSW *shape = get_shape(0);
+		GodotShape3D *shape = get_shape(0);
 		remove_shape(shape);
 		memdelete(shape);
 	}
 }
 
-void SoftBody3DSW::destroy() {
+void GodotSoftBody3D::destroy() {
+	soft_mesh = RID();
+
 	map_visual_to_physics.clear();
 
 	node_tree.clear();
@@ -1238,7 +1250,7 @@ void SoftBody3DSW::destroy() {
 	deinitialize_shape();
 }
 
-void SoftBodyShape3DSW::update_bounds() {
+void GodotSoftBodyShape3D::update_bounds() {
 	ERR_FAIL_COND(!soft_body);
 
 	AABB collision_aabb = soft_body->get_bounds();
@@ -1246,13 +1258,13 @@ void SoftBodyShape3DSW::update_bounds() {
 	configure(collision_aabb);
 }
 
-SoftBodyShape3DSW::SoftBodyShape3DSW(SoftBody3DSW *p_soft_body) {
+GodotSoftBodyShape3D::GodotSoftBodyShape3D(GodotSoftBody3D *p_soft_body) {
 	soft_body = p_soft_body;
 	update_bounds();
 }
 
 struct _SoftBodyIntersectSegmentInfo {
-	const SoftBody3DSW *soft_body = nullptr;
+	const GodotSoftBody3D *soft_body = nullptr;
 	Vector3 from;
 	Vector3 dir;
 	Vector3 hit_position;
@@ -1280,7 +1292,7 @@ struct _SoftBodyIntersectSegmentInfo {
 	}
 };
 
-bool SoftBodyShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const {
+bool GodotSoftBodyShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, bool p_hit_back_faces) const {
 	_SoftBodyIntersectSegmentInfo query_info;
 	query_info.soft_body = soft_body;
 	query_info.from = p_begin;
@@ -1297,10 +1309,10 @@ bool SoftBodyShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3
 	return false;
 }
 
-bool SoftBodyShape3DSW::intersect_point(const Vector3 &p_point) const {
+bool GodotSoftBodyShape3D::intersect_point(const Vector3 &p_point) const {
 	return false;
 }
 
-Vector3 SoftBodyShape3DSW::get_closest_point_to(const Vector3 &p_point) const {
+Vector3 GodotSoftBodyShape3D::get_closest_point_to(const Vector3 &p_point) const {
 	ERR_FAIL_V_MSG(Vector3(), "Get closest point is not supported for soft bodies.");
 }
diff --git a/servers/physics_3d/soft_body_3d_sw.h b/servers/physics_3d/godot_soft_body_3d.h
index 58fd234fde..14ddc419cf 100644
--- a/servers/physics_3d/soft_body_3d_sw.h
+++ b/servers/physics_3d/godot_soft_body_3d.h
@@ -1,5 +1,5 @@
 /*************************************************************************/
-/*  soft_body_3d_sw.h                                                    */
+/*  godot_soft_body_3d.h                                                 */
 /*************************************************************************/
 /*                       This file is part of:                           */
 /*                           GODOT ENGINE                                */
@@ -28,11 +28,11 @@
 /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
 /*************************************************************************/
 
-#ifndef SOFT_BODY_3D_SW_H
-#define SOFT_BODY_3D_SW_H
+#ifndef GODOT_SOFT_BODY_3D_H
+#define GODOT_SOFT_BODY_3D_H
 
-#include "area_3d_sw.h"
-#include "collision_object_3d_sw.h"
+#include "godot_area_3d.h"
+#include "godot_collision_object_3d.h"
 
 #include "core/math/aabb.h"
 #include "core/math/dynamic_bvh.h"
@@ -40,12 +40,11 @@
 #include "core/templates/local_vector.h"
 #include "core/templates/set.h"
 #include "core/templates/vset.h"
-#include "scene/resources/mesh.h"
 
-class Constraint3DSW;
+class GodotConstraint3D;
 
-class SoftBody3DSW : public CollisionObject3DSW {
-	Ref<Mesh> soft_mesh;
+class GodotSoftBody3D : public GodotCollisionObject3D {
+	RID soft_mesh;
 
 	struct Node {
 		Vector3 s; // Source position
@@ -102,11 +101,9 @@ class SoftBody3DSW : public CollisionObject3DSW {
 	real_t drag_coefficient = 0.0; // [0,1]
 	LocalVector<int> pinned_vertices;
 
-	Vector3 gravity;
+	SelfList<GodotSoftBody3D> active_list;
 
-	SelfList<SoftBody3DSW> active_list;
-
-	Set<Constraint3DSW *> constraints;
+	Set<GodotConstraint3D *> constraints;
 
 	Vector<AreaCMP> areas;
 
@@ -114,20 +111,19 @@ class SoftBody3DSW : public CollisionObject3DSW {
 
 	uint64_t island_step = 0;
 
-	_FORCE_INLINE_ void _compute_area_gravity(const Area3DSW *p_area);
-	_FORCE_INLINE_ Vector3 _compute_area_windforce(const Area3DSW *p_area, const Face *p_face);
+	_FORCE_INLINE_ Vector3 _compute_area_windforce(const GodotArea3D *p_area, const Face *p_face);
 
 public:
-	SoftBody3DSW();
+	GodotSoftBody3D();
 
 	const AABB &get_bounds() const { return bounds; }
 
 	void set_state(PhysicsServer3D::BodyState p_state, const Variant &p_variant);
 	Variant get_state(PhysicsServer3D::BodyState p_state) const;
 
-	_FORCE_INLINE_ void add_constraint(Constraint3DSW *p_constraint) { constraints.insert(p_constraint); }
-	_FORCE_INLINE_ void remove_constraint(Constraint3DSW *p_constraint) { constraints.erase(p_constraint); }
-	_FORCE_INLINE_ const Set<Constraint3DSW *> &get_constraints() const { return constraints; }
+	_FORCE_INLINE_ void add_constraint(GodotConstraint3D *p_constraint) { constraints.insert(p_constraint); }
+	_FORCE_INLINE_ void remove_constraint(GodotConstraint3D *p_constraint) { constraints.erase(p_constraint); }
+	_FORCE_INLINE_ const Set<GodotConstraint3D *> &get_constraints() const { return constraints; }
 	_FORCE_INLINE_ void clear_constraints() { constraints.clear(); }
 
 	_FORCE_INLINE_ void add_exception(const RID &p_exception) { exceptions.insert(p_exception); }
@@ -138,7 +134,7 @@ public:
 	_FORCE_INLINE_ uint64_t get_island_step() const { return island_step; }
 	_FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step = p_step; }
 
-	_FORCE_INLINE_ void add_area(Area3DSW *p_area) {
+	_FORCE_INLINE_ void add_area(GodotArea3D *p_area) {
 		int index = areas.find(AreaCMP(p_area));
 		if (index > -1) {
 			areas.write[index].refCount += 1;
@@ -147,19 +143,19 @@ public:
 		}
 	}
 
-	_FORCE_INLINE_ void remove_area(Area3DSW *p_area) {
+	_FORCE_INLINE_ void remove_area(GodotArea3D *p_area) {
 		int index = areas.find(AreaCMP(p_area));
 		if (index > -1) {
 			areas.write[index].refCount -= 1;
 			if (areas[index].refCount < 1) {
-				areas.remove(index);
+				areas.remove_at(index);
 			}
 		}
 	}
 
-	virtual void set_space(Space3DSW *p_space);
+	virtual void set_space(GodotSpace3D *p_space);
 
-	void set_mesh(const Ref<Mesh> &p_mesh);
+	void set_mesh(RID p_mesh);
 
 	void update_rendering_server(RenderingServerHandler *p_rendering_server_handler);
 
@@ -233,7 +229,7 @@ private:
 
 	void add_velocity(const Vector3 &p_velocity);
 
-	void apply_forces(bool p_has_wind_forces);
+	void apply_forces(const LocalVector<GodotArea3D *> &p_wind_areas);
 
 	bool create_from_trimesh(const Vector<int> &p_indices, const Vector<Vector3> &p_vertices);
 	void generate_bending_constraints(int p_distance);
@@ -252,29 +248,29 @@ private:
 	void destroy();
 };
 
-class SoftBodyShape3DSW : public Shape3DSW {
-	SoftBody3DSW *soft_body = nullptr;
+class GodotSoftBodyShape3D : public GodotShape3D {
+	GodotSoftBody3D *soft_body = nullptr;
 
 public:
-	SoftBody3DSW *get_soft_body() const { return soft_body; }
+	GodotSoftBody3D *get_soft_body() const { return soft_body; }
 
-	virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_SOFT_BODY; }
-	virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { r_min = r_max = 0.0; }
-	virtual Vector3 get_support(const Vector3 &p_normal) const { return Vector3(); }
-	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { r_amount = 0; }
+	virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_SOFT_BODY; }
+	virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const override { r_min = r_max = 0.0; }
+	virtual Vector3 get_support(const Vector3 &p_normal) const override { return Vector3(); }
+	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override { r_amount = 0; }
 
-	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const;
-	virtual bool intersect_point(const Vector3 &p_point) const;
-	virtual Vector3 get_closest_point_to(const Vector3 &p_point) const;
-	virtual Vector3 get_moment_of_inertia(real_t p_mass) const { return Vector3(); }
+	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal, bool p_hit_back_faces) const override;
+	virtual bool intersect_point(const Vector3 &p_point) const override;
+	virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override;
+	virtual Vector3 get_moment_of_inertia(real_t p_mass) const override { return Vector3(); }
 
-	virtual void set_data(const Variant &p_data) {}
-	virtual Variant get_data() const { return Variant(); }
+	virtual void set_data(const Variant &p_data) override {}
+	virtual Variant get_data() const override { return Variant(); }
 
 	void update_bounds();
 
-	SoftBodyShape3DSW(SoftBody3DSW *p_soft_body);
-	~SoftBodyShape3DSW() {}
+	GodotSoftBodyShape3D(GodotSoftBody3D *p_soft_body);
+	~GodotSoftBodyShape3D() {}
 };
 
-#endif // SOFT_BODY_3D_SW_H
+#endif // GODOT_SOFT_BODY_3D_H
diff --git a/servers/physics_3d/space_3d_sw.cpp b/servers/physics_3d/godot_space_3d.cpp
index 37dee436df..f503273c88 100644
--- a/servers/physics_3d/space_3d_sw.cpp
+++ b/servers/physics_3d/godot_space_3d.cpp
@@ -1,5 +1,5 @@
 /*************************************************************************/
-/*  space_3d_sw.cpp                                                      */
+/*  godot_space_3d.cpp                                                   */
 /*************************************************************************/
 /*                       This file is part of:                           */
 /*                           GODOT ENGINE                                */
@@ -28,35 +28,38 @@
 /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
 /*************************************************************************/
 
-#include "space_3d_sw.h"
+#include "godot_space_3d.h"
+
+#include "godot_collision_solver_3d.h"
+#include "godot_physics_server_3d.h"
 
-#include "collision_solver_3d_sw.h"
 #include "core/config/project_settings.h"
-#include "physics_server_3d_sw.h"
 
-_FORCE_INLINE_ static bool _can_collide_with(CollisionObject3DSW *p_object, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) {
+#define TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR 0.05
+
+_FORCE_INLINE_ static bool _can_collide_with(GodotCollisionObject3D *p_object, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) {
 	if (!(p_object->get_collision_layer() & p_collision_mask)) {
 		return false;
 	}
 
-	if (p_object->get_type() == CollisionObject3DSW::TYPE_AREA && !p_collide_with_areas) {
+	if (p_object->get_type() == GodotCollisionObject3D::TYPE_AREA && !p_collide_with_areas) {
 		return false;
 	}
 
-	if (p_object->get_type() == CollisionObject3DSW::TYPE_BODY && !p_collide_with_bodies) {
+	if (p_object->get_type() == GodotCollisionObject3D::TYPE_BODY && !p_collide_with_bodies) {
 		return false;
 	}
 
-	if (p_object->get_type() == CollisionObject3DSW::TYPE_SOFT_BODY && !p_collide_with_bodies) {
+	if (p_object->get_type() == GodotCollisionObject3D::TYPE_SOFT_BODY && !p_collide_with_bodies) {
 		return false;
 	}
 
 	return true;
 }
 
-int PhysicsDirectSpaceState3DSW::intersect_point(const Vector3 &p_point, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) {
+int GodotPhysicsDirectSpaceState3D::intersect_point(const PointParameters &p_parameters, ShapeResult *r_results, int p_result_max) {
 	ERR_FAIL_COND_V(space->locked, false);
-	int amount = space->broadphase->cull_point(p_point, space->intersection_query_results, Space3DSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
+	int amount = space->broadphase->cull_point(p_parameters.position, space->intersection_query_results, GodotSpace3D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
 	int cc = 0;
 
 	//Transform3D ai = p_xform.affine_inverse();
@@ -66,23 +69,23 @@ int PhysicsDirectSpaceState3DSW::intersect_point(const Vector3 &p_point, ShapeRe
 			break;
 		}
 
-		if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) {
+		if (!_can_collide_with(space->intersection_query_results[i], p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas)) {
 			continue;
 		}
 
 		//area can't be picked by ray (default)
 
-		if (p_exclude.has(space->intersection_query_results[i]->get_self())) {
+		if (p_parameters.exclude.has(space->intersection_query_results[i]->get_self())) {
 			continue;
 		}
 
-		const CollisionObject3DSW *col_obj = space->intersection_query_results[i];
+		const GodotCollisionObject3D *col_obj = space->intersection_query_results[i];
 		int shape_idx = space->intersection_query_subindex_results[i];
 
 		Transform3D inv_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
 		inv_xform.affine_invert();
 
-		if (!col_obj->get_shape(shape_idx)->intersect_point(inv_xform.xform(p_point))) {
+		if (!col_obj->get_shape(shape_idx)->intersect_point(inv_xform.xform(p_parameters.position))) {
 			continue;
 		}
 
@@ -101,39 +104,39 @@ int PhysicsDirectSpaceState3DSW::intersect_point(const Vector3 &p_point, ShapeRe
 	return cc;
 }
 
-bool PhysicsDirectSpaceState3DSW::intersect_ray(const Vector3 &p_from, const Vector3 &p_to, RayResult &r_result, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, bool p_pick_ray) {
+bool GodotPhysicsDirectSpaceState3D::intersect_ray(const RayParameters &p_parameters, RayResult &r_result) {
 	ERR_FAIL_COND_V(space->locked, false);
 
 	Vector3 begin, end;
 	Vector3 normal;
-	begin = p_from;
-	end = p_to;
+	begin = p_parameters.from;
+	end = p_parameters.to;
 	normal = (end - begin).normalized();
 
-	int amount = space->broadphase->cull_segment(begin, end, space->intersection_query_results, Space3DSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
+	int amount = space->broadphase->cull_segment(begin, end, space->intersection_query_results, GodotSpace3D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
 
 	//todo, create another array that references results, compute AABBs and check closest point to ray origin, sort, and stop evaluating results when beyond first collision
 
 	bool collided = false;
 	Vector3 res_point, res_normal;
 	int res_shape;
-	const CollisionObject3DSW *res_obj;
+	const GodotCollisionObject3D *res_obj;
 	real_t min_d = 1e10;
 
 	for (int i = 0; i < amount; i++) {
-		if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) {
+		if (!_can_collide_with(space->intersection_query_results[i], p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas)) {
 			continue;
 		}
 
-		if (p_pick_ray && !(space->intersection_query_results[i]->is_ray_pickable())) {
+		if (p_parameters.pick_ray && !(space->intersection_query_results[i]->is_ray_pickable())) {
 			continue;
 		}
 
-		if (p_exclude.has(space->intersection_query_results[i]->get_self())) {
+		if (p_parameters.exclude.has(space->intersection_query_results[i]->get_self())) {
 			continue;
 		}
 
-		const CollisionObject3DSW *col_obj = space->intersection_query_results[i];
+		const GodotCollisionObject3D *col_obj = space->intersection_query_results[i];
 
 		int shape_idx = space->intersection_query_subindex_results[i];
 		Transform3D inv_xform = col_obj->get_shape_inv_transform(shape_idx) * col_obj->get_inv_transform();
@@ -141,11 +144,27 @@ bool PhysicsDirectSpaceState3DSW::intersect_ray(const Vector3 &p_from, const Vec
 		Vector3 local_from = inv_xform.xform(begin);
 		Vector3 local_to = inv_xform.xform(end);
 
-		const Shape3DSW *shape = col_obj->get_shape(shape_idx);
+		const GodotShape3D *shape = col_obj->get_shape(shape_idx);
 
 		Vector3 shape_point, shape_normal;
 
-		if (shape->intersect_segment(local_from, local_to, shape_point, shape_normal)) {
+		if (shape->intersect_point(local_from)) {
+			if (p_parameters.hit_from_inside) {
+				// Hit shape at starting point.
+				min_d = 0;
+				res_point = local_from;
+				res_normal = Vector3();
+				res_shape = shape_idx;
+				res_obj = col_obj;
+				collided = true;
+				break;
+			} else {
+				// Ignore shape when starting inside.
+				continue;
+			}
+		}
+
+		if (shape->intersect_segment(local_from, local_to, shape_point, shape_normal, p_parameters.hit_back_faces)) {
 			Transform3D xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
 			shape_point = xform.xform(shape_point);
 
@@ -180,17 +199,17 @@ bool PhysicsDirectSpaceState3DSW::intersect_ray(const Vector3 &p_from, const Vec
 	return true;
 }
 
-int PhysicsDirectSpaceState3DSW::intersect_shape(const RID &p_shape, const Transform3D &p_xform, real_t p_margin, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) {
+int GodotPhysicsDirectSpaceState3D::intersect_shape(const ShapeParameters &p_parameters, ShapeResult *r_results, int p_result_max) {
 	if (p_result_max <= 0) {
 		return 0;
 	}
 
-	Shape3DSW *shape = PhysicsServer3DSW::singletonsw->shape_owner.getornull(p_shape);
+	GodotShape3D *shape = GodotPhysicsServer3D::godot_singleton->shape_owner.get_or_null(p_parameters.shape_rid);
 	ERR_FAIL_COND_V(!shape, 0);
 
-	AABB aabb = p_xform.xform(shape->get_aabb());
+	AABB aabb = p_parameters.transform.xform(shape->get_aabb());
 
-	int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, Space3DSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
+	int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, GodotSpace3D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
 
 	int cc = 0;
 
@@ -201,20 +220,20 @@ int PhysicsDirectSpaceState3DSW::intersect_shape(const RID &p_shape, const Trans
 			break;
 		}
 
-		if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) {
+		if (!_can_collide_with(space->intersection_query_results[i], p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas)) {
 			continue;
 		}
 
 		//area can't be picked by ray (default)
 
-		if (p_exclude.has(space->intersection_query_results[i]->get_self())) {
+		if (p_parameters.exclude.has(space->intersection_query_results[i]->get_self())) {
 			continue;
 		}
 
-		const CollisionObject3DSW *col_obj = space->intersection_query_results[i];
+		const GodotCollisionObject3D *col_obj = space->intersection_query_results[i];
 		int shape_idx = space->intersection_query_subindex_results[i];
 
-		if (!CollisionSolver3DSW::solve_static(shape, p_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), nullptr, nullptr, nullptr, p_margin, 0)) {
+		if (!GodotCollisionSolver3D::solve_static(shape, p_parameters.transform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), nullptr, nullptr, nullptr, p_parameters.margin, 0)) {
 			continue;
 		}
 
@@ -235,40 +254,40 @@ int PhysicsDirectSpaceState3DSW::intersect_shape(const RID &p_shape, const Trans
 	return cc;
 }
 
-bool PhysicsDirectSpaceState3DSW::cast_motion(const RID &p_shape, const Transform3D &p_xform, const Vector3 &p_motion, real_t p_margin, real_t &p_closest_safe, real_t &p_closest_unsafe, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, ShapeRestInfo *r_info) {
-	Shape3DSW *shape = PhysicsServer3DSW::singletonsw->shape_owner.getornull(p_shape);
+bool GodotPhysicsDirectSpaceState3D::cast_motion(const ShapeParameters &p_parameters, real_t &p_closest_safe, real_t &p_closest_unsafe, ShapeRestInfo *r_info) {
+	GodotShape3D *shape = GodotPhysicsServer3D::godot_singleton->shape_owner.get_or_null(p_parameters.shape_rid);
 	ERR_FAIL_COND_V(!shape, false);
 
-	AABB aabb = p_xform.xform(shape->get_aabb());
-	aabb = aabb.merge(AABB(aabb.position + p_motion, aabb.size)); //motion
-	aabb = aabb.grow(p_margin);
+	AABB aabb = p_parameters.transform.xform(shape->get_aabb());
+	aabb = aabb.merge(AABB(aabb.position + p_parameters.motion, aabb.size)); //motion
+	aabb = aabb.grow(p_parameters.margin);
 
-	int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, Space3DSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
+	int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, GodotSpace3D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
 
 	real_t best_safe = 1;
 	real_t best_unsafe = 1;
 
-	Transform3D xform_inv = p_xform.affine_inverse();
-	MotionShape3DSW mshape;
+	Transform3D xform_inv = p_parameters.transform.affine_inverse();
+	GodotMotionShape3D mshape;
 	mshape.shape = shape;
-	mshape.motion = xform_inv.basis.xform(p_motion);
+	mshape.motion = xform_inv.basis.xform(p_parameters.motion);
 
 	bool best_first = true;
 
-	Vector3 motion_normal = p_motion.normalized();
+	Vector3 motion_normal = p_parameters.motion.normalized();
 
 	Vector3 closest_A, closest_B;
 
 	for (int i = 0; i < amount; i++) {
-		if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) {
+		if (!_can_collide_with(space->intersection_query_results[i], p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas)) {
 			continue;
 		}
 
-		if (p_exclude.has(space->intersection_query_results[i]->get_self())) {
+		if (p_parameters.exclude.has(space->intersection_query_results[i]->get_self())) {
 			continue; //ignore excluded
 		}
 
-		const CollisionObject3DSW *col_obj = space->intersection_query_results[i];
+		const GodotCollisionObject3D *col_obj = space->intersection_query_results[i];
 		int shape_idx = space->intersection_query_subindex_results[i];
 
 		Vector3 point_A, point_B;
@@ -276,14 +295,14 @@ bool PhysicsDirectSpaceState3DSW::cast_motion(const RID &p_shape, const Transfor
 
 		Transform3D col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
 		//test initial overlap, does it collide if going all the way?
-		if (CollisionSolver3DSW::solve_distance(&mshape, p_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, aabb, &sep_axis)) {
+		if (GodotCollisionSolver3D::solve_distance(&mshape, p_parameters.transform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, aabb, &sep_axis)) {
 			continue;
 		}
 
 		//test initial overlap, ignore objects it's inside of.
 		sep_axis = motion_normal;
 
-		if (!CollisionSolver3DSW::solve_distance(shape, p_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, aabb, &sep_axis)) {
+		if (!GodotCollisionSolver3D::solve_distance(shape, p_parameters.transform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, aabb, &sep_axis)) {
 			continue;
 		}
 
@@ -294,11 +313,11 @@ bool PhysicsDirectSpaceState3DSW::cast_motion(const RID &p_shape, const Transfor
 		for (int j = 0; j < 8; j++) { //steps should be customizable..
 			real_t fraction = low + (hi - low) * fraction_coeff;
 
-			mshape.motion = xform_inv.basis.xform(p_motion * fraction);
+			mshape.motion = xform_inv.basis.xform(p_parameters.motion * fraction);
 
 			Vector3 lA, lB;
 			Vector3 sep = motion_normal; //important optimization for this to work fast enough
-			bool collided = !CollisionSolver3DSW::solve_distance(&mshape, p_xform, col_obj->get_shape(shape_idx), col_obj_xform, lA, lB, aabb, &sep);
+			bool collided = !GodotCollisionSolver3D::solve_distance(&mshape, p_parameters.transform, col_obj->get_shape(shape_idx), col_obj_xform, lA, lB, aabb, &sep);
 
 			if (collided) {
 				hi = fraction;
@@ -340,8 +359,8 @@ bool PhysicsDirectSpaceState3DSW::cast_motion(const RID &p_shape, const Transfor
 			r_info->point = closest_B;
 			r_info->normal = (closest_A - closest_B).normalized();
 			best_first = false;
-			if (col_obj->get_type() == CollisionObject3DSW::TYPE_BODY) {
-				const Body3DSW *body = static_cast<const Body3DSW *>(col_obj);
+			if (col_obj->get_type() == GodotCollisionObject3D::TYPE_BODY) {
+				const GodotBody3D *body = static_cast<const GodotBody3D *>(col_obj);
 				Vector3 rel_vec = closest_B - (body->get_transform().origin + body->get_center_of_mass());
 				r_info->linear_velocity = body->get_linear_velocity() + (body->get_angular_velocity()).cross(rel_vec);
 			}
@@ -354,44 +373,44 @@ bool PhysicsDirectSpaceState3DSW::cast_motion(const RID &p_shape, const Transfor
 	return true;
 }
 
-bool PhysicsDirectSpaceState3DSW::collide_shape(RID p_shape, const Transform3D &p_shape_xform, real_t p_margin, Vector3 *r_results, int p_result_max, int &r_result_count, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) {
+bool GodotPhysicsDirectSpaceState3D::collide_shape(const ShapeParameters &p_parameters, Vector3 *r_results, int p_result_max, int &r_result_count) {
 	if (p_result_max <= 0) {
 		return false;
 	}
 
-	Shape3DSW *shape = PhysicsServer3DSW::singletonsw->shape_owner.getornull(p_shape);
+	GodotShape3D *shape = GodotPhysicsServer3D::godot_singleton->shape_owner.get_or_null(p_parameters.shape_rid);
 	ERR_FAIL_COND_V(!shape, 0);
 
-	AABB aabb = p_shape_xform.xform(shape->get_aabb());
-	aabb = aabb.grow(p_margin);
+	AABB aabb = p_parameters.transform.xform(shape->get_aabb());
+	aabb = aabb.grow(p_parameters.margin);
 
-	int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, Space3DSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
+	int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, GodotSpace3D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
 
 	bool collided = false;
 	r_result_count = 0;
 
-	PhysicsServer3DSW::CollCbkData cbk;
+	GodotPhysicsServer3D::CollCbkData cbk;
 	cbk.max = p_result_max;
 	cbk.amount = 0;
 	cbk.ptr = r_results;
-	CollisionSolver3DSW::CallbackResult cbkres = PhysicsServer3DSW::_shape_col_cbk;
+	GodotCollisionSolver3D::CallbackResult cbkres = GodotPhysicsServer3D::_shape_col_cbk;
 
-	PhysicsServer3DSW::CollCbkData *cbkptr = &cbk;
+	GodotPhysicsServer3D::CollCbkData *cbkptr = &cbk;
 
 	for (int i = 0; i < amount; i++) {
-		if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) {
+		if (!_can_collide_with(space->intersection_query_results[i], p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas)) {
 			continue;
 		}
 
-		const CollisionObject3DSW *col_obj = space->intersection_query_results[i];
+		const GodotCollisionObject3D *col_obj = space->intersection_query_results[i];
 
-		if (p_exclude.has(col_obj->get_self())) {
+		if (p_parameters.exclude.has(col_obj->get_self())) {
 			continue;
 		}
 
 		int shape_idx = space->intersection_query_subindex_results[i];
 
-		if (CollisionSolver3DSW::solve_static(shape, p_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), cbkres, cbkptr, nullptr, p_margin)) {
+		if (GodotCollisionSolver3D::solve_static(shape, p_parameters.transform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), cbkres, cbkptr, nullptr, p_parameters.margin)) {
 			collided = true;
 		}
 	}
@@ -401,17 +420,27 @@ bool PhysicsDirectSpaceState3DSW::collide_shape(RID p_shape, const Transform3D &
 	return collided;
 }
 
+struct _RestResultData {
+	const GodotCollisionObject3D *object = nullptr;
+	int local_shape = 0;
+	int shape = 0;
+	Vector3 contact;
+	Vector3 normal;
+	real_t len = 0.0;
+};
+
 struct _RestCallbackData {
-	const CollisionObject3DSW *object;
-	const CollisionObject3DSW *best_object;
-	int local_shape;
-	int best_local_shape;
-	int shape;
-	int best_shape;
-	Vector3 best_contact;
-	Vector3 best_normal;
-	real_t best_len;
-	real_t min_allowed_depth;
+	const GodotCollisionObject3D *object = nullptr;
+	int local_shape = 0;
+	int shape = 0;
+
+	real_t min_allowed_depth = 0.0;
+
+	_RestResultData best_result;
+
+	int max_results = 0;
+	int result_count = 0;
+	_RestResultData *other_results = nullptr;
 };
 
 static void _rest_cbk_result(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata) {
@@ -422,41 +451,82 @@ static void _rest_cbk_result(const Vector3 &p_point_A, int p_index_A, const Vect
 	if (len < rd->min_allowed_depth) {
 		return;
 	}
-	if (len <= rd->best_len) {
+
+	bool is_best_result = (len > rd->best_result.len);
+
+	if (rd->other_results && rd->result_count > 0) {
+		// Consider as new result by default.
+		int prev_result_count = rd->result_count++;
+
+		int result_index = 0;
+		real_t tested_len = is_best_result ? rd->best_result.len : len;
+		for (; result_index < prev_result_count - 1; ++result_index) {
+			if (tested_len > rd->other_results[result_index].len) {
+				// Re-using a previous result.
+				rd->result_count--;
+				break;
+			}
+		}
+
+		if (result_index < rd->max_results - 1) {
+			_RestResultData &result = rd->other_results[result_index];
+
+			if (is_best_result) {
+				// Keep the previous best result as separate result.
+				result = rd->best_result;
+			} else {
+				// Keep this result as separate result.
+				result.len = len;
+				result.contact = p_point_B;
+				result.normal = contact_rel / len;
+				result.object = rd->object;
+				result.shape = rd->shape;
+				result.local_shape = rd->local_shape;
+			}
+		} else {
+			// Discarding this result.
+			rd->result_count--;
+		}
+	} else if (is_best_result) {
+		rd->result_count = 1;
+	}
+
+	if (!is_best_result) {
 		return;
 	}
 
-	rd->best_len = len;
-	rd->best_contact = p_point_B;
-	rd->best_normal = contact_rel / len;
-	rd->best_object = rd->object;
-	rd->best_shape = rd->shape;
-	rd->best_local_shape = rd->local_shape;
+	rd->best_result.len = len;
+	rd->best_result.contact = p_point_B;
+	rd->best_result.normal = contact_rel / len;
+	rd->best_result.object = rd->object;
+	rd->best_result.shape = rd->shape;
+	rd->best_result.local_shape = rd->local_shape;
 }
 
-bool PhysicsDirectSpaceState3DSW::rest_info(RID p_shape, const Transform3D &p_shape_xform, real_t p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) {
-	Shape3DSW *shape = PhysicsServer3DSW::singletonsw->shape_owner.getornull(p_shape);
+bool GodotPhysicsDirectSpaceState3D::rest_info(const ShapeParameters &p_parameters, ShapeRestInfo *r_info) {
+	GodotShape3D *shape = GodotPhysicsServer3D::godot_singleton->shape_owner.get_or_null(p_parameters.shape_rid);
 	ERR_FAIL_COND_V(!shape, 0);
 
-	AABB aabb = p_shape_xform.xform(shape->get_aabb());
-	aabb = aabb.grow(p_margin);
+	AABB aabb = p_parameters.transform.xform(shape->get_aabb());
+	aabb = aabb.grow(p_parameters.margin);
 
-	int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, Space3DSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
+	int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, GodotSpace3D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
 
 	_RestCallbackData rcd;
-	rcd.best_len = 0;
-	rcd.best_object = nullptr;
-	rcd.best_shape = 0;
-	rcd.min_allowed_depth = space->test_motion_min_contact_depth;
+
+	// Allowed depth can't be lower than motion length, in order to handle contacts at low speed.
+	real_t motion_length = p_parameters.motion.length();
+	real_t min_contact_depth = p_parameters.margin * TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR;
+	rcd.min_allowed_depth = MIN(motion_length, min_contact_depth);
 
 	for (int i = 0; i < amount; i++) {
-		if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) {
+		if (!_can_collide_with(space->intersection_query_results[i], p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas)) {
 			continue;
 		}
 
-		const CollisionObject3DSW *col_obj = space->intersection_query_results[i];
+		const GodotCollisionObject3D *col_obj = space->intersection_query_results[i];
 
-		if (p_exclude.has(col_obj->get_self())) {
+		if (p_parameters.exclude.has(col_obj->get_self())) {
 			continue;
 		}
 
@@ -464,24 +534,24 @@ bool PhysicsDirectSpaceState3DSW::rest_info(RID p_shape, const Transform3D &p_sh
 
 		rcd.object = col_obj;
 		rcd.shape = shape_idx;
-		bool sc = CollisionSolver3DSW::solve_static(shape, p_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), _rest_cbk_result, &rcd, nullptr, p_margin);
+		bool sc = GodotCollisionSolver3D::solve_static(shape, p_parameters.transform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), _rest_cbk_result, &rcd, nullptr, p_parameters.margin);
 		if (!sc) {
 			continue;
 		}
 	}
 
-	if (rcd.best_len == 0 || !rcd.best_object) {
+	if (rcd.best_result.len == 0 || !rcd.best_result.object) {
 		return false;
 	}
 
-	r_info->collider_id = rcd.best_object->get_instance_id();
-	r_info->shape = rcd.best_shape;
-	r_info->normal = rcd.best_normal;
-	r_info->point = rcd.best_contact;
-	r_info->rid = rcd.best_object->get_self();
-	if (rcd.best_object->get_type() == CollisionObject3DSW::TYPE_BODY) {
-		const Body3DSW *body = static_cast<const Body3DSW *>(rcd.best_object);
-		Vector3 rel_vec = rcd.best_contact - (body->get_transform().origin + body->get_center_of_mass());
+	r_info->collider_id = rcd.best_result.object->get_instance_id();
+	r_info->shape = rcd.best_result.shape;
+	r_info->normal = rcd.best_result.normal;
+	r_info->point = rcd.best_result.contact;
+	r_info->rid = rcd.best_result.object->get_self();
+	if (rcd.best_result.object->get_type() == GodotCollisionObject3D::TYPE_BODY) {
+		const GodotBody3D *body = static_cast<const GodotBody3D *>(rcd.best_result.object);
+		Vector3 rel_vec = rcd.best_result.contact - (body->get_transform().origin + body->get_center_of_mass());
 		r_info->linear_velocity = body->get_linear_velocity() + (body->get_angular_velocity()).cross(rel_vec);
 
 	} else {
@@ -491,10 +561,10 @@ bool PhysicsDirectSpaceState3DSW::rest_info(RID p_shape, const Transform3D &p_sh
 	return true;
 }
 
-Vector3 PhysicsDirectSpaceState3DSW::get_closest_point_to_object_volume(RID p_object, const Vector3 p_point) const {
-	CollisionObject3DSW *obj = PhysicsServer3DSW::singletonsw->area_owner.getornull(p_object);
+Vector3 GodotPhysicsDirectSpaceState3D::get_closest_point_to_object_volume(RID p_object, const Vector3 p_point) const {
+	GodotCollisionObject3D *obj = GodotPhysicsServer3D::godot_singleton->area_owner.get_or_null(p_object);
 	if (!obj) {
-		obj = PhysicsServer3DSW::singletonsw->body_owner.getornull(p_object);
+		obj = GodotPhysicsServer3D::godot_singleton->body_owner.get_or_null(p_object);
 	}
 	ERR_FAIL_COND_V(!obj, Vector3());
 
@@ -511,7 +581,7 @@ Vector3 PhysicsDirectSpaceState3DSW::get_closest_point_to_object_volume(RID p_ob
 		}
 
 		Transform3D shape_xform = obj->get_transform() * obj->get_shape_transform(i);
-		Shape3DSW *shape = obj->get_shape(i);
+		GodotShape3D *shape = obj->get_shape(i);
 
 		Vector3 point = shape->get_closest_point_to(shape_xform.affine_inverse().xform(p_point));
 		point = shape_xform.xform(point);
@@ -531,13 +601,13 @@ Vector3 PhysicsDirectSpaceState3DSW::get_closest_point_to_object_volume(RID p_ob
 	}
 }
 
-PhysicsDirectSpaceState3DSW::PhysicsDirectSpaceState3DSW() {
+GodotPhysicsDirectSpaceState3D::GodotPhysicsDirectSpaceState3D() {
 	space = nullptr;
 }
 
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////
 
-int Space3DSW::_cull_aabb_for_body(Body3DSW *p_body, const AABB &p_aabb) {
+int GodotSpace3D::_cull_aabb_for_body(GodotBody3D *p_body, const AABB &p_aabb) {
 	int amount = broadphase->cull_aabb(p_aabb, intersection_query_results, INTERSECTION_QUERY_MAX, intersection_query_subindex_results);
 
 	for (int i = 0; i < amount; i++) {
@@ -545,13 +615,13 @@ int Space3DSW::_cull_aabb_for_body(Body3DSW *p_body, const AABB &p_aabb) {
 
 		if (intersection_query_results[i] == p_body) {
 			keep = false;
-		} else if (intersection_query_results[i]->get_type() == CollisionObject3DSW::TYPE_AREA) {
+		} else if (intersection_query_results[i]->get_type() == GodotCollisionObject3D::TYPE_AREA) {
 			keep = false;
-		} else if (intersection_query_results[i]->get_type() == CollisionObject3DSW::TYPE_SOFT_BODY) {
+		} else if (intersection_query_results[i]->get_type() == GodotCollisionObject3D::TYPE_SOFT_BODY) {
 			keep = false;
-		} else if (!p_body->collides_with(static_cast<Body3DSW *>(intersection_query_results[i]))) {
+		} else if (!p_body->collides_with(static_cast<GodotBody3D *>(intersection_query_results[i]))) {
 			keep = false;
-		} else if (static_cast<Body3DSW *>(intersection_query_results[i])->has_exception(p_body->get_self()) || p_body->has_exception(intersection_query_results[i]->get_self())) {
+		} else if (static_cast<GodotBody3D *>(intersection_query_results[i])->has_exception(p_body->get_self()) || p_body->has_exception(intersection_query_results[i]->get_self())) {
 			keep = false;
 		}
 
@@ -569,7 +639,7 @@ int Space3DSW::_cull_aabb_for_body(Body3DSW *p_body, const AABB &p_aabb) {
 	return amount;
 }
 
-bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, const Vector3 &p_motion, real_t p_margin, PhysicsServer3D::MotionResult *r_result, bool p_collide_separation_ray, const Set<RID> &p_exclude) {
+bool GodotSpace3D::test_body_motion(GodotBody3D *p_body, const PhysicsServer3D::MotionParameters &p_parameters, PhysicsServer3D::MotionResult *r_result) {
 	//give me back regular physics engine logic
 	//this is madness
 	//and most people using this function will think
@@ -577,10 +647,12 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, co
 	//this took about a week to get right..
 	//but is it right? who knows at this point..
 
+	ERR_FAIL_INDEX_V(p_parameters.max_collisions, PhysicsServer3D::MotionResult::MAX_COLLISIONS, false);
+
 	if (r_result) {
-		r_result->collider_id = ObjectID();
-		r_result->collider_shape = 0;
+		*r_result = PhysicsServer3D::MotionResult();
 	}
+
 	AABB body_aabb;
 	bool shapes_found = false;
 
@@ -599,21 +671,22 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, co
 
 	if (!shapes_found) {
 		if (r_result) {
-			*r_result = PhysicsServer3D::MotionResult();
-			r_result->travel = p_motion;
+			r_result->travel = p_parameters.motion;
 		}
 
 		return false;
 	}
 
 	// Undo the currently transform the physics server is aware of and apply the provided one
-	body_aabb = p_from.xform(p_body->get_inv_transform().xform(body_aabb));
-	body_aabb = body_aabb.grow(p_margin);
+	body_aabb = p_parameters.from.xform(p_body->get_inv_transform().xform(body_aabb));
+	body_aabb = body_aabb.grow(p_parameters.margin);
+
+	real_t min_contact_depth = p_parameters.margin * TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR;
 
-	real_t motion_length = p_motion.length();
-	Vector3 motion_normal = p_motion / motion_length;
+	real_t motion_length = p_parameters.motion.length();
+	Vector3 motion_normal = p_parameters.motion / motion_length;
 
-	Transform3D body_transform = p_from;
+	Transform3D body_transform = p_parameters.from;
 
 	bool recovered = false;
 
@@ -625,13 +698,13 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, co
 		Vector3 sr[max_results * 2];
 
 		do {
-			PhysicsServer3DSW::CollCbkData cbk;
+			GodotPhysicsServer3D::CollCbkData cbk;
 			cbk.max = max_results;
 			cbk.amount = 0;
 			cbk.ptr = sr;
 
-			PhysicsServer3DSW::CollCbkData *cbkptr = &cbk;
-			CollisionSolver3DSW::CallbackResult cbkres = PhysicsServer3DSW::_shape_col_cbk;
+			GodotPhysicsServer3D::CollCbkData *cbkptr = &cbk;
+			GodotCollisionSolver3D::CallbackResult cbkres = GodotPhysicsServer3D::_shape_col_cbk;
 
 			bool collided = false;
 
@@ -643,17 +716,20 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, co
 				}
 
 				Transform3D body_shape_xform = body_transform * p_body->get_shape_transform(j);
-				Shape3DSW *body_shape = p_body->get_shape(j);
+				GodotShape3D *body_shape = p_body->get_shape(j);
 
 				for (int i = 0; i < amount; i++) {
-					const CollisionObject3DSW *col_obj = intersection_query_results[i];
-					if (p_exclude.has(col_obj->get_self())) {
+					const GodotCollisionObject3D *col_obj = intersection_query_results[i];
+					if (p_parameters.exclude_bodies.has(col_obj->get_self())) {
+						continue;
+					}
+					if (p_parameters.exclude_objects.has(col_obj->get_instance_id())) {
 						continue;
 					}
 
 					int shape_idx = intersection_query_subindex_results[i];
 
-					if (CollisionSolver3DSW::solve_static(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), cbkres, cbkptr, nullptr, p_margin)) {
+					if (GodotCollisionSolver3D::solve_static(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), cbkres, cbkptr, nullptr, p_parameters.margin)) {
 						collided = cbk.amount > 0;
 					}
 				}
@@ -663,8 +739,9 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, co
 				break;
 			}
 
-			Vector3 recover_motion;
+			recovered = true;
 
+			Vector3 recover_motion;
 			for (int i = 0; i < cbk.amount; i++) {
 				Vector3 a = sr[i * 2 + 0];
 				Vector3 b = sr[i * 2 + 1];
@@ -675,9 +752,9 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, co
 
 				// Compute depth on recovered motion.
 				real_t depth = n.dot(a + recover_motion) - d;
-				if (depth > 0.0) {
+				if (depth > min_contact_depth + CMP_EPSILON) {
 					// Only recover if there is penetration.
-					recover_motion -= n * depth * 0.4;
+					recover_motion -= n * (depth - min_contact_depth) * 0.4;
 				}
 			}
 
@@ -686,8 +763,6 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, co
 				break;
 			}
 
-			recovered = true;
-
 			body_transform.origin += recover_motion;
 			body_aabb.position += recover_motion;
 
@@ -704,7 +779,7 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, co
 		// STEP 2 ATTEMPT MOTION
 
 		AABB motion_aabb = body_aabb;
-		motion_aabb.position += p_motion;
+		motion_aabb.position += p_parameters.motion;
 		motion_aabb = motion_aabb.merge(body_aabb);
 
 		int amount = _cull_aabb_for_body(p_body, motion_aabb);
@@ -714,13 +789,13 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, co
 				continue;
 			}
 
-			Shape3DSW *body_shape = p_body->get_shape(j);
+			GodotShape3D *body_shape = p_body->get_shape(j);
 
 			// Colliding separation rays allows to properly snap to the ground,
 			// otherwise it's not needed in regular motion.
-			if (!p_collide_separation_ray && (body_shape->get_type() == PhysicsServer3D::SHAPE_SEPARATION_RAY)) {
+			if (!p_parameters.collide_separation_ray && (body_shape->get_type() == PhysicsServer3D::SHAPE_SEPARATION_RAY)) {
 				// When slide on slope is on, separation ray shape acts like a regular shape.
-				if (!static_cast<SeparationRayShape3DSW *>(body_shape)->get_slide_on_slope()) {
+				if (!static_cast<GodotSeparationRayShape3D *>(body_shape)->get_slide_on_slope()) {
 					continue;
 				}
 			}
@@ -728,9 +803,9 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, co
 			Transform3D body_shape_xform = body_transform * p_body->get_shape_transform(j);
 
 			Transform3D body_shape_xform_inv = body_shape_xform.affine_inverse();
-			MotionShape3DSW mshape;
+			GodotMotionShape3D mshape;
 			mshape.shape = body_shape;
-			mshape.motion = body_shape_xform_inv.basis.xform(p_motion);
+			mshape.motion = body_shape_xform_inv.basis.xform(p_parameters.motion);
 
 			bool stuck = false;
 
@@ -738,8 +813,11 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, co
 			real_t best_unsafe = 1;
 
 			for (int i = 0; i < amount; i++) {
-				const CollisionObject3DSW *col_obj = intersection_query_results[i];
-				if (p_exclude.has(col_obj->get_self())) {
+				const GodotCollisionObject3D *col_obj = intersection_query_results[i];
+				if (p_parameters.exclude_bodies.has(col_obj->get_self())) {
+					continue;
+				}
+				if (p_parameters.exclude_objects.has(col_obj->get_instance_id())) {
 					continue;
 				}
 
@@ -751,12 +829,12 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, co
 
 				Transform3D col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
 				//test initial overlap, does it collide if going all the way?
-				if (CollisionSolver3DSW::solve_distance(&mshape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, motion_aabb, &sep_axis)) {
+				if (GodotCollisionSolver3D::solve_distance(&mshape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, motion_aabb, &sep_axis)) {
 					continue;
 				}
 				sep_axis = motion_normal;
 
-				if (!CollisionSolver3DSW::solve_distance(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, motion_aabb, &sep_axis)) {
+				if (!GodotCollisionSolver3D::solve_distance(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, motion_aabb, &sep_axis)) {
 					stuck = true;
 					break;
 				}
@@ -768,11 +846,11 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, co
 				for (int k = 0; k < 8; k++) { //steps should be customizable..
 					real_t fraction = low + (hi - low) * fraction_coeff;
 
-					mshape.motion = body_shape_xform_inv.basis.xform(p_motion * fraction);
+					mshape.motion = body_shape_xform_inv.basis.xform(p_parameters.motion * fraction);
 
 					Vector3 lA, lB;
 					Vector3 sep = motion_normal; //important optimization for this to work fast enough
-					bool collided = !CollisionSolver3DSW::solve_distance(&mshape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj_xform, lA, lB, motion_aabb, &sep);
+					bool collided = !GodotCollisionSolver3D::solve_distance(&mshape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj_xform, lA, lB, motion_aabb, &sep);
 
 					if (collided) {
 						hi = fraction;
@@ -830,15 +908,18 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, co
 
 		//it collided, let's get the rest info in unsafe advance
 		Transform3D ugt = body_transform;
-		ugt.origin += p_motion * unsafe;
+		ugt.origin += p_parameters.motion * unsafe;
+
+		_RestResultData results[PhysicsServer3D::MotionResult::MAX_COLLISIONS];
 
 		_RestCallbackData rcd;
-		rcd.best_len = 0;
-		rcd.best_object = nullptr;
-		rcd.best_shape = 0;
+		if (p_parameters.max_collisions > 1) {
+			rcd.max_results = p_parameters.max_collisions;
+			rcd.other_results = results;
+		}
 
 		// Allowed depth can't be lower than motion length, in order to handle contacts at low speed.
-		rcd.min_allowed_depth = MIN(motion_length, test_motion_min_contact_depth);
+		rcd.min_allowed_depth = MIN(motion_length, min_contact_depth);
 
 		int from_shape = best_shape != -1 ? best_shape : 0;
 		int to_shape = best_shape != -1 ? best_shape + 1 : p_body->get_shape_count();
@@ -849,49 +930,61 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, co
 			}
 
 			Transform3D body_shape_xform = ugt * p_body->get_shape_transform(j);
-			Shape3DSW *body_shape = p_body->get_shape(j);
+			GodotShape3D *body_shape = p_body->get_shape(j);
 
-			body_aabb.position += p_motion * unsafe;
+			body_aabb.position += p_parameters.motion * unsafe;
 
 			int amount = _cull_aabb_for_body(p_body, body_aabb);
 
 			for (int i = 0; i < amount; i++) {
-				const CollisionObject3DSW *col_obj = intersection_query_results[i];
-				if (p_exclude.has(col_obj->get_self())) {
+				const GodotCollisionObject3D *col_obj = intersection_query_results[i];
+				if (p_parameters.exclude_bodies.has(col_obj->get_self())) {
 					continue;
 				}
+				if (p_parameters.exclude_objects.has(col_obj->get_instance_id())) {
+					continue;
+				}
+
 				int shape_idx = intersection_query_subindex_results[i];
 
 				rcd.object = col_obj;
 				rcd.shape = shape_idx;
-				bool sc = CollisionSolver3DSW::solve_static(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), _rest_cbk_result, &rcd, nullptr, p_margin);
+				bool sc = GodotCollisionSolver3D::solve_static(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), _rest_cbk_result, &rcd, nullptr, p_parameters.margin);
 				if (!sc) {
 					continue;
 				}
 			}
 		}
 
-		if (rcd.best_len != 0) {
+		if (rcd.result_count > 0) {
 			if (r_result) {
-				r_result->collider = rcd.best_object->get_self();
-				r_result->collider_id = rcd.best_object->get_instance_id();
-				r_result->collider_shape = rcd.best_shape;
-				r_result->collision_local_shape = rcd.best_local_shape;
-				r_result->collision_normal = rcd.best_normal;
-				r_result->collision_point = rcd.best_contact;
-				r_result->collision_depth = rcd.best_len;
-				r_result->collision_safe_fraction = safe;
-				r_result->collision_unsafe_fraction = unsafe;
-				//r_result->collider_metadata = rcd.best_object->get_shape_metadata(rcd.best_shape);
+				for (int collision_index = 0; collision_index < rcd.result_count; ++collision_index) {
+					const _RestResultData &result = (collision_index > 0) ? rcd.other_results[collision_index - 1] : rcd.best_result;
+
+					PhysicsServer3D::MotionCollision &collision = r_result->collisions[collision_index];
+
+					collision.collider = result.object->get_self();
+					collision.collider_id = result.object->get_instance_id();
+					collision.collider_shape = result.shape;
+					collision.local_shape = result.local_shape;
+					collision.normal = result.normal;
+					collision.position = result.contact;
+					collision.depth = result.len;
+
+					const GodotBody3D *body = static_cast<const GodotBody3D *>(result.object);
 
-				const Body3DSW *body = static_cast<const Body3DSW *>(rcd.best_object);
+					Vector3 rel_vec = result.contact - (body->get_transform().origin + body->get_center_of_mass());
+					collision.collider_velocity = body->get_linear_velocity() + (body->get_angular_velocity()).cross(rel_vec);
+				}
+
+				r_result->travel = safe * p_parameters.motion;
+				r_result->remainder = p_parameters.motion - safe * p_parameters.motion;
+				r_result->travel += (body_transform.get_origin() - p_parameters.from.get_origin());
 
-				Vector3 rel_vec = rcd.best_contact - (body->get_transform().origin + body->get_center_of_mass());
-				r_result->collider_velocity = body->get_linear_velocity() + (body->get_angular_velocity()).cross(rel_vec);
+				r_result->collision_safe_fraction = safe;
+				r_result->collision_unsafe_fraction = unsafe;
 
-				r_result->travel = safe * p_motion;
-				r_result->remainder = p_motion - safe * p_motion;
-				r_result->travel += (body_transform.get_origin() - p_from.get_origin());
+				r_result->collision_count = rcd.result_count;
 			}
 
 			collided = true;
@@ -899,52 +992,55 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, co
 	}
 
 	if (!collided && r_result) {
-		r_result->travel = p_motion;
+		r_result->travel = p_parameters.motion;
 		r_result->remainder = Vector3();
-		r_result->travel += (body_transform.get_origin() - p_from.get_origin());
+		r_result->travel += (body_transform.get_origin() - p_parameters.from.get_origin());
+
+		r_result->collision_safe_fraction = 1.0;
+		r_result->collision_unsafe_fraction = 1.0;
 	}
 
 	return collided;
 }
 
-void *Space3DSW::_broadphase_pair(CollisionObject3DSW *A, int p_subindex_A, CollisionObject3DSW *B, int p_subindex_B, void *p_self) {
+void *GodotSpace3D::_broadphase_pair(GodotCollisionObject3D *A, int p_subindex_A, GodotCollisionObject3D *B, int p_subindex_B, void *p_self) {
 	if (!A->interacts_with(B)) {
 		return nullptr;
 	}
 
-	CollisionObject3DSW::Type type_A = A->get_type();
-	CollisionObject3DSW::Type type_B = B->get_type();
+	GodotCollisionObject3D::Type type_A = A->get_type();
+	GodotCollisionObject3D::Type type_B = B->get_type();
 	if (type_A > type_B) {
 		SWAP(A, B);
 		SWAP(p_subindex_A, p_subindex_B);
 		SWAP(type_A, type_B);
 	}
 
-	Space3DSW *self = (Space3DSW *)p_self;
+	GodotSpace3D *self = (GodotSpace3D *)p_self;
 
 	self->collision_pairs++;
 
-	if (type_A == CollisionObject3DSW::TYPE_AREA) {
-		Area3DSW *area = static_cast<Area3DSW *>(A);
-		if (type_B == CollisionObject3DSW::TYPE_AREA) {
-			Area3DSW *area_b = static_cast<Area3DSW *>(B);
-			Area2Pair3DSW *area2_pair = memnew(Area2Pair3DSW(area_b, p_subindex_B, area, p_subindex_A));
+	if (type_A == GodotCollisionObject3D::TYPE_AREA) {
+		GodotArea3D *area = static_cast<GodotArea3D *>(A);
+		if (type_B == GodotCollisionObject3D::TYPE_AREA) {
+			GodotArea3D *area_b = static_cast<GodotArea3D *>(B);
+			GodotArea2Pair3D *area2_pair = memnew(GodotArea2Pair3D(area_b, p_subindex_B, area, p_subindex_A));
 			return area2_pair;
-		} else if (type_B == CollisionObject3DSW::TYPE_SOFT_BODY) {
-			SoftBody3DSW *softbody = static_cast<SoftBody3DSW *>(B);
-			AreaSoftBodyPair3DSW *soft_area_pair = memnew(AreaSoftBodyPair3DSW(softbody, p_subindex_B, area, p_subindex_A));
+		} else if (type_B == GodotCollisionObject3D::TYPE_SOFT_BODY) {
+			GodotSoftBody3D *softbody = static_cast<GodotSoftBody3D *>(B);
+			GodotAreaSoftBodyPair3D *soft_area_pair = memnew(GodotAreaSoftBodyPair3D(softbody, p_subindex_B, area, p_subindex_A));
 			return soft_area_pair;
 		} else {
-			Body3DSW *body = static_cast<Body3DSW *>(B);
-			AreaPair3DSW *area_pair = memnew(AreaPair3DSW(body, p_subindex_B, area, p_subindex_A));
+			GodotBody3D *body = static_cast<GodotBody3D *>(B);
+			GodotAreaPair3D *area_pair = memnew(GodotAreaPair3D(body, p_subindex_B, area, p_subindex_A));
 			return area_pair;
 		}
-	} else if (type_A == CollisionObject3DSW::TYPE_BODY) {
-		if (type_B == CollisionObject3DSW::TYPE_SOFT_BODY) {
-			BodySoftBodyPair3DSW *soft_pair = memnew(BodySoftBodyPair3DSW((Body3DSW *)A, p_subindex_A, (SoftBody3DSW *)B));
+	} else if (type_A == GodotCollisionObject3D::TYPE_BODY) {
+		if (type_B == GodotCollisionObject3D::TYPE_SOFT_BODY) {
+			GodotBodySoftBodyPair3D *soft_pair = memnew(GodotBodySoftBodyPair3D((GodotBody3D *)A, p_subindex_A, (GodotSoftBody3D *)B));
 			return soft_pair;
 		} else {
-			BodyPair3DSW *b = memnew(BodyPair3DSW((Body3DSW *)A, p_subindex_A, (Body3DSW *)B, p_subindex_B));
+			GodotBodyPair3D *b = memnew(GodotBodyPair3D((GodotBody3D *)A, p_subindex_A, (GodotBody3D *)B, p_subindex_B));
 			return b;
 		}
 	} else {
@@ -954,122 +1050,122 @@ void *Space3DSW::_broadphase_pair(CollisionObject3DSW *A, int p_subindex_A, Coll
 	return nullptr;
 }
 
-void Space3DSW::_broadphase_unpair(CollisionObject3DSW *A, int p_subindex_A, CollisionObject3DSW *B, int p_subindex_B, void *p_data, void *p_self) {
+void GodotSpace3D::_broadphase_unpair(GodotCollisionObject3D *A, int p_subindex_A, GodotCollisionObject3D *B, int p_subindex_B, void *p_data, void *p_self) {
 	if (!p_data) {
 		return;
 	}
 
-	Space3DSW *self = (Space3DSW *)p_self;
+	GodotSpace3D *self = (GodotSpace3D *)p_self;
 	self->collision_pairs--;
-	Constraint3DSW *c = (Constraint3DSW *)p_data;
+	GodotConstraint3D *c = (GodotConstraint3D *)p_data;
 	memdelete(c);
 }
 
-const SelfList<Body3DSW>::List &Space3DSW::get_active_body_list() const {
+const SelfList<GodotBody3D>::List &GodotSpace3D::get_active_body_list() const {
 	return active_list;
 }
 
-void Space3DSW::body_add_to_active_list(SelfList<Body3DSW> *p_body) {
+void GodotSpace3D::body_add_to_active_list(SelfList<GodotBody3D> *p_body) {
 	active_list.add(p_body);
 }
 
-void Space3DSW::body_remove_from_active_list(SelfList<Body3DSW> *p_body) {
+void GodotSpace3D::body_remove_from_active_list(SelfList<GodotBody3D> *p_body) {
 	active_list.remove(p_body);
 }
 
-void Space3DSW::body_add_to_inertia_update_list(SelfList<Body3DSW> *p_body) {
-	inertia_update_list.add(p_body);
+void GodotSpace3D::body_add_to_mass_properties_update_list(SelfList<GodotBody3D> *p_body) {
+	mass_properties_update_list.add(p_body);
 }
 
-void Space3DSW::body_remove_from_inertia_update_list(SelfList<Body3DSW> *p_body) {
-	inertia_update_list.remove(p_body);
+void GodotSpace3D::body_remove_from_mass_properties_update_list(SelfList<GodotBody3D> *p_body) {
+	mass_properties_update_list.remove(p_body);
 }
 
-BroadPhase3DSW *Space3DSW::get_broadphase() {
+GodotBroadPhase3D *GodotSpace3D::get_broadphase() {
 	return broadphase;
 }
 
-void Space3DSW::add_object(CollisionObject3DSW *p_object) {
+void GodotSpace3D::add_object(GodotCollisionObject3D *p_object) {
 	ERR_FAIL_COND(objects.has(p_object));
 	objects.insert(p_object);
 }
 
-void Space3DSW::remove_object(CollisionObject3DSW *p_object) {
+void GodotSpace3D::remove_object(GodotCollisionObject3D *p_object) {
 	ERR_FAIL_COND(!objects.has(p_object));
 	objects.erase(p_object);
 }
 
-const Set<CollisionObject3DSW *> &Space3DSW::get_objects() const {
+const Set<GodotCollisionObject3D *> &GodotSpace3D::get_objects() const {
 	return objects;
 }
 
-void Space3DSW::body_add_to_state_query_list(SelfList<Body3DSW> *p_body) {
+void GodotSpace3D::body_add_to_state_query_list(SelfList<GodotBody3D> *p_body) {
 	state_query_list.add(p_body);
 }
 
-void Space3DSW::body_remove_from_state_query_list(SelfList<Body3DSW> *p_body) {
+void GodotSpace3D::body_remove_from_state_query_list(SelfList<GodotBody3D> *p_body) {
 	state_query_list.remove(p_body);
 }
 
-void Space3DSW::area_add_to_monitor_query_list(SelfList<Area3DSW> *p_area) {
+void GodotSpace3D::area_add_to_monitor_query_list(SelfList<GodotArea3D> *p_area) {
 	monitor_query_list.add(p_area);
 }
 
-void Space3DSW::area_remove_from_monitor_query_list(SelfList<Area3DSW> *p_area) {
+void GodotSpace3D::area_remove_from_monitor_query_list(SelfList<GodotArea3D> *p_area) {
 	monitor_query_list.remove(p_area);
 }
 
-void Space3DSW::area_add_to_moved_list(SelfList<Area3DSW> *p_area) {
+void GodotSpace3D::area_add_to_moved_list(SelfList<GodotArea3D> *p_area) {
 	area_moved_list.add(p_area);
 }
 
-void Space3DSW::area_remove_from_moved_list(SelfList<Area3DSW> *p_area) {
+void GodotSpace3D::area_remove_from_moved_list(SelfList<GodotArea3D> *p_area) {
 	area_moved_list.remove(p_area);
 }
 
-const SelfList<Area3DSW>::List &Space3DSW::get_moved_area_list() const {
+const SelfList<GodotArea3D>::List &GodotSpace3D::get_moved_area_list() const {
 	return area_moved_list;
 }
 
-const SelfList<SoftBody3DSW>::List &Space3DSW::get_active_soft_body_list() const {
+const SelfList<GodotSoftBody3D>::List &GodotSpace3D::get_active_soft_body_list() const {
 	return active_soft_body_list;
 }
 
-void Space3DSW::soft_body_add_to_active_list(SelfList<SoftBody3DSW> *p_soft_body) {
+void GodotSpace3D::soft_body_add_to_active_list(SelfList<GodotSoftBody3D> *p_soft_body) {
 	active_soft_body_list.add(p_soft_body);
 }
 
-void Space3DSW::soft_body_remove_from_active_list(SelfList<SoftBody3DSW> *p_soft_body) {
+void GodotSpace3D::soft_body_remove_from_active_list(SelfList<GodotSoftBody3D> *p_soft_body) {
 	active_soft_body_list.remove(p_soft_body);
 }
 
-void Space3DSW::call_queries() {
+void GodotSpace3D::call_queries() {
 	while (state_query_list.first()) {
-		Body3DSW *b = state_query_list.first()->self();
+		GodotBody3D *b = state_query_list.first()->self();
 		state_query_list.remove(state_query_list.first());
 		b->call_queries();
 	}
 
 	while (monitor_query_list.first()) {
-		Area3DSW *a = monitor_query_list.first()->self();
+		GodotArea3D *a = monitor_query_list.first()->self();
 		monitor_query_list.remove(monitor_query_list.first());
 		a->call_queries();
 	}
 }
 
-void Space3DSW::setup() {
+void GodotSpace3D::setup() {
 	contact_debug_count = 0;
-	while (inertia_update_list.first()) {
-		inertia_update_list.first()->self()->update_inertias();
-		inertia_update_list.remove(inertia_update_list.first());
+	while (mass_properties_update_list.first()) {
+		mass_properties_update_list.first()->self()->update_mass_properties();
+		mass_properties_update_list.remove(mass_properties_update_list.first());
 	}
 }
 
-void Space3DSW::update() {
+void GodotSpace3D::update() {
 	broadphase->update();
 }
 
-void Space3DSW::set_param(PhysicsServer3D::SpaceParameter p_param, real_t p_value) {
+void GodotSpace3D::set_param(PhysicsServer3D::SpaceParameter p_param, real_t p_value) {
 	switch (p_param) {
 		case PhysicsServer3D::SPACE_PARAM_CONTACT_RECYCLE_RADIUS:
 			contact_recycle_radius = p_value;
@@ -1095,13 +1191,10 @@ void Space3DSW::set_param(PhysicsServer3D::SpaceParameter p_param, real_t p_valu
 		case PhysicsServer3D::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS:
 			constraint_bias = p_value;
 			break;
-		case PhysicsServer3D::SPACE_PARAM_TEST_MOTION_MIN_CONTACT_DEPTH:
-			test_motion_min_contact_depth = p_value;
-			break;
 	}
 }
 
-real_t Space3DSW::get_param(PhysicsServer3D::SpaceParameter p_param) const {
+real_t GodotSpace3D::get_param(PhysicsServer3D::SpaceParameter p_param) const {
 	switch (p_param) {
 		case PhysicsServer3D::SPACE_PARAM_CONTACT_RECYCLE_RADIUS:
 			return contact_recycle_radius;
@@ -1119,61 +1212,42 @@ real_t Space3DSW::get_param(PhysicsServer3D::SpaceParameter p_param) const {
 			return body_angular_velocity_damp_ratio;
 		case PhysicsServer3D::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS:
 			return constraint_bias;
-		case PhysicsServer3D::SPACE_PARAM_TEST_MOTION_MIN_CONTACT_DEPTH:
-			return test_motion_min_contact_depth;
 	}
 	return 0;
 }
 
-void Space3DSW::lock() {
+void GodotSpace3D::lock() {
 	locked = true;
 }
 
-void Space3DSW::unlock() {
+void GodotSpace3D::unlock() {
 	locked = false;
 }
 
-bool Space3DSW::is_locked() const {
+bool GodotSpace3D::is_locked() const {
 	return locked;
 }
 
-PhysicsDirectSpaceState3DSW *Space3DSW::get_direct_state() {
+GodotPhysicsDirectSpaceState3D *GodotSpace3D::get_direct_state() {
 	return direct_access;
 }
 
-Space3DSW::Space3DSW() {
-	collision_pairs = 0;
-	active_objects = 0;
-	island_count = 0;
-	contact_debug_count = 0;
-
-	locked = false;
-	contact_recycle_radius = 0.01;
-	contact_max_separation = 0.05;
-	contact_max_allowed_penetration = 0.01;
-	test_motion_min_contact_depth = 0.00001;
-
-	constraint_bias = 0.01;
+GodotSpace3D::GodotSpace3D() {
 	body_linear_velocity_sleep_threshold = GLOBAL_DEF("physics/3d/sleep_threshold_linear", 0.1);
 	body_angular_velocity_sleep_threshold = GLOBAL_DEF("physics/3d/sleep_threshold_angular", Math::deg2rad(8.0));
 	body_time_to_sleep = GLOBAL_DEF("physics/3d/time_before_sleep", 0.5);
 	ProjectSettings::get_singleton()->set_custom_property_info("physics/3d/time_before_sleep", PropertyInfo(Variant::FLOAT, "physics/3d/time_before_sleep", PROPERTY_HINT_RANGE, "0,5,0.01,or_greater"));
 	body_angular_velocity_damp_ratio = 10;
 
-	broadphase = BroadPhase3DSW::create_func();
+	broadphase = GodotBroadPhase3D::create_func();
 	broadphase->set_pair_callback(_broadphase_pair, this);
 	broadphase->set_unpair_callback(_broadphase_unpair, this);
-	area = nullptr;
 
-	direct_access = memnew(PhysicsDirectSpaceState3DSW);
+	direct_access = memnew(GodotPhysicsDirectSpaceState3D);
 	direct_access->space = this;
-
-	for (int i = 0; i < ELAPSED_TIME_MAX; i++) {
-		elapsed_time[i] = 0;
-	}
 }
 
-Space3DSW::~Space3DSW() {
+GodotSpace3D::~GodotSpace3D() {
 	memdelete(broadphase);
 	memdelete(direct_access);
 }
diff --git a/servers/physics_3d/space_3d_sw.h b/servers/physics_3d/godot_space_3d.h
index eff494a7bc..aa5e965751 100644
--- a/servers/physics_3d/space_3d_sw.h
+++ b/servers/physics_3d/godot_space_3d.h
@@ -1,5 +1,5 @@
 /*************************************************************************/
-/*  space_3d_sw.h                                                        */
+/*  godot_space_3d.h                                                     */
 /*************************************************************************/
 /*                       This file is part of:                           */
 /*                           GODOT ENGINE                                */
@@ -28,38 +28,39 @@
 /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
 /*************************************************************************/
 
-#ifndef SPACE_SW_H
-#define SPACE_SW_H
+#ifndef GODOT_SPACE_3D_H
+#define GODOT_SPACE_3D_H
+
+#include "godot_area_3d.h"
+#include "godot_area_pair_3d.h"
+#include "godot_body_3d.h"
+#include "godot_body_pair_3d.h"
+#include "godot_broad_phase_3d.h"
+#include "godot_collision_object_3d.h"
+#include "godot_soft_body_3d.h"
 
-#include "area_3d_sw.h"
-#include "area_pair_3d_sw.h"
-#include "body_3d_sw.h"
-#include "body_pair_3d_sw.h"
-#include "broad_phase_3d_sw.h"
-#include "collision_object_3d_sw.h"
 #include "core/config/project_settings.h"
 #include "core/templates/hash_map.h"
 #include "core/typedefs.h"
-#include "soft_body_3d_sw.h"
 
-class PhysicsDirectSpaceState3DSW : public PhysicsDirectSpaceState3D {
-	GDCLASS(PhysicsDirectSpaceState3DSW, PhysicsDirectSpaceState3D);
+class GodotPhysicsDirectSpaceState3D : public PhysicsDirectSpaceState3D {
+	GDCLASS(GodotPhysicsDirectSpaceState3D, PhysicsDirectSpaceState3D);
 
 public:
-	Space3DSW *space;
-
-	virtual int intersect_point(const Vector3 &p_point, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override;
-	virtual bool intersect_ray(const Vector3 &p_from, const Vector3 &p_to, RayResult &r_result, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false, bool p_pick_ray = false) override;
-	virtual int intersect_shape(const RID &p_shape, const Transform3D &p_xform, real_t p_margin, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override;
-	virtual bool cast_motion(const RID &p_shape, const Transform3D &p_xform, const Vector3 &p_motion, real_t p_margin, real_t &p_closest_safe, real_t &p_closest_unsafe, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false, ShapeRestInfo *r_info = nullptr) override;
-	virtual bool collide_shape(RID p_shape, const Transform3D &p_shape_xform, real_t p_margin, Vector3 *r_results, int p_result_max, int &r_result_count, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override;
-	virtual bool rest_info(RID p_shape, const Transform3D &p_shape_xform, real_t p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = 0xFFFFFFFF, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override;
+	GodotSpace3D *space;
+
+	virtual int intersect_point(const PointParameters &p_parameters, ShapeResult *r_results, int p_result_max) override;
+	virtual bool intersect_ray(const RayParameters &p_parameters, RayResult &r_result) override;
+	virtual int intersect_shape(const ShapeParameters &p_parameters, ShapeResult *r_results, int p_result_max) override;
+	virtual bool cast_motion(const ShapeParameters &p_parameters, real_t &p_closest_safe, real_t &p_closest_unsafe, ShapeRestInfo *r_info = nullptr) override;
+	virtual bool collide_shape(const ShapeParameters &p_parameters, Vector3 *r_results, int p_result_max, int &r_result_count) override;
+	virtual bool rest_info(const ShapeParameters &p_parameters, ShapeRestInfo *r_info) override;
 	virtual Vector3 get_closest_point_to_object_volume(RID p_object, const Vector3 p_point) const override;
 
-	PhysicsDirectSpaceState3DSW();
+	GodotPhysicsDirectSpaceState3D();
 };
 
-class Space3DSW {
+class GodotSpace3D {
 public:
 	enum ElapsedTime {
 		ELAPSED_TIME_INTEGRATE_FORCES,
@@ -72,37 +73,36 @@ public:
 	};
 
 private:
-	uint64_t elapsed_time[ELAPSED_TIME_MAX];
+	uint64_t elapsed_time[ELAPSED_TIME_MAX] = {};
 
-	PhysicsDirectSpaceState3DSW *direct_access;
+	GodotPhysicsDirectSpaceState3D *direct_access;
 	RID self;
 
-	BroadPhase3DSW *broadphase;
-	SelfList<Body3DSW>::List active_list;
-	SelfList<Body3DSW>::List inertia_update_list;
-	SelfList<Body3DSW>::List state_query_list;
-	SelfList<Area3DSW>::List monitor_query_list;
-	SelfList<Area3DSW>::List area_moved_list;
-	SelfList<SoftBody3DSW>::List active_soft_body_list;
+	GodotBroadPhase3D *broadphase;
+	SelfList<GodotBody3D>::List active_list;
+	SelfList<GodotBody3D>::List mass_properties_update_list;
+	SelfList<GodotBody3D>::List state_query_list;
+	SelfList<GodotArea3D>::List monitor_query_list;
+	SelfList<GodotArea3D>::List area_moved_list;
+	SelfList<GodotSoftBody3D>::List active_soft_body_list;
 
-	static void *_broadphase_pair(CollisionObject3DSW *A, int p_subindex_A, CollisionObject3DSW *B, int p_subindex_B, void *p_self);
-	static void _broadphase_unpair(CollisionObject3DSW *A, int p_subindex_A, CollisionObject3DSW *B, int p_subindex_B, void *p_data, void *p_self);
+	static void *_broadphase_pair(GodotCollisionObject3D *A, int p_subindex_A, GodotCollisionObject3D *B, int p_subindex_B, void *p_self);
+	static void _broadphase_unpair(GodotCollisionObject3D *A, int p_subindex_A, GodotCollisionObject3D *B, int p_subindex_B, void *p_data, void *p_self);
 
-	Set<CollisionObject3DSW *> objects;
+	Set<GodotCollisionObject3D *> objects;
 
-	Area3DSW *area;
+	GodotArea3D *area = nullptr;
 
-	real_t contact_recycle_radius;
-	real_t contact_max_separation;
-	real_t contact_max_allowed_penetration;
-	real_t constraint_bias;
-	real_t test_motion_min_contact_depth;
+	real_t contact_recycle_radius = 0.01;
+	real_t contact_max_separation = 0.05;
+	real_t contact_max_allowed_penetration = 0.01;
+	real_t constraint_bias = 0.01;
 
 	enum {
 		INTERSECTION_QUERY_MAX = 2048
 	};
 
-	CollisionObject3DSW *intersection_query_results[INTERSECTION_QUERY_MAX];
+	GodotCollisionObject3D *intersection_query_results[INTERSECTION_QUERY_MAX];
 	int intersection_query_subindex_results[INTERSECTION_QUERY_MAX];
 
 	real_t body_linear_velocity_sleep_threshold;
@@ -110,54 +110,54 @@ private:
 	real_t body_time_to_sleep;
 	real_t body_angular_velocity_damp_ratio;
 
-	bool locked;
+	bool locked = false;
 
 	real_t last_step = 0.001;
 
-	int island_count;
-	int active_objects;
-	int collision_pairs;
+	int island_count = 0;
+	int active_objects = 0;
+	int collision_pairs = 0;
 
 	RID static_global_body;
 
 	Vector<Vector3> contact_debug;
-	int contact_debug_count;
+	int contact_debug_count = 0;
 
-	friend class PhysicsDirectSpaceState3DSW;
+	friend class GodotPhysicsDirectSpaceState3D;
 
-	int _cull_aabb_for_body(Body3DSW *p_body, const AABB &p_aabb);
+	int _cull_aabb_for_body(GodotBody3D *p_body, const AABB &p_aabb);
 
 public:
 	_FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; }
 	_FORCE_INLINE_ RID get_self() const { return self; }
 
-	void set_default_area(Area3DSW *p_area) { area = p_area; }
-	Area3DSW *get_default_area() const { return area; }
+	void set_default_area(GodotArea3D *p_area) { area = p_area; }
+	GodotArea3D *get_default_area() const { return area; }
 
-	const SelfList<Body3DSW>::List &get_active_body_list() const;
-	void body_add_to_active_list(SelfList<Body3DSW> *p_body);
-	void body_remove_from_active_list(SelfList<Body3DSW> *p_body);
-	void body_add_to_inertia_update_list(SelfList<Body3DSW> *p_body);
-	void body_remove_from_inertia_update_list(SelfList<Body3DSW> *p_body);
+	const SelfList<GodotBody3D>::List &get_active_body_list() const;
+	void body_add_to_active_list(SelfList<GodotBody3D> *p_body);
+	void body_remove_from_active_list(SelfList<GodotBody3D> *p_body);
+	void body_add_to_mass_properties_update_list(SelfList<GodotBody3D> *p_body);
+	void body_remove_from_mass_properties_update_list(SelfList<GodotBody3D> *p_body);
 
-	void body_add_to_state_query_list(SelfList<Body3DSW> *p_body);
-	void body_remove_from_state_query_list(SelfList<Body3DSW> *p_body);
+	void body_add_to_state_query_list(SelfList<GodotBody3D> *p_body);
+	void body_remove_from_state_query_list(SelfList<GodotBody3D> *p_body);
 
-	void area_add_to_monitor_query_list(SelfList<Area3DSW> *p_area);
-	void area_remove_from_monitor_query_list(SelfList<Area3DSW> *p_area);
-	void area_add_to_moved_list(SelfList<Area3DSW> *p_area);
-	void area_remove_from_moved_list(SelfList<Area3DSW> *p_area);
-	const SelfList<Area3DSW>::List &get_moved_area_list() const;
+	void area_add_to_monitor_query_list(SelfList<GodotArea3D> *p_area);
+	void area_remove_from_monitor_query_list(SelfList<GodotArea3D> *p_area);
+	void area_add_to_moved_list(SelfList<GodotArea3D> *p_area);
+	void area_remove_from_moved_list(SelfList<GodotArea3D> *p_area);
+	const SelfList<GodotArea3D>::List &get_moved_area_list() const;
 
-	const SelfList<SoftBody3DSW>::List &get_active_soft_body_list() const;
-	void soft_body_add_to_active_list(SelfList<SoftBody3DSW> *p_soft_body);
-	void soft_body_remove_from_active_list(SelfList<SoftBody3DSW> *p_soft_body);
+	const SelfList<GodotSoftBody3D>::List &get_active_soft_body_list() const;
+	void soft_body_add_to_active_list(SelfList<GodotSoftBody3D> *p_soft_body);
+	void soft_body_remove_from_active_list(SelfList<GodotSoftBody3D> *p_soft_body);
 
-	BroadPhase3DSW *get_broadphase();
+	GodotBroadPhase3D *get_broadphase();
 
-	void add_object(CollisionObject3DSW *p_object);
-	void remove_object(CollisionObject3DSW *p_object);
-	const Set<CollisionObject3DSW *> &get_objects() const;
+	void add_object(GodotCollisionObject3D *p_object);
+	void remove_object(GodotCollisionObject3D *p_object);
+	const Set<GodotCollisionObject3D *> &get_objects() const;
 
 	_FORCE_INLINE_ real_t get_contact_recycle_radius() const { return contact_recycle_radius; }
 	_FORCE_INLINE_ real_t get_contact_max_separation() const { return contact_max_separation; }
@@ -190,7 +190,7 @@ public:
 
 	int get_collision_pairs() const { return collision_pairs; }
 
-	PhysicsDirectSpaceState3DSW *get_direct_state();
+	GodotPhysicsDirectSpaceState3D *get_direct_state();
 
 	void set_debug_contacts(int p_amount) { contact_debug.resize(p_amount); }
 	_FORCE_INLINE_ bool is_debugging_contacts() const { return !contact_debug.is_empty(); }
@@ -208,10 +208,10 @@ public:
 	void set_elapsed_time(ElapsedTime p_time, uint64_t p_msec) { elapsed_time[p_time] = p_msec; }
 	uint64_t get_elapsed_time(ElapsedTime p_time) const { return elapsed_time[p_time]; }
 
-	bool test_body_motion(Body3DSW *p_body, const Transform3D &p_from, const Vector3 &p_motion, real_t p_margin, PhysicsServer3D::MotionResult *r_result, bool p_collide_separation_ray = false, const Set<RID> &p_exclude = Set<RID>());
+	bool test_body_motion(GodotBody3D *p_body, const PhysicsServer3D::MotionParameters &p_parameters, PhysicsServer3D::MotionResult *r_result);
 
-	Space3DSW();
-	~Space3DSW();
+	GodotSpace3D();
+	~GodotSpace3D();
 };
 
-#endif // SPACE__SW_H
+#endif // GODOT_SPACE_3D_H
diff --git a/servers/physics_3d/step_3d_sw.cpp b/servers/physics_3d/godot_step_3d.cpp
index d0604b0aa0..5453c4415c 100644
--- a/servers/physics_3d/step_3d_sw.cpp
+++ b/servers/physics_3d/godot_step_3d.cpp
@@ -1,5 +1,5 @@
 /*************************************************************************/
-/*  step_3d_sw.cpp                                                       */
+/*  godot_step_3d.cpp                                                    */
 /*************************************************************************/
 /*                       This file is part of:                           */
 /*                           GODOT ENGINE                                */
@@ -28,8 +28,9 @@
 /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
 /*************************************************************************/
 
-#include "step_3d_sw.h"
-#include "joints_3d_sw.h"
+#include "godot_step_3d.h"
+
+#include "godot_joint_3d.h"
 
 #include "core/os/os.h"
 
@@ -39,7 +40,7 @@
 #define ISLAND_SIZE_RESERVE 512
 #define CONSTRAINT_COUNT_RESERVE 1024
 
-void Step3DSW::_populate_island(Body3DSW *p_body, LocalVector<Body3DSW *> &p_body_island, LocalVector<Constraint3DSW *> &p_constraint_island) {
+void GodotStep3D::_populate_island(GodotBody3D *p_body, LocalVector<GodotBody3D *> &p_body_island, LocalVector<GodotConstraint3D *> &p_constraint_island) {
 	p_body->set_island_step(_step);
 
 	if (p_body->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC) {
@@ -47,8 +48,8 @@ void Step3DSW::_populate_island(Body3DSW *p_body, LocalVector<Body3DSW *> &p_bod
 		p_body_island.push_back(p_body);
 	}
 
-	for (Map<Constraint3DSW *, int>::Element *E = p_body->get_constraint_map().front(); E; E = E->next()) {
-		Constraint3DSW *constraint = (Constraint3DSW *)E->key();
+	for (const KeyValue<GodotConstraint3D *, int> &E : p_body->get_constraint_map()) {
+		GodotConstraint3D *constraint = (GodotConstraint3D *)E.key;
 		if (constraint->get_island_step() == _step) {
 			continue; // Already processed.
 		}
@@ -59,10 +60,10 @@ void Step3DSW::_populate_island(Body3DSW *p_body, LocalVector<Body3DSW *> &p_bod
 
 		// Find connected rigid bodies.
 		for (int i = 0; i < constraint->get_body_count(); i++) {
-			if (i == E->get()) {
+			if (i == E.value) {
 				continue;
 			}
-			Body3DSW *other_body = constraint->get_body_ptr()[i];
+			GodotBody3D *other_body = constraint->get_body_ptr()[i];
 			if (other_body->get_island_step() == _step) {
 				continue; // Already processed.
 			}
@@ -74,7 +75,7 @@ void Step3DSW::_populate_island(Body3DSW *p_body, LocalVector<Body3DSW *> &p_bod
 
 		// Find connected soft bodies.
 		for (int i = 0; i < constraint->get_soft_body_count(); i++) {
-			SoftBody3DSW *soft_body = constraint->get_soft_body_ptr(i);
+			GodotSoftBody3D *soft_body = constraint->get_soft_body_ptr(i);
 			if (soft_body->get_island_step() == _step) {
 				continue; // Already processed.
 			}
@@ -83,11 +84,11 @@ void Step3DSW::_populate_island(Body3DSW *p_body, LocalVector<Body3DSW *> &p_bod
 	}
 }
 
-void Step3DSW::_populate_island_soft_body(SoftBody3DSW *p_soft_body, LocalVector<Body3DSW *> &p_body_island, LocalVector<Constraint3DSW *> &p_constraint_island) {
+void GodotStep3D::_populate_island_soft_body(GodotSoftBody3D *p_soft_body, LocalVector<GodotBody3D *> &p_body_island, LocalVector<GodotConstraint3D *> &p_constraint_island) {
 	p_soft_body->set_island_step(_step);
 
-	for (Set<Constraint3DSW *>::Element *E = p_soft_body->get_constraints().front(); E; E = E->next()) {
-		Constraint3DSW *constraint = (Constraint3DSW *)E->get();
+	for (Set<GodotConstraint3D *>::Element *E = p_soft_body->get_constraints().front(); E; E = E->next()) {
+		GodotConstraint3D *constraint = (GodotConstraint3D *)E->get();
 		if (constraint->get_island_step() == _step) {
 			continue; // Already processed.
 		}
@@ -98,7 +99,7 @@ void Step3DSW::_populate_island_soft_body(SoftBody3DSW *p_soft_body, LocalVector
 
 		// Find connected rigid bodies.
 		for (int i = 0; i < constraint->get_body_count(); i++) {
-			Body3DSW *body = constraint->get_body_ptr()[i];
+			GodotBody3D *body = constraint->get_body_ptr()[i];
 			if (body->get_island_step() == _step) {
 				continue; // Already processed.
 			}
@@ -110,16 +111,16 @@ void Step3DSW::_populate_island_soft_body(SoftBody3DSW *p_soft_body, LocalVector
 	}
 }
 
-void Step3DSW::_setup_contraint(uint32_t p_constraint_index, void *p_userdata) {
-	Constraint3DSW *constraint = all_constraints[p_constraint_index];
+void GodotStep3D::_setup_contraint(uint32_t p_constraint_index, void *p_userdata) {
+	GodotConstraint3D *constraint = all_constraints[p_constraint_index];
 	constraint->setup(delta);
 }
 
-void Step3DSW::_pre_solve_island(LocalVector<Constraint3DSW *> &p_constraint_island) const {
+void GodotStep3D::_pre_solve_island(LocalVector<GodotConstraint3D *> &p_constraint_island) const {
 	uint32_t constraint_count = p_constraint_island.size();
 	uint32_t valid_constraint_count = 0;
 	for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) {
-		Constraint3DSW *constraint = p_constraint_island[constraint_index];
+		GodotConstraint3D *constraint = p_constraint_island[constraint_index];
 		if (p_constraint_island[constraint_index]->pre_solve(delta)) {
 			// Keep this constraint for solving.
 			p_constraint_island[valid_constraint_count++] = constraint;
@@ -128,8 +129,8 @@ void Step3DSW::_pre_solve_island(LocalVector<Constraint3DSW *> &p_constraint_isl
 	p_constraint_island.resize(valid_constraint_count);
 }
 
-void Step3DSW::_solve_island(uint32_t p_island_index, void *p_userdata) {
-	LocalVector<Constraint3DSW *> &constraint_island = constraint_islands[p_island_index];
+void GodotStep3D::_solve_island(uint32_t p_island_index, void *p_userdata) {
+	LocalVector<GodotConstraint3D *> &constraint_island = constraint_islands[p_island_index];
 
 	int current_priority = 1;
 
@@ -146,7 +147,7 @@ void Step3DSW::_solve_island(uint32_t p_island_index, void *p_userdata) {
 		uint32_t priority_constraint_count = 0;
 		++current_priority;
 		for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) {
-			Constraint3DSW *constraint = constraint_island[constraint_index];
+			GodotConstraint3D *constraint = constraint_island[constraint_index];
 			if (constraint->get_priority() >= current_priority) {
 				// Keep this constraint for the next iteration.
 				constraint_island[priority_constraint_count++] = constraint;
@@ -156,12 +157,12 @@ void Step3DSW::_solve_island(uint32_t p_island_index, void *p_userdata) {
 	}
 }
 
-void Step3DSW::_check_suspend(const LocalVector<Body3DSW *> &p_body_island) const {
+void GodotStep3D::_check_suspend(const LocalVector<GodotBody3D *> &p_body_island) const {
 	bool can_sleep = true;
 
 	uint32_t body_count = p_body_island.size();
 	for (uint32_t body_index = 0; body_index < body_count; ++body_index) {
-		Body3DSW *body = p_body_island[body_index];
+		GodotBody3D *body = p_body_island[body_index];
 
 		if (!body->sleep_test(delta)) {
 			can_sleep = false;
@@ -170,7 +171,7 @@ void Step3DSW::_check_suspend(const LocalVector<Body3DSW *> &p_body_island) cons
 
 	// Put all to sleep or wake up everyone.
 	for (uint32_t body_index = 0; body_index < body_count; ++body_index) {
-		Body3DSW *body = p_body_island[body_index];
+		GodotBody3D *body = p_body_island[body_index];
 
 		bool active = body->is_active();
 
@@ -180,7 +181,7 @@ void Step3DSW::_check_suspend(const LocalVector<Body3DSW *> &p_body_island) cons
 	}
 }
 
-void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
+void GodotStep3D::step(GodotSpace3D *p_space, real_t p_delta, int p_iterations) {
 	p_space->lock(); // can't access space during this
 
 	p_space->setup(); //update inertias, etc
@@ -190,9 +191,9 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
 	iterations = p_iterations;
 	delta = p_delta;
 
-	const SelfList<Body3DSW>::List *body_list = &p_space->get_active_body_list();
+	const SelfList<GodotBody3D>::List *body_list = &p_space->get_active_body_list();
 
-	const SelfList<SoftBody3DSW>::List *soft_body_list = &p_space->get_active_soft_body_list();
+	const SelfList<GodotSoftBody3D>::List *soft_body_list = &p_space->get_active_soft_body_list();
 
 	/* INTEGRATE FORCES */
 
@@ -201,7 +202,7 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
 
 	int active_count = 0;
 
-	const SelfList<Body3DSW> *b = body_list->first();
+	const SelfList<GodotBody3D> *b = body_list->first();
 	while (b) {
 		b->self()->integrate_forces(p_delta);
 		b = b->next();
@@ -210,7 +211,7 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
 
 	/* UPDATE SOFT BODY MOTION */
 
-	const SelfList<SoftBody3DSW> *sb = soft_body_list->first();
+	const SelfList<GodotSoftBody3D> *sb = soft_body_list->first();
 	while (sb) {
 		sb->self()->predict_motion(p_delta);
 		sb = sb->next();
@@ -219,9 +220,12 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
 
 	p_space->set_active_objects(active_count);
 
+	// Update the broadphase to register collision pairs.
+	p_space->update();
+
 	{ //profile
 		profile_endtime = OS::get_singleton()->get_ticks_usec();
-		p_space->set_elapsed_time(Space3DSW::ELAPSED_TIME_INTEGRATE_FORCES, profile_endtime - profile_begtime);
+		p_space->set_elapsed_time(GodotSpace3D::ELAPSED_TIME_INTEGRATE_FORCES, profile_endtime - profile_begtime);
 		profile_begtime = profile_endtime;
 	}
 
@@ -229,11 +233,11 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
 
 	uint32_t island_count = 0;
 
-	const SelfList<Area3DSW>::List &aml = p_space->get_moved_area_list();
+	const SelfList<GodotArea3D>::List &aml = p_space->get_moved_area_list();
 
 	while (aml.first()) {
-		for (const Set<Constraint3DSW *>::Element *E = aml.first()->self()->get_constraints().front(); E; E = E->next()) {
-			Constraint3DSW *constraint = E->get();
+		for (const Set<GodotConstraint3D *>::Element *E = aml.first()->self()->get_constraints().front(); E; E = E->next()) {
+			GodotConstraint3D *constraint = E->get();
 			if (constraint->get_island_step() == _step) {
 				continue;
 			}
@@ -244,13 +248,13 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
 			if (constraint_islands.size() < island_count) {
 				constraint_islands.resize(island_count);
 			}
-			LocalVector<Constraint3DSW *> &constraint_island = constraint_islands[island_count - 1];
+			LocalVector<GodotConstraint3D *> &constraint_island = constraint_islands[island_count - 1];
 			constraint_island.clear();
 
 			all_constraints.push_back(constraint);
 			constraint_island.push_back(constraint);
 		}
-		p_space->area_remove_from_moved_list((SelfList<Area3DSW> *)aml.first()); //faster to remove here
+		p_space->area_remove_from_moved_list((SelfList<GodotArea3D> *)aml.first()); //faster to remove here
 	}
 
 	/* GENERATE CONSTRAINT ISLANDS FOR ACTIVE RIGID BODIES */
@@ -260,14 +264,14 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
 	uint32_t body_island_count = 0;
 
 	while (b) {
-		Body3DSW *body = b->self();
+		GodotBody3D *body = b->self();
 
 		if (body->get_island_step() != _step) {
 			++body_island_count;
 			if (body_islands.size() < body_island_count) {
 				body_islands.resize(body_island_count);
 			}
-			LocalVector<Body3DSW *> &body_island = body_islands[body_island_count - 1];
+			LocalVector<GodotBody3D *> &body_island = body_islands[body_island_count - 1];
 			body_island.clear();
 			body_island.reserve(BODY_ISLAND_SIZE_RESERVE);
 
@@ -275,7 +279,7 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
 			if (constraint_islands.size() < island_count) {
 				constraint_islands.resize(island_count);
 			}
-			LocalVector<Constraint3DSW *> &constraint_island = constraint_islands[island_count - 1];
+			LocalVector<GodotConstraint3D *> &constraint_island = constraint_islands[island_count - 1];
 			constraint_island.clear();
 			constraint_island.reserve(ISLAND_SIZE_RESERVE);
 
@@ -296,14 +300,14 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
 
 	sb = soft_body_list->first();
 	while (sb) {
-		SoftBody3DSW *soft_body = sb->self();
+		GodotSoftBody3D *soft_body = sb->self();
 
 		if (soft_body->get_island_step() != _step) {
 			++body_island_count;
 			if (body_islands.size() < body_island_count) {
 				body_islands.resize(body_island_count);
 			}
-			LocalVector<Body3DSW *> &body_island = body_islands[body_island_count - 1];
+			LocalVector<GodotBody3D *> &body_island = body_islands[body_island_count - 1];
 			body_island.clear();
 			body_island.reserve(BODY_ISLAND_SIZE_RESERVE);
 
@@ -311,7 +315,7 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
 			if (constraint_islands.size() < island_count) {
 				constraint_islands.resize(island_count);
 			}
-			LocalVector<Constraint3DSW *> &constraint_island = constraint_islands[island_count - 1];
+			LocalVector<GodotConstraint3D *> &constraint_island = constraint_islands[island_count - 1];
 			constraint_island.clear();
 			constraint_island.reserve(ISLAND_SIZE_RESERVE);
 
@@ -332,18 +336,18 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
 
 	{ //profile
 		profile_endtime = OS::get_singleton()->get_ticks_usec();
-		p_space->set_elapsed_time(Space3DSW::ELAPSED_TIME_GENERATE_ISLANDS, profile_endtime - profile_begtime);
+		p_space->set_elapsed_time(GodotSpace3D::ELAPSED_TIME_GENERATE_ISLANDS, profile_endtime - profile_begtime);
 		profile_begtime = profile_endtime;
 	}
 
 	/* SETUP CONSTRAINTS / PROCESS COLLISIONS */
 
 	uint32_t total_contraint_count = all_constraints.size();
-	work_pool.do_work(total_contraint_count, this, &Step3DSW::_setup_contraint, nullptr);
+	work_pool.do_work(total_contraint_count, this, &GodotStep3D::_setup_contraint, nullptr);
 
 	{ //profile
 		profile_endtime = OS::get_singleton()->get_ticks_usec();
-		p_space->set_elapsed_time(Space3DSW::ELAPSED_TIME_SETUP_CONSTRAINTS, profile_endtime - profile_begtime);
+		p_space->set_elapsed_time(GodotSpace3D::ELAPSED_TIME_SETUP_CONSTRAINTS, profile_endtime - profile_begtime);
 		profile_begtime = profile_endtime;
 	}
 
@@ -358,15 +362,11 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
 
 	// Warning: _solve_island modifies the constraint islands for optimization purpose,
 	// their content is not reliable after these calls and shouldn't be used anymore.
-	if (island_count > 1) {
-		work_pool.do_work(island_count, this, &Step3DSW::_solve_island, nullptr);
-	} else if (island_count > 0) {
-		_solve_island(0);
-	}
+	work_pool.do_work(island_count, this, &GodotStep3D::_solve_island, nullptr);
 
 	{ //profile
 		profile_endtime = OS::get_singleton()->get_ticks_usec();
-		p_space->set_elapsed_time(Space3DSW::ELAPSED_TIME_SOLVE_CONSTRAINTS, profile_endtime - profile_begtime);
+		p_space->set_elapsed_time(GodotSpace3D::ELAPSED_TIME_SOLVE_CONSTRAINTS, profile_endtime - profile_begtime);
 		profile_begtime = profile_endtime;
 	}
 
@@ -374,7 +374,7 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
 
 	b = body_list->first();
 	while (b) {
-		const SelfList<Body3DSW> *n = b->next();
+		const SelfList<GodotBody3D> *n = b->next();
 		b->self()->integrate_velocities(p_delta);
 		b = n;
 	}
@@ -395,20 +395,17 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
 
 	{ //profile
 		profile_endtime = OS::get_singleton()->get_ticks_usec();
-		p_space->set_elapsed_time(Space3DSW::ELAPSED_TIME_INTEGRATE_VELOCITIES, profile_endtime - profile_begtime);
+		p_space->set_elapsed_time(GodotSpace3D::ELAPSED_TIME_INTEGRATE_VELOCITIES, profile_endtime - profile_begtime);
 		profile_begtime = profile_endtime;
 	}
 
 	all_constraints.clear();
 
-	p_space->update();
 	p_space->unlock();
 	_step++;
 }
 
-Step3DSW::Step3DSW() {
-	_step = 1;
-
+GodotStep3D::GodotStep3D() {
 	body_islands.reserve(BODY_ISLAND_COUNT_RESERVE);
 	constraint_islands.reserve(ISLAND_COUNT_RESERVE);
 	all_constraints.reserve(CONSTRAINT_COUNT_RESERVE);
@@ -416,6 +413,6 @@ Step3DSW::Step3DSW() {
 	work_pool.init();
 }
 
-Step3DSW::~Step3DSW() {
+GodotStep3D::~GodotStep3D() {
 	work_pool.finish();
 }
diff --git a/servers/physics_3d/step_3d_sw.h b/servers/physics_3d/godot_step_3d.h
index 9c60004b24..23ede4feff 100644
--- a/servers/physics_3d/step_3d_sw.h
+++ b/servers/physics_3d/godot_step_3d.h
@@ -1,5 +1,5 @@
 /*************************************************************************/
-/*  step_3d_sw.h                                                         */
+/*  godot_step_3d.h                                                      */
 /*************************************************************************/
 /*                       This file is part of:                           */
 /*                           GODOT ENGINE                                */
@@ -28,37 +28,37 @@
 /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
 /*************************************************************************/
 
-#ifndef STEP_SW_H
-#define STEP_SW_H
+#ifndef GODOT_STEP_3D_H
+#define GODOT_STEP_3D_H
 
-#include "space_3d_sw.h"
+#include "godot_space_3d.h"
 
 #include "core/templates/local_vector.h"
 #include "core/templates/thread_work_pool.h"
 
-class Step3DSW {
-	uint64_t _step;
+class GodotStep3D {
+	uint64_t _step = 1;
 
 	int iterations = 0;
 	real_t delta = 0.0;
 
 	ThreadWorkPool work_pool;
 
-	LocalVector<LocalVector<Body3DSW *>> body_islands;
-	LocalVector<LocalVector<Constraint3DSW *>> constraint_islands;
-	LocalVector<Constraint3DSW *> all_constraints;
+	LocalVector<LocalVector<GodotBody3D *>> body_islands;
+	LocalVector<LocalVector<GodotConstraint3D *>> constraint_islands;
+	LocalVector<GodotConstraint3D *> all_constraints;
 
-	void _populate_island(Body3DSW *p_body, LocalVector<Body3DSW *> &p_body_island, LocalVector<Constraint3DSW *> &p_constraint_island);
-	void _populate_island_soft_body(SoftBody3DSW *p_soft_body, LocalVector<Body3DSW *> &p_body_island, LocalVector<Constraint3DSW *> &p_constraint_island);
+	void _populate_island(GodotBody3D *p_body, LocalVector<GodotBody3D *> &p_body_island, LocalVector<GodotConstraint3D *> &p_constraint_island);
+	void _populate_island_soft_body(GodotSoftBody3D *p_soft_body, LocalVector<GodotBody3D *> &p_body_island, LocalVector<GodotConstraint3D *> &p_constraint_island);
 	void _setup_contraint(uint32_t p_constraint_index, void *p_userdata = nullptr);
-	void _pre_solve_island(LocalVector<Constraint3DSW *> &p_constraint_island) const;
+	void _pre_solve_island(LocalVector<GodotConstraint3D *> &p_constraint_island) const;
 	void _solve_island(uint32_t p_island_index, void *p_userdata = nullptr);
-	void _check_suspend(const LocalVector<Body3DSW *> &p_body_island) const;
+	void _check_suspend(const LocalVector<GodotBody3D *> &p_body_island) const;
 
 public:
-	void step(Space3DSW *p_space, real_t p_delta, int p_iterations);
-	Step3DSW();
-	~Step3DSW();
+	void step(GodotSpace3D *p_space, real_t p_delta, int p_iterations);
+	GodotStep3D();
+	~GodotStep3D();
 };
 
-#endif // STEP__SW_H
+#endif // GODOT_STEP_3D_H
diff --git a/servers/physics_3d/joints/cone_twist_joint_3d_sw.cpp b/servers/physics_3d/joints/godot_cone_twist_joint_3d.cpp
index 7315e9c709..864086c956 100644
--- a/servers/physics_3d/joints/cone_twist_joint_3d_sw.cpp
+++ b/servers/physics_3d/joints/godot_cone_twist_joint_3d.cpp
@@ -1,5 +1,5 @@
 /*************************************************************************/
-/*  cone_twist_joint_3d_sw.cpp                                           */
+/*  godot_cone_twist_joint_3d.cpp                                        */
 /*************************************************************************/
 /*                       This file is part of:                           */
 /*                           GODOT ENGINE                                */
@@ -49,7 +49,7 @@ subject to the following restrictions:
 Written by: Marcus Hennix
 */
 
-#include "cone_twist_joint_3d_sw.h"
+#include "godot_cone_twist_joint_3d.h"
 
 static void plane_space(const Vector3 &n, Vector3 &p, Vector3 &q) {
 	if (Math::abs(n.z) > Math_SQRT12) {
@@ -84,31 +84,19 @@ static _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x) {
 	return (y < 0.0f) ? -angle : angle;
 }
 
-ConeTwistJoint3DSW::ConeTwistJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform3D &rbAFrame, const Transform3D &rbBFrame) :
-		Joint3DSW(_arr, 2) {
+GodotConeTwistJoint3D::GodotConeTwistJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Transform3D &rbAFrame, const Transform3D &rbBFrame) :
+		GodotJoint3D(_arr, 2) {
 	A = rbA;
 	B = rbB;
 
 	m_rbAFrame = rbAFrame;
 	m_rbBFrame = rbBFrame;
 
-	m_swingSpan1 = Math_TAU / 8.0;
-	m_swingSpan2 = Math_TAU / 8.0;
-	m_twistSpan = Math_TAU;
-	m_biasFactor = 0.3f;
-	m_relaxationFactor = 1.0f;
-
-	m_angularOnly = false;
-	m_solveTwistLimit = false;
-	m_solveSwingLimit = false;
-
 	A->add_constraint(this, 0);
 	B->add_constraint(this, 1);
-
-	m_appliedImpulse = 0;
 }
 
-bool ConeTwistJoint3DSW::setup(real_t p_timestep) {
+bool GodotConeTwistJoint3D::setup(real_t p_timestep) {
 	dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
 	dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
 
@@ -141,16 +129,18 @@ bool ConeTwistJoint3DSW::setup(real_t p_timestep) {
 		plane_space(normal[0], normal[1], normal[2]);
 
 		for (int i = 0; i < 3; i++) {
-			memnew_placement(&m_jac[i], JacobianEntry3DSW(
-												A->get_principal_inertia_axes().transposed(),
-												B->get_principal_inertia_axes().transposed(),
-												pivotAInW - A->get_transform().origin - A->get_center_of_mass(),
-												pivotBInW - B->get_transform().origin - B->get_center_of_mass(),
-												normal[i],
-												A->get_inv_inertia(),
-												A->get_inv_mass(),
-												B->get_inv_inertia(),
-												B->get_inv_mass()));
+			memnew_placement(
+					&m_jac[i],
+					GodotJacobianEntry3D(
+							A->get_principal_inertia_axes().transposed(),
+							B->get_principal_inertia_axes().transposed(),
+							pivotAInW - A->get_transform().origin - A->get_center_of_mass(),
+							pivotBInW - B->get_transform().origin - B->get_center_of_mass(),
+							normal[i],
+							A->get_inv_inertia(),
+							A->get_inv_mass(),
+							B->get_inv_inertia(),
+							B->get_inv_mass()));
 		}
 	}
 
@@ -204,8 +194,7 @@ bool ConeTwistJoint3DSW::setup(real_t p_timestep) {
 		real_t swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f;
 		m_swingAxis *= swingAxisSign;
 
-		m_kSwing = real_t(1.) / (A->compute_angular_impulse_denominator(m_swingAxis) +
-										B->compute_angular_impulse_denominator(m_swingAxis));
+		m_kSwing = real_t(1.) / (A->compute_angular_impulse_denominator(m_swingAxis) + B->compute_angular_impulse_denominator(m_swingAxis));
 	}
 
 	// Twist limits
@@ -224,8 +213,7 @@ bool ConeTwistJoint3DSW::setup(real_t p_timestep) {
 			m_twistAxis.normalize();
 			m_twistAxis *= -1.0f;
 
-			m_kTwist = real_t(1.) / (A->compute_angular_impulse_denominator(m_twistAxis) +
-											B->compute_angular_impulse_denominator(m_twistAxis));
+			m_kTwist = real_t(1.) / (A->compute_angular_impulse_denominator(m_twistAxis) + B->compute_angular_impulse_denominator(m_twistAxis));
 
 		} else if (twist > m_twistSpan * lockedFreeFactor) {
 			m_twistCorrection = (twist - m_twistSpan);
@@ -234,15 +222,14 @@ bool ConeTwistJoint3DSW::setup(real_t p_timestep) {
 			m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
 			m_twistAxis.normalize();
 
-			m_kTwist = real_t(1.) / (A->compute_angular_impulse_denominator(m_twistAxis) +
-											B->compute_angular_impulse_denominator(m_twistAxis));
+			m_kTwist = real_t(1.) / (A->compute_angular_impulse_denominator(m_twistAxis) + B->compute_angular_impulse_denominator(m_twistAxis));
 		}
 	}
 
 	return true;
 }
 
-void ConeTwistJoint3DSW::solve(real_t p_timestep) {
+void GodotConeTwistJoint3D::solve(real_t p_timestep) {
 	Vector3 pivotAInW = A->get_transform().xform(m_rbAFrame.origin);
 	Vector3 pivotBInW = B->get_transform().xform(m_rbBFrame.origin);
 
@@ -324,7 +311,7 @@ void ConeTwistJoint3DSW::solve(real_t p_timestep) {
 	}
 }
 
-void ConeTwistJoint3DSW::set_param(PhysicsServer3D::ConeTwistJointParam p_param, real_t p_value) {
+void GodotConeTwistJoint3D::set_param(PhysicsServer3D::ConeTwistJointParam p_param, real_t p_value) {
 	switch (p_param) {
 		case PhysicsServer3D::CONE_TWIST_JOINT_SWING_SPAN: {
 			m_swingSpan1 = p_value;
@@ -347,7 +334,7 @@ void ConeTwistJoint3DSW::set_param(PhysicsServer3D::ConeTwistJointParam p_param,
 	}
 }
 
-real_t ConeTwistJoint3DSW::get_param(PhysicsServer3D::ConeTwistJointParam p_param) const {
+real_t GodotConeTwistJoint3D::get_param(PhysicsServer3D::ConeTwistJointParam p_param) const {
 	switch (p_param) {
 		case PhysicsServer3D::CONE_TWIST_JOINT_SWING_SPAN: {
 			return m_swingSpan1;
diff --git a/servers/physics_3d/joints/cone_twist_joint_3d_sw.h b/servers/physics_3d/joints/godot_cone_twist_joint_3d.h
index 608847352c..999d0f0692 100644
--- a/servers/physics_3d/joints/cone_twist_joint_3d_sw.h
+++ b/servers/physics_3d/joints/godot_cone_twist_joint_3d.h
@@ -1,5 +1,5 @@
 /*************************************************************************/
-/*  cone_twist_joint_3d_sw.h                                             */
+/*  godot_cone_twist_joint_3d.h                                          */
 /*************************************************************************/
 /*                       This file is part of:                           */
 /*                           GODOT ENGINE                                */
@@ -34,7 +34,7 @@ Adapted to Godot from the Bullet library.
 
 /*
 Bullet Continuous Collision Detection and Physics Library
-ConeTwistJointSW is Copyright (c) 2007 Starbreeze Studios
+GodotConeTwistJoint3D is Copyright (c) 2007 Starbreeze Studios
 
 This software is provided 'as-is', without any express or implied warranty.
 In no event will the authors be held liable for any damages arising from the use of this software.
@@ -49,57 +49,57 @@ subject to the following restrictions:
 Written by: Marcus Hennix
 */
 
-#ifndef CONE_TWIST_JOINT_SW_H
-#define CONE_TWIST_JOINT_SW_H
+#ifndef GODOT_CONE_TWIST_JOINT_3D_H
+#define GODOT_CONE_TWIST_JOINT_3D_H
 
-#include "servers/physics_3d/joints/jacobian_entry_3d_sw.h"
-#include "servers/physics_3d/joints_3d_sw.h"
+#include "servers/physics_3d/godot_joint_3d.h"
+#include "servers/physics_3d/joints/godot_jacobian_entry_3d.h"
 
-///ConeTwistJointSW can be used to simulate ragdoll joints (upper arm, leg etc)
-class ConeTwistJoint3DSW : public Joint3DSW {
+// GodotConeTwistJoint3D can be used to simulate ragdoll joints (upper arm, leg etc).
+class GodotConeTwistJoint3D : public GodotJoint3D {
 #ifdef IN_PARALLELL_SOLVER
 public:
 #endif
 
 	union {
 		struct {
-			Body3DSW *A;
-			Body3DSW *B;
+			GodotBody3D *A;
+			GodotBody3D *B;
 		};
 
-		Body3DSW *_arr[2];
+		GodotBody3D *_arr[2] = { nullptr, nullptr };
 	};
 
-	JacobianEntry3DSW m_jac[3]; //3 orthogonal linear constraints
+	GodotJacobianEntry3D m_jac[3] = {}; //3 orthogonal linear constraints
 
-	real_t m_appliedImpulse;
+	real_t m_appliedImpulse = 0.0;
 	Transform3D m_rbAFrame;
 	Transform3D m_rbBFrame;
 
-	real_t m_limitSoftness;
-	real_t m_biasFactor;
-	real_t m_relaxationFactor;
+	real_t m_limitSoftness = 0.0;
+	real_t m_biasFactor = 0.3;
+	real_t m_relaxationFactor = 1.0;
 
-	real_t m_swingSpan1;
-	real_t m_swingSpan2;
-	real_t m_twistSpan;
+	real_t m_swingSpan1 = Math_TAU / 8.0;
+	real_t m_swingSpan2 = 0.0;
+	real_t m_twistSpan = 0.0;
 
 	Vector3 m_swingAxis;
 	Vector3 m_twistAxis;
 
-	real_t m_kSwing;
-	real_t m_kTwist;
+	real_t m_kSwing = 0.0;
+	real_t m_kTwist = 0.0;
 
-	real_t m_twistLimitSign;
-	real_t m_swingCorrection;
-	real_t m_twistCorrection;
+	real_t m_twistLimitSign = 0.0;
+	real_t m_swingCorrection = 0.0;
+	real_t m_twistCorrection = 0.0;
 
-	real_t m_accSwingLimitImpulse;
-	real_t m_accTwistLimitImpulse;
+	real_t m_accSwingLimitImpulse = 0.0;
+	real_t m_accTwistLimitImpulse = 0.0;
 
-	bool m_angularOnly;
-	bool m_solveTwistLimit;
-	bool m_solveSwingLimit;
+	bool m_angularOnly = false;
+	bool m_solveTwistLimit = false;
+	bool m_solveSwingLimit = false;
 
 public:
 	virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_CONE_TWIST; }
@@ -107,7 +107,7 @@ public:
 	virtual bool setup(real_t p_step) override;
 	virtual void solve(real_t p_step) override;
 
-	ConeTwistJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform3D &rbAFrame, const Transform3D &rbBFrame);
+	GodotConeTwistJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Transform3D &rbAFrame, const Transform3D &rbBFrame);
 
 	void setAngularOnly(bool angularOnly) {
 		m_angularOnly = angularOnly;
@@ -139,4 +139,4 @@ public:
 	real_t get_param(PhysicsServer3D::ConeTwistJointParam p_param) const;
 };
 
-#endif // CONE_TWIST_JOINT_SW_H
+#endif // GODOT_CONE_TWIST_JOINT_3D_H
diff --git a/servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp b/servers/physics_3d/joints/godot_generic_6dof_joint_3d.cpp
index d2b64ce6e3..915bb528e9 100644
--- a/servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp
+++ b/servers/physics_3d/joints/godot_generic_6dof_joint_3d.cpp
@@ -1,5 +1,5 @@
 /*************************************************************************/
-/*  generic_6dof_joint_3d_sw.cpp                                         */
+/*  godot_generic_6dof_joint_3d.cpp                                      */
 /*************************************************************************/
 /*                       This file is part of:                           */
 /*                           GODOT ENGINE                                */
@@ -34,7 +34,7 @@ Adapted to Godot from the Bullet library.
 
 /*
 Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans  https://bulletphysics.org
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
 
 This software is provided 'as-is', without any express or implied warranty.
 In no event will the authors be held liable for any damages arising from the use of this software.
@@ -49,18 +49,18 @@ subject to the following restrictions:
 
 /*
 2007-09-09
-Generic6DOFJointSW Refactored by Francisco Le?n
+GodotGeneric6DOFJoint3D Refactored by Francisco Le?n
 email: projectileman@yahoo.com
 http://gimpact.sf.net
 */
 
-#include "generic_6dof_joint_3d_sw.h"
+#include "godot_generic_6dof_joint_3d.h"
 
 #define GENERIC_D6_DISABLE_WARMSTARTING 1
 
-//////////////////////////// G6DOFRotationalLimitMotorSW ////////////////////////////////////
+//////////////////////////// GodotG6DOFRotationalLimitMotor3D ////////////////////////////////////
 
-int G6DOFRotationalLimitMotor3DSW::testLimitValue(real_t test_value) {
+int GodotG6DOFRotationalLimitMotor3D::testLimitValue(real_t test_value) {
 	if (m_loLimit > m_hiLimit) {
 		m_currentLimit = 0; //Free from violation
 		return 0;
@@ -80,9 +80,9 @@ int G6DOFRotationalLimitMotor3DSW::testLimitValue(real_t test_value) {
 	return 0;
 }
 
-real_t G6DOFRotationalLimitMotor3DSW::solveAngularLimits(
+real_t GodotG6DOFRotationalLimitMotor3D::solveAngularLimits(
 		real_t timeStep, Vector3 &axis, real_t jacDiagABInv,
-		Body3DSW *body0, Body3DSW *body1, bool p_body0_dynamic, bool p_body1_dynamic) {
+		GodotBody3D *body0, GodotBody3D *body1, bool p_body0_dynamic, bool p_body1_dynamic) {
 	if (!needApplyTorques()) {
 		return 0.0f;
 	}
@@ -148,14 +148,13 @@ real_t G6DOFRotationalLimitMotor3DSW::solveAngularLimits(
 	return clippedMotorImpulse;
 }
 
-//////////////////////////// End G6DOFRotationalLimitMotorSW ////////////////////////////////////
+//////////////////////////// GodotG6DOFTranslationalLimitMotor3D ////////////////////////////////////
 
-//////////////////////////// G6DOFTranslationalLimitMotorSW ////////////////////////////////////
-real_t G6DOFTranslationalLimitMotor3DSW::solveLinearAxis(
+real_t GodotG6DOFTranslationalLimitMotor3D::solveLinearAxis(
 		real_t timeStep,
 		real_t jacDiagABInv,
-		Body3DSW *body1, const Vector3 &pointInA,
-		Body3DSW *body2, const Vector3 &pointInB,
+		GodotBody3D *body1, const Vector3 &pointInA,
+		GodotBody3D *body2, const Vector3 &pointInB,
 		bool p_body1_dynamic, bool p_body2_dynamic,
 		int limit_index,
 		const Vector3 &axis_normal_on_a,
@@ -217,10 +216,10 @@ real_t G6DOFTranslationalLimitMotor3DSW::solveLinearAxis(
 	return normalImpulse;
 }
 
-//////////////////////////// G6DOFTranslationalLimitMotorSW ////////////////////////////////////
+//////////////////////////// GodotGeneric6DOFJoint3D ////////////////////////////////////
 
-Generic6DOFJoint3DSW::Generic6DOFJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform3D &frameInA, const Transform3D &frameInB, bool useLinearReferenceFrameA) :
-		Joint3DSW(_arr, 2),
+GodotGeneric6DOFJoint3D::GodotGeneric6DOFJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Transform3D &frameInA, const Transform3D &frameInB, bool useLinearReferenceFrameA) :
+		GodotJoint3D(_arr, 2),
 		m_frameInA(frameInA),
 		m_frameInB(frameInB),
 		m_useLinearReferenceFrameA(useLinearReferenceFrameA) {
@@ -230,10 +229,10 @@ Generic6DOFJoint3DSW::Generic6DOFJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const T
 	B->add_constraint(this, 1);
 }
 
-void Generic6DOFJoint3DSW::calculateAngleInfo() {
+void GodotGeneric6DOFJoint3D::calculateAngleInfo() {
 	Basis relative_frame = m_calculatedTransformB.basis.inverse() * m_calculatedTransformA.basis;
 
-	m_calculatedAxisAngleDiff = relative_frame.get_euler_xyz();
+	m_calculatedAxisAngleDiff = relative_frame.get_euler(Basis::EULER_ORDER_XYZ);
 
 	// in euler angle mode we do not actually constrain the angular velocity
 	// along the axes axis[0] and axis[2] (although we do use axis[1]) :
@@ -270,38 +269,43 @@ void Generic6DOFJoint3DSW::calculateAngleInfo() {
 	*/
 }
 
-void Generic6DOFJoint3DSW::calculateTransforms() {
+void GodotGeneric6DOFJoint3D::calculateTransforms() {
 	m_calculatedTransformA = A->get_transform() * m_frameInA;
 	m_calculatedTransformB = B->get_transform() * m_frameInB;
 
 	calculateAngleInfo();
 }
 
-void Generic6DOFJoint3DSW::buildLinearJacobian(
-		JacobianEntry3DSW &jacLinear, const Vector3 &normalWorld,
+void GodotGeneric6DOFJoint3D::buildLinearJacobian(
+		GodotJacobianEntry3D &jacLinear, const Vector3 &normalWorld,
 		const Vector3 &pivotAInW, const Vector3 &pivotBInW) {
-	memnew_placement(&jacLinear, JacobianEntry3DSW(
-										 A->get_principal_inertia_axes().transposed(),
-										 B->get_principal_inertia_axes().transposed(),
-										 pivotAInW - A->get_transform().origin - A->get_center_of_mass(),
-										 pivotBInW - B->get_transform().origin - B->get_center_of_mass(),
-										 normalWorld,
-										 A->get_inv_inertia(),
-										 A->get_inv_mass(),
-										 B->get_inv_inertia(),
-										 B->get_inv_mass()));
+	memnew_placement(
+			&jacLinear,
+			GodotJacobianEntry3D(
+					A->get_principal_inertia_axes().transposed(),
+					B->get_principal_inertia_axes().transposed(),
+					pivotAInW - A->get_transform().origin - A->get_center_of_mass(),
+					pivotBInW - B->get_transform().origin - B->get_center_of_mass(),
+					normalWorld,
+					A->get_inv_inertia(),
+					A->get_inv_mass(),
+					B->get_inv_inertia(),
+					B->get_inv_mass()));
 }
 
-void Generic6DOFJoint3DSW::buildAngularJacobian(
-		JacobianEntry3DSW &jacAngular, const Vector3 &jointAxisW) {
-	memnew_placement(&jacAngular, JacobianEntry3DSW(jointAxisW,
-										  A->get_principal_inertia_axes().transposed(),
-										  B->get_principal_inertia_axes().transposed(),
-										  A->get_inv_inertia(),
-										  B->get_inv_inertia()));
+void GodotGeneric6DOFJoint3D::buildAngularJacobian(
+		GodotJacobianEntry3D &jacAngular, const Vector3 &jointAxisW) {
+	memnew_placement(
+			&jacAngular,
+			GodotJacobianEntry3D(
+					jointAxisW,
+					A->get_principal_inertia_axes().transposed(),
+					B->get_principal_inertia_axes().transposed(),
+					A->get_inv_inertia(),
+					B->get_inv_inertia()));
 }
 
-bool Generic6DOFJoint3DSW::testAngularLimitMotor(int axis_index) {
+bool GodotGeneric6DOFJoint3D::testAngularLimitMotor(int axis_index) {
 	real_t angle = m_calculatedAxisAngleDiff[axis_index];
 
 	//test limits
@@ -309,7 +313,7 @@ bool Generic6DOFJoint3DSW::testAngularLimitMotor(int axis_index) {
 	return m_angularLimits[axis_index].needApplyTorques();
 }
 
-bool Generic6DOFJoint3DSW::setup(real_t p_timestep) {
+bool GodotGeneric6DOFJoint3D::setup(real_t p_timestep) {
 	dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
 	dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
 
@@ -365,7 +369,7 @@ bool Generic6DOFJoint3DSW::setup(real_t p_timestep) {
 	return true;
 }
 
-void Generic6DOFJoint3DSW::solve(real_t p_timestep) {
+void GodotGeneric6DOFJoint3D::solve(real_t p_timestep) {
 	m_timeStep = p_timestep;
 
 	//calculateTransforms();
@@ -414,19 +418,19 @@ void Generic6DOFJoint3DSW::solve(real_t p_timestep) {
 	}
 }
 
-void Generic6DOFJoint3DSW::updateRHS(real_t timeStep) {
+void GodotGeneric6DOFJoint3D::updateRHS(real_t timeStep) {
 	(void)timeStep;
 }
 
-Vector3 Generic6DOFJoint3DSW::getAxis(int axis_index) const {
+Vector3 GodotGeneric6DOFJoint3D::getAxis(int axis_index) const {
 	return m_calculatedAxis[axis_index];
 }
 
-real_t Generic6DOFJoint3DSW::getAngle(int axis_index) const {
+real_t GodotGeneric6DOFJoint3D::getAngle(int axis_index) const {
 	return m_calculatedAxisAngleDiff[axis_index];
 }
 
-void Generic6DOFJoint3DSW::calcAnchorPos() {
+void GodotGeneric6DOFJoint3D::calcAnchorPos() {
 	real_t imA = A->get_inv_mass();
 	real_t imB = B->get_inv_mass();
 	real_t weight;
@@ -438,9 +442,9 @@ void Generic6DOFJoint3DSW::calcAnchorPos() {
 	const Vector3 &pA = m_calculatedTransformA.origin;
 	const Vector3 &pB = m_calculatedTransformB.origin;
 	m_AnchorPos = pA * weight + pB * (real_t(1.0) - weight);
-} // Generic6DOFJointSW::calcAnchorPos()
+}
 
-void Generic6DOFJoint3DSW::set_param(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisParam p_param, real_t p_value) {
+void GodotGeneric6DOFJoint3D::set_param(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisParam p_param, real_t p_value) {
 	ERR_FAIL_INDEX(p_axis, 3);
 	switch (p_param) {
 		case PhysicsServer3D::G6DOF_JOINT_LINEAR_LOWER_LIMIT: {
@@ -527,7 +531,7 @@ void Generic6DOFJoint3DSW::set_param(Vector3::Axis p_axis, PhysicsServer3D::G6DO
 	}
 }
 
-real_t Generic6DOFJoint3DSW::get_param(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisParam p_param) const {
+real_t GodotGeneric6DOFJoint3D::get_param(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisParam p_param) const {
 	ERR_FAIL_INDEX_V(p_axis, 3, 0);
 	switch (p_param) {
 		case PhysicsServer3D::G6DOF_JOINT_LINEAR_LOWER_LIMIT: {
@@ -615,7 +619,7 @@ real_t Generic6DOFJoint3DSW::get_param(Vector3::Axis p_axis, PhysicsServer3D::G6
 	return 0;
 }
 
-void Generic6DOFJoint3DSW::set_flag(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisFlag p_flag, bool p_value) {
+void GodotGeneric6DOFJoint3D::set_flag(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisFlag p_flag, bool p_value) {
 	ERR_FAIL_INDEX(p_axis, 3);
 
 	switch (p_flag) {
@@ -642,7 +646,7 @@ void Generic6DOFJoint3DSW::set_flag(Vector3::Axis p_axis, PhysicsServer3D::G6DOF
 	}
 }
 
-bool Generic6DOFJoint3DSW::get_flag(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisFlag p_flag) const {
+bool GodotGeneric6DOFJoint3D::get_flag(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisFlag p_flag) const {
 	ERR_FAIL_INDEX_V(p_axis, 3, 0);
 	switch (p_flag) {
 		case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT: {
diff --git a/servers/physics_3d/joints/generic_6dof_joint_3d_sw.h b/servers/physics_3d/joints/godot_generic_6dof_joint_3d.h
index c2a0443aff..f37b5b981b 100644
--- a/servers/physics_3d/joints/generic_6dof_joint_3d_sw.h
+++ b/servers/physics_3d/joints/godot_generic_6dof_joint_3d.h
@@ -1,5 +1,5 @@
 /*************************************************************************/
-/*  generic_6dof_joint_3d_sw.h                                           */
+/*  godot_generic_6dof_joint_3d.h                                        */
 /*************************************************************************/
 /*                       This file is part of:                           */
 /*                           GODOT ENGINE                                */
@@ -32,15 +32,15 @@
 Adapted to Godot from the Bullet library.
 */
 
-#ifndef GENERIC_6DOF_JOINT_SW_H
-#define GENERIC_6DOF_JOINT_SW_H
+#ifndef GODOT_GENERIC_6DOF_JOINT_3D_H
+#define GODOT_GENERIC_6DOF_JOINT_3D_H
 
-#include "servers/physics_3d/joints/jacobian_entry_3d_sw.h"
-#include "servers/physics_3d/joints_3d_sw.h"
+#include "servers/physics_3d/godot_joint_3d.h"
+#include "servers/physics_3d/joints/godot_jacobian_entry_3d.h"
 
 /*
 Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans  https://bulletphysics.org
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
 
 This software is provided 'as-is', without any express or implied warranty.
 In no event will the authors be held liable for any damages arising from the use of this software.
@@ -55,108 +55,75 @@ subject to the following restrictions:
 
 /*
 2007-09-09
-Generic6DOFJointSW Refactored by Francisco Le?n
+GodotGeneric6DOFJoint3D Refactored by Francisco Le?n
 email: projectileman@yahoo.com
 http://gimpact.sf.net
 */
 
 //! Rotation Limit structure for generic joints
-class G6DOFRotationalLimitMotor3DSW {
+class GodotG6DOFRotationalLimitMotor3D {
 public:
 	//! limit_parameters
 	//!@{
-	real_t m_loLimit; //!< joint limit
-	real_t m_hiLimit; //!< joint limit
-	real_t m_targetVelocity; //!< target motor velocity
-	real_t m_maxMotorForce; //!< max force on motor
-	real_t m_maxLimitForce; //!< max force on limit
-	real_t m_damping; //!< Damping.
-	real_t m_limitSoftness; //! Relaxation factor
-	real_t m_ERP; //!< Error tolerance factor when joint is at limit
-	real_t m_bounce; //!< restitution factor
-	bool m_enableMotor;
-	bool m_enableLimit;
+	real_t m_loLimit = -1e30; //!< joint limit
+	real_t m_hiLimit = 1e30; //!< joint limit
+	real_t m_targetVelocity = 0.0; //!< target motor velocity
+	real_t m_maxMotorForce = 0.1; //!< max force on motor
+	real_t m_maxLimitForce = 300.0; //!< max force on limit
+	real_t m_damping = 1.0; //!< Damping.
+	real_t m_limitSoftness = 0.5; //! Relaxation factor
+	real_t m_ERP = 0.5; //!< Error tolerance factor when joint is at limit
+	real_t m_bounce = 0.0; //!< restitution factor
+	bool m_enableMotor = false;
+	bool m_enableLimit = false;
 
 	//!@}
 
 	//! temp_variables
 	//!@{
-	real_t m_currentLimitError; //!< How much is violated this limit
-	int m_currentLimit; //!< 0=free, 1=at lo limit, 2=at hi limit
-	real_t m_accumulatedImpulse;
+	real_t m_currentLimitError = 0.0; //!< How much is violated this limit
+	int m_currentLimit = 0; //!< 0=free, 1=at lo limit, 2=at hi limit
+	real_t m_accumulatedImpulse = 0.0;
 	//!@}
 
-	G6DOFRotationalLimitMotor3DSW() {
-		m_accumulatedImpulse = 0.f;
-		m_targetVelocity = 0;
-		m_maxMotorForce = 0.1f;
-		m_maxLimitForce = 300.0f;
-		m_loLimit = -1e30;
-		m_hiLimit = 1e30;
-		m_ERP = 0.5f;
-		m_bounce = 0.0f;
-		m_damping = 1.0f;
-		m_limitSoftness = 0.5f;
-		m_currentLimit = 0;
-		m_currentLimitError = 0;
-		m_enableMotor = false;
-		m_enableLimit = false;
-	}
+	GodotG6DOFRotationalLimitMotor3D() {}
 
-	//! Is limited
 	bool isLimited() {
 		return (m_loLimit < m_hiLimit);
 	}
 
-	//! Need apply correction
+	// Need apply correction.
 	bool needApplyTorques() {
 		return (m_enableMotor || m_currentLimit != 0);
 	}
 
-	//! calculates error
-	/*!
-	calculates m_currentLimit and m_currentLimitError.
-	*/
+	// Calculates m_currentLimit and m_currentLimitError.
 	int testLimitValue(real_t test_value);
 
-	//! apply the correction impulses for two bodies
-	real_t solveAngularLimits(real_t timeStep, Vector3 &axis, real_t jacDiagABInv, Body3DSW *body0, Body3DSW *body1, bool p_body0_dynamic, bool p_body1_dynamic);
+	// Apply the correction impulses for two bodies.
+	real_t solveAngularLimits(real_t timeStep, Vector3 &axis, real_t jacDiagABInv, GodotBody3D *body0, GodotBody3D *body1, bool p_body0_dynamic, bool p_body1_dynamic);
 };
 
-class G6DOFTranslationalLimitMotor3DSW {
+class GodotG6DOFTranslationalLimitMotor3D {
 public:
-	Vector3 m_lowerLimit; //!< the constraint lower limits
-	Vector3 m_upperLimit; //!< the constraint upper limits
-	Vector3 m_accumulatedImpulse;
+	Vector3 m_lowerLimit = Vector3(0.0, 0.0, 0.0); //!< the constraint lower limits
+	Vector3 m_upperLimit = Vector3(0.0, 0.0, 0.0); //!< the constraint upper limits
+	Vector3 m_accumulatedImpulse = Vector3(0.0, 0.0, 0.0);
 	//! Linear_Limit_parameters
 	//!@{
-	Vector3 m_limitSoftness; //!< Softness for linear limit
-	Vector3 m_damping; //!< Damping for linear limit
-	Vector3 m_restitution; //! Bounce parameter for linear limit
+	Vector3 m_limitSoftness = Vector3(0.7, 0.7, 0.7); //!< Softness for linear limit
+	Vector3 m_damping = Vector3(1.0, 1.0, 1.0); //!< Damping for linear limit
+	Vector3 m_restitution = Vector3(0.5, 0.5, 0.5); //! Bounce parameter for linear limit
 	//!@}
-	bool enable_limit[3];
-
-	G6DOFTranslationalLimitMotor3DSW() {
-		m_lowerLimit = Vector3(0.f, 0.f, 0.f);
-		m_upperLimit = Vector3(0.f, 0.f, 0.f);
-		m_accumulatedImpulse = Vector3(0.f, 0.f, 0.f);
-
-		m_limitSoftness = Vector3(1, 1, 1) * 0.7f;
-		m_damping = Vector3(1, 1, 1) * real_t(1.0f);
-		m_restitution = Vector3(1, 1, 1) * real_t(0.5f);
-
-		enable_limit[0] = true;
-		enable_limit[1] = true;
-		enable_limit[2] = true;
-	}
+	bool enable_limit[3] = { true, true, true };
 
 	//! Test limit
 	/*!
-    - free means upper < lower,
-    - locked means upper == lower
-    - limited means upper > lower
-    - limitIndex: first 3 are linear, next 3 are angular
-    */
+	 * - free means upper < lower,
+	 * - locked means upper == lower
+	 * - limited means upper > lower
+	 * - limitIndex: first 3 are linear, next 3 are angular
+	 */
 	inline bool isLimited(int limitIndex) {
 		return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
 	}
@@ -164,23 +131,23 @@ public:
 	real_t solveLinearAxis(
 			real_t timeStep,
 			real_t jacDiagABInv,
-			Body3DSW *body1, const Vector3 &pointInA,
-			Body3DSW *body2, const Vector3 &pointInB,
+			GodotBody3D *body1, const Vector3 &pointInA,
+			GodotBody3D *body2, const Vector3 &pointInB,
 			bool p_body1_dynamic, bool p_body2_dynamic,
 			int limit_index,
 			const Vector3 &axis_normal_on_a,
 			const Vector3 &anchorPos);
 };
 
-class Generic6DOFJoint3DSW : public Joint3DSW {
+class GodotGeneric6DOFJoint3D : public GodotJoint3D {
 protected:
 	union {
 		struct {
-			Body3DSW *A;
-			Body3DSW *B;
+			GodotBody3D *A;
+			GodotBody3D *B;
 		};
 
-		Body3DSW *_arr[2];
+		GodotBody3D *_arr[2] = { nullptr, nullptr };
 	};
 
 	//! relative_frames
@@ -191,24 +158,24 @@ protected:
 
 	//! Jacobians
 	//!@{
-	JacobianEntry3DSW m_jacLinear[3]; //!< 3 orthogonal linear constraints
-	JacobianEntry3DSW m_jacAng[3]; //!< 3 orthogonal angular constraints
+	GodotJacobianEntry3D m_jacLinear[3]; //!< 3 orthogonal linear constraints
+	GodotJacobianEntry3D m_jacAng[3]; //!< 3 orthogonal angular constraints
 	//!@}
 
 	//! Linear_Limit_parameters
 	//!@{
-	G6DOFTranslationalLimitMotor3DSW m_linearLimits;
+	GodotG6DOFTranslationalLimitMotor3D m_linearLimits;
 	//!@}
 
 	//! hinge_parameters
 	//!@{
-	G6DOFRotationalLimitMotor3DSW m_angularLimits[3];
+	GodotG6DOFRotationalLimitMotor3D m_angularLimits[3];
 	//!@}
 
 protected:
 	//! temporal variables
 	//!@{
-	real_t m_timeStep;
+	real_t m_timeStep = 0.0;
 	Transform3D m_calculatedTransformA;
 	Transform3D m_calculatedTransformB;
 	Vector3 m_calculatedAxisAngleDiff;
@@ -216,49 +183,39 @@ protected:
 
 	Vector3 m_AnchorPos; // point between pivots of bodies A and B to solve linear axes
 
-	bool m_useLinearReferenceFrameA;
+	bool m_useLinearReferenceFrameA = false;
 
 	//!@}
 
-	Generic6DOFJoint3DSW(Generic6DOFJoint3DSW const &) = delete;
-	void operator=(Generic6DOFJoint3DSW const &) = delete;
+	GodotGeneric6DOFJoint3D(GodotGeneric6DOFJoint3D const &) = delete;
+	void operator=(GodotGeneric6DOFJoint3D const &) = delete;
 
 	void buildLinearJacobian(
-			JacobianEntry3DSW &jacLinear, const Vector3 &normalWorld,
+			GodotJacobianEntry3D &jacLinear, const Vector3 &normalWorld,
 			const Vector3 &pivotAInW, const Vector3 &pivotBInW);
 
-	void buildAngularJacobian(JacobianEntry3DSW &jacAngular, const Vector3 &jointAxisW);
+	void buildAngularJacobian(GodotJacobianEntry3D &jacAngular, const Vector3 &jointAxisW);
 
 	//! calcs the euler angles between the two bodies.
 	void calculateAngleInfo();
 
 public:
-	Generic6DOFJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform3D &frameInA, const Transform3D &frameInB, bool useLinearReferenceFrameA);
+	GodotGeneric6DOFJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Transform3D &frameInA, const Transform3D &frameInB, bool useLinearReferenceFrameA);
 
 	virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_6DOF; }
 
 	virtual bool setup(real_t p_step) override;
 	virtual void solve(real_t p_step) override;
 
-	//! Calcs global transform of the offsets
-	/*!
-	Calcs the global transform for the joint offset for body A an B, and also calcs the agle differences between the bodies.
-	\sa Generic6DOFJointSW.getCalculatedTransformA , Generic6DOFJointSW.getCalculatedTransformB, Generic6DOFJointSW.calculateAngleInfo
-	*/
+	// Calcs the global transform for the joint offset for body A an B, and also calcs the angle differences between the bodies.
 	void calculateTransforms();
 
-	//! Gets the global transform of the offset for body A
-	/*!
-    \sa Generic6DOFJointSW.getFrameOffsetA, Generic6DOFJointSW.getFrameOffsetB, Generic6DOFJointSW.calculateAngleInfo.
-    */
+	// Gets the global transform of the offset for body A.
 	const Transform3D &getCalculatedTransformA() const {
 		return m_calculatedTransformA;
 	}
 
-	//! Gets the global transform of the offset for body B
-	/*!
-    \sa Generic6DOFJointSW.getFrameOffsetA, Generic6DOFJointSW.getFrameOffsetB, Generic6DOFJointSW.calculateAngleInfo.
-    */
+	// Gets the global transform of the offset for body B.
 	const Transform3D &getCalculatedTransformB() const {
 		return m_calculatedTransformB;
 	}
@@ -279,27 +236,16 @@ public:
 		return m_frameInB;
 	}
 
-	//! performs Jacobian calculation, and also calculates angle differences and axis
-
+	// Performs Jacobian calculation, and also calculates angle differences and axis.
 	void updateRHS(real_t timeStep);
 
-	//! Get the rotation axis in global coordinates
-	/*!
-	\pre Generic6DOFJointSW.buildJacobian must be called previously.
-	*/
+	// Get the rotation axis in global coordinates.
 	Vector3 getAxis(int axis_index) const;
 
-	//! Get the relative Euler angle
-	/*!
-	\pre Generic6DOFJointSW.buildJacobian must be called previously.
-	*/
+	// Get the relative Euler angle.
 	real_t getAngle(int axis_index) const;
 
-	//! Test angular limit.
-	/*!
-	Calculates angular correction and returns true if limit needs to be corrected.
-	\pre Generic6DOFJointSW.buildJacobian must be called previously.
-	*/
+	// Calculates angular correction and returns true if limit needs to be corrected.
 	bool testAngularLimitMotor(int axis_index);
 
 	void setLinearLowerLimit(const Vector3 &linearLower) {
@@ -322,17 +268,17 @@ public:
 		m_angularLimits[2].m_hiLimit = angularUpper.z;
 	}
 
-	//! Retrieves the angular limit information.
-	G6DOFRotationalLimitMotor3DSW *getRotationalLimitMotor(int index) {
+	// Retrieves the angular limit information.
+	GodotG6DOFRotationalLimitMotor3D *getRotationalLimitMotor(int index) {
 		return &m_angularLimits[index];
 	}
 
-	//! Retrieves the limit information.
-	G6DOFTranslationalLimitMotor3DSW *getTranslationalLimitMotor() {
+	// Retrieves the limit information.
+	GodotG6DOFTranslationalLimitMotor3D *getTranslationalLimitMotor() {
 		return &m_linearLimits;
 	}
 
-	//first 3 are linear, next 3 are angular
+	// First 3 are linear, next 3 are angular.
 	void setLimit(int axis, real_t lo, real_t hi) {
 		if (axis < 3) {
 			m_linearLimits.m_lowerLimit[axis] = lo;
@@ -345,11 +291,11 @@ public:
 
 	//! Test limit
 	/*!
-    - free means upper < lower,
-    - locked means upper == lower
-    - limited means upper > lower
-    - limitIndex: first 3 are linear, next 3 are angular
-    */
+	 * - free means upper < lower,
+	 * - locked means upper == lower
+	 * - limited means upper > lower
+	 * - limitIndex: first 3 are linear, next 3 are angular
+	 */
 	bool isLimited(int limitIndex) {
 		if (limitIndex < 3) {
 			return m_linearLimits.isLimited(limitIndex);
@@ -357,10 +303,10 @@ public:
 		return m_angularLimits[limitIndex - 3].isLimited();
 	}
 
-	const Body3DSW *getRigidBodyA() const {
+	const GodotBody3D *getRigidBodyA() const {
 		return A;
 	}
-	const Body3DSW *getRigidBodyB() const {
+	const GodotBody3D *getRigidBodyB() const {
 		return B;
 	}
 
@@ -373,4 +319,4 @@ public:
 	bool get_flag(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisFlag p_flag) const;
 };
 
-#endif // GENERIC_6DOF_JOINT_SW_H
+#endif // GODOT_GENERIC_6DOF_JOINT_3D_H
diff --git a/servers/physics_3d/joints/hinge_joint_3d_sw.cpp b/servers/physics_3d/joints/godot_hinge_joint_3d.cpp
index e2bf2845fe..cf77129a30 100644
--- a/servers/physics_3d/joints/hinge_joint_3d_sw.cpp
+++ b/servers/physics_3d/joints/godot_hinge_joint_3d.cpp
@@ -1,5 +1,5 @@
 /*************************************************************************/
-/*  hinge_joint_3d_sw.cpp                                                */
+/*  godot_hinge_joint_3d.cpp                                             */
 /*************************************************************************/
 /*                       This file is part of:                           */
 /*                           GODOT ENGINE                                */
@@ -34,7 +34,7 @@ Adapted to Godot from the Bullet library.
 
 /*
 Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans  https://bulletphysics.org
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
 
 This software is provided 'as-is', without any express or implied warranty.
 In no event will the authors be held liable for any damages arising from the use of this software.
@@ -47,7 +47,7 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-#include "hinge_joint_3d_sw.h"
+#include "godot_hinge_joint_3d.h"
 
 static void plane_space(const Vector3 &n, Vector3 &p, Vector3 &q) {
 	if (Math::abs(n.z) > Math_SQRT12) {
@@ -67,8 +67,8 @@ static void plane_space(const Vector3 &n, Vector3 &p, Vector3 &q) {
 	}
 }
 
-HingeJoint3DSW::HingeJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform3D &frameA, const Transform3D &frameB) :
-		Joint3DSW(_arr, 2) {
+GodotHingeJoint3D::GodotHingeJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Transform3D &frameA, const Transform3D &frameB) :
+		GodotJoint3D(_arr, 2) {
 	A = rbA;
 	B = rbB;
 
@@ -79,28 +79,13 @@ HingeJoint3DSW::HingeJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform3D &
 	m_rbBFrame.basis[1][2] *= real_t(-1.);
 	m_rbBFrame.basis[2][2] *= real_t(-1.);
 
-	//start with free
-	m_lowerLimit = Math_PI;
-	m_upperLimit = -Math_PI;
-
-	m_useLimit = false;
-	m_biasFactor = 0.3f;
-	m_relaxationFactor = 1.0f;
-	m_limitSoftness = 0.9f;
-	m_solveLimit = false;
-
-	tau = 0.3;
-
-	m_angularOnly = false;
-	m_enableAngularMotor = false;
-
 	A->add_constraint(this, 0);
 	B->add_constraint(this, 1);
 }
 
-HingeJoint3DSW::HingeJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Vector3 &pivotInA, const Vector3 &pivotInB,
+GodotHingeJoint3D::GodotHingeJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Vector3 &pivotInA, const Vector3 &pivotInB,
 		const Vector3 &axisInA, const Vector3 &axisInB) :
-		Joint3DSW(_arr, 2) {
+		GodotJoint3D(_arr, 2) {
 	A = rbA;
 	B = rbB;
 
@@ -135,26 +120,11 @@ HingeJoint3DSW::HingeJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Vector3 &pivo
 			rbAxisB1.y, rbAxisB2.y, -axisInB.y,
 			rbAxisB1.z, rbAxisB2.z, -axisInB.z);
 
-	//start with free
-	m_lowerLimit = Math_PI;
-	m_upperLimit = -Math_PI;
-
-	m_useLimit = false;
-	m_biasFactor = 0.3f;
-	m_relaxationFactor = 1.0f;
-	m_limitSoftness = 0.9f;
-	m_solveLimit = false;
-
-	tau = 0.3;
-
-	m_angularOnly = false;
-	m_enableAngularMotor = false;
-
 	A->add_constraint(this, 0);
 	B->add_constraint(this, 1);
 }
 
-bool HingeJoint3DSW::setup(real_t p_step) {
+bool GodotHingeJoint3D::setup(real_t p_step) {
 	dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
 	dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
 
@@ -179,16 +149,18 @@ bool HingeJoint3DSW::setup(real_t p_step) {
 		plane_space(normal[0], normal[1], normal[2]);
 
 		for (int i = 0; i < 3; i++) {
-			memnew_placement(&m_jac[i], JacobianEntry3DSW(
-												A->get_principal_inertia_axes().transposed(),
-												B->get_principal_inertia_axes().transposed(),
-												pivotAInW - A->get_transform().origin - A->get_center_of_mass(),
-												pivotBInW - B->get_transform().origin - B->get_center_of_mass(),
-												normal[i],
-												A->get_inv_inertia(),
-												A->get_inv_mass(),
-												B->get_inv_inertia(),
-												B->get_inv_mass()));
+			memnew_placement(
+					&m_jac[i],
+					GodotJacobianEntry3D(
+							A->get_principal_inertia_axes().transposed(),
+							B->get_principal_inertia_axes().transposed(),
+							pivotAInW - A->get_transform().origin - A->get_center_of_mass(),
+							pivotBInW - B->get_transform().origin - B->get_center_of_mass(),
+							normal[i],
+							A->get_inv_inertia(),
+							A->get_inv_mass(),
+							B->get_inv_inertia(),
+							B->get_inv_mass()));
 		}
 	}
 
@@ -205,23 +177,32 @@ bool HingeJoint3DSW::setup(real_t p_step) {
 	Vector3 jointAxis1 = A->get_transform().basis.xform(jointAxis1local);
 	Vector3 hingeAxisWorld = A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(2));
 
-	memnew_placement(&m_jacAng[0], JacobianEntry3DSW(jointAxis0,
-										   A->get_principal_inertia_axes().transposed(),
-										   B->get_principal_inertia_axes().transposed(),
-										   A->get_inv_inertia(),
-										   B->get_inv_inertia()));
-
-	memnew_placement(&m_jacAng[1], JacobianEntry3DSW(jointAxis1,
-										   A->get_principal_inertia_axes().transposed(),
-										   B->get_principal_inertia_axes().transposed(),
-										   A->get_inv_inertia(),
-										   B->get_inv_inertia()));
-
-	memnew_placement(&m_jacAng[2], JacobianEntry3DSW(hingeAxisWorld,
-										   A->get_principal_inertia_axes().transposed(),
-										   B->get_principal_inertia_axes().transposed(),
-										   A->get_inv_inertia(),
-										   B->get_inv_inertia()));
+	memnew_placement(
+			&m_jacAng[0],
+			GodotJacobianEntry3D(
+					jointAxis0,
+					A->get_principal_inertia_axes().transposed(),
+					B->get_principal_inertia_axes().transposed(),
+					A->get_inv_inertia(),
+					B->get_inv_inertia()));
+
+	memnew_placement(
+			&m_jacAng[1],
+			GodotJacobianEntry3D(
+					jointAxis1,
+					A->get_principal_inertia_axes().transposed(),
+					B->get_principal_inertia_axes().transposed(),
+					A->get_inv_inertia(),
+					B->get_inv_inertia()));
+
+	memnew_placement(
+			&m_jacAng[2],
+			GodotJacobianEntry3D(
+					hingeAxisWorld,
+					A->get_principal_inertia_axes().transposed(),
+					B->get_principal_inertia_axes().transposed(),
+					A->get_inv_inertia(),
+					B->get_inv_inertia()));
 
 	// Compute limit information
 	real_t hingeAngle = get_hinge_angle();
@@ -250,13 +231,12 @@ bool HingeJoint3DSW::setup(real_t p_step) {
 
 	//Compute K = J*W*J' for hinge axis
 	Vector3 axisA = A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(2));
-	m_kHinge = 1.0f / (A->compute_angular_impulse_denominator(axisA) +
-							  B->compute_angular_impulse_denominator(axisA));
+	m_kHinge = 1.0f / (A->compute_angular_impulse_denominator(axisA) + B->compute_angular_impulse_denominator(axisA));
 
 	return true;
 }
 
-void HingeJoint3DSW::solve(real_t p_step) {
+void GodotHingeJoint3D::solve(real_t p_step) {
 	Vector3 pivotAInW = A->get_transform().xform(m_rbAFrame.origin);
 	Vector3 pivotBInW = B->get_transform().xform(m_rbBFrame.origin);
 
@@ -314,7 +294,7 @@ void HingeJoint3DSW::solve(real_t p_step) {
 			if (len > real_t(0.00001)) {
 				Vector3 normal = velrelOrthog.normalized();
 				real_t denom = A->compute_angular_impulse_denominator(normal) +
-							   B->compute_angular_impulse_denominator(normal);
+						B->compute_angular_impulse_denominator(normal);
 				// scale for mass and relaxation
 				velrelOrthog *= (real_t(1.) / denom) * m_relaxationFactor;
 			}
@@ -325,7 +305,7 @@ void HingeJoint3DSW::solve(real_t p_step) {
 			if (len2 > real_t(0.00001)) {
 				Vector3 normal2 = angularError.normalized();
 				real_t denom2 = A->compute_angular_impulse_denominator(normal2) +
-								B->compute_angular_impulse_denominator(normal2);
+						B->compute_angular_impulse_denominator(normal2);
 				angularError *= (real_t(1.) / denom2) * relaxation;
 			}
 
@@ -407,7 +387,7 @@ static _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x) {
 	return (y < 0.0f) ? -angle : angle;
 }
 
-real_t HingeJoint3DSW::get_hinge_angle() {
+real_t GodotHingeJoint3D::get_hinge_angle() {
 	const Vector3 refAxis0 = A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(0));
 	const Vector3 refAxis1 = A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(1));
 	const Vector3 swingAxis = B->get_transform().basis.xform(m_rbBFrame.basis.get_axis(1));
@@ -415,7 +395,7 @@ real_t HingeJoint3DSW::get_hinge_angle() {
 	return atan2fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
 }
 
-void HingeJoint3DSW::set_param(PhysicsServer3D::HingeJointParam p_param, real_t p_value) {
+void GodotHingeJoint3D::set_param(PhysicsServer3D::HingeJointParam p_param, real_t p_value) {
 	switch (p_param) {
 		case PhysicsServer3D::HINGE_JOINT_BIAS:
 			tau = p_value;
@@ -446,7 +426,7 @@ void HingeJoint3DSW::set_param(PhysicsServer3D::HingeJointParam p_param, real_t
 	}
 }
 
-real_t HingeJoint3DSW::get_param(PhysicsServer3D::HingeJointParam p_param) const {
+real_t GodotHingeJoint3D::get_param(PhysicsServer3D::HingeJointParam p_param) const {
 	switch (p_param) {
 		case PhysicsServer3D::HINGE_JOINT_BIAS:
 			return tau;
@@ -471,7 +451,7 @@ real_t HingeJoint3DSW::get_param(PhysicsServer3D::HingeJointParam p_param) const
 	return 0;
 }
 
-void HingeJoint3DSW::set_flag(PhysicsServer3D::HingeJointFlag p_flag, bool p_value) {
+void GodotHingeJoint3D::set_flag(PhysicsServer3D::HingeJointFlag p_flag, bool p_value) {
 	switch (p_flag) {
 		case PhysicsServer3D::HINGE_JOINT_FLAG_USE_LIMIT:
 			m_useLimit = p_value;
@@ -484,7 +464,7 @@ void HingeJoint3DSW::set_flag(PhysicsServer3D::HingeJointFlag p_flag, bool p_val
 	}
 }
 
-bool HingeJoint3DSW::get_flag(PhysicsServer3D::HingeJointFlag p_flag) const {
+bool GodotHingeJoint3D::get_flag(PhysicsServer3D::HingeJointFlag p_flag) const {
 	switch (p_flag) {
 		case PhysicsServer3D::HINGE_JOINT_FLAG_USE_LIMIT:
 			return m_useLimit;
diff --git a/servers/physics_3d/joints/hinge_joint_3d_sw.h b/servers/physics_3d/joints/godot_hinge_joint_3d.h
index 572c35266f..ff1fbe0f25 100644
--- a/servers/physics_3d/joints/hinge_joint_3d_sw.h
+++ b/servers/physics_3d/joints/godot_hinge_joint_3d.h
@@ -1,5 +1,5 @@
 /*************************************************************************/
-/*  hinge_joint_3d_sw.h                                                  */
+/*  godot_hinge_joint_3d.h                                               */
 /*************************************************************************/
 /*                       This file is part of:                           */
 /*                           GODOT ENGINE                                */
@@ -32,15 +32,15 @@
 Adapted to Godot from the Bullet library.
 */
 
-#ifndef HINGE_JOINT_SW_H
-#define HINGE_JOINT_SW_H
+#ifndef GODOT_HINGE_JOINT_3D_H
+#define GODOT_HINGE_JOINT_3D_H
 
-#include "servers/physics_3d/joints/jacobian_entry_3d_sw.h"
-#include "servers/physics_3d/joints_3d_sw.h"
+#include "servers/physics_3d/godot_joint_3d.h"
+#include "servers/physics_3d/joints/godot_jacobian_entry_3d.h"
 
 /*
 Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans  https://bulletphysics.org
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
 
 This software is provided 'as-is', without any express or implied warranty.
 In no event will the authors be held liable for any damages arising from the use of this software.
@@ -53,47 +53,47 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-class HingeJoint3DSW : public Joint3DSW {
+class GodotHingeJoint3D : public GodotJoint3D {
 	union {
 		struct {
-			Body3DSW *A;
-			Body3DSW *B;
+			GodotBody3D *A;
+			GodotBody3D *B;
 		};
 
-		Body3DSW *_arr[2];
+		GodotBody3D *_arr[2] = {};
 	};
 
-	JacobianEntry3DSW m_jac[3]; //3 orthogonal linear constraints
-	JacobianEntry3DSW m_jacAng[3]; //2 orthogonal angular constraints+ 1 for limit/motor
+	GodotJacobianEntry3D m_jac[3]; //3 orthogonal linear constraints
+	GodotJacobianEntry3D m_jacAng[3]; //2 orthogonal angular constraints+ 1 for limit/motor
 
 	Transform3D m_rbAFrame; // constraint axii. Assumes z is hinge axis.
 	Transform3D m_rbBFrame;
 
-	real_t m_motorTargetVelocity;
-	real_t m_maxMotorImpulse;
+	real_t m_motorTargetVelocity = 0.0;
+	real_t m_maxMotorImpulse = 0.0;
 
-	real_t m_limitSoftness;
-	real_t m_biasFactor;
-	real_t m_relaxationFactor;
+	real_t m_limitSoftness = 0.9;
+	real_t m_biasFactor = 0.3;
+	real_t m_relaxationFactor = 1.0;
 
-	real_t m_lowerLimit;
-	real_t m_upperLimit;
+	real_t m_lowerLimit = Math_PI;
+	real_t m_upperLimit = -Math_PI;
 
-	real_t m_kHinge;
+	real_t m_kHinge = 0.0;
 
-	real_t m_limitSign;
-	real_t m_correction;
+	real_t m_limitSign = 0.0;
+	real_t m_correction = 0.0;
 
-	real_t m_accLimitImpulse;
+	real_t m_accLimitImpulse = 0.0;
 
-	real_t tau;
+	real_t tau = 0.3;
 
-	bool m_useLimit;
-	bool m_angularOnly;
-	bool m_enableAngularMotor;
-	bool m_solveLimit;
+	bool m_useLimit = false;
+	bool m_angularOnly = false;
+	bool m_enableAngularMotor = false;
+	bool m_solveLimit = false;
 
-	real_t m_appliedImpulse;
+	real_t m_appliedImpulse = 0.0;
 
 public:
 	virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_HINGE; }
@@ -109,8 +109,8 @@ public:
 	void set_flag(PhysicsServer3D::HingeJointFlag p_flag, bool p_value);
 	bool get_flag(PhysicsServer3D::HingeJointFlag p_flag) const;
 
-	HingeJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform3D &frameA, const Transform3D &frameB);
-	HingeJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Vector3 &pivotInA, const Vector3 &pivotInB, const Vector3 &axisInA, const Vector3 &axisInB);
+	GodotHingeJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Transform3D &frameA, const Transform3D &frameB);
+	GodotHingeJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Vector3 &pivotInA, const Vector3 &pivotInB, const Vector3 &axisInA, const Vector3 &axisInB);
 };
 
-#endif // HINGE_JOINT_SW_H
+#endif // GODOT_HINGE_JOINT_3D_H
diff --git a/servers/physics_3d/joints/jacobian_entry_3d_sw.h b/servers/physics_3d/joints/godot_jacobian_entry_3d.h
index 30c80db23f..90a77a9b61 100644
--- a/servers/physics_3d/joints/jacobian_entry_3d_sw.h
+++ b/servers/physics_3d/joints/godot_jacobian_entry_3d.h
@@ -1,5 +1,5 @@
 /*************************************************************************/
-/*  jacobian_entry_3d_sw.h                                               */
+/*  godot_jacobian_entry_3d.h                                            */
 /*************************************************************************/
 /*                       This file is part of:                           */
 /*                           GODOT ENGINE                                */
@@ -32,12 +32,12 @@
 Adapted to Godot from the Bullet library.
 */
 
-#ifndef JACOBIAN_ENTRY_SW_H
-#define JACOBIAN_ENTRY_SW_H
+#ifndef GODOT_JACOBIAN_ENTRY_3D_H
+#define GODOT_JACOBIAN_ENTRY_3D_H
 
 /*
 Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans  https://bulletphysics.org
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
 
 This software is provided 'as-is', without any express or implied warranty.
 In no event will the authors be held liable for any damages arising from the use of this software.
@@ -52,11 +52,11 @@ subject to the following restrictions:
 
 #include "core/math/transform_3d.h"
 
-class JacobianEntry3DSW {
+class GodotJacobianEntry3D {
 public:
-	JacobianEntry3DSW() {}
+	GodotJacobianEntry3D() {}
 	//constraint between two different rigidbodies
-	JacobianEntry3DSW(
+	GodotJacobianEntry3D(
 			const Basis &world2A,
 			const Basis &world2B,
 			const Vector3 &rel_pos1, const Vector3 &rel_pos2,
@@ -76,7 +76,7 @@ public:
 	}
 
 	//angular constraint between two different rigidbodies
-	JacobianEntry3DSW(const Vector3 &jointAxis,
+	GodotJacobianEntry3D(const Vector3 &jointAxis,
 			const Basis &world2A,
 			const Basis &world2B,
 			const Vector3 &inertiaInvA,
@@ -92,7 +92,7 @@ public:
 	}
 
 	//angular constraint between two different rigidbodies
-	JacobianEntry3DSW(const Vector3 &axisInA,
+	GodotJacobianEntry3D(const Vector3 &axisInA,
 			const Vector3 &axisInB,
 			const Vector3 &inertiaInvA,
 			const Vector3 &inertiaInvB) :
@@ -107,7 +107,7 @@ public:
 	}
 
 	//constraint on one rigidbody
-	JacobianEntry3DSW(
+	GodotJacobianEntry3D(
 			const Basis &world2A,
 			const Vector3 &rel_pos1, const Vector3 &rel_pos2,
 			const Vector3 &jointAxis,
@@ -126,16 +126,16 @@ public:
 	real_t getDiagonal() const { return m_Adiag; }
 
 	// for two constraints on the same rigidbody (for example vehicle friction)
-	real_t getNonDiagonal(const JacobianEntry3DSW &jacB, const real_t massInvA) const {
-		const JacobianEntry3DSW &jacA = *this;
+	real_t getNonDiagonal(const GodotJacobianEntry3D &jacB, const real_t massInvA) const {
+		const GodotJacobianEntry3D &jacA = *this;
 		real_t lin = massInvA * jacA.m_linearJointAxis.dot(jacB.m_linearJointAxis);
 		real_t ang = jacA.m_0MinvJt.dot(jacB.m_aJ);
 		return lin + ang;
 	}
 
 	// for two constraints on sharing two same rigidbodies (for example two contact points between two rigidbodies)
-	real_t getNonDiagonal(const JacobianEntry3DSW &jacB, const real_t massInvA, const real_t massInvB) const {
-		const JacobianEntry3DSW &jacA = *this;
+	real_t getNonDiagonal(const GodotJacobianEntry3D &jacB, const real_t massInvA, const real_t massInvB) const {
+		const GodotJacobianEntry3D &jacA = *this;
 		Vector3 lin = jacA.m_linearJointAxis * jacB.m_linearJointAxis;
 		Vector3 ang0 = jacA.m_0MinvJt * jacB.m_aJ;
 		Vector3 ang1 = jacA.m_1MinvJt * jacB.m_bJ;
@@ -163,7 +163,7 @@ public:
 	Vector3 m_0MinvJt;
 	Vector3 m_1MinvJt;
 	//Optimization: can be stored in the w/last component of one of the vectors
-	real_t m_Adiag;
+	real_t m_Adiag = 1.0;
 };
 
-#endif // JACOBIAN_ENTRY_SW_H
+#endif // GODOT_JACOBIAN_ENTRY_3D_H
diff --git a/servers/physics_3d/joints/pin_joint_3d_sw.cpp b/servers/physics_3d/joints/godot_pin_joint_3d.cpp
index 7a713c1161..e9e81b61a7 100644
--- a/servers/physics_3d/joints/pin_joint_3d_sw.cpp
+++ b/servers/physics_3d/joints/godot_pin_joint_3d.cpp
@@ -1,5 +1,5 @@
 /*************************************************************************/
-/*  pin_joint_3d_sw.cpp                                                  */
+/*  godot_pin_joint_3d.cpp                                               */
 /*************************************************************************/
 /*                       This file is part of:                           */
 /*                           GODOT ENGINE                                */
@@ -34,7 +34,7 @@ Adapted to Godot from the Bullet library.
 
 /*
 Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans  https://bulletphysics.org
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
 
 This software is provided 'as-is', without any express or implied warranty.
 In no event will the authors be held liable for any damages arising from the use of this software.
@@ -47,9 +47,9 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-#include "pin_joint_3d_sw.h"
+#include "godot_pin_joint_3d.h"
 
-bool PinJoint3DSW::setup(real_t p_step) {
+bool GodotPinJoint3D::setup(real_t p_step) {
 	dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
 	dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
 
@@ -63,23 +63,25 @@ bool PinJoint3DSW::setup(real_t p_step) {
 
 	for (int i = 0; i < 3; i++) {
 		normal[i] = 1;
-		memnew_placement(&m_jac[i], JacobianEntry3DSW(
-											A->get_principal_inertia_axes().transposed(),
-											B->get_principal_inertia_axes().transposed(),
-											A->get_transform().xform(m_pivotInA) - A->get_transform().origin - A->get_center_of_mass(),
-											B->get_transform().xform(m_pivotInB) - B->get_transform().origin - B->get_center_of_mass(),
-											normal,
-											A->get_inv_inertia(),
-											A->get_inv_mass(),
-											B->get_inv_inertia(),
-											B->get_inv_mass()));
+		memnew_placement(
+				&m_jac[i],
+				GodotJacobianEntry3D(
+						A->get_principal_inertia_axes().transposed(),
+						B->get_principal_inertia_axes().transposed(),
+						A->get_transform().xform(m_pivotInA) - A->get_transform().origin - A->get_center_of_mass(),
+						B->get_transform().xform(m_pivotInB) - B->get_transform().origin - B->get_center_of_mass(),
+						normal,
+						A->get_inv_inertia(),
+						A->get_inv_mass(),
+						B->get_inv_inertia(),
+						B->get_inv_mass()));
 		normal[i] = 0;
 	}
 
 	return true;
 }
 
-void PinJoint3DSW::solve(real_t p_step) {
+void GodotPinJoint3D::solve(real_t p_step) {
 	Vector3 pivotAInW = A->get_transform().xform(m_pivotInA);
 	Vector3 pivotBInW = B->get_transform().xform(m_pivotInB);
 
@@ -137,7 +139,7 @@ void PinJoint3DSW::solve(real_t p_step) {
 	}
 }
 
-void PinJoint3DSW::set_param(PhysicsServer3D::PinJointParam p_param, real_t p_value) {
+void GodotPinJoint3D::set_param(PhysicsServer3D::PinJointParam p_param, real_t p_value) {
 	switch (p_param) {
 		case PhysicsServer3D::PIN_JOINT_BIAS:
 			m_tau = p_value;
@@ -151,7 +153,7 @@ void PinJoint3DSW::set_param(PhysicsServer3D::PinJointParam p_param, real_t p_va
 	}
 }
 
-real_t PinJoint3DSW::get_param(PhysicsServer3D::PinJointParam p_param) const {
+real_t GodotPinJoint3D::get_param(PhysicsServer3D::PinJointParam p_param) const {
 	switch (p_param) {
 		case PhysicsServer3D::PIN_JOINT_BIAS:
 			return m_tau;
@@ -164,21 +166,16 @@ real_t PinJoint3DSW::get_param(PhysicsServer3D::PinJointParam p_param) const {
 	return 0;
 }
 
-PinJoint3DSW::PinJoint3DSW(Body3DSW *p_body_a, const Vector3 &p_pos_a, Body3DSW *p_body_b, const Vector3 &p_pos_b) :
-		Joint3DSW(_arr, 2) {
+GodotPinJoint3D::GodotPinJoint3D(GodotBody3D *p_body_a, const Vector3 &p_pos_a, GodotBody3D *p_body_b, const Vector3 &p_pos_b) :
+		GodotJoint3D(_arr, 2) {
 	A = p_body_a;
 	B = p_body_b;
 	m_pivotInA = p_pos_a;
 	m_pivotInB = p_pos_b;
 
-	m_tau = 0.3;
-	m_damping = 1;
-	m_impulseClamp = 0;
-	m_appliedImpulse = 0;
-
 	A->add_constraint(this, 0);
 	B->add_constraint(this, 1);
 }
 
-PinJoint3DSW::~PinJoint3DSW() {
+GodotPinJoint3D::~GodotPinJoint3D() {
 }
diff --git a/servers/physics_3d/joints/pin_joint_3d_sw.h b/servers/physics_3d/joints/godot_pin_joint_3d.h
index 09deefc5c4..17e2e6e973 100644
--- a/servers/physics_3d/joints/pin_joint_3d_sw.h
+++ b/servers/physics_3d/joints/godot_pin_joint_3d.h
@@ -1,5 +1,5 @@
 /*************************************************************************/
-/*  pin_joint_3d_sw.h                                                    */
+/*  godot_pin_joint_3d.h                                                 */
 /*************************************************************************/
 /*                       This file is part of:                           */
 /*                           GODOT ENGINE                                */
@@ -32,15 +32,15 @@
 Adapted to Godot from the Bullet library.
 */
 
-#ifndef PIN_JOINT_SW_H
-#define PIN_JOINT_SW_H
+#ifndef GODOT_PIN_JOINT_3D_H
+#define GODOT_PIN_JOINT_3D_H
 
-#include "servers/physics_3d/joints/jacobian_entry_3d_sw.h"
-#include "servers/physics_3d/joints_3d_sw.h"
+#include "servers/physics_3d/godot_joint_3d.h"
+#include "servers/physics_3d/joints/godot_jacobian_entry_3d.h"
 
 /*
 Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans  https://bulletphysics.org
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
 
 This software is provided 'as-is', without any express or implied warranty.
 In no event will the authors be held liable for any damages arising from the use of this software.
@@ -53,22 +53,22 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-class PinJoint3DSW : public Joint3DSW {
+class GodotPinJoint3D : public GodotJoint3D {
 	union {
 		struct {
-			Body3DSW *A;
-			Body3DSW *B;
+			GodotBody3D *A;
+			GodotBody3D *B;
 		};
 
-		Body3DSW *_arr[2];
+		GodotBody3D *_arr[2] = {};
 	};
 
-	real_t m_tau; //bias
-	real_t m_damping;
-	real_t m_impulseClamp;
-	real_t m_appliedImpulse;
+	real_t m_tau = 0.3; //bias
+	real_t m_damping = 1.0;
+	real_t m_impulseClamp = 0.0;
+	real_t m_appliedImpulse = 0.0;
 
-	JacobianEntry3DSW m_jac[3]; //3 orthogonal linear constraints
+	GodotJacobianEntry3D m_jac[3] = {}; //3 orthogonal linear constraints
 
 	Vector3 m_pivotInA;
 	Vector3 m_pivotInB;
@@ -88,8 +88,8 @@ public:
 	Vector3 get_position_a() { return m_pivotInA; }
 	Vector3 get_position_b() { return m_pivotInB; }
 
-	PinJoint3DSW(Body3DSW *p_body_a, const Vector3 &p_pos_a, Body3DSW *p_body_b, const Vector3 &p_pos_b);
-	~PinJoint3DSW();
+	GodotPinJoint3D(GodotBody3D *p_body_a, const Vector3 &p_pos_a, GodotBody3D *p_body_b, const Vector3 &p_pos_b);
+	~GodotPinJoint3D();
 };
 
-#endif // PIN_JOINT_SW_H
+#endif // GODOT_PIN_JOINT_3D_H
diff --git a/servers/physics_3d/joints/slider_joint_3d_sw.cpp b/servers/physics_3d/joints/godot_slider_joint_3d.cpp
index 9f01196c30..1f463ad24c 100644
--- a/servers/physics_3d/joints/slider_joint_3d_sw.cpp
+++ b/servers/physics_3d/joints/godot_slider_joint_3d.cpp
@@ -1,5 +1,5 @@
 /*************************************************************************/
-/*  slider_joint_3d_sw.cpp                                               */
+/*  godot_slider_joint_3d.cpp                                            */
 /*************************************************************************/
 /*                       This file is part of:                           */
 /*                           GODOT ENGINE                                */
@@ -34,7 +34,7 @@ Adapted to Godot from the Bullet library.
 
 /*
 Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans  https://bulletphysics.org
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
 
 This software is provided 'as-is', without any express or implied warranty.
 In no event will the authors be held liable for any damages arising from the use of this software.
@@ -53,7 +53,7 @@ April 04, 2008
 
 */
 
-#include "slider_joint_3d_sw.h"
+#include "godot_slider_joint_3d.h"
 
 //-----------------------------------------------------------------------------
 
@@ -72,47 +72,12 @@ static _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x) {
 	return (y < 0.0f) ? -angle : angle;
 }
 
-void SliderJoint3DSW::initParams() {
-	m_lowerLinLimit = real_t(1.0);
-	m_upperLinLimit = real_t(-1.0);
-	m_lowerAngLimit = real_t(0.);
-	m_upperAngLimit = real_t(0.);
-	m_softnessDirLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
-	m_restitutionDirLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
-	m_dampingDirLin = real_t(0.);
-	m_softnessDirAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
-	m_restitutionDirAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
-	m_dampingDirAng = real_t(0.);
-	m_softnessOrthoLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
-	m_restitutionOrthoLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
-	m_dampingOrthoLin = SLIDER_CONSTRAINT_DEF_DAMPING;
-	m_softnessOrthoAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
-	m_restitutionOrthoAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
-	m_dampingOrthoAng = SLIDER_CONSTRAINT_DEF_DAMPING;
-	m_softnessLimLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
-	m_restitutionLimLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
-	m_dampingLimLin = SLIDER_CONSTRAINT_DEF_DAMPING;
-	m_softnessLimAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
-	m_restitutionLimAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
-	m_dampingLimAng = SLIDER_CONSTRAINT_DEF_DAMPING;
-
-	m_poweredLinMotor = false;
-	m_targetLinMotorVelocity = real_t(0.);
-	m_maxLinMotorForce = real_t(0.);
-	m_accumulatedLinMotorImpulse = real_t(0.0);
-
-	m_poweredAngMotor = false;
-	m_targetAngMotorVelocity = real_t(0.);
-	m_maxAngMotorForce = real_t(0.);
-	m_accumulatedAngMotorImpulse = real_t(0.0);
-} // SliderJointSW::initParams()
-
 //-----------------------------------------------------------------------------
 
 //-----------------------------------------------------------------------------
 
-SliderJoint3DSW::SliderJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform3D &frameInA, const Transform3D &frameInB) :
-		Joint3DSW(_arr, 2),
+GodotSliderJoint3D::GodotSliderJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Transform3D &frameInA, const Transform3D &frameInB) :
+		GodotJoint3D(_arr, 2),
 		m_frameInA(frameInA),
 		m_frameInB(frameInB) {
 	A = rbA;
@@ -120,13 +85,11 @@ SliderJoint3DSW::SliderJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform3D
 
 	A->add_constraint(this, 0);
 	B->add_constraint(this, 1);
-
-	initParams();
-} // SliderJointSW::SliderJointSW()
+}
 
 //-----------------------------------------------------------------------------
 
-bool SliderJoint3DSW::setup(real_t p_step) {
+bool GodotSliderJoint3D::setup(real_t p_step) {
 	dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
 	dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
 
@@ -149,16 +112,18 @@ bool SliderJoint3DSW::setup(real_t p_step) {
 	//linear part
 	for (i = 0; i < 3; i++) {
 		normalWorld = m_calculatedTransformA.basis.get_axis(i);
-		memnew_placement(&m_jacLin[i], JacobianEntry3DSW(
-											   A->get_principal_inertia_axes().transposed(),
-											   B->get_principal_inertia_axes().transposed(),
-											   m_relPosA - A->get_center_of_mass(),
-											   m_relPosB - B->get_center_of_mass(),
-											   normalWorld,
-											   A->get_inv_inertia(),
-											   A->get_inv_mass(),
-											   B->get_inv_inertia(),
-											   B->get_inv_mass()));
+		memnew_placement(
+				&m_jacLin[i],
+				GodotJacobianEntry3D(
+						A->get_principal_inertia_axes().transposed(),
+						B->get_principal_inertia_axes().transposed(),
+						m_relPosA - A->get_center_of_mass(),
+						m_relPosB - B->get_center_of_mass(),
+						normalWorld,
+						A->get_inv_inertia(),
+						A->get_inv_mass(),
+						B->get_inv_inertia(),
+						B->get_inv_mass()));
 		m_jacLinDiagABInv[i] = real_t(1.) / m_jacLin[i].getDiagonal();
 		m_depth[i] = m_delta.dot(normalWorld);
 	}
@@ -166,12 +131,14 @@ bool SliderJoint3DSW::setup(real_t p_step) {
 	// angular part
 	for (i = 0; i < 3; i++) {
 		normalWorld = m_calculatedTransformA.basis.get_axis(i);
-		memnew_placement(&m_jacAng[i], JacobianEntry3DSW(
-											   normalWorld,
-											   A->get_principal_inertia_axes().transposed(),
-											   B->get_principal_inertia_axes().transposed(),
-											   A->get_inv_inertia(),
-											   B->get_inv_inertia()));
+		memnew_placement(
+				&m_jacAng[i],
+				GodotJacobianEntry3D(
+						normalWorld,
+						A->get_principal_inertia_axes().transposed(),
+						B->get_principal_inertia_axes().transposed(),
+						A->get_inv_inertia(),
+						B->get_inv_inertia()));
 	}
 	testAngLimits();
 	Vector3 axisA = m_calculatedTransformA.basis.get_axis(0);
@@ -181,11 +148,11 @@ bool SliderJoint3DSW::setup(real_t p_step) {
 	m_accumulatedAngMotorImpulse = real_t(0.0);
 
 	return true;
-} // SliderJointSW::buildJacobianInt()
+}
 
 //-----------------------------------------------------------------------------
 
-void SliderJoint3DSW::solve(real_t p_step) {
+void GodotSliderJoint3D::solve(real_t p_step) {
 	int i;
 	// linear
 	Vector3 velA = A->get_velocity_in_local_point(m_relPosA);
@@ -321,13 +288,11 @@ void SliderJoint3DSW::solve(real_t p_step) {
 			}
 		}
 	}
-} // SliderJointSW::solveConstraint()
-
-//-----------------------------------------------------------------------------
+}
 
 //-----------------------------------------------------------------------------
 
-void SliderJoint3DSW::calculateTransforms() {
+void GodotSliderJoint3D::calculateTransforms() {
 	m_calculatedTransformA = A->get_transform() * m_frameInA;
 	m_calculatedTransformB = B->get_transform() * m_frameInB;
 	m_realPivotAInW = m_calculatedTransformA.origin;
@@ -342,11 +307,11 @@ void SliderJoint3DSW::calculateTransforms() {
 		normalWorld = m_calculatedTransformA.basis.get_axis(i);
 		m_depth[i] = m_delta.dot(normalWorld);
 	}
-} // SliderJointSW::calculateTransforms()
+}
 
 //-----------------------------------------------------------------------------
 
-void SliderJoint3DSW::testLinLimits() {
+void GodotSliderJoint3D::testLinLimits() {
 	m_solveLinLim = false;
 	m_linPos = m_depth[0];
 	if (m_lowerLinLimit <= m_upperLinLimit) {
@@ -362,11 +327,11 @@ void SliderJoint3DSW::testLinLimits() {
 	} else {
 		m_depth[0] = real_t(0.);
 	}
-} // SliderJointSW::testLinLimits()
+}
 
 //-----------------------------------------------------------------------------
 
-void SliderJoint3DSW::testAngLimits() {
+void GodotSliderJoint3D::testAngLimits() {
 	m_angDepth = real_t(0.);
 	m_solveAngLim = false;
 	if (m_lowerAngLimit <= m_upperAngLimit) {
@@ -382,26 +347,26 @@ void SliderJoint3DSW::testAngLimits() {
 			m_solveAngLim = true;
 		}
 	}
-} // SliderJointSW::testAngLimits()
+}
 
 //-----------------------------------------------------------------------------
 
-Vector3 SliderJoint3DSW::getAncorInA() {
+Vector3 GodotSliderJoint3D::getAncorInA() {
 	Vector3 ancorInA;
 	ancorInA = m_realPivotAInW + (m_lowerLinLimit + m_upperLinLimit) * real_t(0.5) * m_sliderAxis;
 	ancorInA = A->get_transform().inverse().xform(ancorInA);
 	return ancorInA;
-} // SliderJointSW::getAncorInA()
+}
 
 //-----------------------------------------------------------------------------
 
-Vector3 SliderJoint3DSW::getAncorInB() {
+Vector3 GodotSliderJoint3D::getAncorInB() {
 	Vector3 ancorInB;
 	ancorInB = m_frameInB.origin;
 	return ancorInB;
-} // SliderJointSW::getAncorInB();
+}
 
-void SliderJoint3DSW::set_param(PhysicsServer3D::SliderJointParam p_param, real_t p_value) {
+void GodotSliderJoint3D::set_param(PhysicsServer3D::SliderJointParam p_param, real_t p_value) {
 	switch (p_param) {
 		case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_UPPER:
 			m_upperLinLimit = p_value;
@@ -476,7 +441,7 @@ void SliderJoint3DSW::set_param(PhysicsServer3D::SliderJointParam p_param, real_
 	}
 }
 
-real_t SliderJoint3DSW::get_param(PhysicsServer3D::SliderJointParam p_param) const {
+real_t GodotSliderJoint3D::get_param(PhysicsServer3D::SliderJointParam p_param) const {
 	switch (p_param) {
 		case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_UPPER:
 			return m_upperLinLimit;
diff --git a/servers/physics_3d/joints/slider_joint_3d_sw.h b/servers/physics_3d/joints/godot_slider_joint_3d.h
index f09476f570..9baaf1fa40 100644
--- a/servers/physics_3d/joints/slider_joint_3d_sw.h
+++ b/servers/physics_3d/joints/godot_slider_joint_3d.h
@@ -1,5 +1,5 @@
 /*************************************************************************/
-/*  slider_joint_3d_sw.h                                                 */
+/*  godot_slider_joint_3d.h                                              */
 /*************************************************************************/
 /*                       This file is part of:                           */
 /*                           GODOT ENGINE                                */
@@ -32,15 +32,15 @@
 Adapted to Godot from the Bullet library.
 */
 
-#ifndef SLIDER_JOINT_SW_H
-#define SLIDER_JOINT_SW_H
+#ifndef GODOT_SLIDER_JOINT_3D_H
+#define GODOT_SLIDER_JOINT_3D_H
 
-#include "servers/physics_3d/joints/jacobian_entry_3d_sw.h"
-#include "servers/physics_3d/joints_3d_sw.h"
+#include "servers/physics_3d/godot_joint_3d.h"
+#include "servers/physics_3d/joints/godot_jacobian_entry_3d.h"
 
 /*
 Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans  https://bulletphysics.org
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
 
 This software is provided 'as-is', without any express or implied warranty.
 In no event will the authors be held liable for any damages arising from the use of this software.
@@ -65,61 +65,61 @@ April 04, 2008
 
 //-----------------------------------------------------------------------------
 
-class SliderJoint3DSW : public Joint3DSW {
+class GodotSliderJoint3D : public GodotJoint3D {
 protected:
 	union {
 		struct {
-			Body3DSW *A;
-			Body3DSW *B;
+			GodotBody3D *A;
+			GodotBody3D *B;
 		};
 
-		Body3DSW *_arr[2];
+		GodotBody3D *_arr[2] = { nullptr, nullptr };
 	};
 
 	Transform3D m_frameInA;
 	Transform3D m_frameInB;
 
 	// linear limits
-	real_t m_lowerLinLimit;
-	real_t m_upperLinLimit;
+	real_t m_lowerLinLimit = 1.0;
+	real_t m_upperLinLimit = -1.0;
 	// angular limits
-	real_t m_lowerAngLimit;
-	real_t m_upperAngLimit;
+	real_t m_lowerAngLimit = 0.0;
+	real_t m_upperAngLimit = 0.0;
 	// softness, restitution and damping for different cases
 	// DirLin - moving inside linear limits
 	// LimLin - hitting linear limit
 	// DirAng - moving inside angular limits
 	// LimAng - hitting angular limit
 	// OrthoLin, OrthoAng - against constraint axis
-	real_t m_softnessDirLin;
-	real_t m_restitutionDirLin;
-	real_t m_dampingDirLin;
-	real_t m_softnessDirAng;
-	real_t m_restitutionDirAng;
-	real_t m_dampingDirAng;
-	real_t m_softnessLimLin;
-	real_t m_restitutionLimLin;
-	real_t m_dampingLimLin;
-	real_t m_softnessLimAng;
-	real_t m_restitutionLimAng;
-	real_t m_dampingLimAng;
-	real_t m_softnessOrthoLin;
-	real_t m_restitutionOrthoLin;
-	real_t m_dampingOrthoLin;
-	real_t m_softnessOrthoAng;
-	real_t m_restitutionOrthoAng;
-	real_t m_dampingOrthoAng;
+	real_t m_softnessDirLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
+	real_t m_restitutionDirLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
+	real_t m_dampingDirLin = 0.0;
+	real_t m_softnessDirAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
+	real_t m_restitutionDirAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
+	real_t m_dampingDirAng = 0.0;
+	real_t m_softnessLimLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
+	real_t m_restitutionLimLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
+	real_t m_dampingLimLin = SLIDER_CONSTRAINT_DEF_DAMPING;
+	real_t m_softnessLimAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
+	real_t m_restitutionLimAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
+	real_t m_dampingLimAng = SLIDER_CONSTRAINT_DEF_DAMPING;
+	real_t m_softnessOrthoLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
+	real_t m_restitutionOrthoLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
+	real_t m_dampingOrthoLin = SLIDER_CONSTRAINT_DEF_DAMPING;
+	real_t m_softnessOrthoAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
+	real_t m_restitutionOrthoAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
+	real_t m_dampingOrthoAng = SLIDER_CONSTRAINT_DEF_DAMPING;
 
 	// for interlal use
-	bool m_solveLinLim;
-	bool m_solveAngLim;
+	bool m_solveLinLim = false;
+	bool m_solveAngLim = false;
 
-	JacobianEntry3DSW m_jacLin[3];
-	real_t m_jacLinDiagABInv[3];
+	GodotJacobianEntry3D m_jacLin[3] = {};
+	real_t m_jacLinDiagABInv[3] = {};
 
-	JacobianEntry3DSW m_jacAng[3];
+	GodotJacobianEntry3D m_jacAng[3] = {};
 
-	real_t m_timeStep;
+	real_t m_timeStep = 0.0;
 	Transform3D m_calculatedTransformA;
 	Transform3D m_calculatedTransformB;
 
@@ -132,33 +132,30 @@ protected:
 	Vector3 m_relPosA;
 	Vector3 m_relPosB;
 
-	real_t m_linPos;
+	real_t m_linPos = 0.0;
 
-	real_t m_angDepth;
-	real_t m_kAngle;
+	real_t m_angDepth = 0.0;
+	real_t m_kAngle = 0.0;
 
-	bool m_poweredLinMotor;
-	real_t m_targetLinMotorVelocity;
-	real_t m_maxLinMotorForce;
-	real_t m_accumulatedLinMotorImpulse;
+	bool m_poweredLinMotor = false;
+	real_t m_targetLinMotorVelocity = 0.0;
+	real_t m_maxLinMotorForce = 0.0;
+	real_t m_accumulatedLinMotorImpulse = 0.0;
 
-	bool m_poweredAngMotor;
-	real_t m_targetAngMotorVelocity;
-	real_t m_maxAngMotorForce;
-	real_t m_accumulatedAngMotorImpulse;
-
-	//------------------------
-	void initParams();
+	bool m_poweredAngMotor = false;
+	real_t m_targetAngMotorVelocity = 0.0;
+	real_t m_maxAngMotorForce = 0.0;
+	real_t m_accumulatedAngMotorImpulse = 0.0;
 
 public:
 	// constructors
-	SliderJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform3D &frameInA, const Transform3D &frameInB);
+	GodotSliderJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Transform3D &frameInA, const Transform3D &frameInB);
 	//SliderJointSW();
 	// overrides
 
 	// access
-	const Body3DSW *getRigidBodyA() const { return A; }
-	const Body3DSW *getRigidBodyB() const { return B; }
+	const GodotBody3D *getRigidBodyA() const { return A; }
+	const GodotBody3D *getRigidBodyB() const { return B; }
 	const Transform3D &getCalculatedTransformA() const { return m_calculatedTransformA; }
 	const Transform3D &getCalculatedTransformB() const { return m_calculatedTransformB; }
 	const Transform3D &getFrameOffsetA() const { return m_frameInA; }
@@ -246,4 +243,4 @@ public:
 	virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_SLIDER; }
 };
 
-#endif // SLIDER_JOINT_SW_H
+#endif // GODOT_SLIDER_JOINT_3D_H
diff --git a/servers/physics_3d/physics_server_3d_sw.cpp b/servers/physics_3d/physics_server_3d_sw.cpp
deleted file mode 100644
index a214e80c6c..0000000000
--- a/servers/physics_3d/physics_server_3d_sw.cpp
+++ /dev/null
@@ -1,1747 +0,0 @@
-/*************************************************************************/
-/*  physics_server_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 "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"
-#include "joints/cone_twist_joint_3d_sw.h"
-#include "joints/generic_6dof_joint_3d_sw.h"
-#include "joints/hinge_joint_3d_sw.h"
-#include "joints/pin_joint_3d_sw.h"
-#include "joints/slider_joint_3d_sw.h"
-
-#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::plane_shape_create() {
-	Shape3DSW *shape = memnew(PlaneShape3DSW);
-	RID rid = shape_owner.make_rid(shape);
-	shape->set_self(rid);
-	return rid;
-}
-RID PhysicsServer3DSW::separation_ray_shape_create() {
-	Shape3DSW *shape = memnew(SeparationRayShape3DSW);
-	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() {
-	Shape3DSW *shape = memnew(CylinderShape3DSW);
-	RID rid = shape_owner.make_rid(shape);
-	shape->set_self(rid);
-	return 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);
-	ERR_FAIL_COND(!shape);
-	shape->set_data(p_data);
-};
-
-void PhysicsServer3DSW::shape_set_custom_solver_bias(RID p_shape, real_t p_bias) {
-	Shape3DSW *shape = shape_owner.getornull(p_shape);
-	ERR_FAIL_COND(!shape);
-	shape->set_custom_bias(p_bias);
-}
-
-PhysicsServer3D::ShapeType PhysicsServer3DSW::shape_get_type(RID p_shape) const {
-	const Shape3DSW *shape = shape_owner.getornull(p_shape);
-	ERR_FAIL_COND_V(!shape, SHAPE_CUSTOM);
-	return shape->get_type();
-};
-
-Variant PhysicsServer3DSW::shape_get_data(RID p_shape) const {
-	const Shape3DSW *shape = shape_owner.getornull(p_shape);
-	ERR_FAIL_COND_V(!shape, Variant());
-	ERR_FAIL_COND_V(!shape->is_configured(), Variant());
-	return shape->get_data();
-};
-
-void PhysicsServer3DSW::shape_set_margin(RID p_shape, real_t p_margin) {
-}
-
-real_t PhysicsServer3DSW::shape_get_margin(RID p_shape) const {
-	return 0.0;
-}
-
-real_t PhysicsServer3DSW::shape_get_custom_solver_bias(RID p_shape) const {
-	const Shape3DSW *shape = shape_owner.getornull(p_shape);
-	ERR_FAIL_COND_V(!shape, 0);
-	return shape->get_custom_bias();
-}
-
-RID PhysicsServer3DSW::space_create() {
-	Space3DSW *space = memnew(Space3DSW);
-	RID id = space_owner.make_rid(space);
-	space->set_self(id);
-	RID area_id = area_create();
-	Area3DSW *area = area_owner.getornull(area_id);
-	ERR_FAIL_COND_V(!area, RID());
-	space->set_default_area(area);
-	area->set_space(space);
-	area->set_priority(-1);
-	RID sgb = body_create();
-	body_set_space(sgb, id);
-	body_set_mode(sgb, BODY_MODE_STATIC);
-	space->set_static_global_body(sgb);
-
-	return id;
-};
-
-void PhysicsServer3DSW::space_set_active(RID p_space, bool p_active) {
-	Space3DSW *space = space_owner.getornull(p_space);
-	ERR_FAIL_COND(!space);
-	if (p_active) {
-		active_spaces.insert(space);
-	} else {
-		active_spaces.erase(space);
-	}
-}
-
-bool PhysicsServer3DSW::space_is_active(RID p_space) const {
-	const Space3DSW *space = space_owner.getornull(p_space);
-	ERR_FAIL_COND_V(!space, false);
-
-	return active_spaces.has(space);
-}
-
-void PhysicsServer3DSW::space_set_param(RID p_space, SpaceParameter p_param, real_t p_value) {
-	Space3DSW *space = space_owner.getornull(p_space);
-	ERR_FAIL_COND(!space);
-
-	space->set_param(p_param, p_value);
-}
-
-real_t PhysicsServer3DSW::space_get_param(RID p_space, SpaceParameter p_param) const {
-	const Space3DSW *space = space_owner.getornull(p_space);
-	ERR_FAIL_COND_V(!space, 0);
-	return space->get_param(p_param);
-}
-
-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((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();
-}
-
-void PhysicsServer3DSW::space_set_debug_contacts(RID p_space, int p_max_contacts) {
-	Space3DSW *space = space_owner.getornull(p_space);
-	ERR_FAIL_COND(!space);
-	space->set_debug_contacts(p_max_contacts);
-}
-
-Vector<Vector3> PhysicsServer3DSW::space_get_contacts(RID p_space) const {
-	Space3DSW *space = space_owner.getornull(p_space);
-	ERR_FAIL_COND_V(!space, Vector<Vector3>());
-	return space->get_debug_contacts();
-}
-
-int PhysicsServer3DSW::space_get_contact_count(RID p_space) const {
-	Space3DSW *space = space_owner.getornull(p_space);
-	ERR_FAIL_COND_V(!space, 0);
-	return space->get_debug_contact_count();
-}
-
-RID PhysicsServer3DSW::area_create() {
-	Area3DSW *area = memnew(Area3DSW);
-	RID rid = area_owner.make_rid(area);
-	area->set_self(rid);
-	return rid;
-};
-
-void PhysicsServer3DSW::area_set_space(RID p_area, RID p_space) {
-	Area3DSW *area = area_owner.getornull(p_area);
-	ERR_FAIL_COND(!area);
-
-	Space3DSW *space = nullptr;
-	if (p_space.is_valid()) {
-		space = space_owner.getornull(p_space);
-		ERR_FAIL_COND(!space);
-	}
-
-	if (area->get_space() == space) {
-		return; //pointless
-	}
-
-	area->clear_constraints();
-	area->set_space(space);
-};
-
-RID PhysicsServer3DSW::area_get_space(RID p_area) const {
-	Area3DSW *area = area_owner.getornull(p_area);
-	ERR_FAIL_COND_V(!area, RID());
-
-	Space3DSW *space = area->get_space();
-	if (!space) {
-		return RID();
-	}
-	return space->get_self();
-};
-
-void PhysicsServer3DSW::area_set_space_override_mode(RID p_area, AreaSpaceOverrideMode p_mode) {
-	Area3DSW *area = area_owner.getornull(p_area);
-	ERR_FAIL_COND(!area);
-
-	area->set_space_override_mode(p_mode);
-}
-
-PhysicsServer3D::AreaSpaceOverrideMode PhysicsServer3DSW::area_get_space_override_mode(RID p_area) const {
-	const Area3DSW *area = area_owner.getornull(p_area);
-	ERR_FAIL_COND_V(!area, AREA_SPACE_OVERRIDE_DISABLED);
-
-	return area->get_space_override_mode();
-}
-
-void PhysicsServer3DSW::area_add_shape(RID p_area, RID p_shape, const Transform3D &p_transform, bool p_disabled) {
-	Area3DSW *area = area_owner.getornull(p_area);
-	ERR_FAIL_COND(!area);
-
-	Shape3DSW *shape = shape_owner.getornull(p_shape);
-	ERR_FAIL_COND(!shape);
-
-	area->add_shape(shape, p_transform, p_disabled);
-}
-
-void PhysicsServer3DSW::area_set_shape(RID p_area, int p_shape_idx, RID p_shape) {
-	Area3DSW *area = area_owner.getornull(p_area);
-	ERR_FAIL_COND(!area);
-
-	Shape3DSW *shape = shape_owner.getornull(p_shape);
-	ERR_FAIL_COND(!shape);
-	ERR_FAIL_COND(!shape->is_configured());
-
-	area->set_shape(p_shape_idx, shape);
-}
-
-void PhysicsServer3DSW::area_set_shape_transform(RID p_area, int p_shape_idx, const Transform3D &p_transform) {
-	Area3DSW *area = area_owner.getornull(p_area);
-	ERR_FAIL_COND(!area);
-
-	area->set_shape_transform(p_shape_idx, p_transform);
-}
-
-int PhysicsServer3DSW::area_get_shape_count(RID p_area) const {
-	Area3DSW *area = area_owner.getornull(p_area);
-	ERR_FAIL_COND_V(!area, -1);
-
-	return area->get_shape_count();
-}
-
-RID PhysicsServer3DSW::area_get_shape(RID p_area, int p_shape_idx) const {
-	Area3DSW *area = area_owner.getornull(p_area);
-	ERR_FAIL_COND_V(!area, RID());
-
-	Shape3DSW *shape = area->get_shape(p_shape_idx);
-	ERR_FAIL_COND_V(!shape, RID());
-
-	return shape->get_self();
-}
-
-Transform3D PhysicsServer3DSW::area_get_shape_transform(RID p_area, int p_shape_idx) const {
-	Area3DSW *area = area_owner.getornull(p_area);
-	ERR_FAIL_COND_V(!area, Transform3D());
-
-	return area->get_shape_transform(p_shape_idx);
-}
-
-void PhysicsServer3DSW::area_remove_shape(RID p_area, int p_shape_idx) {
-	Area3DSW *area = area_owner.getornull(p_area);
-	ERR_FAIL_COND(!area);
-
-	area->remove_shape(p_shape_idx);
-}
-
-void PhysicsServer3DSW::area_clear_shapes(RID p_area) {
-	Area3DSW *area = area_owner.getornull(p_area);
-	ERR_FAIL_COND(!area);
-
-	while (area->get_shape_count()) {
-		area->remove_shape(0);
-	}
-}
-
-void PhysicsServer3DSW::area_set_shape_disabled(RID p_area, int p_shape_idx, bool p_disabled) {
-	Area3DSW *area = area_owner.getornull(p_area);
-	ERR_FAIL_COND(!area);
-	ERR_FAIL_INDEX(p_shape_idx, area->get_shape_count());
-	FLUSH_QUERY_CHECK(area);
-	area->set_shape_disabled(p_shape_idx, p_disabled);
-}
-
-void PhysicsServer3DSW::area_attach_object_instance_id(RID p_area, ObjectID p_id) {
-	if (space_owner.owns(p_area)) {
-		Space3DSW *space = space_owner.getornull(p_area);
-		p_area = space->get_default_area()->get_self();
-	}
-	Area3DSW *area = area_owner.getornull(p_area);
-	ERR_FAIL_COND(!area);
-	area->set_instance_id(p_id);
-}
-
-ObjectID PhysicsServer3DSW::area_get_object_instance_id(RID p_area) const {
-	if (space_owner.owns(p_area)) {
-		Space3DSW *space = space_owner.getornull(p_area);
-		p_area = space->get_default_area()->get_self();
-	}
-	Area3DSW *area = area_owner.getornull(p_area);
-	ERR_FAIL_COND_V(!area, ObjectID());
-	return area->get_instance_id();
-}
-
-void PhysicsServer3DSW::area_set_param(RID p_area, AreaParameter p_param, const Variant &p_value) {
-	if (space_owner.owns(p_area)) {
-		Space3DSW *space = space_owner.getornull(p_area);
-		p_area = space->get_default_area()->get_self();
-	}
-	Area3DSW *area = area_owner.getornull(p_area);
-	ERR_FAIL_COND(!area);
-	area->set_param(p_param, p_value);
-};
-
-void PhysicsServer3DSW::area_set_transform(RID p_area, const Transform3D &p_transform) {
-	Area3DSW *area = area_owner.getornull(p_area);
-	ERR_FAIL_COND(!area);
-	area->set_transform(p_transform);
-};
-
-Variant PhysicsServer3DSW::area_get_param(RID p_area, AreaParameter p_param) const {
-	if (space_owner.owns(p_area)) {
-		Space3DSW *space = space_owner.getornull(p_area);
-		p_area = space->get_default_area()->get_self();
-	}
-	Area3DSW *area = area_owner.getornull(p_area);
-	ERR_FAIL_COND_V(!area, Variant());
-
-	return area->get_param(p_param);
-};
-
-Transform3D PhysicsServer3DSW::area_get_transform(RID p_area) const {
-	Area3DSW *area = area_owner.getornull(p_area);
-	ERR_FAIL_COND_V(!area, Transform3D());
-
-	return area->get_transform();
-};
-
-void PhysicsServer3DSW::area_set_collision_layer(RID p_area, uint32_t p_layer) {
-	Area3DSW *area = area_owner.getornull(p_area);
-	ERR_FAIL_COND(!area);
-
-	area->set_collision_layer(p_layer);
-}
-
-void PhysicsServer3DSW::area_set_collision_mask(RID p_area, uint32_t p_mask) {
-	Area3DSW *area = area_owner.getornull(p_area);
-	ERR_FAIL_COND(!area);
-
-	area->set_collision_mask(p_mask);
-}
-
-void PhysicsServer3DSW::area_set_monitorable(RID p_area, bool p_monitorable) {
-	Area3DSW *area = area_owner.getornull(p_area);
-	ERR_FAIL_COND(!area);
-	FLUSH_QUERY_CHECK(area);
-
-	area->set_monitorable(p_monitorable);
-}
-
-void PhysicsServer3DSW::area_set_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) {
-	Area3DSW *area = area_owner.getornull(p_area);
-	ERR_FAIL_COND(!area);
-
-	area->set_monitor_callback(p_receiver ? p_receiver->get_instance_id() : ObjectID(), p_method);
-}
-
-void PhysicsServer3DSW::area_set_ray_pickable(RID p_area, bool p_enable) {
-	Area3DSW *area = area_owner.getornull(p_area);
-	ERR_FAIL_COND(!area);
-
-	area->set_ray_pickable(p_enable);
-}
-
-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);
-
-	area->set_area_monitor_callback(p_receiver ? p_receiver->get_instance_id() : ObjectID(), p_method);
-}
-
-/* BODY API */
-
-RID PhysicsServer3DSW::body_create() {
-	Body3DSW *body = memnew(Body3DSW);
-	RID rid = body_owner.make_rid(body);
-	body->set_self(rid);
-	return rid;
-};
-
-void PhysicsServer3DSW::body_set_space(RID p_body, RID p_space) {
-	Body3DSW *body = body_owner.getornull(p_body);
-	ERR_FAIL_COND(!body);
-
-	Space3DSW *space = nullptr;
-	if (p_space.is_valid()) {
-		space = space_owner.getornull(p_space);
-		ERR_FAIL_COND(!space);
-	}
-
-	if (body->get_space() == space) {
-		return; //pointless
-	}
-
-	body->clear_constraint_map();
-	body->set_space(space);
-};
-
-RID PhysicsServer3DSW::body_get_space(RID p_body) const {
-	Body3DSW *body = body_owner.getornull(p_body);
-	ERR_FAIL_COND_V(!body, RID());
-
-	Space3DSW *space = body->get_space();
-	if (!space) {
-		return RID();
-	}
-	return space->get_self();
-};
-
-void PhysicsServer3DSW::body_set_mode(RID p_body, BodyMode p_mode) {
-	Body3DSW *body = body_owner.getornull(p_body);
-	ERR_FAIL_COND(!body);
-
-	body->set_mode(p_mode);
-};
-
-PhysicsServer3D::BodyMode PhysicsServer3DSW::body_get_mode(RID p_body) const {
-	Body3DSW *body = body_owner.getornull(p_body);
-	ERR_FAIL_COND_V(!body, BODY_MODE_STATIC);
-
-	return body->get_mode();
-};
-
-void PhysicsServer3DSW::body_add_shape(RID p_body, RID p_shape, const Transform3D &p_transform, bool p_disabled) {
-	Body3DSW *body = body_owner.getornull(p_body);
-	ERR_FAIL_COND(!body);
-
-	Shape3DSW *shape = shape_owner.getornull(p_shape);
-	ERR_FAIL_COND(!shape);
-
-	body->add_shape(shape, p_transform, p_disabled);
-}
-
-void PhysicsServer3DSW::body_set_shape(RID p_body, int p_shape_idx, RID p_shape) {
-	Body3DSW *body = body_owner.getornull(p_body);
-	ERR_FAIL_COND(!body);
-
-	Shape3DSW *shape = shape_owner.getornull(p_shape);
-	ERR_FAIL_COND(!shape);
-	ERR_FAIL_COND(!shape->is_configured());
-
-	body->set_shape(p_shape_idx, shape);
-}
-void PhysicsServer3DSW::body_set_shape_transform(RID p_body, int p_shape_idx, const Transform3D &p_transform) {
-	Body3DSW *body = body_owner.getornull(p_body);
-	ERR_FAIL_COND(!body);
-
-	body->set_shape_transform(p_shape_idx, p_transform);
-}
-
-int PhysicsServer3DSW::body_get_shape_count(RID p_body) const {
-	Body3DSW *body = body_owner.getornull(p_body);
-	ERR_FAIL_COND_V(!body, -1);
-
-	return body->get_shape_count();
-}
-
-RID PhysicsServer3DSW::body_get_shape(RID p_body, int p_shape_idx) const {
-	Body3DSW *body = body_owner.getornull(p_body);
-	ERR_FAIL_COND_V(!body, RID());
-
-	Shape3DSW *shape = body->get_shape(p_shape_idx);
-	ERR_FAIL_COND_V(!shape, RID());
-
-	return shape->get_self();
-}
-
-void PhysicsServer3DSW::body_set_shape_disabled(RID p_body, int p_shape_idx, bool p_disabled) {
-	Body3DSW *body = body_owner.getornull(p_body);
-	ERR_FAIL_COND(!body);
-	ERR_FAIL_INDEX(p_shape_idx, body->get_shape_count());
-	FLUSH_QUERY_CHECK(body);
-
-	body->set_shape_disabled(p_shape_idx, p_disabled);
-}
-
-Transform3D PhysicsServer3DSW::body_get_shape_transform(RID p_body, int p_shape_idx) const {
-	Body3DSW *body = body_owner.getornull(p_body);
-	ERR_FAIL_COND_V(!body, Transform3D());
-
-	return body->get_shape_transform(p_shape_idx);
-}
-
-void PhysicsServer3DSW::body_remove_shape(RID p_body, int p_shape_idx) {
-	Body3DSW *body = body_owner.getornull(p_body);
-	ERR_FAIL_COND(!body);
-
-	body->remove_shape(p_shape_idx);
-}
-
-void PhysicsServer3DSW::body_clear_shapes(RID p_body) {
-	Body3DSW *body = body_owner.getornull(p_body);
-	ERR_FAIL_COND(!body);
-
-	while (body->get_shape_count()) {
-		body->remove_shape(0);
-	}
-}
-
-void PhysicsServer3DSW::body_set_enable_continuous_collision_detection(RID p_body, bool p_enable) {
-	Body3DSW *body = body_owner.getornull(p_body);
-	ERR_FAIL_COND(!body);
-
-	body->set_continuous_collision_detection(p_enable);
-}
-
-bool PhysicsServer3DSW::body_is_continuous_collision_detection_enabled(RID p_body) const {
-	Body3DSW *body = body_owner.getornull(p_body);
-	ERR_FAIL_COND_V(!body, false);
-
-	return body->is_continuous_collision_detection_enabled();
-}
-
-void PhysicsServer3DSW::body_set_collision_layer(RID p_body, uint32_t p_layer) {
-	Body3DSW *body = body_owner.getornull(p_body);
-	ERR_FAIL_COND(!body);
-
-	body->set_collision_layer(p_layer);
-	body->wakeup();
-}
-
-uint32_t PhysicsServer3DSW::body_get_collision_layer(RID p_body) const {
-	const Body3DSW *body = body_owner.getornull(p_body);
-	ERR_FAIL_COND_V(!body, 0);
-
-	return body->get_collision_layer();
-}
-
-void PhysicsServer3DSW::body_set_collision_mask(RID p_body, uint32_t p_mask) {
-	Body3DSW *body = body_owner.getornull(p_body);
-	ERR_FAIL_COND(!body);
-
-	body->set_collision_mask(p_mask);
-	body->wakeup();
-}
-
-uint32_t PhysicsServer3DSW::body_get_collision_mask(RID p_body) const {
-	const Body3DSW *body = body_owner.getornull(p_body);
-	ERR_FAIL_COND_V(!body, 0);
-
-	return body->get_collision_mask();
-}
-
-void PhysicsServer3DSW::body_attach_object_instance_id(RID p_body, ObjectID p_id) {
-	Body3DSW *body = body_owner.getornull(p_body);
-	if (body) {
-		body->set_instance_id(p_id);
-		return;
-	}
-
-	SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
-	if (soft_body) {
-		soft_body->set_instance_id(p_id);
-		return;
-	}
-
-	ERR_FAIL_MSG("Invalid ID.");
-};
-
-ObjectID PhysicsServer3DSW::body_get_object_instance_id(RID p_body) const {
-	Body3DSW *body = body_owner.getornull(p_body);
-	ERR_FAIL_COND_V(!body, ObjectID());
-
-	return body->get_instance_id();
-};
-
-void PhysicsServer3DSW::body_set_user_flags(RID p_body, uint32_t p_flags) {
-	Body3DSW *body = body_owner.getornull(p_body);
-	ERR_FAIL_COND(!body);
-};
-
-uint32_t PhysicsServer3DSW::body_get_user_flags(RID p_body) const {
-	Body3DSW *body = body_owner.getornull(p_body);
-	ERR_FAIL_COND_V(!body, 0);
-
-	return 0;
-};
-
-void PhysicsServer3DSW::body_set_param(RID p_body, BodyParameter p_param, real_t p_value) {
-	Body3DSW *body = body_owner.getornull(p_body);
-	ERR_FAIL_COND(!body);
-
-	body->set_param(p_param, p_value);
-};
-
-real_t PhysicsServer3DSW::body_get_param(RID p_body, BodyParameter p_param) const {
-	Body3DSW *body = body_owner.getornull(p_body);
-	ERR_FAIL_COND_V(!body, 0);
-
-	return body->get_param(p_param);
-};
-
-void PhysicsServer3DSW::body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) {
-	Body3DSW *body = body_owner.getornull(p_body);
-	ERR_FAIL_COND(!body);
-
-	body->set_state(p_state, p_variant);
-};
-
-Variant PhysicsServer3DSW::body_get_state(RID p_body, BodyState p_state) const {
-	Body3DSW *body = body_owner.getornull(p_body);
-	ERR_FAIL_COND_V(!body, Variant());
-
-	return body->get_state(p_state);
-};
-
-void PhysicsServer3DSW::body_set_applied_force(RID p_body, const Vector3 &p_force) {
-	Body3DSW *body = body_owner.getornull(p_body);
-	ERR_FAIL_COND(!body);
-
-	body->set_applied_force(p_force);
-	body->wakeup();
-};
-
-Vector3 PhysicsServer3DSW::body_get_applied_force(RID p_body) const {
-	Body3DSW *body = body_owner.getornull(p_body);
-	ERR_FAIL_COND_V(!body, Vector3());
-	return body->get_applied_force();
-};
-
-void PhysicsServer3DSW::body_set_applied_torque(RID p_body, const Vector3 &p_torque) {
-	Body3DSW *body = body_owner.getornull(p_body);
-	ERR_FAIL_COND(!body);
-
-	body->set_applied_torque(p_torque);
-	body->wakeup();
-};
-
-Vector3 PhysicsServer3DSW::body_get_applied_torque(RID p_body) const {
-	Body3DSW *body = body_owner.getornull(p_body);
-	ERR_FAIL_COND_V(!body, Vector3());
-
-	return body->get_applied_torque();
-};
-
-void PhysicsServer3DSW::body_add_central_force(RID p_body, const Vector3 &p_force) {
-	Body3DSW *body = body_owner.getornull(p_body);
-	ERR_FAIL_COND(!body);
-
-	body->add_central_force(p_force);
-	body->wakeup();
-}
-
-void PhysicsServer3DSW::body_add_force(RID p_body, const Vector3 &p_force, const Vector3 &p_position) {
-	Body3DSW *body = body_owner.getornull(p_body);
-	ERR_FAIL_COND(!body);
-
-	body->add_force(p_force, p_position);
-	body->wakeup();
-};
-
-void PhysicsServer3DSW::body_add_torque(RID p_body, const Vector3 &p_torque) {
-	Body3DSW *body = body_owner.getornull(p_body);
-	ERR_FAIL_COND(!body);
-
-	body->add_torque(p_torque);
-	body->wakeup();
-};
-
-void PhysicsServer3DSW::body_apply_central_impulse(RID p_body, const Vector3 &p_impulse) {
-	Body3DSW *body = body_owner.getornull(p_body);
-	ERR_FAIL_COND(!body);
-
-	_update_shapes();
-
-	body->apply_central_impulse(p_impulse);
-	body->wakeup();
-}
-
-void PhysicsServer3DSW::body_apply_impulse(RID p_body, const Vector3 &p_impulse, const Vector3 &p_position) {
-	Body3DSW *body = body_owner.getornull(p_body);
-	ERR_FAIL_COND(!body);
-
-	_update_shapes();
-
-	body->apply_impulse(p_impulse, p_position);
-	body->wakeup();
-};
-
-void PhysicsServer3DSW::body_apply_torque_impulse(RID p_body, const Vector3 &p_impulse) {
-	Body3DSW *body = body_owner.getornull(p_body);
-	ERR_FAIL_COND(!body);
-
-	_update_shapes();
-
-	body->apply_torque_impulse(p_impulse);
-	body->wakeup();
-};
-
-void PhysicsServer3DSW::body_set_axis_velocity(RID p_body, const Vector3 &p_axis_velocity) {
-	Body3DSW *body = body_owner.getornull(p_body);
-	ERR_FAIL_COND(!body);
-
-	_update_shapes();
-
-	Vector3 v = body->get_linear_velocity();
-	Vector3 axis = p_axis_velocity.normalized();
-	v -= axis * axis.dot(v);
-	v += p_axis_velocity;
-	body->set_linear_velocity(v);
-	body->wakeup();
-};
-
-void PhysicsServer3DSW::body_set_axis_lock(RID p_body, BodyAxis p_axis, bool p_lock) {
-	Body3DSW *body = body_owner.getornull(p_body);
-	ERR_FAIL_COND(!body);
-
-	body->set_axis_lock(p_axis, p_lock);
-	body->wakeup();
-}
-
-bool PhysicsServer3DSW::body_is_axis_locked(RID p_body, BodyAxis p_axis) const {
-	const Body3DSW *body = body_owner.getornull(p_body);
-	ERR_FAIL_COND_V(!body, 0);
-	return body->is_axis_locked(p_axis);
-}
-
-void PhysicsServer3DSW::body_add_collision_exception(RID p_body, RID p_body_b) {
-	Body3DSW *body = body_owner.getornull(p_body);
-	ERR_FAIL_COND(!body);
-
-	body->add_exception(p_body_b);
-	body->wakeup();
-};
-
-void PhysicsServer3DSW::body_remove_collision_exception(RID p_body, RID p_body_b) {
-	Body3DSW *body = body_owner.getornull(p_body);
-	ERR_FAIL_COND(!body);
-
-	body->remove_exception(p_body_b);
-	body->wakeup();
-};
-
-void PhysicsServer3DSW::body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions) {
-	Body3DSW *body = body_owner.getornull(p_body);
-	ERR_FAIL_COND(!body);
-
-	for (int i = 0; i < body->get_exceptions().size(); i++) {
-		p_exceptions->push_back(body->get_exceptions()[i]);
-	}
-};
-
-void PhysicsServer3DSW::body_set_contacts_reported_depth_threshold(RID p_body, real_t p_threshold) {
-	Body3DSW *body = body_owner.getornull(p_body);
-	ERR_FAIL_COND(!body);
-};
-
-real_t PhysicsServer3DSW::body_get_contacts_reported_depth_threshold(RID p_body) const {
-	Body3DSW *body = body_owner.getornull(p_body);
-	ERR_FAIL_COND_V(!body, 0);
-	return 0;
-};
-
-void PhysicsServer3DSW::body_set_omit_force_integration(RID p_body, bool p_omit) {
-	Body3DSW *body = body_owner.getornull(p_body);
-	ERR_FAIL_COND(!body);
-
-	body->set_omit_force_integration(p_omit);
-};
-
-bool PhysicsServer3DSW::body_is_omitting_force_integration(RID p_body) const {
-	Body3DSW *body = body_owner.getornull(p_body);
-	ERR_FAIL_COND_V(!body, false);
-	return body->get_omit_force_integration();
-};
-
-void PhysicsServer3DSW::body_set_max_contacts_reported(RID p_body, int p_contacts) {
-	Body3DSW *body = body_owner.getornull(p_body);
-	ERR_FAIL_COND(!body);
-	body->set_max_contacts_reported(p_contacts);
-}
-
-int PhysicsServer3DSW::body_get_max_contacts_reported(RID p_body) const {
-	Body3DSW *body = body_owner.getornull(p_body);
-	ERR_FAIL_COND_V(!body, -1);
-	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);
-	body->set_force_integration_callback(p_callable, p_udata);
-}
-
-void PhysicsServer3DSW::body_set_ray_pickable(RID p_body, bool p_enable) {
-	Body3DSW *body = body_owner.getornull(p_body);
-	ERR_FAIL_COND(!body);
-	body->set_ray_pickable(p_enable);
-}
-
-bool PhysicsServer3DSW::body_test_motion(RID p_body, const Transform3D &p_from, const Vector3 &p_motion, real_t p_margin, MotionResult *r_result, bool p_collide_separation_ray, const Set<RID> &p_exclude) {
-	Body3DSW *body = body_owner.getornull(p_body);
-	ERR_FAIL_COND_V(!body, false);
-	ERR_FAIL_COND_V(!body->get_space(), false);
-	ERR_FAIL_COND_V(body->get_space()->is_locked(), false);
-
-	_update_shapes();
-
-	return body->get_space()->test_body_motion(body, p_from, p_motion, p_margin, r_result, p_collide_separation_ray, p_exclude);
-}
-
-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_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.");
-
-	return body->get_direct_state();
-}
-
-/* SOFT BODY */
-
-RID PhysicsServer3DSW::soft_body_create() {
-	SoftBody3DSW *soft_body = memnew(SoftBody3DSW);
-	RID rid = soft_body_owner.make_rid(soft_body);
-	soft_body->set_self(rid);
-	return rid;
-}
-
-void PhysicsServer3DSW::soft_body_update_rendering_server(RID p_body, RenderingServerHandler *p_rendering_server_handler) {
-	SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
-	ERR_FAIL_COND(!soft_body);
-
-	soft_body->update_rendering_server(p_rendering_server_handler);
-}
-
-void PhysicsServer3DSW::soft_body_set_space(RID p_body, RID p_space) {
-	SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
-	ERR_FAIL_COND(!soft_body);
-
-	Space3DSW *space = nullptr;
-	if (p_space.is_valid()) {
-		space = space_owner.getornull(p_space);
-		ERR_FAIL_COND(!space);
-	}
-
-	if (soft_body->get_space() == space) {
-		return;
-	}
-
-	soft_body->set_space(space);
-}
-
-RID PhysicsServer3DSW::soft_body_get_space(RID p_body) const {
-	SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
-	ERR_FAIL_COND_V(!soft_body, RID());
-
-	Space3DSW *space = soft_body->get_space();
-	if (!space) {
-		return RID();
-	}
-	return space->get_self();
-}
-
-void PhysicsServer3DSW::soft_body_set_collision_layer(RID p_body, uint32_t p_layer) {
-	SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
-	ERR_FAIL_COND(!soft_body);
-
-	soft_body->set_collision_layer(p_layer);
-}
-
-uint32_t PhysicsServer3DSW::soft_body_get_collision_layer(RID p_body) const {
-	SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
-	ERR_FAIL_COND_V(!soft_body, 0);
-
-	return soft_body->get_collision_layer();
-}
-
-void PhysicsServer3DSW::soft_body_set_collision_mask(RID p_body, uint32_t p_mask) {
-	SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
-	ERR_FAIL_COND(!soft_body);
-
-	soft_body->set_collision_mask(p_mask);
-}
-
-uint32_t PhysicsServer3DSW::soft_body_get_collision_mask(RID p_body) const {
-	SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
-	ERR_FAIL_COND_V(!soft_body, 0);
-
-	return soft_body->get_collision_mask();
-}
-
-void PhysicsServer3DSW::soft_body_add_collision_exception(RID p_body, RID p_body_b) {
-	SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
-	ERR_FAIL_COND(!soft_body);
-
-	soft_body->add_exception(p_body_b);
-}
-
-void PhysicsServer3DSW::soft_body_remove_collision_exception(RID p_body, RID p_body_b) {
-	SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
-	ERR_FAIL_COND(!soft_body);
-
-	soft_body->remove_exception(p_body_b);
-}
-
-void PhysicsServer3DSW::soft_body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions) {
-	SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
-	ERR_FAIL_COND(!soft_body);
-
-	for (int i = 0; i < soft_body->get_exceptions().size(); i++) {
-		p_exceptions->push_back(soft_body->get_exceptions()[i]);
-	}
-}
-
-void PhysicsServer3DSW::soft_body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) {
-	SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
-	ERR_FAIL_COND(!soft_body);
-
-	soft_body->set_state(p_state, p_variant);
-}
-
-Variant PhysicsServer3DSW::soft_body_get_state(RID p_body, BodyState p_state) const {
-	SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
-	ERR_FAIL_COND_V(!soft_body, Variant());
-
-	return soft_body->get_state(p_state);
-}
-
-void PhysicsServer3DSW::soft_body_set_transform(RID p_body, const Transform3D &p_transform) {
-	SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
-	ERR_FAIL_COND(!soft_body);
-
-	soft_body->set_state(BODY_STATE_TRANSFORM, p_transform);
-}
-
-void PhysicsServer3DSW::soft_body_set_ray_pickable(RID p_body, bool p_enable) {
-	SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
-	ERR_FAIL_COND(!soft_body);
-
-	soft_body->set_ray_pickable(p_enable);
-}
-
-void PhysicsServer3DSW::soft_body_set_simulation_precision(RID p_body, int p_simulation_precision) {
-	SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
-	ERR_FAIL_COND(!soft_body);
-
-	soft_body->set_iteration_count(p_simulation_precision);
-}
-
-int PhysicsServer3DSW::soft_body_get_simulation_precision(RID p_body) const {
-	SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
-	ERR_FAIL_COND_V(!soft_body, 0.f);
-
-	return soft_body->get_iteration_count();
-}
-
-void PhysicsServer3DSW::soft_body_set_total_mass(RID p_body, real_t p_total_mass) {
-	SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
-	ERR_FAIL_COND(!soft_body);
-
-	soft_body->set_total_mass(p_total_mass);
-}
-
-real_t PhysicsServer3DSW::soft_body_get_total_mass(RID p_body) const {
-	SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
-	ERR_FAIL_COND_V(!soft_body, 0.f);
-
-	return soft_body->get_total_mass();
-}
-
-void PhysicsServer3DSW::soft_body_set_linear_stiffness(RID p_body, real_t p_stiffness) {
-	SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
-	ERR_FAIL_COND(!soft_body);
-
-	soft_body->set_linear_stiffness(p_stiffness);
-}
-
-real_t PhysicsServer3DSW::soft_body_get_linear_stiffness(RID p_body) const {
-	SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
-	ERR_FAIL_COND_V(!soft_body, 0.f);
-
-	return soft_body->get_linear_stiffness();
-}
-
-void PhysicsServer3DSW::soft_body_set_pressure_coefficient(RID p_body, real_t p_pressure_coefficient) {
-	SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
-	ERR_FAIL_COND(!soft_body);
-
-	soft_body->set_pressure_coefficient(p_pressure_coefficient);
-}
-
-real_t PhysicsServer3DSW::soft_body_get_pressure_coefficient(RID p_body) const {
-	SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
-	ERR_FAIL_COND_V(!soft_body, 0.f);
-
-	return soft_body->get_pressure_coefficient();
-}
-
-void PhysicsServer3DSW::soft_body_set_damping_coefficient(RID p_body, real_t p_damping_coefficient) {
-	SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
-	ERR_FAIL_COND(!soft_body);
-
-	soft_body->set_damping_coefficient(p_damping_coefficient);
-}
-
-real_t PhysicsServer3DSW::soft_body_get_damping_coefficient(RID p_body) const {
-	SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
-	ERR_FAIL_COND_V(!soft_body, 0.f);
-
-	return soft_body->get_damping_coefficient();
-}
-
-void PhysicsServer3DSW::soft_body_set_drag_coefficient(RID p_body, real_t p_drag_coefficient) {
-	SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
-	ERR_FAIL_COND(!soft_body);
-
-	soft_body->set_drag_coefficient(p_drag_coefficient);
-}
-
-real_t PhysicsServer3DSW::soft_body_get_drag_coefficient(RID p_body) const {
-	SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
-	ERR_FAIL_COND_V(!soft_body, 0.f);
-
-	return soft_body->get_drag_coefficient();
-}
-
-void PhysicsServer3DSW::soft_body_set_mesh(RID p_body, const REF &p_mesh) {
-	SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
-	ERR_FAIL_COND(!soft_body);
-
-	soft_body->set_mesh(p_mesh);
-}
-
-AABB PhysicsServer3DSW::soft_body_get_bounds(RID p_body) const {
-	SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
-	ERR_FAIL_COND_V(!soft_body, AABB());
-
-	return soft_body->get_bounds();
-}
-
-void PhysicsServer3DSW::soft_body_move_point(RID p_body, int p_point_index, const Vector3 &p_global_position) {
-	SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
-	ERR_FAIL_COND(!soft_body);
-
-	soft_body->set_vertex_position(p_point_index, p_global_position);
-}
-
-Vector3 PhysicsServer3DSW::soft_body_get_point_global_position(RID p_body, int p_point_index) const {
-	SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
-	ERR_FAIL_COND_V(!soft_body, Vector3());
-
-	return soft_body->get_vertex_position(p_point_index);
-}
-
-void PhysicsServer3DSW::soft_body_remove_all_pinned_points(RID p_body) {
-	SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
-	ERR_FAIL_COND(!soft_body);
-
-	soft_body->unpin_all_vertices();
-}
-
-void PhysicsServer3DSW::soft_body_pin_point(RID p_body, int p_point_index, bool p_pin) {
-	SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
-	ERR_FAIL_COND(!soft_body);
-
-	if (p_pin) {
-		soft_body->pin_vertex(p_point_index);
-	} else {
-		soft_body->unpin_vertex(p_point_index);
-	}
-}
-
-bool PhysicsServer3DSW::soft_body_is_point_pinned(RID p_body, int p_point_index) const {
-	SoftBody3DSW *soft_body = soft_body_owner.getornull(p_body);
-	ERR_FAIL_COND_V(!soft_body, false);
-
-	return soft_body->is_vertex_pinned(p_point_index);
-}
-
-/* JOINT API */
-
-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(!body_A);
-
-	if (!p_body_B.is_valid()) {
-		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(!body_B);
-
-	ERR_FAIL_COND(body_A == body_B);
-
-	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));
-
-	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_TYPE_PIN);
-	PinJoint3DSW *pin_joint = static_cast<PinJoint3DSW *>(joint);
-	pin_joint->set_param(p_param, p_value);
-}
-
-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_TYPE_PIN, 0);
-	PinJoint3DSW *pin_joint = static_cast<PinJoint3DSW *>(joint);
-	return pin_joint->get_param(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_TYPE_PIN);
-	PinJoint3DSW *pin_joint = static_cast<PinJoint3DSW *>(joint);
-	pin_joint->set_pos_a(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_TYPE_PIN, Vector3());
-	PinJoint3DSW *pin_joint = static_cast<PinJoint3DSW *>(joint);
-	return pin_joint->get_position_a();
-}
-
-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_TYPE_PIN);
-	PinJoint3DSW *pin_joint = static_cast<PinJoint3DSW *>(joint);
-	pin_joint->set_pos_b(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_TYPE_PIN, Vector3());
-	PinJoint3DSW *pin_joint = static_cast<PinJoint3DSW *>(joint);
-	return pin_joint->get_position_b();
-}
-
-void PhysicsServer3DSW::joint_make_hinge(RID p_joint, RID p_body_A, const Transform3D &p_frame_A, RID p_body_B, const Transform3D &p_frame_B) {
-	Body3DSW *body_A = body_owner.getornull(p_body_A);
-	ERR_FAIL_COND(!body_A);
-
-	if (!p_body_B.is_valid()) {
-		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(!body_B);
-
-	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));
-
-	joint->copy_settings_from(prev_joint);
-	joint_owner.replace(p_joint, joint);
-	memdelete(prev_joint);
-}
-
-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(!body_A);
-
-	if (!p_body_B.is_valid()) {
-		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(!body_B);
-
-	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_pivot_A, p_pivot_B, p_axis_A, p_axis_B));
-
-	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_TYPE_HINGE);
-	HingeJoint3DSW *hinge_joint = static_cast<HingeJoint3DSW *>(joint);
-	hinge_joint->set_param(p_param, p_value);
-}
-
-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_TYPE_HINGE, 0);
-	HingeJoint3DSW *hinge_joint = static_cast<HingeJoint3DSW *>(joint);
-	return hinge_joint->get_param(p_param);
-}
-
-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_TYPE_HINGE);
-	HingeJoint3DSW *hinge_joint = static_cast<HingeJoint3DSW *>(joint);
-	hinge_joint->set_flag(p_flag, p_value);
-}
-
-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_TYPE_HINGE, false);
-	HingeJoint3DSW *hinge_joint = static_cast<HingeJoint3DSW *>(joint);
-	return hinge_joint->get_flag(p_flag);
-}
-
-void PhysicsServer3DSW::joint_set_solver_priority(RID p_joint, int p_priority) {
-	Joint3DSW *joint = joint_owner.getornull(p_joint);
-	ERR_FAIL_COND(!joint);
-	joint->set_priority(p_priority);
-}
-
-int PhysicsServer3DSW::joint_get_solver_priority(RID p_joint) const {
-	Joint3DSW *joint = joint_owner.getornull(p_joint);
-	ERR_FAIL_COND_V(!joint, 0);
-	return joint->get_priority();
-}
-
-void PhysicsServer3DSW::joint_disable_collisions_between_bodies(RID p_joint, const bool p_disable) {
-	Joint3DSW *joint = joint_owner.getornull(p_joint);
-	ERR_FAIL_COND(!joint);
-
-	joint->disable_collisions_between_bodies(p_disable);
-
-	if (2 == joint->get_body_count()) {
-		Body3DSW *body_a = *joint->get_body_ptr();
-		Body3DSW *body_b = *(joint->get_body_ptr() + 1);
-
-		if (p_disable) {
-			body_add_collision_exception(body_a->get_self(), body_b->get_self());
-			body_add_collision_exception(body_b->get_self(), body_a->get_self());
-		} else {
-			body_remove_collision_exception(body_a->get_self(), body_b->get_self());
-			body_remove_collision_exception(body_b->get_self(), body_a->get_self());
-		}
-	}
-}
-
-bool PhysicsServer3DSW::joint_is_disabled_collisions_between_bodies(RID p_joint) const {
-	Joint3DSW *joint = joint_owner.getornull(p_joint);
-	ERR_FAIL_COND_V(!joint, true);
-
-	return joint->is_disabled_collisions_between_bodies();
-}
-
-PhysicsServer3DSW::JointType PhysicsServer3DSW::joint_get_type(RID p_joint) const {
-	Joint3DSW *joint = joint_owner.getornull(p_joint);
-	ERR_FAIL_COND_V(!joint, JOINT_TYPE_PIN);
-	return joint->get_type();
-}
-
-void PhysicsServer3DSW::joint_make_slider(RID p_joint, RID p_body_A, const Transform3D &p_local_frame_A, RID p_body_B, const Transform3D &p_local_frame_B) {
-	Body3DSW *body_A = body_owner.getornull(p_body_A);
-	ERR_FAIL_COND(!body_A);
-
-	if (!p_body_B.is_valid()) {
-		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(!body_B);
-
-	ERR_FAIL_COND(body_A == body_B);
-
-	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));
-
-	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_TYPE_SLIDER);
-	SliderJoint3DSW *slider_joint = static_cast<SliderJoint3DSW *>(joint);
-	slider_joint->set_param(p_param, p_value);
-}
-
-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_TYPE_CONE_TWIST, 0);
-	SliderJoint3DSW *slider_joint = static_cast<SliderJoint3DSW *>(joint);
-	return slider_joint->get_param(p_param);
-}
-
-void PhysicsServer3DSW::joint_make_cone_twist(RID p_joint, RID p_body_A, const Transform3D &p_local_frame_A, RID p_body_B, const Transform3D &p_local_frame_B) {
-	Body3DSW *body_A = body_owner.getornull(p_body_A);
-	ERR_FAIL_COND(!body_A);
-
-	if (!p_body_B.is_valid()) {
-		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(!body_B);
-
-	ERR_FAIL_COND(body_A == body_B);
-
-	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));
-
-	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_TYPE_CONE_TWIST);
-	ConeTwistJoint3DSW *cone_twist_joint = static_cast<ConeTwistJoint3DSW *>(joint);
-	cone_twist_joint->set_param(p_param, p_value);
-}
-
-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_TYPE_CONE_TWIST, 0);
-	ConeTwistJoint3DSW *cone_twist_joint = static_cast<ConeTwistJoint3DSW *>(joint);
-	return cone_twist_joint->get_param(p_param);
-}
-
-void PhysicsServer3DSW::joint_make_generic_6dof(RID p_joint, RID p_body_A, const Transform3D &p_local_frame_A, RID p_body_B, const Transform3D &p_local_frame_B) {
-	Body3DSW *body_A = body_owner.getornull(p_body_A);
-	ERR_FAIL_COND(!body_A);
-
-	if (!p_body_B.is_valid()) {
-		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(!body_B);
-
-	ERR_FAIL_COND(body_A == body_B);
-
-	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));
-
-	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_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) const {
-	Joint3DSW *joint = joint_owner.getornull(p_joint);
-	ERR_FAIL_COND_V(!joint, 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);
-}
-
-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_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) const {
-	Joint3DSW *joint = joint_owner.getornull(p_joint);
-	ERR_FAIL_COND_V(!joint, 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);
-}
-
-void PhysicsServer3DSW::free(RID p_rid) {
-	_update_shapes(); //just in case
-
-	if (shape_owner.owns(p_rid)) {
-		Shape3DSW *shape = shape_owner.getornull(p_rid);
-
-		while (shape->get_owners().size()) {
-			ShapeOwner3DSW *so = shape->get_owners().front()->key();
-			so->remove_shape(shape);
-		}
-
-		shape_owner.free(p_rid);
-		memdelete(shape);
-	} else if (body_owner.owns(p_rid)) {
-		Body3DSW *body = body_owner.getornull(p_rid);
-
-		/*
-		if (body->get_state_query())
-			_clear_query(body->get_state_query());
-
-		if (body->get_direct_state_query())
-			_clear_query(body->get_direct_state_query());
-		*/
-
-		body->set_space(nullptr);
-
-		while (body->get_shape_count()) {
-			body->remove_shape(0);
-		}
-
-		body_owner.free(p_rid);
-		memdelete(body);
-	} else if (soft_body_owner.owns(p_rid)) {
-		SoftBody3DSW *soft_body = soft_body_owner.getornull(p_rid);
-
-		soft_body->set_space(nullptr);
-
-		soft_body_owner.free(p_rid);
-		memdelete(soft_body);
-	} else if (area_owner.owns(p_rid)) {
-		Area3DSW *area = area_owner.getornull(p_rid);
-
-		/*
-		if (area->get_monitor_query())
-			_clear_query(area->get_monitor_query());
-		*/
-
-		area->set_space(nullptr);
-
-		while (area->get_shape_count()) {
-			area->remove_shape(0);
-		}
-
-		area_owner.free(p_rid);
-		memdelete(area);
-	} else if (space_owner.owns(p_rid)) {
-		Space3DSW *space = space_owner.getornull(p_rid);
-
-		while (space->get_objects().size()) {
-			CollisionObject3DSW *co = (CollisionObject3DSW *)space->get_objects().front()->get();
-			co->set_space(nullptr);
-		}
-
-		active_spaces.erase(space);
-		free(space->get_default_area()->get_self());
-		free(space->get_static_global_body());
-
-		space_owner.free(p_rid);
-		memdelete(space);
-	} else if (joint_owner.owns(p_rid)) {
-		Joint3DSW *joint = joint_owner.getornull(p_rid);
-
-		joint_owner.free(p_rid);
-		memdelete(joint);
-
-	} else {
-		ERR_FAIL_MSG("Invalid ID.");
-	}
-};
-
-void PhysicsServer3DSW::set_active(bool p_active) {
-	active = p_active;
-};
-
-void PhysicsServer3DSW::set_collision_iterations(int p_iterations) {
-	iterations = p_iterations;
-};
-
-void PhysicsServer3DSW::init() {
-	iterations = 8; // 8?
-	stepper = memnew(Step3DSW);
-};
-
-void PhysicsServer3DSW::step(real_t p_step) {
-#ifndef _3D_DISABLED
-
-	if (!active) {
-		return;
-	}
-
-	_update_shapes();
-
-	island_count = 0;
-	active_objects = 0;
-	collision_pairs = 0;
-	for (Set<const Space3DSW *>::Element *E = active_spaces.front(); E; E = E->next()) {
-		stepper->step((Space3DSW *)E->get(), p_step, iterations);
-		island_count += E->get()->get_island_count();
-		active_objects += E->get()->get_active_objects();
-		collision_pairs += E->get()->get_collision_pairs();
-	}
-#endif
-}
-
-void PhysicsServer3DSW::sync() {
-	doing_sync = true;
-};
-
-void PhysicsServer3DSW::flush_queries() {
-#ifndef _3D_DISABLED
-
-	if (!active) {
-		return;
-	}
-
-	flushing_queries = true;
-
-	uint64_t time_beg = OS::get_singleton()->get_ticks_usec();
-
-	for (Set<const Space3DSW *>::Element *E = active_spaces.front(); E; E = E->next()) {
-		Space3DSW *space = (Space3DSW *)E->get();
-		space->call_queries();
-	}
-
-	flushing_queries = false;
-
-	if (EngineDebugger::is_profiling("servers")) {
-		uint64_t total_time[Space3DSW::ELAPSED_TIME_MAX];
-		static const char *time_name[Space3DSW::ELAPSED_TIME_MAX] = {
-			"integrate_forces",
-			"generate_islands",
-			"setup_constraints",
-			"solve_constraints",
-			"integrate_velocities"
-		};
-
-		for (int i = 0; i < Space3DSW::ELAPSED_TIME_MAX; i++) {
-			total_time[i] = 0;
-		}
-
-		for (Set<const Space3DSW *>::Element *E = active_spaces.front(); E; E = E->next()) {
-			for (int i = 0; i < Space3DSW::ELAPSED_TIME_MAX; i++) {
-				total_time[i] += E->get()->get_elapsed_time(Space3DSW::ElapsedTime(i));
-			}
-		}
-
-		Array values;
-		values.resize(Space3DSW::ELAPSED_TIME_MAX * 2);
-		for (int i = 0; i < Space3DSW::ELAPSED_TIME_MAX; i++) {
-			values[i * 2 + 0] = time_name[i];
-			values[i * 2 + 1] = USEC_TO_SEC(total_time[i]);
-		}
-		values.push_back("flush_queries");
-		values.push_back(USEC_TO_SEC(OS::get_singleton()->get_ticks_usec() - time_beg));
-
-		values.push_front("physics");
-		EngineDebugger::profiler_add_frame_data("servers", values);
-	}
-#endif
-};
-
-void PhysicsServer3DSW::end_sync() {
-	doing_sync = false;
-};
-
-void PhysicsServer3DSW::finish() {
-	memdelete(stepper);
-};
-
-int PhysicsServer3DSW::get_process_info(ProcessInfo p_info) {
-	switch (p_info) {
-		case INFO_ACTIVE_OBJECTS: {
-			return active_objects;
-		} break;
-		case INFO_COLLISION_PAIRS: {
-			return collision_pairs;
-		} break;
-		case INFO_ISLAND_COUNT: {
-			return island_count;
-		} break;
-	}
-
-	return 0;
-}
-
-void PhysicsServer3DSW::_update_shapes() {
-	while (pending_shape_update_list.first()) {
-		pending_shape_update_list.first()->self()->_shape_changed();
-		pending_shape_update_list.remove(pending_shape_update_list.first());
-	}
-}
-
-void PhysicsServer3DSW::_shape_col_cbk(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata) {
-	CollCbkData *cbk = (CollCbkData *)p_userdata;
-
-	if (cbk->max == 0) {
-		return;
-	}
-
-	if (cbk->amount == cbk->max) {
-		//find least deep
-		real_t min_depth = 1e20;
-		int min_depth_idx = 0;
-		for (int i = 0; i < cbk->amount; i++) {
-			real_t d = cbk->ptr[i * 2 + 0].distance_squared_to(cbk->ptr[i * 2 + 1]);
-			if (d < min_depth) {
-				min_depth = d;
-				min_depth_idx = i;
-			}
-		}
-
-		real_t d = p_point_A.distance_squared_to(p_point_B);
-		if (d < min_depth) {
-			return;
-		}
-		cbk->ptr[min_depth_idx * 2 + 0] = p_point_A;
-		cbk->ptr[min_depth_idx * 2 + 1] = p_point_B;
-
-	} else {
-		cbk->ptr[cbk->amount * 2 + 0] = p_point_A;
-		cbk->ptr[cbk->amount * 2 + 1] = p_point_B;
-		cbk->amount++;
-	}
-}
-
-PhysicsServer3DSW *PhysicsServer3DSW::singletonsw = nullptr;
-PhysicsServer3DSW::PhysicsServer3DSW(bool p_using_threads) {
-	singletonsw = this;
-	BroadPhase3DSW::create_func = BroadPhase3DBVH::_create;
-
-	island_count = 0;
-	active_objects = 0;
-	collision_pairs = 0;
-	using_threads = p_using_threads;
-	active = true;
-	flushing_queries = false;
-	doing_sync = false;
-};
diff --git a/servers/physics_3d/physics_server_3d_wrap_mt.cpp b/servers/physics_3d/physics_server_3d_wrap_mt.cpp
deleted file mode 100644
index 0a89c1a9c9..0000000000
--- a/servers/physics_3d/physics_server_3d_wrap_mt.cpp
+++ /dev/null
@@ -1,140 +0,0 @@
-/*************************************************************************/
-/*  physics_server_3d_wrap_mt.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 "physics_server_3d_wrap_mt.h"
-
-#include "core/os/os.h"
-
-void PhysicsServer3DWrapMT::thread_exit() {
-	exit = true;
-}
-
-void PhysicsServer3DWrapMT::thread_step(real_t p_delta) {
-	physics_3d_server->step(p_delta);
-	step_sem.post();
-}
-
-void PhysicsServer3DWrapMT::_thread_callback(void *_instance) {
-	PhysicsServer3DWrapMT *vsmt = reinterpret_cast<PhysicsServer3DWrapMT *>(_instance);
-
-	vsmt->thread_loop();
-}
-
-void PhysicsServer3DWrapMT::thread_loop() {
-	server_thread = Thread::get_caller_id();
-
-	physics_3d_server->init();
-
-	exit = false;
-	step_thread_up = true;
-	while (!exit) {
-		// flush commands one by one, until exit is requested
-		command_queue.wait_and_flush();
-	}
-
-	command_queue.flush_all(); // flush all
-
-	physics_3d_server->finish();
-}
-
-/* EVENT QUEUING */
-
-void PhysicsServer3DWrapMT::step(real_t p_step) {
-	if (create_thread) {
-		command_queue.push(this, &PhysicsServer3DWrapMT::thread_step, p_step);
-	} else {
-		command_queue.flush_all(); //flush all pending from other threads
-		physics_3d_server->step(p_step);
-	}
-}
-
-void PhysicsServer3DWrapMT::sync() {
-	if (create_thread) {
-		if (first_frame) {
-			first_frame = false;
-		} else {
-			step_sem.wait(); //must not wait if a step was not issued
-		}
-	}
-	physics_3d_server->sync();
-}
-
-void PhysicsServer3DWrapMT::flush_queries() {
-	physics_3d_server->flush_queries();
-}
-
-void PhysicsServer3DWrapMT::end_sync() {
-	physics_3d_server->end_sync();
-}
-
-void PhysicsServer3DWrapMT::init() {
-	if (create_thread) {
-		//OS::get_singleton()->release_rendering_thread();
-		thread.start(_thread_callback, this);
-		while (!step_thread_up) {
-			OS::get_singleton()->delay_usec(1000);
-		}
-	} else {
-		physics_3d_server->init();
-	}
-}
-
-void PhysicsServer3DWrapMT::finish() {
-	if (thread.is_started()) {
-		command_queue.push(this, &PhysicsServer3DWrapMT::thread_exit);
-		thread.wait_to_finish();
-	} else {
-		physics_3d_server->finish();
-	}
-}
-
-PhysicsServer3DWrapMT::PhysicsServer3DWrapMT(PhysicsServer3D *p_contained, bool p_create_thread) :
-		command_queue(p_create_thread) {
-	physics_3d_server = p_contained;
-	create_thread = p_create_thread;
-	step_pending = 0;
-	step_thread_up = false;
-
-	pool_max_size = GLOBAL_GET("memory/limits/multithreaded_server/rid_pool_prealloc");
-
-	if (!p_create_thread) {
-		server_thread = Thread::get_caller_id();
-	} else {
-		server_thread = 0;
-	}
-
-	main_thread = Thread::get_caller_id();
-	first_frame = true;
-}
-
-PhysicsServer3DWrapMT::~PhysicsServer3DWrapMT() {
-	memdelete(physics_3d_server);
-	//finish();
-}
diff --git a/servers/physics_3d/physics_server_3d_wrap_mt.h b/servers/physics_3d/physics_server_3d_wrap_mt.h
deleted file mode 100644
index 8865bd4bd2..0000000000
--- a/servers/physics_3d/physics_server_3d_wrap_mt.h
+++ /dev/null
@@ -1,407 +0,0 @@
-/*************************************************************************/
-/*  physics_server_3d_wrap_mt.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 PHYSICS3DSERVERWRAPMT_H
-#define PHYSICS3DSERVERWRAPMT_H
-
-#include "core/config/project_settings.h"
-#include "core/os/thread.h"
-#include "core/templates/command_queue_mt.h"
-#include "servers/physics_server_3d.h"
-
-#ifdef DEBUG_SYNC
-#define SYNC_DEBUG print_line("sync on: " + String(__FUNCTION__));
-#else
-#define SYNC_DEBUG
-#endif
-
-class PhysicsServer3DWrapMT : public PhysicsServer3D {
-	mutable PhysicsServer3D *physics_3d_server;
-
-	mutable CommandQueueMT command_queue;
-
-	static void _thread_callback(void *_instance);
-	void thread_loop();
-
-	Thread::ID server_thread;
-	Thread::ID main_thread;
-	volatile bool exit = false;
-	Thread thread;
-	volatile bool step_thread_up = false;
-	bool create_thread = false;
-
-	Semaphore step_sem;
-	int step_pending;
-	void thread_step(real_t p_delta);
-	void thread_flush();
-
-	void thread_exit();
-
-	bool first_frame = true;
-
-	Mutex alloc_mutex;
-	int pool_max_size = 0;
-
-public:
-#define ServerName PhysicsServer3D
-#define ServerNameWrapMT PhysicsServer3DWrapMT
-#define server_name physics_3d_server
-#define WRITE_ACTION
-
-#include "servers/server_wrap_mt_common.h"
-
-	//FUNC1RID(shape,ShapeType); todo fix
-	FUNCRID(plane_shape)
-	FUNCRID(separation_ray_shape)
-	FUNCRID(sphere_shape)
-	FUNCRID(box_shape)
-	FUNCRID(capsule_shape)
-	FUNCRID(cylinder_shape)
-	FUNCRID(convex_polygon_shape)
-	FUNCRID(concave_polygon_shape)
-	FUNCRID(heightmap_shape)
-	FUNCRID(custom_shape)
-
-	FUNC2(shape_set_data, RID, const Variant &);
-	FUNC2(shape_set_custom_solver_bias, RID, real_t);
-
-	FUNC2(shape_set_margin, RID, real_t)
-	FUNC1RC(real_t, shape_get_margin, RID)
-
-	FUNC1RC(ShapeType, shape_get_type, RID);
-	FUNC1RC(Variant, shape_get_data, RID);
-	FUNC1RC(real_t, shape_get_custom_solver_bias, RID);
-#if 0
-	//these work well, but should be used from the main thread only
-	bool shape_collide(RID p_shape_A, const Transform &p_xform_A, const Vector3 &p_motion_A, RID p_shape_B, const Transform &p_xform_B, const Vector3 &p_motion_B, Vector3 *r_results, int p_result_max, int &r_result_count) {
-		ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), false);
-		return physics_3d_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);
-	}
-#endif
-	/* SPACE API */
-
-	FUNCRID(space);
-	FUNC2(space_set_active, RID, bool);
-	FUNC1RC(bool, space_is_active, RID);
-
-	FUNC3(space_set_param, RID, SpaceParameter, real_t);
-	FUNC2RC(real_t, space_get_param, RID, SpaceParameter);
-
-	// this function only works on physics process, errors and returns null otherwise
-	PhysicsDirectSpaceState3D *space_get_direct_state(RID p_space) override {
-		ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), nullptr);
-		return physics_3d_server->space_get_direct_state(p_space);
-	}
-
-	FUNC2(space_set_debug_contacts, RID, int);
-	virtual Vector<Vector3> space_get_contacts(RID p_space) const override {
-		ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), Vector<Vector3>());
-		return physics_3d_server->space_get_contacts(p_space);
-	}
-
-	virtual int space_get_contact_count(RID p_space) const override {
-		ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), 0);
-		return physics_3d_server->space_get_contact_count(p_space);
-	}
-
-	/* AREA API */
-
-	//FUNC0RID(area);
-	FUNCRID(area);
-
-	FUNC2(area_set_space, RID, RID);
-	FUNC1RC(RID, area_get_space, RID);
-
-	FUNC2(area_set_space_override_mode, RID, AreaSpaceOverrideMode);
-	FUNC1RC(AreaSpaceOverrideMode, area_get_space_override_mode, RID);
-
-	FUNC4(area_add_shape, RID, RID, const Transform3D &, bool);
-	FUNC3(area_set_shape, RID, int, RID);
-	FUNC3(area_set_shape_transform, RID, int, const Transform3D &);
-	FUNC3(area_set_shape_disabled, RID, int, bool);
-
-	FUNC1RC(int, area_get_shape_count, RID);
-	FUNC2RC(RID, area_get_shape, RID, int);
-	FUNC2RC(Transform3D, area_get_shape_transform, RID, int);
-	FUNC2(area_remove_shape, RID, int);
-	FUNC1(area_clear_shapes, RID);
-
-	FUNC2(area_attach_object_instance_id, RID, ObjectID);
-	FUNC1RC(ObjectID, area_get_object_instance_id, RID);
-
-	FUNC3(area_set_param, RID, AreaParameter, const Variant &);
-	FUNC2(area_set_transform, RID, const Transform3D &);
-
-	FUNC2RC(Variant, area_get_param, RID, AreaParameter);
-	FUNC1RC(Transform3D, area_get_transform, RID);
-
-	FUNC2(area_set_collision_mask, RID, uint32_t);
-	FUNC2(area_set_collision_layer, RID, uint32_t);
-
-	FUNC2(area_set_monitorable, RID, bool);
-	FUNC2(area_set_ray_pickable, RID, bool);
-
-	FUNC3(area_set_monitor_callback, RID, Object *, const StringName &);
-	FUNC3(area_set_area_monitor_callback, RID, Object *, const StringName &);
-
-	/* BODY API */
-
-	//FUNC2RID(body,BodyMode,bool);
-	FUNCRID(body)
-
-	FUNC2(body_set_space, RID, RID);
-	FUNC1RC(RID, body_get_space, RID);
-
-	FUNC2(body_set_mode, RID, BodyMode);
-	FUNC1RC(BodyMode, body_get_mode, RID);
-
-	FUNC4(body_add_shape, RID, RID, const Transform3D &, bool);
-	FUNC3(body_set_shape, RID, int, RID);
-	FUNC3(body_set_shape_transform, RID, int, const Transform3D &);
-
-	FUNC1RC(int, body_get_shape_count, RID);
-	FUNC2RC(Transform3D, body_get_shape_transform, RID, int);
-	FUNC2RC(RID, body_get_shape, RID, int);
-
-	FUNC3(body_set_shape_disabled, RID, int, bool);
-
-	FUNC2(body_remove_shape, RID, int);
-	FUNC1(body_clear_shapes, RID);
-
-	FUNC2(body_attach_object_instance_id, RID, ObjectID);
-	FUNC1RC(ObjectID, body_get_object_instance_id, RID);
-
-	FUNC2(body_set_enable_continuous_collision_detection, RID, bool);
-	FUNC1RC(bool, body_is_continuous_collision_detection_enabled, RID);
-
-	FUNC2(body_set_collision_layer, RID, uint32_t);
-	FUNC1RC(uint32_t, body_get_collision_layer, RID);
-
-	FUNC2(body_set_collision_mask, RID, uint32_t);
-	FUNC1RC(uint32_t, body_get_collision_mask, RID);
-
-	FUNC2(body_set_user_flags, RID, uint32_t);
-	FUNC1RC(uint32_t, body_get_user_flags, RID);
-
-	FUNC3(body_set_param, RID, BodyParameter, real_t);
-	FUNC2RC(real_t, body_get_param, RID, BodyParameter);
-
-	FUNC3(body_set_state, RID, BodyState, const Variant &);
-	FUNC2RC(Variant, body_get_state, RID, BodyState);
-
-	FUNC2(body_set_applied_force, RID, const Vector3 &);
-	FUNC1RC(Vector3, body_get_applied_force, RID);
-
-	FUNC2(body_set_applied_torque, RID, const Vector3 &);
-	FUNC1RC(Vector3, body_get_applied_torque, RID);
-
-	FUNC2(body_add_central_force, RID, const Vector3 &);
-	FUNC3(body_add_force, RID, const Vector3 &, const Vector3 &);
-	FUNC2(body_add_torque, RID, const Vector3 &);
-	FUNC2(body_apply_torque_impulse, RID, const Vector3 &);
-	FUNC2(body_apply_central_impulse, RID, const Vector3 &);
-	FUNC3(body_apply_impulse, RID, const Vector3 &, const Vector3 &);
-	FUNC2(body_set_axis_velocity, RID, const Vector3 &);
-
-	FUNC3(body_set_axis_lock, RID, BodyAxis, bool);
-	FUNC2RC(bool, body_is_axis_locked, RID, BodyAxis);
-
-	FUNC2(body_add_collision_exception, RID, RID);
-	FUNC2(body_remove_collision_exception, RID, RID);
-	FUNC2S(body_get_collision_exceptions, RID, List<RID> *);
-
-	FUNC2(body_set_max_contacts_reported, RID, int);
-	FUNC1RC(int, body_get_max_contacts_reported, RID);
-
-	FUNC2(body_set_contacts_reported_depth_threshold, RID, real_t);
-	FUNC1RC(real_t, body_get_contacts_reported_depth_threshold, RID);
-
-	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);
-
-	bool body_test_motion(RID p_body, const Transform3D &p_from, const Vector3 &p_motion, real_t p_margin = 0.001, MotionResult *r_result = nullptr, bool p_collide_separation_ray = false, const Set<RID> &p_exclude = Set<RID>()) override {
-		ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), false);
-		return physics_3d_server->body_test_motion(p_body, p_from, p_motion, p_margin, r_result, p_collide_separation_ray, p_exclude);
-	}
-
-	// this function only works on physics process, errors and returns null otherwise
-	PhysicsDirectBodyState3D *body_get_direct_state(RID p_body) override {
-		ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), nullptr);
-		return physics_3d_server->body_get_direct_state(p_body);
-	}
-
-	/* SOFT BODY API */
-
-	FUNCRID(soft_body)
-
-	FUNC2(soft_body_update_rendering_server, RID, class RenderingServerHandler *)
-
-	FUNC2(soft_body_set_space, RID, RID)
-	FUNC1RC(RID, soft_body_get_space, RID)
-
-	FUNC2(soft_body_set_ray_pickable, RID, bool);
-
-	FUNC2(soft_body_set_collision_layer, RID, uint32_t)
-	FUNC1RC(uint32_t, soft_body_get_collision_layer, RID)
-
-	FUNC2(soft_body_set_collision_mask, RID, uint32_t)
-	FUNC1RC(uint32_t, soft_body_get_collision_mask, RID)
-
-	FUNC2(soft_body_add_collision_exception, RID, RID)
-	FUNC2(soft_body_remove_collision_exception, RID, RID)
-	FUNC2S(soft_body_get_collision_exceptions, RID, List<RID> *)
-
-	FUNC3(soft_body_set_state, RID, BodyState, const Variant &);
-	FUNC2RC(Variant, soft_body_get_state, RID, BodyState);
-
-	FUNC2(soft_body_set_transform, RID, const Transform3D &);
-
-	FUNC2(soft_body_set_simulation_precision, RID, int);
-	FUNC1RC(int, soft_body_get_simulation_precision, RID);
-
-	FUNC2(soft_body_set_total_mass, RID, real_t);
-	FUNC1RC(real_t, soft_body_get_total_mass, RID);
-
-	FUNC2(soft_body_set_linear_stiffness, RID, real_t);
-	FUNC1RC(real_t, soft_body_get_linear_stiffness, RID);
-
-	FUNC2(soft_body_set_pressure_coefficient, RID, real_t);
-	FUNC1RC(real_t, soft_body_get_pressure_coefficient, RID);
-
-	FUNC2(soft_body_set_damping_coefficient, RID, real_t);
-	FUNC1RC(real_t, soft_body_get_damping_coefficient, RID);
-
-	FUNC2(soft_body_set_drag_coefficient, RID, real_t);
-	FUNC1RC(real_t, soft_body_get_drag_coefficient, RID);
-
-	FUNC2(soft_body_set_mesh, RID, const REF &);
-
-	FUNC1RC(AABB, soft_body_get_bounds, RID);
-
-	FUNC3(soft_body_move_point, RID, int, const Vector3 &);
-	FUNC2RC(Vector3, soft_body_get_point_global_position, RID, int);
-
-	FUNC1(soft_body_remove_all_pinned_points, RID);
-	FUNC3(soft_body_pin_point, RID, int, bool);
-	FUNC2RC(bool, soft_body_is_point_pinned, RID, int);
-
-	/* JOINT API */
-
-	FUNCRID(joint)
-
-	FUNC1(joint_clear, RID)
-
-	FUNC5(joint_make_pin, RID, RID, const Vector3 &, RID, const Vector3 &)
-
-	FUNC3(pin_joint_set_param, RID, PinJointParam, real_t)
-	FUNC2RC(real_t, pin_joint_get_param, RID, PinJointParam)
-
-	FUNC2(pin_joint_set_local_a, RID, const Vector3 &)
-	FUNC1RC(Vector3, pin_joint_get_local_a, RID)
-
-	FUNC2(pin_joint_set_local_b, RID, const Vector3 &)
-	FUNC1RC(Vector3, pin_joint_get_local_b, RID)
-
-	FUNC5(joint_make_hinge, RID, RID, const Transform3D &, RID, const Transform3D &)
-	FUNC7(joint_make_hinge_simple, RID, RID, const Vector3 &, const Vector3 &, RID, const Vector3 &, const Vector3 &)
-
-	FUNC3(hinge_joint_set_param, RID, HingeJointParam, real_t)
-	FUNC2RC(real_t, hinge_joint_get_param, RID, HingeJointParam)
-
-	FUNC3(hinge_joint_set_flag, RID, HingeJointFlag, bool)
-	FUNC2RC(bool, hinge_joint_get_flag, RID, HingeJointFlag)
-
-	FUNC5(joint_make_slider, RID, RID, const Transform3D &, RID, const Transform3D &)
-
-	FUNC3(slider_joint_set_param, RID, SliderJointParam, real_t)
-	FUNC2RC(real_t, slider_joint_get_param, RID, SliderJointParam)
-
-	FUNC5(joint_make_cone_twist, RID, RID, const Transform3D &, RID, const Transform3D &)
-
-	FUNC3(cone_twist_joint_set_param, RID, ConeTwistJointParam, real_t)
-	FUNC2RC(real_t, cone_twist_joint_get_param, RID, ConeTwistJointParam)
-
-	FUNC5(joint_make_generic_6dof, RID, RID, const Transform3D &, RID, const Transform3D &)
-
-	FUNC4(generic_6dof_joint_set_param, RID, Vector3::Axis, G6DOFJointAxisParam, real_t)
-	FUNC3RC(real_t, generic_6dof_joint_get_param, RID, Vector3::Axis, G6DOFJointAxisParam)
-
-	FUNC4(generic_6dof_joint_set_flag, RID, Vector3::Axis, G6DOFJointAxisFlag, bool)
-	FUNC3RC(bool, generic_6dof_joint_get_flag, RID, Vector3::Axis, G6DOFJointAxisFlag)
-
-	FUNC1RC(JointType, joint_get_type, RID);
-
-	FUNC2(joint_set_solver_priority, RID, int);
-	FUNC1RC(int, joint_get_solver_priority, RID);
-
-	FUNC2(joint_disable_collisions_between_bodies, RID, const bool);
-	FUNC1RC(bool, joint_is_disabled_collisions_between_bodies, RID);
-
-	/* MISC */
-
-	FUNC1(free, RID);
-	FUNC1(set_active, bool);
-	FUNC1(set_collision_iterations, int);
-
-	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 override {
-		return physics_3d_server->is_flushing_queries();
-	}
-
-	int get_process_info(ProcessInfo p_info) override {
-		return physics_3d_server->get_process_info(p_info);
-	}
-
-	PhysicsServer3DWrapMT(PhysicsServer3D *p_contained, bool p_create_thread);
-	~PhysicsServer3DWrapMT();
-
-#undef ServerNameWrapMT
-#undef ServerName
-#undef server_name
-#undef WRITE_ACTION
-};
-
-#ifdef DEBUG_SYNC
-#undef DEBUG_SYNC
-#endif
-#undef SYNC_DEBUG
-
-#endif // PHYSICS3DSERVERWRAPMT_H