19 files changed, 420 insertions, 495 deletions

diff --git a/servers/physics_3d/area_3d_sw.cpp b/servers/physics_3d/area_3d_sw.cpp
index a9f5c4aec3..364f63e4ad 100644
--- a/servers/physics_3d/area_3d_sw.cpp
+++ b/servers/physics_3d/area_3d_sw.cpp
@@ -30,8 +30,16 @@
 
 #include "area_3d_sw.h"
 #include "body_3d_sw.h"
+#include "soft_body_3d_sw.h"
 #include "space_3d_sw.h"
 
+Area3DSW::BodyKey::BodyKey(SoftBody3DSW *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) {
 	rid = p_body->get_self();
 	instance_id = p_body->get_instance_id();
diff --git a/servers/physics_3d/area_3d_sw.h b/servers/physics_3d/area_3d_sw.h
index 12f7545c08..5959ee1e95 100644
--- a/servers/physics_3d/area_3d_sw.h
+++ b/servers/physics_3d/area_3d_sw.h
@@ -38,6 +38,7 @@
 
 class Space3DSW;
 class Body3DSW;
+class SoftBody3DSW;
 class Constraint3DSW;
 
 class Area3DSW : public CollisionObject3DSW {
@@ -80,6 +81,7 @@ 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);
 	};
@@ -91,6 +93,7 @@ class Area3DSW : public CollisionObject3DSW {
 		_FORCE_INLINE_ BodyState() { state = 0; }
 	};
 
+	Map<BodyKey, BodyState> monitored_soft_bodies;
 	Map<BodyKey, BodyState> monitored_bodies;
 	Map<BodyKey, BodyState> monitored_areas;
 
@@ -115,6 +118,9 @@ public:
 	_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_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_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);
 
@@ -166,6 +172,22 @@ public:
 	~Area3DSW();
 };
 
+void Area3DSW::add_soft_body_to_query(SoftBody3DSW *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()) {
+		_queue_monitor_update();
+	}
+}
+
+void Area3DSW::remove_soft_body_from_query(SoftBody3DSW *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()) {
+		_queue_monitor_update();
+	}
+}
+
 void Area3DSW::add_body_to_query(Body3DSW *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();
@@ -198,4 +220,16 @@ void Area3DSW::remove_area_from_query(Area3DSW *p_area, uint32_t p_area_shape, u
 	}
 }
 
+struct AreaCMP {
+	Area3DSW *area;
+	int refCount;
+	_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) {
+		area = p_area;
+		refCount = 1;
+	}
+};
+
 #endif // AREA__SW_H
diff --git a/servers/physics_3d/area_pair_3d_sw.cpp b/servers/physics_3d/area_pair_3d_sw.cpp
index f43c2c965e..e740565da6 100644
--- a/servers/physics_3d/area_pair_3d_sw.cpp
+++ b/servers/physics_3d/area_pair_3d_sw.cpp
@@ -181,3 +181,85 @@ Area2Pair3DSW::~Area2Pair3DSW() {
 	area_a->remove_constraint(this);
 	area_b->remove_constraint(this);
 }
+
+////////////////////////////////////////////////////
+
+bool AreaSoftBodyPair3DSW::setup(real_t p_step) {
+	bool result = false;
+	if (
+			area->interacts_with(soft_body) &&
+			CollisionSolver3DSW::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),
+					area->get_transform() * area->get_shape_transform(area_shape),
+					nullptr,
+					this)) {
+		result = true;
+	}
+
+	process_collision = false;
+	if (result != colliding) {
+		if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) {
+			process_collision = true;
+		} else if (area->has_monitor_callback()) {
+			process_collision = true;
+		}
+
+		colliding = result;
+	}
+
+	return process_collision;
+}
+
+bool AreaSoftBodyPair3DSW::pre_solve(real_t p_step) {
+	if (!process_collision) {
+		return false;
+	}
+
+	if (colliding) {
+		if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) {
+			soft_body->add_area(area);
+		}
+
+		if (area->has_monitor_callback()) {
+			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) {
+			soft_body->remove_area(area);
+		}
+
+		if (area->has_monitor_callback()) {
+			area->remove_soft_body_from_query(soft_body, soft_body_shape, area_shape);
+		}
+	}
+
+	return false; // Never do any post solving.
+}
+
+void AreaSoftBodyPair3DSW::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) {
+	soft_body = p_soft_body;
+	area = p_area;
+	soft_body_shape = p_soft_body_shape;
+	area_shape = p_area_shape;
+	soft_body->add_constraint(this);
+	area->add_constraint(this);
+}
+
+AreaSoftBodyPair3DSW::~AreaSoftBodyPair3DSW() {
+	if (colliding) {
+		if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) {
+			soft_body->remove_area(area);
+		}
+		if (area->has_monitor_callback()) {
+			area->remove_soft_body_from_query(soft_body, soft_body_shape, area_shape);
+		}
+	}
+	soft_body->remove_constraint(this);
+	area->remove_constraint(this);
+}
diff --git a/servers/physics_3d/area_pair_3d_sw.h b/servers/physics_3d/area_pair_3d_sw.h
index 596d893082..8cc9e9ad63 100644
--- a/servers/physics_3d/area_pair_3d_sw.h
+++ b/servers/physics_3d/area_pair_3d_sw.h
@@ -34,6 +34,7 @@
 #include "area_3d_sw.h"
 #include "body_3d_sw.h"
 #include "constraint_3d_sw.h"
+#include "soft_body_3d_sw.h"
 
 class AreaPair3DSW : public Constraint3DSW {
 	Body3DSW *body;
@@ -69,4 +70,21 @@ public:
 	~Area2Pair3DSW();
 };
 
+class AreaSoftBodyPair3DSW : public Constraint3DSW {
+	SoftBody3DSW *soft_body;
+	Area3DSW *area;
+	int soft_body_shape;
+	int area_shape;
+	bool colliding = false;
+	bool process_collision = 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();
+};
+
 #endif // AREA_PAIR__SW_H
diff --git a/servers/physics_3d/body_3d_sw.h b/servers/physics_3d/body_3d_sw.h
index 0fa31c5037..efb114a325 100644
--- a/servers/physics_3d/body_3d_sw.h
+++ b/servers/physics_3d/body_3d_sw.h
@@ -96,18 +96,6 @@ class Body3DSW : public CollisionObject3DSW {
 
 	Map<Constraint3DSW *, int> constraint_map;
 
-	struct AreaCMP {
-		Area3DSW *area;
-		int refCount;
-		_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) {
-			area = p_area;
-			refCount = 1;
-		}
-	};
-
 	Vector<AreaCMP> areas;
 
 	struct Contact {
@@ -389,6 +377,8 @@ public:
 	virtual void set_transform(const Transform3D &p_transform) override { body->set_state(PhysicsServer3D::BODY_STATE_TRANSFORM, p_transform); }
 	virtual Transform3D get_transform() const override { return body->get_transform(); }
 
+	virtual Vector3 get_velocity_at_local_position(const Vector3 &p_position) const override { return body->get_velocity_in_local_point(p_position); }
+
 	virtual void add_central_force(const Vector3 &p_force) override { body->add_central_force(p_force); }
 	virtual void add_force(const Vector3 &p_force, const Vector3 &p_position = Vector3()) override {
 		body->add_force(p_force, p_position);
diff --git a/servers/physics_3d/body_pair_3d_sw.cpp b/servers/physics_3d/body_pair_3d_sw.cpp
index af680f23db..c27a2ecced 100644
--- a/servers/physics_3d/body_pair_3d_sw.cpp
+++ b/servers/physics_3d/body_pair_3d_sw.cpp
@@ -212,16 +212,16 @@ real_t combine_friction(Body3DSW *A, Body3DSW *B) {
 }
 
 bool BodyPair3DSW::setup(real_t p_step) {
-	dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
-	dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
-
 	if (!A->interacts_with(B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self())) {
 		collided = false;
 		return false;
 	}
 
+	collide_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC) && A->collides_with(B);
+	collide_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC) && B->collides_with(A);
+
 	report_contacts_only = false;
-	if (!dynamic_A && !dynamic_B) {
+	if (!collide_A && !collide_B) {
 		if ((A->get_max_contacts_reported() > 0) || (B->get_max_contacts_reported() > 0)) {
 			report_contacts_only = true;
 		} else {
@@ -250,11 +250,11 @@ bool BodyPair3DSW::setup(real_t p_step) {
 	if (!collided) {
 		//test ccd (currently just a raycast)
 
-		if (A->is_continuous_collision_detection_enabled() && dynamic_A && !dynamic_B) {
+		if (A->is_continuous_collision_detection_enabled() && collide_A) {
 			_test_ccd(p_step, A, shape_A, xform_A, B, shape_B, xform_B);
 		}
 
-		if (B->is_continuous_collision_detection_enabled() && dynamic_B && !dynamic_A) {
+		if (B->is_continuous_collision_detection_enabled() && collide_B) {
 			_test_ccd(p_step, B, shape_B, xform_B, A, shape_A, xform_A);
 		}
 
@@ -293,6 +293,15 @@ bool BodyPair3DSW::pre_solve(real_t p_step) {
 	const Basis &basis_A = A->get_transform().basis;
 	const Basis &basis_B = B->get_transform().basis;
 
+	Basis zero_basis;
+	zero_basis.set_zero();
+
+	const Basis &inv_inertia_tensor_A = collide_A ? A->get_inv_inertia_tensor() : zero_basis;
+	const Basis &inv_inertia_tensor_B = collide_B ? B->get_inv_inertia_tensor() : zero_basis;
+
+	real_t inv_mass_A = collide_A ? A->get_inv_mass() : 0.0;
+	real_t inv_mass_B = collide_B ? B->get_inv_mass() : 0.0;
+
 	for (int i = 0; i < contact_count; i++) {
 		Contact &c = contacts[i];
 		c.active = false;
@@ -303,7 +312,7 @@ bool BodyPair3DSW::pre_solve(real_t p_step) {
 		Vector3 axis = global_A - global_B;
 		real_t depth = axis.dot(c.normal);
 
-		if (depth <= 0) {
+		if (depth <= 0.0) {
 			continue;
 		}
 
@@ -339,9 +348,9 @@ bool BodyPair3DSW::pre_solve(real_t p_step) {
 		do_process = true;
 
 		// Precompute normal mass, tangent mass, and bias.
-		Vector3 inertia_A = A->get_inv_inertia_tensor().xform(c.rA.cross(c.normal));
-		Vector3 inertia_B = B->get_inv_inertia_tensor().xform(c.rB.cross(c.normal));
-		real_t kNormal = A->get_inv_mass() + B->get_inv_mass();
+		Vector3 inertia_A = inv_inertia_tensor_A.xform(c.rA.cross(c.normal));
+		Vector3 inertia_B = inv_inertia_tensor_B.xform(c.rB.cross(c.normal));
+		real_t kNormal = inv_mass_A + inv_mass_B;
 		kNormal += c.normal.dot(inertia_A.cross(c.rA)) + c.normal.dot(inertia_B.cross(c.rB));
 		c.mass_normal = 1.0f / kNormal;
 
@@ -349,10 +358,10 @@ bool BodyPair3DSW::pre_solve(real_t p_step) {
 		c.depth = depth;
 
 		Vector3 j_vec = c.normal * c.acc_normal_impulse + c.acc_tangent_impulse;
-		if (dynamic_A) {
+		if (collide_A) {
 			A->apply_impulse(-j_vec, c.rA + A->get_center_of_mass());
 		}
-		if (dynamic_B) {
+		if (collide_B) {
 			B->apply_impulse(j_vec, c.rB + B->get_center_of_mass());
 		}
 		c.acc_bias_impulse = 0;
@@ -378,6 +387,15 @@ void BodyPair3DSW::solve(real_t p_step) {
 
 	const real_t max_bias_av = MAX_BIAS_ROTATION / p_step;
 
+	Basis zero_basis;
+	zero_basis.set_zero();
+
+	const Basis &inv_inertia_tensor_A = collide_A ? A->get_inv_inertia_tensor() : zero_basis;
+	const Basis &inv_inertia_tensor_B = collide_B ? B->get_inv_inertia_tensor() : zero_basis;
+
+	real_t inv_mass_A = collide_A ? A->get_inv_mass() : 0.0;
+	real_t inv_mass_B = collide_B ? B->get_inv_mass() : 0.0;
+
 	for (int i = 0; i < contact_count; i++) {
 		Contact &c = contacts[i];
 		if (!c.active) {
@@ -401,10 +419,10 @@ void BodyPair3DSW::solve(real_t p_step) {
 
 			Vector3 jb = c.normal * (c.acc_bias_impulse - jbnOld);
 
-			if (dynamic_A) {
+			if (collide_A) {
 				A->apply_bias_impulse(-jb, c.rA + A->get_center_of_mass(), max_bias_av);
 			}
-			if (dynamic_B) {
+			if (collide_B) {
 				B->apply_bias_impulse(jb, c.rB + B->get_center_of_mass(), max_bias_av);
 			}
 
@@ -415,16 +433,16 @@ void BodyPair3DSW::solve(real_t p_step) {
 			vbn = dbv.dot(c.normal);
 
 			if (Math::abs(-vbn + c.bias) > MIN_VELOCITY) {
-				real_t jbn_com = (-vbn + c.bias) / (A->get_inv_mass() + B->get_inv_mass());
+				real_t jbn_com = (-vbn + c.bias) / (inv_mass_A + inv_mass_B);
 				real_t jbnOld_com = c.acc_bias_impulse_center_of_mass;
 				c.acc_bias_impulse_center_of_mass = MAX(jbnOld_com + jbn_com, 0.0f);
 
 				Vector3 jb_com = c.normal * (c.acc_bias_impulse_center_of_mass - jbnOld_com);
 
-				if (dynamic_A) {
+				if (collide_A) {
 					A->apply_bias_impulse(-jb_com, A->get_center_of_mass(), 0.0f);
 				}
-				if (dynamic_B) {
+				if (collide_B) {
 					B->apply_bias_impulse(jb_com, B->get_center_of_mass(), 0.0f);
 				}
 			}
@@ -446,10 +464,10 @@ void BodyPair3DSW::solve(real_t p_step) {
 
 			Vector3 j = c.normal * (c.acc_normal_impulse - jnOld);
 
-			if (dynamic_A) {
+			if (collide_A) {
 				A->apply_impulse(-j, c.rA + A->get_center_of_mass());
 			}
-			if (dynamic_B) {
+			if (collide_B) {
 				B->apply_impulse(j, c.rB + B->get_center_of_mass());
 			}
 
@@ -473,11 +491,11 @@ void BodyPair3DSW::solve(real_t p_step) {
 		if (tvl > MIN_VELOCITY) {
 			tv /= tvl;
 
-			Vector3 temp1 = A->get_inv_inertia_tensor().xform(c.rA.cross(tv));
-			Vector3 temp2 = B->get_inv_inertia_tensor().xform(c.rB.cross(tv));
+			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 /
-					   (A->get_inv_mass() + B->get_inv_mass() + tv.dot(temp1.cross(c.rA) + temp2.cross(c.rB)));
+					   (inv_mass_A + inv_mass_B + tv.dot(temp1.cross(c.rA) + temp2.cross(c.rB)));
 
 			Vector3 jt = t * tv;
 
@@ -493,10 +511,10 @@ void BodyPair3DSW::solve(real_t p_step) {
 
 			jt = c.acc_tangent_impulse - jtOld;
 
-			if (dynamic_A) {
+			if (collide_A) {
 				A->apply_impulse(-jt, c.rA + A->get_center_of_mass());
 			}
-			if (dynamic_B) {
+			if (collide_B) {
 				B->apply_impulse(jt, c.rB + B->get_center_of_mass());
 			}
 
@@ -595,13 +613,23 @@ void BodySoftBodyPair3DSW::validate_contacts() {
 }
 
 bool BodySoftBodyPair3DSW::setup(real_t p_step) {
-	body_dynamic = (body->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
-
 	if (!body->interacts_with(soft_body) || body->has_exception(soft_body->get_self()) || soft_body->has_exception(body->get_self())) {
 		collided = false;
 		return false;
 	}
 
+	body_collides = (body->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC) && body->collides_with(soft_body);
+	soft_body_collides = soft_body->collides_with(body);
+
+	if (!body_collides && !soft_body_collides) {
+		if (body->get_max_contacts_reported() > 0) {
+			report_contacts_only = true;
+		} else {
+			collided = false;
+			return false;
+		}
+	}
+
 	const Transform3D &xform_Au = body->get_transform();
 	Transform3D xform_A = xform_Au * body->get_shape_transform(body_shape);
 
@@ -639,13 +667,20 @@ bool BodySoftBodyPair3DSW::pre_solve(real_t p_step) {
 
 	const Transform3D &transform_A = body->get_transform();
 
+	Basis zero_basis;
+	zero_basis.set_zero();
+
+	const Basis &body_inv_inertia_tensor = body_collides ? body->get_inv_inertia_tensor() : zero_basis;
+
+	real_t body_inv_mass = body_collides ? body->get_inv_mass() : 0.0;
+
 	uint32_t contact_count = contacts.size();
 	for (uint32_t contact_index = 0; contact_index < contact_count; ++contact_index) {
 		Contact &c = contacts[contact_index];
 		c.active = false;
 
-		real_t node_inv_mass = soft_body->get_node_inv_mass(c.index_B);
-		if (node_inv_mass == 0.0) {
+		real_t node_inv_mass = soft_body_collides ? soft_body->get_node_inv_mass(c.index_B) : 0.0;
+		if ((node_inv_mass == 0.0) && (body_inv_mass == 0.0)) {
 			continue;
 		}
 
@@ -654,15 +689,11 @@ bool BodySoftBodyPair3DSW::pre_solve(real_t p_step) {
 		Vector3 axis = global_A - global_B;
 		real_t depth = axis.dot(c.normal);
 
-		if (depth <= 0) {
+		if (depth <= 0.0) {
 			continue;
 		}
 
-		c.active = true;
-		do_process = true;
-
 #ifdef DEBUG_ENABLED
-
 		if (space->is_debugging_contacts()) {
 			space->add_debug_contact(global_A);
 			space->add_debug_contact(global_B);
@@ -677,13 +708,21 @@ bool BodySoftBodyPair3DSW::pre_solve(real_t p_step) {
 			body->add_contact(global_A, -c.normal, depth, body_shape, global_B, 0, soft_body->get_instance_id(), soft_body->get_self(), crA);
 		}
 
-		if (body_dynamic) {
+		if (report_contacts_only) {
+			collided = false;
+			continue;
+		}
+
+		c.active = true;
+		do_process = true;
+
+		if (body_collides) {
 			body->set_active(true);
 		}
 
 		// Precompute normal mass, tangent mass, and bias.
-		Vector3 inertia_A = body->get_inv_inertia_tensor().xform(c.rA.cross(c.normal));
-		real_t kNormal = body->get_inv_mass() + node_inv_mass;
+		Vector3 inertia_A = body_inv_inertia_tensor.xform(c.rA.cross(c.normal));
+		real_t kNormal = body_inv_mass + node_inv_mass;
 		kNormal += c.normal.dot(inertia_A.cross(c.rA));
 		c.mass_normal = 1.0f / kNormal;
 
@@ -691,10 +730,12 @@ bool BodySoftBodyPair3DSW::pre_solve(real_t p_step) {
 		c.depth = depth;
 
 		Vector3 j_vec = c.normal * c.acc_normal_impulse + c.acc_tangent_impulse;
-		if (body_dynamic) {
+		if (body_collides) {
 			body->apply_impulse(-j_vec, c.rA + body->get_center_of_mass());
 		}
-		soft_body->apply_node_impulse(c.index_B, j_vec);
+		if (soft_body_collides) {
+			soft_body->apply_node_impulse(c.index_B, j_vec);
+		}
 		c.acc_bias_impulse = 0;
 		c.acc_bias_impulse_center_of_mass = 0;
 
@@ -719,6 +760,13 @@ void BodySoftBodyPair3DSW::solve(real_t p_step) {
 
 	const real_t max_bias_av = MAX_BIAS_ROTATION / p_step;
 
+	Basis zero_basis;
+	zero_basis.set_zero();
+
+	const Basis &body_inv_inertia_tensor = body_collides ? body->get_inv_inertia_tensor() : zero_basis;
+
+	real_t body_inv_mass = body_collides ? body->get_inv_mass() : 0.0;
+
 	uint32_t contact_count = contacts.size();
 	for (uint32_t contact_index = 0; contact_index < contact_count; ++contact_index) {
 		Contact &c = contacts[contact_index];
@@ -728,6 +776,8 @@ void BodySoftBodyPair3DSW::solve(real_t p_step) {
 
 		c.active = false;
 
+		real_t node_inv_mass = soft_body_collides ? soft_body->get_node_inv_mass(c.index_B) : 0.0;
+
 		// Bias impulse.
 		Vector3 crbA = body->get_biased_angular_velocity().cross(c.rA);
 		Vector3 dbv = soft_body->get_node_biased_velocity(c.index_B) - body->get_biased_linear_velocity() - crbA;
@@ -741,10 +791,12 @@ void BodySoftBodyPair3DSW::solve(real_t p_step) {
 
 			Vector3 jb = c.normal * (c.acc_bias_impulse - jbnOld);
 
-			if (body_dynamic) {
+			if (body_collides) {
 				body->apply_bias_impulse(-jb, c.rA + body->get_center_of_mass(), max_bias_av);
 			}
-			soft_body->apply_node_bias_impulse(c.index_B, jb);
+			if (soft_body_collides) {
+				soft_body->apply_node_bias_impulse(c.index_B, jb);
+			}
 
 			crbA = body->get_biased_angular_velocity().cross(c.rA);
 			dbv = soft_body->get_node_biased_velocity(c.index_B) - body->get_biased_linear_velocity() - crbA;
@@ -752,16 +804,18 @@ void BodySoftBodyPair3DSW::solve(real_t p_step) {
 			vbn = dbv.dot(c.normal);
 
 			if (Math::abs(-vbn + c.bias) > MIN_VELOCITY) {
-				real_t jbn_com = (-vbn + c.bias) / (body->get_inv_mass() + soft_body->get_node_inv_mass(c.index_B));
+				real_t jbn_com = (-vbn + c.bias) / (body_inv_mass + node_inv_mass);
 				real_t jbnOld_com = c.acc_bias_impulse_center_of_mass;
 				c.acc_bias_impulse_center_of_mass = MAX(jbnOld_com + jbn_com, 0.0f);
 
 				Vector3 jb_com = c.normal * (c.acc_bias_impulse_center_of_mass - jbnOld_com);
 
-				if (body_dynamic) {
+				if (body_collides) {
 					body->apply_bias_impulse(-jb_com, body->get_center_of_mass(), 0.0f);
 				}
-				soft_body->apply_node_bias_impulse(c.index_B, jb_com);
+				if (soft_body_collides) {
+					soft_body->apply_node_bias_impulse(c.index_B, jb_com);
+				}
 			}
 
 			c.active = true;
@@ -780,10 +834,12 @@ void BodySoftBodyPair3DSW::solve(real_t p_step) {
 
 			Vector3 j = c.normal * (c.acc_normal_impulse - jnOld);
 
-			if (body_dynamic) {
+			if (body_collides) {
 				body->apply_impulse(-j, c.rA + body->get_center_of_mass());
 			}
-			soft_body->apply_node_impulse(c.index_B, j);
+			if (soft_body_collides) {
+				soft_body->apply_node_impulse(c.index_B, j);
+			}
 
 			c.active = true;
 		}
@@ -804,10 +860,10 @@ void BodySoftBodyPair3DSW::solve(real_t p_step) {
 		if (tvl > MIN_VELOCITY) {
 			tv /= tvl;
 
-			Vector3 temp1 = body->get_inv_inertia_tensor().xform(c.rA.cross(tv));
+			Vector3 temp1 = body_inv_inertia_tensor.xform(c.rA.cross(tv));
 
 			real_t t = -tvl /
-					   (body->get_inv_mass() + soft_body->get_node_inv_mass(c.index_B) + tv.dot(temp1.cross(c.rA)));
+					   (body_inv_mass + node_inv_mass + tv.dot(temp1.cross(c.rA)));
 
 			Vector3 jt = t * tv;
 
@@ -823,10 +879,12 @@ void BodySoftBodyPair3DSW::solve(real_t p_step) {
 
 			jt = c.acc_tangent_impulse - jtOld;
 
-			if (body_dynamic) {
+			if (body_collides) {
 				body->apply_impulse(-jt, c.rA + body->get_center_of_mass());
 			}
-			soft_body->apply_node_impulse(c.index_B, jt);
+			if (soft_body_collides) {
+				soft_body->apply_node_impulse(c.index_B, jt);
+			}
 
 			c.active = true;
 		}
diff --git a/servers/physics_3d/body_pair_3d_sw.h b/servers/physics_3d/body_pair_3d_sw.h
index 976982d1f1..19d6a46880 100644
--- a/servers/physics_3d/body_pair_3d_sw.h
+++ b/servers/physics_3d/body_pair_3d_sw.h
@@ -83,8 +83,8 @@ class BodyPair3DSW : public BodyContact3DSW {
 	int shape_A = 0;
 	int shape_B = 0;
 
-	bool dynamic_A = false;
-	bool dynamic_B = false;
+	bool collide_A = false;
+	bool collide_B = false;
 
 	bool report_contacts_only = false;
 
@@ -115,7 +115,10 @@ class BodySoftBodyPair3DSW : public BodyContact3DSW {
 
 	int body_shape = 0;
 
-	bool body_dynamic = false;
+	bool body_collides = false;
+	bool soft_body_collides = false;
+
+	bool report_contacts_only = false;
 
 	LocalVector<Contact> contacts;
 
diff --git a/servers/physics_3d/collision_object_3d_sw.h b/servers/physics_3d/collision_object_3d_sw.h
index fb8dca8bb4..6ffab54645 100644
--- a/servers/physics_3d/collision_object_3d_sw.h
+++ b/servers/physics_3d/collision_object_3d_sw.h
@@ -166,8 +166,8 @@ public:
 	}
 	_FORCE_INLINE_ uint32_t get_collision_mask() const { return collision_mask; }
 
-	_FORCE_INLINE_ bool layer_in_mask(CollisionObject3DSW *p_other) const {
-		return collision_layer & p_other->collision_mask;
+	_FORCE_INLINE_ bool collides_with(CollisionObject3DSW *p_other) const {
+		return p_other->collision_layer & collision_mask;
 	}
 
 	_FORCE_INLINE_ bool interacts_with(CollisionObject3DSW *p_other) const {
diff --git a/servers/physics_3d/collision_solver_3d_sat.cpp b/servers/physics_3d/collision_solver_3d_sat.cpp
index 6a7f2b73c5..6e6a2cb9e7 100644
--- a/servers/physics_3d/collision_solver_3d_sat.cpp
+++ b/servers/physics_3d/collision_solver_3d_sat.cpp
@@ -848,7 +848,7 @@ static void _collision_sphere_capsule(const Shape3DSW *p_a, const Transform3D &p
 
 	//capsule sphere 1, sphere
 
-	Vector3 capsule_axis = p_transform_b.basis.get_axis(1) * (capsule_B->get_height() * 0.5);
+	Vector3 capsule_axis = p_transform_b.basis.get_axis(1) * (capsule_B->get_height() * 0.5 - capsule_B->get_radius());
 
 	Vector3 capsule_ball_1 = p_transform_b.origin + capsule_axis;
 
@@ -956,9 +956,12 @@ static void _collision_sphere_convex_polygon(const Shape3DSW *p_a, const Transfo
 	const Vector3 *vertices = mesh.vertices.ptr();
 	int vertex_count = mesh.vertices.size();
 
+	// Precalculating this makes the transforms faster.
+	Basis b_xform_normal = p_transform_b.basis.inverse().transposed();
+
 	// faces of B
 	for (int i = 0; i < face_count; i++) {
-		Vector3 axis = p_transform_b.xform(faces[i].plane).normal;
+		Vector3 axis = b_xform_normal.xform(faces[i].plane.normal).normalized();
 
 		if (!separator.test_axis(axis)) {
 			return;
@@ -1204,7 +1207,7 @@ static void _collision_box_capsule(const Shape3DSW *p_a, const Transform3D &p_tr
 	// capsule balls, edges of A
 
 	for (int i = 0; i < 2; i++) {
-		Vector3 capsule_axis = p_transform_b.basis.get_axis(1) * (capsule_B->get_height() * 0.5);
+		Vector3 capsule_axis = p_transform_b.basis.get_axis(1) * (capsule_B->get_height() * 0.5 - capsule_B->get_radius());
 
 		Vector3 sphere_pos = p_transform_b.origin + ((i == 0) ? capsule_axis : -capsule_axis);
 
@@ -1379,9 +1382,12 @@ static void _collision_box_convex_polygon(const Shape3DSW *p_a, const Transform3
 		}
 	}
 
+	// Precalculating this makes the transforms faster.
+	Basis b_xform_normal = p_transform_b.basis.inverse().transposed();
+
 	// faces of B
 	for (int i = 0; i < face_count; i++) {
-		Vector3 axis = p_transform_b.xform(faces[i].plane).normal;
+		Vector3 axis = b_xform_normal.xform(faces[i].plane.normal).normalized();
 
 		if (!separator.test_axis(axis)) {
 			return;
@@ -1601,8 +1607,8 @@ static void _collision_capsule_capsule(const Shape3DSW *p_a, const Transform3D &
 
 	// some values
 
-	Vector3 capsule_A_axis = p_transform_a.basis.get_axis(1) * (capsule_A->get_height() * 0.5);
-	Vector3 capsule_B_axis = p_transform_b.basis.get_axis(1) * (capsule_B->get_height() * 0.5);
+	Vector3 capsule_A_axis = p_transform_a.basis.get_axis(1) * (capsule_A->get_height() * 0.5 - capsule_A->get_radius());
+	Vector3 capsule_B_axis = p_transform_b.basis.get_axis(1) * (capsule_B->get_height() * 0.5 - capsule_B->get_radius());
 
 	Vector3 capsule_A_ball_1 = p_transform_a.origin + capsule_A_axis;
 	Vector3 capsule_A_ball_2 = p_transform_a.origin - capsule_A_axis;
@@ -1673,8 +1679,8 @@ static void _collision_capsule_cylinder(const Shape3DSW *p_a, const Transform3D
 
 	Vector3 capsule_A_axis = p_transform_a.basis.get_axis(1);
 
-	Vector3 capsule_A_ball_1 = p_transform_a.origin + capsule_A_axis * (capsule_A->get_height() * 0.5);
-	Vector3 capsule_A_ball_2 = p_transform_a.origin - capsule_A_axis * (capsule_A->get_height() * 0.5);
+	Vector3 capsule_A_ball_1 = p_transform_a.origin + capsule_A_axis * (capsule_A->get_height() * 0.5 - capsule_A->get_radius());
+	Vector3 capsule_A_ball_2 = p_transform_a.origin - capsule_A_axis * (capsule_A->get_height() * 0.5 - capsule_A->get_radius());
 
 	if (!separator.test_axis((p_transform_b.origin - capsule_A_ball_1).cross(cylinder_B_axis).cross(cylinder_B_axis).normalized())) {
 		return;
@@ -1733,9 +1739,12 @@ static void _collision_capsule_convex_polygon(const Shape3DSW *p_a, const Transf
 	int edge_count = mesh.edges.size();
 	const Vector3 *vertices = mesh.vertices.ptr();
 
+	// Precalculating this makes the transforms faster.
+	Basis b_xform_normal = p_transform_b.basis.inverse().transposed();
+
 	// faces of B
 	for (int i = 0; i < face_count; i++) {
-		Vector3 axis = p_transform_b.xform(faces[i].plane).normal;
+		Vector3 axis = b_xform_normal.xform(faces[i].plane.normal).normalized();
 
 		if (!separator.test_axis(axis)) {
 			return;
@@ -1759,7 +1768,7 @@ static void _collision_capsule_convex_polygon(const Shape3DSW *p_a, const Transf
 	for (int i = 0; i < 2; i++) {
 		// edges of B, capsule cylinder
 
-		Vector3 capsule_axis = p_transform_a.basis.get_axis(1) * (capsule_A->get_height() * 0.5);
+		Vector3 capsule_axis = p_transform_a.basis.get_axis(1) * (capsule_A->get_height() * 0.5 - capsule_A->get_radius());
 
 		Vector3 sphere_pos = p_transform_a.origin + ((i == 0) ? capsule_axis : -capsule_axis);
 
@@ -1799,7 +1808,7 @@ static void _collision_capsule_face(const Shape3DSW *p_a, const Transform3D &p_t
 
 	// edges of B, capsule cylinder
 
-	Vector3 capsule_axis = p_transform_a.basis.get_axis(1) * (capsule_A->get_height() * 0.5);
+	Vector3 capsule_axis = p_transform_a.basis.get_axis(1) * (capsule_A->get_height() * 0.5 - capsule_A->get_radius());
 
 	for (int i = 0; i < 3; i++) {
 		// edge-cylinder
@@ -2057,20 +2066,24 @@ static void _collision_convex_polygon_convex_polygon(const Shape3DSW *p_a, const
 	const Vector3 *vertices_B = mesh_B.vertices.ptr();
 	int vertex_count_B = mesh_B.vertices.size();
 
+	// Precalculating this makes the transforms faster.
+	Basis a_xform_normal = p_transform_b.basis.inverse().transposed();
+
 	// faces of A
 	for (int i = 0; i < face_count_A; i++) {
-		Vector3 axis = p_transform_a.xform(faces_A[i].plane).normal;
-		//Vector3 axis = p_transform_a.basis.xform( faces_A[i].plane.normal ).normalized();
+		Vector3 axis = a_xform_normal.xform(faces_A[i].plane.normal).normalized();
 
 		if (!separator.test_axis(axis)) {
 			return;
 		}
 	}
 
+	// Precalculating this makes the transforms faster.
+	Basis b_xform_normal = p_transform_b.basis.inverse().transposed();
+
 	// faces of B
 	for (int i = 0; i < face_count_B; i++) {
-		Vector3 axis = p_transform_b.xform(faces_B[i].plane).normal;
-		//Vector3 axis = p_transform_b.basis.xform( faces_B[i].plane.normal ).normalized();
+		Vector3 axis = b_xform_normal.xform(faces_B[i].plane.normal).normalized();
 
 		if (!separator.test_axis(axis)) {
 			return;
@@ -2260,13 +2273,11 @@ bool sat_calculate_penetration(const Shape3DSW *p_shape_A, const Transform3D &p_
 	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_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_RAY, false);
 	ERR_FAIL_COND_V(p_shape_B->is_concave(), false);
 
 	static const CollisionFunc collision_table[6][6] = {
@@ -2371,10 +2382,10 @@ bool sat_calculate_penetration(const Shape3DSW *p_shape_A, const Transform3D &p_
 
 	CollisionFunc collision_func;
 	if (margin_A != 0.0 || margin_B != 0.0) {
-		collision_func = collision_table_margin[type_A - 2][type_B - 2];
+		collision_func = collision_table_margin[type_A - 1][type_B - 1];
 
 	} else {
-		collision_func = collision_table[type_A - 2][type_B - 2];
+		collision_func = collision_table[type_A - 1][type_B - 1];
 	}
 	ERR_FAIL_COND_V(!collision_func, false);
 
diff --git a/servers/physics_3d/collision_solver_3d_sw.cpp b/servers/physics_3d/collision_solver_3d_sw.cpp
index 67330d497e..dcecac1c73 100644
--- a/servers/physics_3d/collision_solver_3d_sw.cpp
+++ b/servers/physics_3d/collision_solver_3d_sw.cpp
@@ -89,39 +89,6 @@ bool CollisionSolver3DSW::solve_static_plane(const Shape3DSW *p_shape_A, const T
 	return found;
 }
 
-bool CollisionSolver3DSW::solve_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) {
-	const RayShape3DSW *ray = static_cast<const RayShape3DSW *>(p_shape_A);
-
-	Vector3 from = p_transform_A.origin;
-	Vector3 to = from + p_transform_A.basis.get_axis(2) * ray->get_length();
-	Vector3 support_A = to;
-
-	Transform3D ai = p_transform_B.affine_inverse();
-
-	from = ai.xform(from);
-	to = ai.xform(to);
-
-	Vector3 p, n;
-	if (!p_shape_B->intersect_segment(from, to, p, n)) {
-		return false;
-	}
-
-	Vector3 support_B = p_transform_B.xform(p);
-	if (ray->get_slips_on_slope()) {
-		Vector3 global_n = ai.basis.xform_inv(n).normalized();
-		support_B = support_A + (support_B - support_A).length() * global_n;
-	}
-
-	if (p_result_callback) {
-		if (p_swap_result) {
-			p_result_callback(support_B, 0, support_A, 0, p_userdata);
-		} else {
-			p_result_callback(support_A, 0, support_B, 0, p_userdata);
-		}
-	}
-	return true;
-}
-
 struct _SoftBodyContactCollisionInfo {
 	int node_index = 0;
 	CollisionSolver3DSW::CallbackResult result_callback = nullptr;
@@ -135,6 +102,10 @@ void CollisionSolver3DSW::soft_body_contact_callback(const Vector3 &p_point_A, i
 
 	++cinfo.contact_count;
 
+	if (!cinfo.result_callback) {
+		return;
+	}
+
 	if (cinfo.swap_result) {
 		cinfo.result_callback(p_point_B, cinfo.node_index, p_point_A, p_index_A, cinfo.userdata);
 	} else {
@@ -347,9 +318,6 @@ bool CollisionSolver3DSW::solve_static(const Shape3DSW *p_shape_A, const Transfo
 		if (type_B == PhysicsServer3D::SHAPE_PLANE) {
 			return false;
 		}
-		if (type_B == PhysicsServer3D::SHAPE_RAY) {
-			return false;
-		}
 		if (type_B == PhysicsServer3D::SHAPE_SOFT_BODY) {
 			return false;
 		}
@@ -360,17 +328,6 @@ bool CollisionSolver3DSW::solve_static(const Shape3DSW *p_shape_A, const Transfo
 			return solve_static_plane(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false);
 		}
 
-	} else if (type_A == PhysicsServer3D::SHAPE_RAY) {
-		if (type_B == PhysicsServer3D::SHAPE_RAY) {
-			return false;
-		}
-
-		if (swap) {
-			return solve_ray(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true);
-		} else {
-			return solve_ray(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false);
-		}
-
 	} else if (type_B == PhysicsServer3D::SHAPE_SOFT_BODY) {
 		if (type_A == PhysicsServer3D::SHAPE_SOFT_BODY) {
 			// Soft Body / Soft Body not supported.
diff --git a/servers/physics_3d/physics_server_3d_sw.cpp b/servers/physics_3d/physics_server_3d_sw.cpp
index c1a9d6259d..fbb374bd74 100644
--- a/servers/physics_3d/physics_server_3d_sw.cpp
+++ b/servers/physics_3d/physics_server_3d_sw.cpp
@@ -48,12 +48,6 @@ RID PhysicsServer3DSW::plane_shape_create() {
 	shape->set_self(rid);
 	return rid;
 }
-RID PhysicsServer3DSW::ray_shape_create() {
-	Shape3DSW *shape = memnew(RayShape3DSW);
-	RID rid = shape_owner.make_rid(shape);
-	shape->set_self(rid);
-	return rid;
-}
 RID PhysicsServer3DSW::sphere_shape_create() {
 	Shape3DSW *shape = memnew(SphereShape3DSW);
 	RID rid = shape_owner.make_rid(shape);
@@ -854,18 +848,7 @@ void PhysicsServer3DSW::body_set_ray_pickable(RID p_body, bool p_enable) {
 	body->set_ray_pickable(p_enable);
 }
 
-bool PhysicsServer3DSW::body_test_motion(RID p_body, const Transform3D &p_from, const Vector3 &p_motion, bool p_infinite_inertia, real_t p_margin, MotionResult *r_result, bool p_exclude_raycast_shapes) {
-	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_infinite_inertia, p_margin, r_result, p_exclude_raycast_shapes);
-}
-
-int PhysicsServer3DSW::body_test_ray_separation(RID p_body, const Transform3D &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, SeparationResult *r_results, int p_result_max, real_t p_margin) {
+bool PhysicsServer3DSW::body_test_motion(RID p_body, const Transform3D &p_from, const Vector3 &p_motion, real_t p_margin, MotionResult *r_result, 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);
@@ -873,7 +856,7 @@ int PhysicsServer3DSW::body_test_ray_separation(RID p_body, const Transform3D &p
 
 	_update_shapes();
 
-	return body->get_space()->test_body_ray_separation(body, p_transform, p_infinite_inertia, r_recover_motion, r_results, p_result_max, p_margin);
+	return body->get_space()->test_body_motion(body, p_from, p_motion, p_margin, r_result, p_exclude);
 }
 
 PhysicsDirectBodyState3D *PhysicsServer3DSW::body_get_direct_state(RID p_body) {
diff --git a/servers/physics_3d/physics_server_3d_sw.h b/servers/physics_3d/physics_server_3d_sw.h
index 0ccd15fbb2..6e59a77e89 100644
--- a/servers/physics_3d/physics_server_3d_sw.h
+++ b/servers/physics_3d/physics_server_3d_sw.h
@@ -83,7 +83,6 @@ 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 ray_shape_create() override;
 	virtual RID sphere_shape_create() override;
 	virtual RID box_shape_create() override;
 	virtual RID capsule_shape_create() override;
@@ -242,8 +241,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, bool p_infinite_inertia, real_t p_margin = 0.001, MotionResult *r_result = nullptr, bool p_exclude_raycast_shapes = true) override;
-	virtual int body_test_ray_separation(RID p_body, const Transform3D &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, SeparationResult *r_results, int p_result_max, real_t p_margin = 0.001) 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, const Set<RID> &p_exclude = Set<RID>()) override;
 
 	// this function only works on physics process, errors and returns null otherwise
 	virtual PhysicsDirectBodyState3D *body_get_direct_state(RID p_body) override;
diff --git a/servers/physics_3d/physics_server_3d_wrap_mt.h b/servers/physics_3d/physics_server_3d_wrap_mt.h
index 9beec22bcd..75174628bf 100644
--- a/servers/physics_3d/physics_server_3d_wrap_mt.h
+++ b/servers/physics_3d/physics_server_3d_wrap_mt.h
@@ -79,7 +79,6 @@ public:
 
 	//FUNC1RID(shape,ShapeType); todo fix
 	FUNCRID(plane_shape)
-	FUNCRID(ray_shape)
 	FUNCRID(sphere_shape)
 	FUNCRID(box_shape)
 	FUNCRID(capsule_shape)
@@ -250,14 +249,9 @@ public:
 
 	FUNC2(body_set_ray_pickable, RID, bool);
 
-	bool body_test_motion(RID p_body, const Transform3D &p_from, const Vector3 &p_motion, bool p_infinite_inertia, real_t p_margin = 0.001, MotionResult *r_result = nullptr, bool p_exclude_raycast_shapes = true) override {
+	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, 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_infinite_inertia, p_margin, r_result, p_exclude_raycast_shapes);
-	}
-
-	int body_test_ray_separation(RID p_body, const Transform3D &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, SeparationResult *r_results, int p_result_max, real_t p_margin = 0.001) override {
-		ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), false);
-		return physics_3d_server->body_test_ray_separation(p_body, p_transform, p_infinite_inertia, r_recover_motion, r_results, p_result_max, p_margin);
+		return physics_3d_server->body_test_motion(p_body, p_from, p_motion, p_margin, r_result, p_exclude);
 	}
 
 	// this function only works on physics process, errors and returns null otherwise
diff --git a/servers/physics_3d/shape_3d_sw.cpp b/servers/physics_3d/shape_3d_sw.cpp
index 04a174f9c8..a84405de81 100644
--- a/servers/physics_3d/shape_3d_sw.cpp
+++ b/servers/physics_3d/shape_3d_sw.cpp
@@ -164,91 +164,6 @@ Variant PlaneShape3DSW::get_data() const {
 PlaneShape3DSW::PlaneShape3DSW() {
 }
 
-//
-
-real_t RayShape3DSW::get_length() const {
-	return length;
-}
-
-bool RayShape3DSW::get_slips_on_slope() const {
-	return slips_on_slope;
-}
-
-void RayShape3DSW::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 RayShape3DSW::get_support(const Vector3 &p_normal) const {
-	if (p_normal.z > 0) {
-		return Vector3(0, 0, length);
-	} else {
-		return Vector3(0, 0, 0);
-	}
-}
-
-void RayShape3DSW::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;
-		r_supports[0] = Vector3(0, 0, 0);
-		r_supports[1] = Vector3(0, 0, length);
-	} else if (p_normal.z > 0) {
-		r_amount = 1;
-		r_type = FEATURE_POINT;
-		*r_supports = Vector3(0, 0, length);
-	} else {
-		r_amount = 1;
-		r_type = FEATURE_POINT;
-		*r_supports = Vector3(0, 0, 0);
-	}
-}
-
-bool RayShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const {
-	return false; //simply not possible
-}
-
-bool RayShape3DSW::intersect_point(const Vector3 &p_point) const {
-	return false; //simply not possible
-}
-
-Vector3 RayShape3DSW::get_closest_point_to(const Vector3 &p_point) const {
-	Vector3 s[2] = {
-		Vector3(0, 0, 0),
-		Vector3(0, 0, length)
-	};
-
-	return Geometry3D::get_closest_point_to_segment(p_point, s);
-}
-
-Vector3 RayShape3DSW::get_moment_of_inertia(real_t p_mass) const {
-	return Vector3();
-}
-
-void RayShape3DSW::_setup(real_t p_length, bool p_slips_on_slope) {
-	length = p_length;
-	slips_on_slope = p_slips_on_slope;
-	configure(AABB(Vector3(0, 0, 0), Vector3(0.1, 0.1, length)));
-}
-
-void RayShape3DSW::set_data(const Variant &p_data) {
-	Dictionary d = p_data;
-	_setup(d["length"], d["slips_on_slope"]);
-}
-
-Variant RayShape3DSW::get_data() const {
-	Dictionary d;
-	d["length"] = length;
-	d["slips_on_slope"] = slips_on_slope;
-	return d;
-}
-
-RayShape3DSW::RayShape3DSW() {
-	length = 1;
-	slips_on_slope = false;
-}
-
 /********** SPHERE *************/
 
 real_t SphereShape3DSW::get_radius() const {
@@ -509,10 +424,10 @@ BoxShape3DSW::BoxShape3DSW() {
 
 void CapsuleShape3DSW::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 = (n.y > 0) ? height : -height;
+	real_t h = height * 0.5 - radius;
 
 	n *= radius;
-	n.y += h * 0.5;
+	n.y += (n.y > 0) ? h : -h;
 
 	r_max = p_normal.dot(p_transform.xform(n));
 	r_min = p_normal.dot(p_transform.xform(-n));
@@ -521,10 +436,10 @@ void CapsuleShape3DSW::project_range(const Vector3 &p_normal, const Transform3D
 Vector3 CapsuleShape3DSW::get_support(const Vector3 &p_normal) const {
 	Vector3 n = p_normal;
 
-	real_t h = (n.y > 0) ? height : -height;
+	real_t h = height * 0.5 - radius;
 
 	n *= radius;
-	n.y += h * 0.5;
+	n.y += (n.y > 0) ? h : -h;
 	return n;
 }
 
@@ -542,15 +457,15 @@ void CapsuleShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3
 		r_amount = 2;
 		r_type = FEATURE_EDGE;
 		r_supports[0] = n;
-		r_supports[0].y += height * 0.5;
+		r_supports[0].y += height * 0.5 - radius;
 		r_supports[1] = n;
-		r_supports[1].y -= height * 0.5;
+		r_supports[1].y -= height * 0.5 - radius;
 
 	} else {
-		real_t h = (d > 0) ? height : -height;
+		real_t h = height * 0.5 - radius;
 
 		n *= radius;
-		n.y += h * 0.5;
+		n.y += (d > 0) ? h : -h;
 		r_amount = 1;
 		r_type = FEATURE_POINT;
 		*r_supports = n;
@@ -569,7 +484,7 @@ bool CapsuleShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &
 
 	// test against cylinder and spheres :-|
 
-	collided = Geometry3D::segment_intersects_cylinder(p_begin, p_end, height, radius, &auxres, &auxn, 1);
+	collided = Geometry3D::segment_intersects_cylinder(p_begin, p_end, height - radius * 2.0, radius, &auxres, &auxn, 1);
 
 	if (collided) {
 		real_t d = norm.dot(auxres);
@@ -581,7 +496,7 @@ bool CapsuleShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &
 		}
 	}
 
-	collided = Geometry3D::segment_intersects_sphere(p_begin, p_end, Vector3(0, height * 0.5, 0), radius, &auxres, &auxn);
+	collided = Geometry3D::segment_intersects_sphere(p_begin, p_end, Vector3(0, height * 0.5 - radius, 0), radius, &auxres, &auxn);
 
 	if (collided) {
 		real_t d = norm.dot(auxres);
@@ -593,7 +508,7 @@ bool CapsuleShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &
 		}
 	}
 
-	collided = Geometry3D::segment_intersects_sphere(p_begin, p_end, Vector3(0, height * -0.5, 0), radius, &auxres, &auxn);
+	collided = Geometry3D::segment_intersects_sphere(p_begin, p_end, Vector3(0, height * -0.5 + radius, 0), radius, &auxres, &auxn);
 
 	if (collided) {
 		real_t d = norm.dot(auxres);
@@ -614,19 +529,19 @@ bool CapsuleShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &
 }
 
 bool CapsuleShape3DSW::intersect_point(const Vector3 &p_point) const {
-	if (Math::abs(p_point.y) < height * 0.5) {
+	if (Math::abs(p_point.y) < height * 0.5 - radius) {
 		return Vector3(p_point.x, 0, p_point.z).length() < radius;
 	} else {
 		Vector3 p = p_point;
-		p.y = Math::abs(p.y) - height * 0.5;
+		p.y = Math::abs(p.y) - height * 0.5 + radius;
 		return p.length() < radius;
 	}
 }
 
 Vector3 CapsuleShape3DSW::get_closest_point_to(const Vector3 &p_point) const {
 	Vector3 s[2] = {
-		Vector3(0, -height * 0.5, 0),
-		Vector3(0, height * 0.5, 0),
+		Vector3(0, -height * 0.5 + radius, 0),
+		Vector3(0, height * 0.5 - radius, 0),
 	};
 
 	Vector3 p = Geometry3D::get_closest_point_to_segment(p_point, s);
@@ -651,7 +566,7 @@ Vector3 CapsuleShape3DSW::get_moment_of_inertia(real_t p_mass) const {
 void CapsuleShape3DSW::_setup(real_t p_height, real_t p_radius) {
 	height = p_height;
 	radius = p_radius;
-	configure(AABB(Vector3(-radius, -height * 0.5 - radius, -radius), Vector3(radius * 2, height + radius * 2.0, radius * 2)));
+	configure(AABB(Vector3(-radius, -height * 0.5, -radius), Vector3(radius * 2, height, radius * 2)));
 }
 
 void CapsuleShape3DSW::set_data(const Variant &p_data) {
@@ -1635,7 +1550,7 @@ ConcavePolygonShape3DSW::ConcavePolygonShape3DSW() {
 
 /* HEIGHT MAP SHAPE */
 
-Vector<float> HeightMapShape3DSW::get_heights() const {
+Vector<real_t> HeightMapShape3DSW::get_heights() const {
 	return heights;
 }
 
@@ -1932,7 +1847,7 @@ 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<float> &p_heights, int p_width, int p_depth, real_t p_min_height, real_t p_max_height) {
+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) {
 	heights = p_heights;
 	width = p_width;
 	depth = p_depth;
@@ -1966,8 +1881,12 @@ void HeightMapShape3DSW::set_data(const Variant &p_data) {
 	ERR_FAIL_COND(depth <= 0.0);
 
 	Variant heights_variant = d["heights"];
-	Vector<float> heights_buffer;
+	Vector<real_t> heights_buffer;
+#ifdef REAL_T_IS_DOUBLE
+	if (heights_variant.get_type() == Variant::PACKED_FLOAT64_ARRAY) {
+#else
 	if (heights_variant.get_type() == Variant::PACKED_FLOAT32_ARRAY) {
+#endif
 		// Ready-to-use heights can be passed.
 		heights_buffer = heights_variant;
 	} else if (heights_variant.get_type() == Variant::OBJECT) {
@@ -1980,13 +1899,17 @@ void HeightMapShape3DSW::set_data(const Variant &p_data) {
 		PackedByteArray im_data = image->get_data();
 		heights_buffer.resize(image->get_width() * image->get_height());
 
-		float *w = heights_buffer.ptrw();
-		float *rp = (float *)im_data.ptr();
+		real_t *w = heights_buffer.ptrw();
+		real_t *rp = (real_t *)im_data.ptr();
 		for (int i = 0; i < heights_buffer.size(); ++i) {
 			w[i] = rp[i];
 		}
 	} else {
+#ifdef REAL_T_IS_DOUBLE
+		ERR_FAIL_MSG("Expected PackedFloat64Array or float Image.");
+#else
 		ERR_FAIL_MSG("Expected PackedFloat32Array or float Image.");
+#endif
 	}
 
 	// Compute min and max heights or use precomputed values.
diff --git a/servers/physics_3d/shape_3d_sw.h b/servers/physics_3d/shape_3d_sw.h
index 0d1b7cc3d7..c11c3a08f6 100644
--- a/servers/physics_3d/shape_3d_sw.h
+++ b/servers/physics_3d/shape_3d_sw.h
@@ -145,34 +145,6 @@ public:
 	PlaneShape3DSW();
 };
 
-class RayShape3DSW : public Shape3DSW {
-	real_t length;
-	bool slips_on_slope;
-
-	void _setup(real_t p_length, bool p_slips_on_slope);
-
-public:
-	real_t get_length() const;
-	bool get_slips_on_slope() const;
-
-	virtual real_t get_area() const { return 0.0; }
-	virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_RAY; }
-	virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const;
-	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;
-
-	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;
-
-	virtual void set_data(const Variant &p_data);
-	virtual Variant get_data() const;
-
-	RayShape3DSW();
-};
-
 class SphereShape3DSW : public Shape3DSW {
 	real_t radius;
 
@@ -235,7 +207,7 @@ 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 { return 4.0 / 3.0 * Math_PI * radius * radius * radius + height * Math_PI * radius * radius; }
+	virtual real_t get_area() const { return 4.0 / 3.0 * Math_PI * radius * radius * radius + (height - radius * 2.0) * Math_PI * radius * radius; }
 
 	virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_CAPSULE; }
 
@@ -389,12 +361,12 @@ public:
 };
 
 struct HeightMapShape3DSW : public ConcaveShape3DSW {
-	Vector<float> heights;
+	Vector<real_t> heights;
 	int width = 0;
 	int depth = 0;
 	Vector3 local_origin;
 
-	_FORCE_INLINE_ float _get_height(int p_x, int p_z) const {
+	_FORCE_INLINE_ real_t _get_height(int p_x, int p_z) const {
 		return heights[(p_z * width) + p_x];
 	}
 
@@ -406,10 +378,10 @@ struct HeightMapShape3DSW : public ConcaveShape3DSW {
 
 	void _get_cell(const Vector3 &p_point, int &r_x, int &r_y, int &r_z) const;
 
-	void _setup(const Vector<float> &p_heights, int p_width, int p_depth, real_t p_min_height, real_t p_max_height);
+	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:
-	Vector<float> get_heights() const;
+	Vector<real_t> get_heights() const;
 	int get_width() const;
 	int get_depth() const;
 
diff --git a/servers/physics_3d/soft_body_3d_sw.cpp b/servers/physics_3d/soft_body_3d_sw.cpp
index 724125bea8..73b81444e1 100644
--- a/servers/physics_3d/soft_body_3d_sw.cpp
+++ b/servers/physics_3d/soft_body_3d_sw.cpp
@@ -928,6 +928,19 @@ void SoftBody3DSW::apply_forces() {
 	}
 }
 
+void SoftBody3DSW::_compute_area_gravity(const Area3DSW *p_area) {
+	if (p_area->is_gravity_point()) {
+		if (p_area->get_gravity_distance_scale() > 0) {
+			Vector3 v = p_area->get_transform().xform(p_area->get_gravity_vector()) - get_transform().get_origin();
+			gravity += v.normalized() * (p_area->get_gravity() / Math::pow(v.length() * p_area->get_gravity_distance_scale() + 1, 2));
+		} else {
+			gravity += (p_area->get_transform().xform(p_area->get_gravity_vector()) - get_transform().get_origin()).normalized() * p_area->get_gravity();
+		}
+	} else {
+		gravity += p_area->get_gravity_vector() * p_area->get_gravity();
+	}
+}
+
 void SoftBody3DSW::predict_motion(real_t p_delta) {
 	const real_t inv_delta = 1.0 / p_delta;
 
@@ -935,9 +948,35 @@ void SoftBody3DSW::predict_motion(real_t p_delta) {
 
 	Area3DSW *def_area = get_space()->get_default_area();
 	ERR_FAIL_COND(!def_area);
+	gravity = def_area->get_gravity_vector() * def_area->get_gravity();
+
+	int ac = areas.size();
+	bool stopped = false;
+
+	if (ac) {
+		areas.sort();
+		const AreaCMP *aa = &areas[0];
+		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(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: {
+				}
+			}
+		}
+	}
 
 	// Apply forces.
-	Vector3 gravity = def_area->get_gravity_vector() * def_area->get_gravity();
 	add_velocity(gravity * p_delta);
 	apply_forces();
 
diff --git a/servers/physics_3d/soft_body_3d_sw.h b/servers/physics_3d/soft_body_3d_sw.h
index ac8bcbf0b9..be0620c506 100644
--- a/servers/physics_3d/soft_body_3d_sw.h
+++ b/servers/physics_3d/soft_body_3d_sw.h
@@ -31,6 +31,7 @@
 #ifndef SOFT_BODY_3D_SW_H
 #define SOFT_BODY_3D_SW_H
 
+#include "area_3d_sw.h"
 #include "collision_object_3d_sw.h"
 
 #include "core/math/aabb.h"
@@ -100,14 +101,20 @@ class SoftBody3DSW : public CollisionObject3DSW {
 	real_t drag_coefficient = 0.0; // [0,1]
 	LocalVector<int> pinned_vertices;
 
+	Vector3 gravity;
+
 	SelfList<SoftBody3DSW> active_list;
 
 	Set<Constraint3DSW *> constraints;
 
+	Vector<AreaCMP> areas;
+
 	VSet<RID> exceptions;
 
 	uint64_t island_step = 0;
 
+	_FORCE_INLINE_ void _compute_area_gravity(const Area3DSW *p_area);
+
 public:
 	SoftBody3DSW();
 
@@ -129,6 +136,25 @@ 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) {
+		int index = areas.find(AreaCMP(p_area));
+		if (index > -1) {
+			areas.write[index].refCount += 1;
+		} else {
+			areas.ordered_insert(AreaCMP(p_area));
+		}
+	}
+
+	_FORCE_INLINE_ void remove_area(Area3DSW *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);
+			}
+		}
+	}
+
 	virtual void set_space(Space3DSW *p_space);
 
 	void set_mesh(const Ref<Mesh> &p_mesh);
diff --git a/servers/physics_3d/space_3d_sw.cpp b/servers/physics_3d/space_3d_sw.cpp
index bc149648d7..f9e55ad525 100644
--- a/servers/physics_3d/space_3d_sw.cpp
+++ b/servers/physics_3d/space_3d_sw.cpp
@@ -549,7 +549,7 @@ int Space3DSW::_cull_aabb_for_body(Body3DSW *p_body, const AABB &p_aabb) {
 			keep = false;
 		} else if (intersection_query_results[i]->get_type() == CollisionObject3DSW::TYPE_SOFT_BODY) {
 			keep = false;
-		} else if (!p_body->layer_in_mask(static_cast<Body3DSW *>(intersection_query_results[i]))) {
+		} else if (!p_body->collides_with(static_cast<Body3DSW *>(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())) {
 			keep = false;
@@ -569,158 +569,7 @@ int Space3DSW::_cull_aabb_for_body(Body3DSW *p_body, const AABB &p_aabb) {
 	return amount;
 }
 
-int Space3DSW::test_body_ray_separation(Body3DSW *p_body, const Transform3D &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, PhysicsServer3D::SeparationResult *r_results, int p_result_max, real_t p_margin) {
-	AABB body_aabb;
-
-	bool shapes_found = false;
-
-	for (int i = 0; i < p_body->get_shape_count(); i++) {
-		if (p_body->is_shape_disabled(i)) {
-			continue;
-		}
-
-		if (!shapes_found) {
-			body_aabb = p_body->get_shape_aabb(i);
-			shapes_found = true;
-		} else {
-			body_aabb = body_aabb.merge(p_body->get_shape_aabb(i));
-		}
-	}
-
-	if (!shapes_found) {
-		return 0;
-	}
-	// Undo the currently transform the physics server is aware of and apply the provided one
-	body_aabb = p_transform.xform(p_body->get_inv_transform().xform(body_aabb));
-	body_aabb = body_aabb.grow(p_margin);
-
-	Transform3D body_transform = p_transform;
-
-	for (int i = 0; i < p_result_max; i++) {
-		//reset results
-		r_results[i].collision_depth = 0;
-	}
-
-	int rays_found = 0;
-
-	{
-		// raycast AND separate
-
-		const int max_results = 32;
-		int recover_attempts = 4;
-		Vector3 sr[max_results * 2];
-		PhysicsServer3DSW::CollCbkData cbk;
-		cbk.max = max_results;
-		PhysicsServer3DSW::CollCbkData *cbkptr = &cbk;
-		CollisionSolver3DSW::CallbackResult cbkres = PhysicsServer3DSW::_shape_col_cbk;
-
-		do {
-			Vector3 recover_motion;
-
-			bool collided = false;
-
-			int amount = _cull_aabb_for_body(p_body, body_aabb);
-
-			for (int j = 0; j < p_body->get_shape_count(); j++) {
-				if (p_body->is_shape_disabled(j)) {
-					continue;
-				}
-
-				Shape3DSW *body_shape = p_body->get_shape(j);
-
-				if (body_shape->get_type() != PhysicsServer3D::SHAPE_RAY) {
-					continue;
-				}
-
-				Transform3D body_shape_xform = body_transform * p_body->get_shape_transform(j);
-
-				for (int i = 0; i < amount; i++) {
-					const CollisionObject3DSW *col_obj = intersection_query_results[i];
-					int shape_idx = intersection_query_subindex_results[i];
-
-					cbk.amount = 0;
-					cbk.ptr = sr;
-
-					if (CollisionObject3DSW::TYPE_BODY == col_obj->get_type()) {
-						const Body3DSW *b = static_cast<const Body3DSW *>(col_obj);
-						if (p_infinite_inertia && PhysicsServer3D::BODY_MODE_STATIC != b->get_mode() && PhysicsServer3D::BODY_MODE_KINEMATIC != b->get_mode()) {
-							continue;
-						}
-					}
-
-					Shape3DSW *against_shape = col_obj->get_shape(shape_idx);
-					if (CollisionSolver3DSW::solve_static(body_shape, body_shape_xform, against_shape, col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), cbkres, cbkptr, nullptr, p_margin)) {
-						if (cbk.amount > 0) {
-							collided = true;
-						}
-
-						int ray_index = -1; //reuse shape
-						for (int k = 0; k < rays_found; k++) {
-							if (r_results[k].collision_local_shape == j) {
-								ray_index = k;
-							}
-						}
-
-						if (ray_index == -1 && rays_found < p_result_max) {
-							ray_index = rays_found;
-							rays_found++;
-						}
-
-						if (ray_index != -1) {
-							PhysicsServer3D::SeparationResult &result = r_results[ray_index];
-
-							for (int k = 0; k < cbk.amount; k++) {
-								Vector3 a = sr[k * 2 + 0];
-								Vector3 b = sr[k * 2 + 1];
-
-								recover_motion += (b - a) / cbk.amount;
-
-								real_t depth = a.distance_to(b);
-								if (depth > result.collision_depth) {
-									result.collision_depth = depth;
-									result.collision_point = b;
-									result.collision_normal = (b - a).normalized();
-									result.collision_local_shape = j;
-									result.collider = col_obj->get_self();
-									result.collider_id = col_obj->get_instance_id();
-									result.collider_shape = shape_idx;
-									//result.collider_metadata = col_obj->get_shape_metadata(shape_idx);
-									if (col_obj->get_type() == CollisionObject3DSW::TYPE_BODY) {
-										Body3DSW *body = (Body3DSW *)col_obj;
-										Vector3 rel_vec = b - (body->get_transform().origin + body->get_center_of_mass());
-										result.collider_velocity = body->get_linear_velocity() + (body->get_angular_velocity()).cross(rel_vec);
-									}
-								}
-							}
-						}
-					}
-				}
-			}
-
-			if (!collided || recover_motion == Vector3()) {
-				break;
-			}
-
-			body_transform.origin += recover_motion;
-			body_aabb.position += recover_motion;
-
-			recover_attempts--;
-		} while (recover_attempts);
-	}
-
-	//optimize results (remove non colliding)
-	for (int i = 0; i < rays_found; i++) {
-		if (r_results[i].collision_depth == 0) {
-			rays_found--;
-			SWAP(r_results[i], r_results[rays_found]);
-		}
-	}
-
-	r_recover_motion = body_transform.origin - p_transform.origin;
-	return rays_found;
-}
-
-bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, const Vector3 &p_motion, bool p_infinite_inertia, real_t p_margin, PhysicsServer3D::MotionResult *r_result, bool p_exclude_raycast_shapes) {
+bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, const Vector3 &p_motion, real_t p_margin, PhysicsServer3D::MotionResult *r_result, const Set<RID> &p_exclude) {
 	//give me back regular physics engine logic
 	//this is madness
 	//and most people using this function will think
@@ -751,7 +600,7 @@ 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->motion = p_motion;
+			r_result->travel = p_motion;
 		}
 
 		return false;
@@ -795,21 +644,15 @@ 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);
-				if (p_exclude_raycast_shapes && body_shape->get_type() == PhysicsServer3D::SHAPE_RAY) {
-					continue;
-				}
 
 				for (int i = 0; i < amount; i++) {
 					const CollisionObject3DSW *col_obj = intersection_query_results[i];
-					int shape_idx = intersection_query_subindex_results[i];
-
-					if (CollisionObject3DSW::TYPE_BODY == col_obj->get_type()) {
-						const Body3DSW *b = static_cast<const Body3DSW *>(col_obj);
-						if (p_infinite_inertia && PhysicsServer3D::BODY_MODE_STATIC != b->get_mode() && PhysicsServer3D::BODY_MODE_KINEMATIC != b->get_mode()) {
-							continue;
-						}
+					if (p_exclude.has(col_obj->get_self())) {
+						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)) {
 						collided = cbk.amount > 0;
 					}
@@ -874,10 +717,6 @@ 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);
 
-			if (p_exclude_raycast_shapes && body_shape->get_type() == PhysicsServer3D::SHAPE_RAY) {
-				continue;
-			}
-
 			Transform3D body_shape_xform_inv = body_shape_xform.affine_inverse();
 			MotionShape3DSW mshape;
 			mshape.shape = body_shape;
@@ -890,15 +729,12 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, co
 
 			for (int i = 0; i < amount; i++) {
 				const CollisionObject3DSW *col_obj = intersection_query_results[i];
-				int shape_idx = intersection_query_subindex_results[i];
-
-				if (CollisionObject3DSW::TYPE_BODY == col_obj->get_type()) {
-					const Body3DSW *b = static_cast<const Body3DSW *>(col_obj);
-					if (p_infinite_inertia && PhysicsServer3D::BODY_MODE_STATIC != b->get_mode() && PhysicsServer3D::BODY_MODE_KINEMATIC != b->get_mode()) {
-						continue;
-					}
+				if (p_exclude.has(col_obj->get_self())) {
+					continue;
 				}
 
+				int shape_idx = intersection_query_subindex_results[i];
+
 				//test initial overlap, does it collide if going all the way?
 				Vector3 point_A, point_B;
 				Vector3 sep_axis = motion_normal;
@@ -1005,24 +841,16 @@ 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);
 
-			if (p_exclude_raycast_shapes && body_shape->get_type() == PhysicsServer3D::SHAPE_RAY) {
-				continue;
-			}
-
 			body_aabb.position += p_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];
-				int shape_idx = intersection_query_subindex_results[i];
-
-				if (CollisionObject3DSW::TYPE_BODY == col_obj->get_type()) {
-					const Body3DSW *b = static_cast<const Body3DSW *>(col_obj);
-					if (p_infinite_inertia && PhysicsServer3D::BODY_MODE_STATIC != b->get_mode() && PhysicsServer3D::BODY_MODE_KINEMATIC != b->get_mode()) {
-						continue;
-					}
+				if (p_exclude.has(col_obj->get_self())) {
+					continue;
 				}
+				int shape_idx = intersection_query_subindex_results[i];
 
 				rcd.object = col_obj;
 				rcd.shape = shape_idx;
@@ -1051,9 +879,9 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, co
 				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->motion = safe * p_motion;
+				r_result->travel = safe * p_motion;
 				r_result->remainder = p_motion - safe * p_motion;
-				r_result->motion += (body_transform.get_origin() - p_from.get_origin());
+				r_result->travel += (body_transform.get_origin() - p_from.get_origin());
 			}
 
 			collided = true;
@@ -1061,9 +889,9 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, co
 	}
 
 	if (!collided && r_result) {
-		r_result->motion = p_motion;
+		r_result->travel = p_motion;
 		r_result->remainder = Vector3();
-		r_result->motion += (body_transform.get_origin() - p_from.get_origin());
+		r_result->travel += (body_transform.get_origin() - p_from.get_origin());
 	}
 
 	return collided;
@@ -1093,7 +921,9 @@ void *Space3DSW::_broadphase_pair(CollisionObject3DSW *A, int p_subindex_A, Coll
 			Area2Pair3DSW *area2_pair = memnew(Area2Pair3DSW(area_b, p_subindex_B, area, p_subindex_A));
 			return area2_pair;
 		} else if (type_B == CollisionObject3DSW::TYPE_SOFT_BODY) {
-			// Area/Soft Body, not supported.
+			SoftBody3DSW *softbody = static_cast<SoftBody3DSW *>(B);
+			AreaSoftBodyPair3DSW *soft_area_pair = memnew(AreaSoftBodyPair3DSW(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));
diff --git a/servers/physics_3d/space_3d_sw.h b/servers/physics_3d/space_3d_sw.h
index 18e93c90cc..9b5b4de069 100644
--- a/servers/physics_3d/space_3d_sw.h
+++ b/servers/physics_3d/space_3d_sw.h
@@ -203,8 +203,7 @@ 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]; }
 
-	int test_body_ray_separation(Body3DSW *p_body, const Transform3D &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, PhysicsServer3D::SeparationResult *r_results, int p_result_max, real_t p_margin);
-	bool test_body_motion(Body3DSW *p_body, const Transform3D &p_from, const Vector3 &p_motion, bool p_infinite_inertia, real_t p_margin, PhysicsServer3D::MotionResult *r_result, bool p_exclude_raycast_shapes);
+	bool test_body_motion(Body3DSW *p_body, const Transform3D &p_from, const Vector3 &p_motion, real_t p_margin, PhysicsServer3D::MotionResult *r_result, const Set<RID> &p_exclude = Set<RID>());
 
 	Space3DSW();
 	~Space3DSW();