Merge pull request #46937 from nekomatata/soft-body-support

SoftBody support in GodotPhysics 3D

18 files changed, 2410 insertions, 122 deletions

diff --git a/servers/physics_3d/body_3d_sw.h b/servers/physics_3d/body_3d_sw.h
index 8e21003a5f..e87ff2364b 100644
--- a/servers/physics_3d/body_3d_sw.h
+++ b/servers/physics_3d/body_3d_sw.h
@@ -290,10 +290,10 @@ public:
 	void update_inertias();
 
 	_FORCE_INLINE_ real_t get_inv_mass() const { return _inv_mass; }
-	_FORCE_INLINE_ Vector3 get_inv_inertia() const { return _inv_inertia; }
-	_FORCE_INLINE_ Basis get_inv_inertia_tensor() const { return _inv_inertia_tensor; }
+	_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_ Vector3 get_gravity() const { return gravity; }
+	_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);
diff --git a/servers/physics_3d/body_pair_3d_sw.cpp b/servers/physics_3d/body_pair_3d_sw.cpp
index 6012ff1522..06bafb1624 100644
--- a/servers/physics_3d/body_pair_3d_sw.cpp
+++ b/servers/physics_3d/body_pair_3d_sw.cpp
@@ -49,12 +49,12 @@
 #define MIN_VELOCITY 0.0001
 #define MAX_BIAS_ROTATION (Math_PI / 8)
 
-void BodyPair3DSW::_contact_added_callback(const Vector3 &p_point_A, const Vector3 &p_point_B, void *p_userdata) {
+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;
-	pair->contact_added_callback(p_point_A, p_point_B);
+	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, const Vector3 &p_point_B) {
+void BodyPair3DSW::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);
@@ -73,6 +73,8 @@ void BodyPair3DSW::contact_added_callback(const Vector3 &p_point_A, const Vector
 	contact.acc_bias_impulse = 0;
 	contact.acc_bias_impulse_center_of_mass = 0;
 	contact.acc_tangent_impulse = Vector3();
+	contact.index_A = p_index_A;
+	contact.index_B = p_index_B;
 	contact.local_A = local_A;
 	contact.local_B = local_B;
 	contact.normal = (p_point_A - p_point_B).normalized();
@@ -456,7 +458,7 @@ void BodyPair3DSW::solve(real_t p_step) {
 }
 
 BodyPair3DSW::BodyPair3DSW(Body3DSW *p_A, int p_shape_A, Body3DSW *p_B, int p_shape_B) :
-		Constraint3DSW(_arr, 2) {
+		BodyContact3DSW(_arr, 2) {
 	A = p_A;
 	B = p_B;
 	shape_A = p_shape_A;
@@ -472,3 +474,305 @@ BodyPair3DSW::~BodyPair3DSW() {
 	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;
+	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) {
+	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);
+
+	Contact contact;
+	contact.index_A = p_index_A;
+	contact.index_B = p_index_B;
+	contact.acc_normal_impulse = 0;
+	contact.acc_bias_impulse = 0;
+	contact.acc_bias_impulse_center_of_mass = 0;
+	contact.acc_tangent_impulse = Vector3();
+	contact.local_A = local_A;
+	contact.local_B = local_B;
+	contact.normal = (p_point_A - p_point_B).normalized();
+	contact.mass_normal = 0;
+
+	// Attempt to determine if the contact will be reused.
+	real_t contact_recycle_radius = space->get_contact_recycle_radius();
+
+	uint32_t contact_count = contacts.size();
+	for (uint32_t contact_index = 0; contact_index < contact_count; ++contact_index) {
+		Contact &c = contacts[contact_index];
+		if (c.index_B == p_index_B) {
+			if (c.local_A.distance_squared_to(local_A) < (contact_recycle_radius * contact_recycle_radius) &&
+					c.local_B.distance_squared_to(local_B) < (contact_recycle_radius * contact_recycle_radius)) {
+				contact.acc_normal_impulse = c.acc_normal_impulse;
+				contact.acc_bias_impulse = c.acc_bias_impulse;
+				contact.acc_bias_impulse_center_of_mass = c.acc_bias_impulse_center_of_mass;
+				contact.acc_tangent_impulse = c.acc_tangent_impulse;
+			}
+			c = contact;
+			return;
+		}
+	}
+
+	contacts.push_back(contact);
+}
+
+void BodySoftBodyPair3DSW::validate_contacts() {
+	// Make sure to erase contacts that are no longer valid.
+	const Transform &transform_A = body->get_transform();
+
+	real_t contact_max_separation = space->get_contact_max_separation();
+
+	uint32_t contact_count = contacts.size();
+	for (uint32_t contact_index = 0; contact_index < contact_count; ++contact_index) {
+		Contact &c = contacts[contact_index];
+
+		Vector3 global_A = transform_A.xform(c.local_A);
+		Vector3 global_B = soft_body->get_node_position(c.index_B) + c.local_B;
+		Vector3 axis = global_A - global_B;
+		real_t depth = axis.dot(c.normal);
+
+		if (depth < -contact_max_separation || (global_B + c.normal * depth - global_A).length() > contact_max_separation) {
+			// Contact no longer needed, remove.
+			if ((contact_index + 1) < contact_count) {
+				// Swap with the last one.
+				SWAP(c, contacts[contact_count - 1]);
+			}
+
+			contact_index--;
+			contact_count--;
+		}
+	}
+
+	contacts.resize(contact_count);
+}
+
+bool BodySoftBodyPair3DSW::setup(real_t p_step) {
+	if (!body->test_collision_mask(soft_body) || body->has_exception(soft_body->get_self()) || soft_body->has_exception(body->get_self())) {
+		collided = false;
+		return false;
+	}
+
+	if (body->is_shape_set_as_disabled(body_shape)) {
+		collided = false;
+		return false;
+	}
+
+	const Transform &xform_Au = body->get_transform();
+	Transform xform_A = xform_Au * body->get_shape_transform(body_shape);
+
+	Transform xform_Bu = soft_body->get_transform();
+	Transform xform_B = xform_Bu * soft_body->get_shape_transform(0);
+
+	validate_contacts();
+
+	Shape3DSW *shape_A_ptr = body->get_shape(body_shape);
+	Shape3DSW *shape_B_ptr = soft_body->get_shape(0);
+
+	bool collided = CollisionSolver3DSW::solve_static(shape_A_ptr, xform_A, shape_B_ptr, xform_B, _contact_added_callback, this, &sep_axis);
+	this->collided = collided;
+
+	real_t max_penetration = space->get_contact_max_allowed_penetration();
+
+	real_t bias = (real_t)0.3;
+	if (shape_A_ptr->get_custom_bias()) {
+		bias = shape_A_ptr->get_custom_bias();
+	}
+
+	real_t inv_dt = 1.0 / p_step;
+
+	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) {
+			continue;
+		}
+
+		Vector3 global_A = xform_Au.xform(c.local_A);
+		Vector3 global_B = soft_body->get_node_position(c.index_B) + c.local_B;
+
+		real_t depth = c.normal.dot(global_A - global_B);
+
+		if (depth <= 0) {
+			continue;
+		}
+
+		c.active = true;
+
+#ifdef DEBUG_ENABLED
+
+		if (space->is_debugging_contacts()) {
+			space->add_debug_contact(global_A);
+			space->add_debug_contact(global_B);
+		}
+#endif
+
+		c.rA = global_A - xform_Au.origin - body->get_center_of_mass();
+		c.rB = global_B;
+
+		if (body->can_report_contacts()) {
+			Vector3 crA = body->get_angular_velocity().cross(c.rA) + body->get_linear_velocity();
+			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->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC) {
+			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;
+		kNormal += c.normal.dot(inertia_A.cross(c.rA));
+		c.mass_normal = 1.0f / kNormal;
+
+		c.bias = -bias * inv_dt * MIN(0.0f, -depth + max_penetration);
+		c.depth = depth;
+
+		Vector3 j_vec = c.normal * c.acc_normal_impulse + c.acc_tangent_impulse;
+		body->apply_impulse(c.rA + body->get_center_of_mass(), -j_vec);
+		soft_body->apply_node_impulse(c.index_B, j_vec);
+		c.acc_bias_impulse = 0;
+		c.acc_bias_impulse_center_of_mass = 0;
+
+		c.bounce = body->get_bounce();
+
+		if (c.bounce) {
+			Vector3 crA = body->get_angular_velocity().cross(c.rA);
+			Vector3 dv = soft_body->get_node_velocity(c.index_B) - body->get_linear_velocity() - crA;
+
+			// Normal impulse.
+			c.bounce = c.bounce * dv.dot(c.normal);
+		}
+	}
+
+	return true;
+}
+
+void BodySoftBodyPair3DSW::solve(real_t p_step) {
+	if (!collided) {
+		return;
+	}
+
+	uint32_t contact_count = contacts.size();
+	for (uint32_t contact_index = 0; contact_index < contact_count; ++contact_index) {
+		Contact &c = contacts[contact_index];
+		if (!c.active) {
+			continue;
+		}
+
+		c.active = false;
+
+		// 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;
+
+		real_t vbn = dbv.dot(c.normal);
+
+		if (Math::abs(-vbn + c.bias) > MIN_VELOCITY) {
+			real_t jbn = (-vbn + c.bias) * c.mass_normal;
+			real_t jbnOld = c.acc_bias_impulse;
+			c.acc_bias_impulse = MAX(jbnOld + jbn, 0.0f);
+
+			Vector3 jb = c.normal * (c.acc_bias_impulse - jbnOld);
+
+			body->apply_bias_impulse(c.rA + body->get_center_of_mass(), -jb, MAX_BIAS_ROTATION / p_step);
+			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;
+
+			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 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);
+
+				body->apply_bias_impulse(body->get_center_of_mass(), -jb_com, 0.0f);
+				soft_body->apply_node_bias_impulse(c.index_B, -jb_com);
+			}
+
+			c.active = true;
+		}
+
+		Vector3 crA = body->get_angular_velocity().cross(c.rA);
+		Vector3 dv = soft_body->get_node_velocity(c.index_B) - body->get_linear_velocity() - crA;
+
+		// Normal impulse.
+		real_t vn = dv.dot(c.normal);
+
+		if (Math::abs(vn) > MIN_VELOCITY) {
+			real_t jn = -(c.bounce + vn) * c.mass_normal;
+			real_t jnOld = c.acc_normal_impulse;
+			c.acc_normal_impulse = MAX(jnOld + jn, 0.0f);
+
+			Vector3 j = c.normal * (c.acc_normal_impulse - jnOld);
+
+			body->apply_impulse(c.rA + body->get_center_of_mass(), -j);
+			soft_body->apply_node_impulse(c.index_B, j);
+
+			c.active = true;
+		}
+
+		// Friction impulse.
+		real_t friction = body->get_friction();
+
+		Vector3 lvA = body->get_linear_velocity() + body->get_angular_velocity().cross(c.rA);
+		Vector3 lvB = soft_body->get_node_velocity(c.index_B);
+		Vector3 dtv = lvB - lvA;
+
+		real_t tn = c.normal.dot(dtv);
+
+		// Tangential velocity.
+		Vector3 tv = dtv - c.normal * tn;
+		real_t tvl = tv.length();
+
+		if (tvl > MIN_VELOCITY) {
+			tv /= tvl;
+
+			Vector3 temp1 = body->get_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)));
+
+			Vector3 jt = t * tv;
+
+			Vector3 jtOld = c.acc_tangent_impulse;
+			c.acc_tangent_impulse += jt;
+
+			real_t fi_len = c.acc_tangent_impulse.length();
+			real_t jtMax = c.acc_normal_impulse * friction;
+
+			if (fi_len > CMP_EPSILON && fi_len > jtMax) {
+				c.acc_tangent_impulse *= jtMax / fi_len;
+			}
+
+			jt = c.acc_tangent_impulse - jtOld;
+
+			body->apply_impulse(c.rA + body->get_center_of_mass(), -jt);
+			soft_body->apply_node_impulse(c.index_B, jt);
+
+			c.active = true;
+		}
+	}
+}
+
+BodySoftBodyPair3DSW::BodySoftBodyPair3DSW(Body3DSW *p_A, int p_shape_A, SoftBody3DSW *p_B) {
+	body = p_A;
+	soft_body = p_B;
+	body_shape = p_shape_A;
+	space = p_A->get_space();
+	body->add_constraint(this, 0);
+	soft_body->add_constraint(this);
+}
+
+BodySoftBodyPair3DSW::~BodySoftBodyPair3DSW() {
+	body->remove_constraint(this);
+	soft_body->remove_constraint(this);
+}
diff --git a/servers/physics_3d/body_pair_3d_sw.h b/servers/physics_3d/body_pair_3d_sw.h
index 4d049eafdc..74dddfa6aa 100644
--- a/servers/physics_3d/body_pair_3d_sw.h
+++ b/servers/physics_3d/body_pair_3d_sw.h
@@ -28,32 +28,20 @@
 /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
 /*************************************************************************/
 
-#ifndef BODY_PAIR_SW_H
-#define BODY_PAIR_SW_H
+#ifndef BODY_PAIR_3D_SW_H
+#define BODY_PAIR_3D_SW_H
 
 #include "body_3d_sw.h"
 #include "constraint_3d_sw.h"
+#include "core/templates/local_vector.h"
+#include "soft_body_3d_sw.h"
 
-class BodyPair3DSW : public Constraint3DSW {
-	enum {
-		MAX_CONTACTS = 4
-	};
-
-	union {
-		struct {
-			Body3DSW *A;
-			Body3DSW *B;
-		};
-
-		Body3DSW *_arr[2];
-	};
-
-	int shape_A;
-	int shape_B;
-
+class BodyContact3DSW : public Constraint3DSW {
+protected:
 	struct Contact {
 		Vector3 position;
 		Vector3 normal;
+		int index_A, index_B;
 		Vector3 local_A, local_B;
 		real_t acc_normal_impulse; // accumulated normal impulse (Pn)
 		Vector3 acc_tangent_impulse; // accumulated tangent impulse (Pt)
@@ -68,22 +56,45 @@ class BodyPair3DSW : public Constraint3DSW {
 		Vector3 rA, rB; // Offset in world orientation with respect to center of mass
 	};
 
+	Vector3 sep_axis;
+	bool collided;
+
+	Space3DSW *space;
+
+	BodyContact3DSW(Body3DSW **p_body_ptr = nullptr, int p_body_count = 0) :
+			Constraint3DSW(p_body_ptr, p_body_count) {
+	}
+};
+
+class BodyPair3DSW : public BodyContact3DSW {
+	enum {
+		MAX_CONTACTS = 4
+	};
+
+	union {
+		struct {
+			Body3DSW *A;
+			Body3DSW *B;
+		};
+
+		Body3DSW *_arr[2];
+	};
+
+	int shape_A;
+	int shape_B;
+
 	Vector3 offset_B; //use local A coordinates to avoid numerical issues on collision detection
 
-	Vector3 sep_axis;
 	Contact contacts[MAX_CONTACTS];
 	int contact_count;
-	bool collided;
 
-	static void _contact_added_callback(const Vector3 &p_point_A, const Vector3 &p_point_B, void *p_userdata);
+	static void _contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata);
 
-	void contact_added_callback(const Vector3 &p_point_A, const Vector3 &p_point_B);
+	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 Transform &p_xform_A, Body3DSW *p_B, int p_shape_B, const Transform &p_xform_B);
 
-	Space3DSW *space;
-
 public:
 	bool setup(real_t p_step);
 	void solve(real_t p_step);
@@ -92,4 +103,26 @@ public:
 	~BodyPair3DSW();
 };
 
-#endif // BODY_PAIR__SW_H
+class BodySoftBodyPair3DSW : public BodyContact3DSW {
+	Body3DSW *body;
+	SoftBody3DSW *soft_body;
+
+	int body_shape;
+
+	LocalVector<Contact> contacts;
+
+	static void _contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata);
+
+	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();
+
+public:
+	bool setup(real_t p_step);
+	void solve(real_t p_step);
+
+	BodySoftBodyPair3DSW(Body3DSW *p_A, int p_shape_A, SoftBody3DSW *p_B);
+	~BodySoftBodyPair3DSW();
+};
+
+#endif // BODY_PAIR_3D_SW_H
diff --git a/servers/physics_3d/collision_object_3d_sw.h b/servers/physics_3d/collision_object_3d_sw.h
index 3847b81381..85221b7746 100644
--- a/servers/physics_3d/collision_object_3d_sw.h
+++ b/servers/physics_3d/collision_object_3d_sw.h
@@ -48,7 +48,8 @@ class CollisionObject3DSW : public ShapeOwner3DSW {
 public:
 	enum Type {
 		TYPE_AREA,
-		TYPE_BODY
+		TYPE_BODY,
+		TYPE_SOFT_BODY,
 	};
 
 private:
@@ -129,8 +130,8 @@ public:
 	_FORCE_INLINE_ const AABB &get_shape_aabb(int p_index) const { return shapes[p_index].aabb_cache; }
 	_FORCE_INLINE_ real_t get_shape_area(int p_index) const { return shapes[p_index].area_cache; }
 
-	_FORCE_INLINE_ Transform get_transform() const { return transform; }
-	_FORCE_INLINE_ Transform get_inv_transform() const { return inv_transform; }
+	_FORCE_INLINE_ const Transform &get_transform() const { return transform; }
+	_FORCE_INLINE_ const Transform &get_inv_transform() const { return inv_transform; }
 	_FORCE_INLINE_ Space3DSW *get_space() const { return space; }
 
 	_FORCE_INLINE_ void set_ray_pickable(bool p_enable) { ray_pickable = p_enable; }
diff --git a/servers/physics_3d/collision_solver_3d_sat.cpp b/servers/physics_3d/collision_solver_3d_sat.cpp
index db48f011c6..9d5448dbfa 100644
--- a/servers/physics_3d/collision_solver_3d_sat.cpp
+++ b/servers/physics_3d/collision_solver_3d_sat.cpp
@@ -74,9 +74,9 @@ struct _CollectorCallback {
 
 	_FORCE_INLINE_ void call(const Vector3 &p_point_A, const Vector3 &p_point_B) {
 		if (swap) {
-			callback(p_point_B, p_point_A, userdata);
+			callback(p_point_B, 0, p_point_A, 0, userdata);
 		} else {
-			callback(p_point_A, p_point_B, userdata);
+			callback(p_point_A, 0, p_point_B, 0, userdata);
 		}
 	}
 };
@@ -685,7 +685,7 @@ public:
 		return true;
 	}
 
-	static _FORCE_INLINE_ void test_contact_points(const Vector3 &p_point_A, const Vector3 &p_point_B, void *p_userdata) {
+	static _FORCE_INLINE_ void test_contact_points(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata) {
 		SeparatorAxisTest<ShapeA, ShapeB, withMargin> *separator = (SeparatorAxisTest<ShapeA, ShapeB, withMargin> *)p_userdata;
 		Vector3 axis = (p_point_B - p_point_A);
 		real_t depth = axis.length();
diff --git a/servers/physics_3d/collision_solver_3d_sw.cpp b/servers/physics_3d/collision_solver_3d_sw.cpp
index fd9ea00d92..f655c4626c 100644
--- a/servers/physics_3d/collision_solver_3d_sw.cpp
+++ b/servers/physics_3d/collision_solver_3d_sw.cpp
@@ -30,6 +30,7 @@
 
 #include "collision_solver_3d_sw.h"
 #include "collision_solver_3d_sat.h"
+#include "soft_body_3d_sw.h"
 
 #include "gjk_epa.h"
 
@@ -78,9 +79,9 @@ bool CollisionSolver3DSW::solve_static_plane(const Shape3DSW *p_shape_A, const T
 
 		if (p_result_callback) {
 			if (p_swap_result) {
-				p_result_callback(supports[i], support_A, p_userdata);
+				p_result_callback(supports[i], 0, support_A, 0, p_userdata);
 			} else {
-				p_result_callback(support_A, supports[i], p_userdata);
+				p_result_callback(support_A, 0, supports[i], 0, p_userdata);
 			}
 		}
 	}
@@ -113,14 +114,148 @@ bool CollisionSolver3DSW::solve_ray(const Shape3DSW *p_shape_A, const Transform
 
 	if (p_result_callback) {
 		if (p_swap_result) {
-			p_result_callback(support_B, support_A, p_userdata);
+			p_result_callback(support_B, 0, support_A, 0, p_userdata);
 		} else {
-			p_result_callback(support_A, support_B, p_userdata);
+			p_result_callback(support_A, 0, support_B, 0, p_userdata);
 		}
 	}
 	return true;
 }
 
+struct _SoftBodyContactCollisionInfo {
+	int node_index = 0;
+	CollisionSolver3DSW::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) {
+	_SoftBodyContactCollisionInfo &cinfo = *(_SoftBodyContactCollisionInfo *)(p_userdata);
+
+	++cinfo.contact_count;
+
+	if (cinfo.swap_result) {
+		cinfo.result_callback(p_point_B, cinfo.node_index, p_point_A, p_index_A, cinfo.userdata);
+	} else {
+		cinfo.result_callback(p_point_A, p_index_A, p_point_B, cinfo.node_index, cinfo.userdata);
+	}
+}
+
+struct _SoftBodyQueryInfo {
+	SoftBody3DSW *soft_body = nullptr;
+	const Shape3DSW *shape_A = nullptr;
+	const Shape3DSW *shape_B = nullptr;
+	Transform transform_A;
+	Transform node_transform;
+	_SoftBodyContactCollisionInfo contact_info;
+#ifdef DEBUG_ENABLED
+	int node_query_count = 0;
+	int convex_query_count = 0;
+#endif
+};
+
+bool CollisionSolver3DSW::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);
+
+	Transform transform_B;
+	transform_B.origin = query_cinfo.node_transform.xform(node_position);
+
+	query_cinfo.contact_info.node_index = p_node_index;
+	solve_static(query_cinfo.shape_A, query_cinfo.transform_A, query_cinfo.shape_B, transform_B, soft_body_contact_callback, &query_cinfo.contact_info);
+
+#ifdef DEBUG_ENABLED
+	++query_cinfo.node_query_count;
+#endif
+
+	// Continue with the query.
+	return false;
+}
+
+void CollisionSolver3DSW::soft_body_concave_callback(void *p_userdata, Shape3DSW *p_convex) {
+	_SoftBodyQueryInfo &query_cinfo = *(_SoftBodyQueryInfo *)(p_userdata);
+
+	query_cinfo.shape_A = p_convex;
+
+	// Calculate AABB for internal soft body query (in world space).
+	AABB shape_aabb;
+	for (int i = 0; i < 3; i++) {
+		Vector3 axis;
+		axis[i] = 1.0;
+
+		real_t smin, smax;
+		p_convex->project_range(axis, query_cinfo.transform_A, smin, smax);
+
+		shape_aabb.position[i] = smin;
+		shape_aabb.size[i] = smax - smin;
+	}
+
+	shape_aabb.grow_by(query_cinfo.soft_body->get_collision_margin());
+
+	query_cinfo.soft_body->query_aabb(shape_aabb, soft_body_query_callback, &query_cinfo);
+
+#ifdef DEBUG_ENABLED
+	++query_cinfo.convex_query_count;
+#endif
+}
+
+bool CollisionSolver3DSW::solve_soft_body(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &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);
+
+	SoftBody3DSW *soft_body = soft_body_shape_B->get_soft_body();
+	const Transform &world_to_local = soft_body->get_inv_transform();
+
+	const real_t collision_margin = soft_body->get_collision_margin();
+
+	SphereShape3DSW sphere_shape;
+	sphere_shape.set_data(collision_margin);
+
+	_SoftBodyQueryInfo query_cinfo;
+	query_cinfo.contact_info.result_callback = p_result_callback;
+	query_cinfo.contact_info.userdata = p_userdata;
+	query_cinfo.contact_info.swap_result = p_swap_result;
+	query_cinfo.soft_body = soft_body;
+	query_cinfo.node_transform = p_transform_B * world_to_local;
+	query_cinfo.shape_A = p_shape_A;
+	query_cinfo.transform_A = p_transform_A;
+	query_cinfo.shape_B = &sphere_shape;
+
+	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);
+
+		AABB soft_body_aabb = soft_body->get_bounds();
+		soft_body_aabb.grow_by(collision_margin);
+
+		// Calculate AABB for internal concave shape query (in local space).
+		AABB local_aabb;
+		for (int i = 0; i < 3; i++) {
+			Vector3 axis(p_transform_A.basis.get_axis(i));
+			real_t axis_scale = 1.0 / axis.length();
+
+			real_t smin = soft_body_aabb.position[i];
+			real_t smax = smin + soft_body_aabb.size[i];
+
+			smin *= axis_scale;
+			smax *= axis_scale;
+
+			local_aabb.position[i] = smin;
+			local_aabb.size[i] = smax - smin;
+		}
+
+		concave_shape_A->cull(local_aabb, soft_body_concave_callback, &query_cinfo);
+	} else {
+		AABB shape_aabb = p_transform_A.xform(p_shape_A->get_aabb());
+		shape_aabb.grow_by(collision_margin);
+
+		soft_body->query_aabb(shape_aabb, soft_body_query_callback, &query_cinfo);
+	}
+
+	return (query_cinfo.contact_info.contact_count > 0);
+}
+
 struct _ConcaveCollisionInfo {
 	const Transform *transform_A;
 	const Shape3DSW *shape_A;
@@ -215,6 +350,9 @@ bool CollisionSolver3DSW::solve_static(const Shape3DSW *p_shape_A, const Transfo
 		if (type_B == PhysicsServer3D::SHAPE_RAY) {
 			return false;
 		}
+		if (type_B == PhysicsServer3D::SHAPE_SOFT_BODY) {
+			return false;
+		}
 
 		if (swap) {
 			return solve_static_plane(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true);
@@ -233,6 +371,18 @@ bool CollisionSolver3DSW::solve_static(const Shape3DSW *p_shape_A, const Transfo
 			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.
+			return false;
+		}
+
+		if (swap) {
+			return solve_soft_body(p_shape_B, p_transform_B, p_shape_A, p_transform_A, p_result_callback, p_userdata, true);
+		} else {
+			return solve_soft_body(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback, p_userdata, false);
+		}
+
 	} else if (concave_B) {
 		if (concave_A) {
 			return false;
diff --git a/servers/physics_3d/collision_solver_3d_sw.h b/servers/physics_3d/collision_solver_3d_sw.h
index 81d87e9773..34ac2c6d3f 100644
--- a/servers/physics_3d/collision_solver_3d_sw.h
+++ b/servers/physics_3d/collision_solver_3d_sw.h
@@ -35,12 +35,16 @@
 
 class CollisionSolver3DSW {
 public:
-	typedef void (*CallbackResult)(const Vector3 &p_point_A, const Vector3 &p_point_B, void *p_userdata);
+	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 void soft_body_concave_callback(void *p_userdata, Shape3DSW *p_convex);
 	static void concave_callback(void *p_userdata, Shape3DSW *p_convex);
 	static bool solve_static_plane(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result);
 	static bool solve_ray(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result);
+	static bool solve_soft_body(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result);
 	static bool solve_concave(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &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 void concave_distance_callback(void *p_userdata, Shape3DSW *p_convex);
 	static bool solve_distance_plane(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B);
diff --git a/servers/physics_3d/gjk_epa.cpp b/servers/physics_3d/gjk_epa.cpp
index e44c92da79..1a8c7f704f 100644
--- a/servers/physics_3d/gjk_epa.cpp
+++ b/servers/physics_3d/gjk_epa.cpp
@@ -1011,9 +1011,9 @@ bool gjk_epa_calculate_penetration(const Shape3DSW *p_shape_A, const Transform &
 	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)) {
 		if (p_result_callback) {
 			if (p_swap) {
-				p_result_callback(res.witnesses[1], res.witnesses[0], p_userdata);
+				p_result_callback(res.witnesses[1], 0, res.witnesses[0], 0, p_userdata);
 			} else {
-				p_result_callback(res.witnesses[0], res.witnesses[1], p_userdata);
+				p_result_callback(res.witnesses[0], 0, res.witnesses[1], 0, p_userdata);
 			}
 		}
 		return true;
diff --git a/servers/physics_3d/physics_server_3d_sw.cpp b/servers/physics_3d/physics_server_3d_sw.cpp
index 6bbef09907..3d0063b0fa 100644
--- a/servers/physics_3d/physics_server_3d_sw.cpp
+++ b/servers/physics_3d/physics_server_3d_sw.cpp
@@ -611,9 +611,18 @@ uint32_t PhysicsServer3DSW::body_get_collision_mask(RID p_body) const {
 
 void PhysicsServer3DSW::body_attach_object_instance_id(RID p_body, ObjectID p_id) {
 	Body3DSW *body = body_owner.getornull(p_body);
-	ERR_FAIL_COND(!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;
+	}
 
-	body->set_instance_id(p_id);
+	ERR_FAIL_MSG("Invalid ID.");
 };
 
 ObjectID PhysicsServer3DSW::body_get_object_instance_id(RID p_body) const {
@@ -893,6 +902,266 @@ PhysicsDirectBodyState3D *PhysicsServer3DSW::body_get_direct_state(RID p_body) {
 	return 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 Transform &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() {
@@ -1278,7 +1547,13 @@ void PhysicsServer3DSW::free(RID p_rid) {
 
 		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);
 
@@ -1444,7 +1719,7 @@ void PhysicsServer3DSW::_update_shapes() {
 	}
 }
 
-void PhysicsServer3DSW::_shape_col_cbk(const Vector3 &p_point_A, const Vector3 &p_point_B, void *p_userdata) {
+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) {
diff --git a/servers/physics_3d/physics_server_3d_sw.h b/servers/physics_3d/physics_server_3d_sw.h
index afda161fa8..f92652bfad 100644
--- a/servers/physics_3d/physics_server_3d_sw.h
+++ b/servers/physics_3d/physics_server_3d_sw.h
@@ -63,6 +63,7 @@ class PhysicsServer3DSW : public PhysicsServer3D {
 	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;
 
 	//void _clear_query(QuerySW *p_query);
@@ -79,7 +80,7 @@ public:
 		Vector3 *ptr;
 	};
 
-	static void _shape_col_cbk(const Vector3 &p_point_A, const Vector3 &p_point_B, void *p_userdata);
+	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;
@@ -252,68 +253,58 @@ public:
 
 	/* SOFT BODY */
 
-	virtual RID soft_body_create() override { return RID(); }
+	virtual RID soft_body_create() override;
 
-	virtual void soft_body_update_rendering_server(RID p_body, class SoftBodyRenderingServerHandler *p_rendering_server_handler) override {}
+	virtual void soft_body_update_rendering_server(RID p_body, RenderingServerHandler *p_rendering_server_handler) override;
 
-	virtual void soft_body_set_space(RID p_body, RID p_space) override {}
-	virtual RID soft_body_get_space(RID p_body) const override { return RID(); }
+	virtual void soft_body_set_space(RID p_body, RID p_space) override;
+	virtual RID soft_body_get_space(RID p_body) const override;
 
-	virtual void soft_body_set_collision_layer(RID p_body, uint32_t p_layer) override {}
-	virtual uint32_t soft_body_get_collision_layer(RID p_body) const override { return 0; }
+	virtual void soft_body_set_collision_layer(RID p_body, uint32_t p_layer) override;
+	virtual uint32_t soft_body_get_collision_layer(RID p_body) const override;
 
-	virtual void soft_body_set_collision_mask(RID p_body, uint32_t p_mask) override {}
-	virtual uint32_t soft_body_get_collision_mask(RID p_body) const override { return 0; }
+	virtual void soft_body_set_collision_mask(RID p_body, uint32_t p_mask) override;
+	virtual uint32_t soft_body_get_collision_mask(RID p_body) const override;
 
-	virtual void soft_body_add_collision_exception(RID p_body, RID p_body_b) override {}
-	virtual void soft_body_remove_collision_exception(RID p_body, RID p_body_b) override {}
-	virtual void soft_body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions) override {}
+	virtual void soft_body_add_collision_exception(RID p_body, RID p_body_b) override;
+	virtual void soft_body_remove_collision_exception(RID p_body, RID p_body_b) override;
+	virtual void soft_body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions) override;
 
-	virtual void soft_body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) override {}
-	virtual Variant soft_body_get_state(RID p_body, BodyState p_state) const override { return Variant(); }
+	virtual void soft_body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) override;
+	virtual Variant soft_body_get_state(RID p_body, BodyState p_state) const override;
 
-	virtual void soft_body_set_transform(RID p_body, const Transform &p_transform) override {}
-	virtual Vector3 soft_body_get_vertex_position(RID p_body, int vertex_index) const override { return Vector3(); }
+	virtual void soft_body_set_transform(RID p_body, const Transform &p_transform) override;
 
-	virtual void soft_body_set_ray_pickable(RID p_body, bool p_enable) override {}
+	virtual void soft_body_set_ray_pickable(RID p_body, bool p_enable) override;
 
-	virtual void soft_body_set_simulation_precision(RID p_body, int p_simulation_precision) override {}
-	virtual int soft_body_get_simulation_precision(RID p_body) const override { return 0; }
+	virtual void soft_body_set_simulation_precision(RID p_body, int p_simulation_precision) override;
+	virtual int soft_body_get_simulation_precision(RID p_body) const override;
 
-	virtual void soft_body_set_total_mass(RID p_body, real_t p_total_mass) override {}
-	virtual real_t soft_body_get_total_mass(RID p_body) const override { return 0.; }
+	virtual void soft_body_set_total_mass(RID p_body, real_t p_total_mass) override;
+	virtual real_t soft_body_get_total_mass(RID p_body) const override;
 
-	virtual void soft_body_set_linear_stiffness(RID p_body, real_t p_stiffness) override {}
-	virtual real_t soft_body_get_linear_stiffness(RID p_body) const override { return 0.; }
+	virtual void soft_body_set_linear_stiffness(RID p_body, real_t p_stiffness) override;
+	virtual real_t soft_body_get_linear_stiffness(RID p_body) const override;
 
-	virtual void soft_body_set_angular_stiffness(RID p_body, real_t p_stiffness) override {}
-	virtual real_t soft_body_get_angular_stiffness(RID p_body) const override { return 0.; }
+	virtual void soft_body_set_pressure_coefficient(RID p_body, real_t p_pressure_coefficient) override;
+	virtual real_t soft_body_get_pressure_coefficient(RID p_body) const override;
 
-	virtual void soft_body_set_volume_stiffness(RID p_body, real_t p_stiffness) override {}
-	virtual real_t soft_body_get_volume_stiffness(RID p_body) const override { return 0.; }
+	virtual void soft_body_set_damping_coefficient(RID p_body, real_t p_damping_coefficient) override;
+	virtual real_t soft_body_get_damping_coefficient(RID p_body) const override;
 
-	virtual void soft_body_set_pressure_coefficient(RID p_body, real_t p_pressure_coefficient) override {}
-	virtual real_t soft_body_get_pressure_coefficient(RID p_body) const override { return 0.; }
+	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_pose_matching_coefficient(RID p_body, real_t p_pose_matching_coefficient) override {}
-	virtual real_t soft_body_get_pose_matching_coefficient(RID p_body) const override { return 0.; }
+	virtual void soft_body_set_mesh(RID p_body, const REF &p_mesh) override;
 
-	virtual void soft_body_set_damping_coefficient(RID p_body, real_t p_damping_coefficient) override {}
-	virtual real_t soft_body_get_damping_coefficient(RID p_body) const override { return 0.; }
+	virtual AABB soft_body_get_bounds(RID p_body) const override;
 
-	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 { return 0.; }
+	virtual void soft_body_move_point(RID p_body, int p_point_index, const Vector3 &p_global_position) override;
+	virtual Vector3 soft_body_get_point_global_position(RID p_body, int p_point_index) const override;
 
-	virtual void soft_body_set_mesh(RID p_body, const REF &p_mesh) override {}
-
-	virtual void soft_body_move_point(RID p_body, int p_point_index, const Vector3 &p_global_position) override {}
-	virtual Vector3 soft_body_get_point_global_position(RID p_body, int p_point_index) const override { return Vector3(); }
-
-	virtual Vector3 soft_body_get_point_offset(RID p_body, int p_point_index) const override { return Vector3(); }
-
-	virtual void soft_body_remove_all_pinned_points(RID p_body) override {}
-	virtual void soft_body_pin_point(RID p_body, int p_point_index, bool p_pin) override {}
-	virtual bool soft_body_is_point_pinned(RID p_body, int p_point_index) const override { return false; }
+	virtual void soft_body_remove_all_pinned_points(RID p_body) override;
+	virtual void soft_body_pin_point(RID p_body, int p_point_index, bool p_pin) override;
+	virtual bool soft_body_is_point_pinned(RID p_body, int p_point_index) const override;
 
 	/* JOINT API */
 
diff --git a/servers/physics_3d/physics_server_3d_wrap_mt.h b/servers/physics_3d/physics_server_3d_wrap_mt.h
index f60e1332d5..49ae60db92 100644
--- a/servers/physics_3d/physics_server_3d_wrap_mt.h
+++ b/servers/physics_3d/physics_server_3d_wrap_mt.h
@@ -273,7 +273,7 @@ public:
 
 	FUNCRID(soft_body)
 
-	FUNC2(soft_body_update_rendering_server, RID, class SoftBodyRenderingServerHandler *)
+	FUNC2(soft_body_update_rendering_server, RID, class RenderingServerHandler *)
 
 	FUNC2(soft_body_set_space, RID, RID)
 	FUNC1RC(RID, soft_body_get_space, RID)
@@ -294,7 +294,6 @@ public:
 	FUNC2RC(Variant, soft_body_get_state, RID, BodyState);
 
 	FUNC2(soft_body_set_transform, RID, const Transform &);
-	FUNC2RC(Vector3, soft_body_get_vertex_position, RID, int);
 
 	FUNC2(soft_body_set_simulation_precision, RID, int);
 	FUNC1RC(int, soft_body_get_simulation_precision, RID);
@@ -305,18 +304,9 @@ public:
 	FUNC2(soft_body_set_linear_stiffness, RID, real_t);
 	FUNC1RC(real_t, soft_body_get_linear_stiffness, RID);
 
-	FUNC2(soft_body_set_angular_stiffness, RID, real_t);
-	FUNC1RC(real_t, soft_body_get_angular_stiffness, RID);
-
-	FUNC2(soft_body_set_volume_stiffness, RID, real_t);
-	FUNC1RC(real_t, soft_body_get_volume_stiffness, RID);
-
 	FUNC2(soft_body_set_pressure_coefficient, RID, real_t);
 	FUNC1RC(real_t, soft_body_get_pressure_coefficient, RID);
 
-	FUNC2(soft_body_set_pose_matching_coefficient, RID, real_t);
-	FUNC1RC(real_t, soft_body_get_pose_matching_coefficient, RID);
-
 	FUNC2(soft_body_set_damping_coefficient, RID, real_t);
 	FUNC1RC(real_t, soft_body_get_damping_coefficient, RID);
 
@@ -325,9 +315,10 @@ public:
 
 	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);
-	FUNC2RC(Vector3, soft_body_get_point_offset, RID, int);
 
 	FUNC1(soft_body_remove_all_pinned_points, RID);
 	FUNC3(soft_body_pin_point, RID, int, bool);
diff --git a/servers/physics_3d/soft_body_3d_sw.cpp b/servers/physics_3d/soft_body_3d_sw.cpp
new file mode 100644
index 0000000000..f63a470cbe
--- /dev/null
+++ b/servers/physics_3d/soft_body_3d_sw.cpp
@@ -0,0 +1,1221 @@
+/*************************************************************************/
+/*  soft_body_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 "soft_body_3d_sw.h"
+#include "space_3d_sw.h"
+
+#include "core/math/geometry_3d.h"
+#include "core/templates/map.h"
+
+// Based on Bullet soft body.
+
+/*
+Bullet Continuous Collision Detection and Physics Library
+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.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+///btSoftBody implementation by Nathanael Presson
+
+SoftBody3DSW::SoftBody3DSW() :
+		CollisionObject3DSW(TYPE_SOFT_BODY),
+		active_list(this) {
+	_set_static(false);
+}
+
+void SoftBody3DSW::_shapes_changed() {
+}
+
+void SoftBody3DSW::set_state(PhysicsServer3D::BodyState p_state, const Variant &p_variant) {
+	switch (p_state) {
+		case PhysicsServer3D::BODY_STATE_TRANSFORM: {
+			_set_transform(p_variant);
+			_set_inv_transform(get_transform().inverse());
+
+			apply_nodes_transform(get_transform());
+
+		} break;
+		case PhysicsServer3D::BODY_STATE_LINEAR_VELOCITY: {
+			// Not supported.
+			ERR_FAIL_MSG("Linear velocity is not supported for Soft bodies.");
+		} break;
+		case PhysicsServer3D::BODY_STATE_ANGULAR_VELOCITY: {
+			ERR_FAIL_MSG("Angular velocity is not supported for Soft bodies.");
+		} break;
+		case PhysicsServer3D::BODY_STATE_SLEEPING: {
+			ERR_FAIL_MSG("Sleeping state is not supported for Soft bodies.");
+		} break;
+		case PhysicsServer3D::BODY_STATE_CAN_SLEEP: {
+			ERR_FAIL_MSG("Sleeping state is not supported for Soft bodies.");
+		} break;
+	}
+}
+
+Variant SoftBody3DSW::get_state(PhysicsServer3D::BodyState p_state) const {
+	switch (p_state) {
+		case PhysicsServer3D::BODY_STATE_TRANSFORM: {
+			return get_transform();
+		} break;
+		case PhysicsServer3D::BODY_STATE_LINEAR_VELOCITY: {
+			ERR_FAIL_V_MSG(Vector3(), "Linear velocity is not supported for Soft bodies.");
+		} break;
+		case PhysicsServer3D::BODY_STATE_ANGULAR_VELOCITY: {
+			ERR_FAIL_V_MSG(Vector3(), "Angular velocity is not supported for Soft bodies.");
+			return Vector3();
+		} break;
+		case PhysicsServer3D::BODY_STATE_SLEEPING: {
+			ERR_FAIL_V_MSG(false, "Sleeping state is not supported for Soft bodies.");
+		} break;
+		case PhysicsServer3D::BODY_STATE_CAN_SLEEP: {
+			ERR_FAIL_V_MSG(false, "Sleeping state is not supported for Soft bodies.");
+		} break;
+	}
+
+	return Variant();
+}
+
+void SoftBody3DSW::set_space(Space3DSW *p_space) {
+	if (get_space()) {
+		get_space()->soft_body_remove_from_active_list(&active_list);
+
+		deinitialize_shape();
+	}
+
+	_set_space(p_space);
+
+	if (get_space()) {
+		get_space()->soft_body_add_to_active_list(&active_list);
+
+		if (bounds != AABB()) {
+			initialize_shape(true);
+		}
+	}
+}
+
+void SoftBody3DSW::set_mesh(const Ref<Mesh> &p_mesh) {
+	destroy();
+
+	soft_mesh = p_mesh;
+
+	if (soft_mesh.is_null()) {
+		return;
+	}
+
+	Array arrays = soft_mesh->surface_get_arrays(0);
+	ERR_FAIL_COND(!(soft_mesh->surface_get_format(0) & RS::ARRAY_FORMAT_INDEX));
+
+	bool success = create_from_trimesh(arrays[RS::ARRAY_INDEX], arrays[RS::ARRAY_VERTEX]);
+	if (!success) {
+		destroy();
+		soft_mesh = Ref<Mesh>();
+	}
+}
+
+void SoftBody3DSW::update_rendering_server(RenderingServerHandler *p_rendering_server_handler) {
+	if (soft_mesh.is_null()) {
+		return;
+	}
+
+	const uint32_t vertex_count = map_visual_to_physics.size();
+	for (uint32_t i = 0; i < vertex_count; ++i) {
+		const uint32_t node_index = map_visual_to_physics[i];
+		const Node &node = nodes[node_index];
+		const Vector3 &vertex_position = node.x;
+		const Vector3 &vertex_normal = node.n;
+
+		p_rendering_server_handler->set_vertex(i, &vertex_position);
+		p_rendering_server_handler->set_normal(i, &vertex_normal);
+	}
+
+	p_rendering_server_handler->set_aabb(bounds);
+}
+
+void SoftBody3DSW::update_normals() {
+	uint32_t i, ni;
+
+	for (i = 0, ni = nodes.size(); i < ni; ++i) {
+		nodes[i].n = Vector3();
+	}
+
+	for (i = 0, ni = faces.size(); i < ni; ++i) {
+		Face &face = faces[i];
+		const Vector3 n = vec3_cross(face.n[0]->x - face.n[2]->x, face.n[0]->x - face.n[1]->x);
+		face.n[0]->n += n;
+		face.n[1]->n += n;
+		face.n[2]->n += n;
+		face.normal = n;
+		face.normal.normalize();
+	}
+
+	for (i = 0, ni = nodes.size(); i < ni; ++i) {
+		Node &node = nodes[i];
+		real_t len = node.n.length();
+		if (len > CMP_EPSILON) {
+			node.n /= len;
+		}
+	}
+}
+
+void SoftBody3DSW::update_bounds() {
+	AABB prev_bounds = bounds;
+	prev_bounds.grow_by(collision_margin);
+
+	bounds = AABB();
+
+	const uint32_t nodes_count = nodes.size();
+	if (nodes_count == 0) {
+		deinitialize_shape();
+		return;
+	}
+
+	bool first = true;
+	bool moved = false;
+	for (uint32_t node_index = 0; node_index < nodes_count; ++node_index) {
+		const Node &node = nodes[node_index];
+		if (!prev_bounds.has_point(node.x)) {
+			moved = true;
+		}
+		if (first) {
+			bounds.position = node.x;
+			first = false;
+		} else {
+			bounds.expand_to(node.x);
+		}
+	}
+
+	if (get_space()) {
+		initialize_shape(moved);
+	}
+}
+
+void SoftBody3DSW::update_constants() {
+	reset_link_rest_lengths();
+	update_link_constants();
+	update_area();
+}
+
+void SoftBody3DSW::update_area() {
+	int i, ni;
+
+	// Face area.
+	for (i = 0, ni = faces.size(); i < ni; ++i) {
+		Face &face = faces[i];
+
+		const Vector3 &x0 = face.n[0]->x;
+		const Vector3 &x1 = face.n[1]->x;
+		const Vector3 &x2 = face.n[2]->x;
+
+		const Vector3 a = x1 - x0;
+		const Vector3 b = x2 - x0;
+		const Vector3 cr = vec3_cross(a, b);
+		face.ra = cr.length();
+	}
+
+	// Node area.
+	LocalVector<int> counts;
+	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;
+	}
+
+	for (i = 0, ni = faces.size(); i < ni; ++i) {
+		const Face &face = faces[i];
+		for (int j = 0; j < 3; ++j) {
+			const int index = (int)(face.n[j] - &nodes[0]);
+			counts[index]++;
+			face.n[j]->area += Math::abs(face.ra);
+		}
+	}
+
+	for (i = 0, ni = nodes.size(); i < ni; ++i) {
+		if (counts[i] > 0) {
+			nodes[i].area /= (real_t)counts[i];
+		} else {
+			nodes[i].area = 0.0;
+		}
+	}
+}
+
+void SoftBody3DSW::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();
+		link.c1 = link.rl * link.rl;
+	}
+}
+
+void SoftBody3DSW::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];
+		link.c0 = (link.n[0]->im + link.n[1]->im) * inv_linear_stiffness;
+	}
+}
+
+void SoftBody3DSW::apply_nodes_transform(const Transform &p_transform) {
+	if (soft_mesh.is_null()) {
+		return;
+	}
+
+	uint32_t node_count = nodes.size();
+	Vector3 leaf_size = Vector3(collision_margin, collision_margin, collision_margin) * 2.0;
+	for (uint32_t node_index = 0; node_index < node_count; ++node_index) {
+		Node &node = nodes[node_index];
+
+		node.x = p_transform.xform(node.x);
+		node.q = node.x;
+		node.v = Vector3();
+		node.bv = Vector3();
+
+		AABB node_aabb(node.x, leaf_size);
+		node_tree.update(node.leaf, node_aabb);
+	}
+
+	face_tree.clear();
+
+	update_normals();
+	update_bounds();
+	update_constants();
+}
+
+Vector3 SoftBody3DSW::get_vertex_position(int p_index) const {
+	if (soft_mesh.is_null()) {
+		return Vector3();
+	}
+
+	ERR_FAIL_INDEX_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) {
+	if (soft_mesh.is_null()) {
+		return;
+	}
+
+	ERR_FAIL_INDEX(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());
+	Node &node = nodes[node_index];
+	node.q = node.x;
+	node.x = p_position;
+}
+
+void SoftBody3DSW::pin_vertex(int p_index) {
+	if (is_vertex_pinned(p_index)) {
+		return;
+	}
+
+	pinned_vertices.push_back(p_index);
+
+	if (!soft_mesh.is_null()) {
+		ERR_FAIL_INDEX(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());
+		Node &node = nodes[node_index];
+		node.im = 0.0;
+	}
+}
+
+void SoftBody3DSW::unpin_vertex(int p_index) {
+	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);
+
+			if (!soft_mesh.is_null()) {
+				ERR_FAIL_INDEX(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());
+				real_t inv_node_mass = nodes.size() * inv_total_mass;
+
+				Node &node = nodes[node_index];
+				node.im = inv_node_mass;
+			}
+
+			return;
+		}
+	}
+}
+
+void SoftBody3DSW::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];
+
+			ERR_CONTINUE(vertex_index >= map_visual_to_physics.size());
+			uint32_t node_index = map_visual_to_physics[vertex_index];
+
+			ERR_CONTINUE(node_index >= nodes.size());
+			Node &node = nodes[node_index];
+			node.im = inv_node_mass;
+		}
+	}
+
+	pinned_vertices.clear();
+}
+
+bool SoftBody3DSW::is_vertex_pinned(int p_index) const {
+	uint32_t pinned_count = pinned_vertices.size();
+	for (uint32_t i = 0; i < pinned_count; ++i) {
+		if (p_index == pinned_vertices[i]) {
+			return true;
+		}
+	}
+
+	return false;
+}
+
+uint32_t SoftBody3DSW::get_node_count() const {
+	return nodes.size();
+}
+
+real_t SoftBody3DSW::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 {
+	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 {
+	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 {
+	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) {
+	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) {
+	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 {
+	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 {
+	ERR_FAIL_COND(p_face_index >= faces.size());
+	const Face &face = faces[p_face_index];
+	r_point_1 = face.n[0]->x;
+	r_point_2 = face.n[1]->x;
+	r_point_3 = face.n[2]->x;
+}
+
+Vector3 SoftBody3DSW::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) {
+	uint32_t node_count = 0;
+	LocalVector<Vector3> vertices;
+	const int visual_vertex_count(p_vertices.size());
+
+	LocalVector<int> triangles;
+	const uint32_t triangle_count(p_indices.size() / 3);
+	triangles.resize(triangle_count * 3);
+
+	// Merge all overlapping vertices and create a map of physical vertices to visual vertices.
+	{
+		// Process vertices.
+		{
+			uint32_t vertex_count = 0;
+			Map<Vector3, uint32_t> unique_vertices;
+
+			vertices.resize(visual_vertex_count);
+			map_visual_to_physics.resize(visual_vertex_count);
+
+			for (int visual_vertex_index = 0; visual_vertex_index < visual_vertex_count; ++visual_vertex_index) {
+				const Vector3 &vertex = p_vertices[visual_vertex_index];
+
+				Map<Vector3, uint32_t>::Element *e = unique_vertices.find(vertex);
+				uint32_t vertex_id;
+				if (e) {
+					// Already existing.
+					vertex_id = e->value();
+				} else {
+					// Create new one.
+					vertex_id = vertex_count++;
+					unique_vertices[vertex] = vertex_id;
+					vertices[vertex_id] = vertex;
+				}
+
+				map_visual_to_physics[visual_vertex_index] = vertex_id;
+			}
+
+			vertices.resize(vertex_count);
+		}
+
+		// Process triangles.
+		{
+			for (uint32_t triangle_index = 0; triangle_index < triangle_count; ++triangle_index) {
+				for (int i = 0; i < 3; ++i) {
+					int visual_index = 3 * triangle_index + i;
+					int physics_index = map_visual_to_physics[p_indices[visual_index]];
+					triangles[visual_index] = physics_index;
+					node_count = MAX((int)node_count, physics_index);
+				}
+			}
+		}
+	}
+
+	++node_count;
+
+	// Create nodes from vertices.
+	nodes.resize(node_count);
+	real_t inv_node_mass = node_count * inv_total_mass;
+	Vector3 leaf_size = Vector3(collision_margin, collision_margin, collision_margin) * 2.0;
+	for (uint32_t i = 0; i < node_count; ++i) {
+		Node &node = nodes[i];
+		node.s = vertices[i];
+		node.x = node.s;
+		node.q = node.s;
+		node.im = inv_node_mass;
+
+		AABB node_aabb(node.x, leaf_size);
+		node.leaf = node_tree.insert(node_aabb, &node);
+
+		node.index = i;
+	}
+
+	// Create links and faces from triangles.
+	LocalVector<bool> chks;
+	chks.resize(node_count * node_count);
+	memset(chks.ptr(), 0, chks.size() * sizeof(bool));
+
+	for (uint32_t i = 0; i < triangle_count * 3; i += 3) {
+		const int idx[] = { triangles[i], triangles[i + 1], triangles[i + 2] };
+
+		for (int j = 2, k = 0; k < 3; j = k++) {
+			int chk = idx[k] * node_count + idx[j];
+			if (!chks[chk]) {
+				chks[chk] = true;
+				int inv_chk = idx[j] * node_count + idx[k];
+				chks[inv_chk] = true;
+
+				append_link(idx[j], idx[k]);
+			}
+		}
+
+		append_face(idx[0], idx[1], idx[2]);
+	}
+
+	// Set pinned nodes.
+	uint32_t pinned_count = pinned_vertices.size();
+	for (uint32_t i = 0; i < pinned_count; ++i) {
+		int pinned_vertex = pinned_vertices[i];
+
+		ERR_CONTINUE(pinned_vertex >= visual_vertex_count);
+		uint32_t node_index = map_visual_to_physics[pinned_vertex];
+
+		ERR_CONTINUE(node_index >= node_count);
+		Node &node = nodes[node_index];
+		node.im = 0.0;
+	}
+
+	generate_bending_constraints(2);
+	reoptimize_link_order();
+
+	update_constants();
+	update_normals();
+	update_bounds();
+
+	return true;
+}
+
+void SoftBody3DSW::generate_bending_constraints(int p_distance) {
+	uint32_t i, j;
+
+	if (p_distance > 1) {
+		// Build graph.
+		const uint32_t n = nodes.size();
+		const unsigned inf = (~(unsigned)0) >> 1;
+		const uint32_t adj_size = n * n;
+		unsigned *adj = memnew_arr(unsigned, adj_size);
+
+#define IDX(_x_, _y_) ((_y_)*n + (_x_))
+		for (j = 0; j < n; ++j) {
+			for (i = 0; i < n; ++i) {
+				int idx_ij = j * n + i;
+				int idx_ji = i * n + j;
+				if (i != j) {
+					adj[idx_ij] = adj[idx_ji] = inf;
+				} else {
+					adj[idx_ij] = adj[idx_ji] = 0;
+				}
+			}
+		}
+		for (i = 0; i < links.size(); ++i) {
+			const int ia = (int)(links[i].n[0] - &nodes[0]);
+			const int ib = (int)(links[i].n[1] - &nodes[0]);
+			int idx = ib * n + ia;
+			int idx_inv = ia * n + ib;
+			adj[idx] = 1;
+			adj[idx_inv] = 1;
+		}
+
+		// Special optimized case for distance == 2.
+		if (p_distance == 2) {
+			LocalVector<LocalVector<int>> node_links;
+
+			// Build node links.
+			node_links.resize(nodes.size());
+
+			for (i = 0; i < links.size(); ++i) {
+				const int ia = (int)(links[i].n[0] - &nodes[0]);
+				const int ib = (int)(links[i].n[1] - &nodes[0]);
+				if (node_links[ia].find(ib) == -1) {
+					node_links[ia].push_back(ib);
+				}
+
+				if (node_links[ib].find(ia) == -1) {
+					node_links[ib].push_back(ia);
+				}
+			}
+			for (uint32_t ii = 0; ii < node_links.size(); ii++) {
+				for (uint32_t jj = 0; jj < node_links[ii].size(); jj++) {
+					int k = node_links[ii][jj];
+					for (uint32_t kk = 0; kk < node_links[k].size(); kk++) {
+						int l = node_links[k][kk];
+						if ((int)ii != l) {
+							int idx_ik = k * n + ii;
+							int idx_kj = l * n + k;
+							const unsigned sum = adj[idx_ik] + adj[idx_kj];
+							ERR_FAIL_COND(sum != 2);
+							int idx_ij = l * n + ii;
+							if (adj[idx_ij] > sum) {
+								int idx_ji = l * n + ii;
+								adj[idx_ij] = adj[idx_ji] = sum;
+							}
+						}
+					}
+				}
+			}
+		} else {
+			// Generic Floyd's algorithm.
+			for (uint32_t k = 0; k < n; ++k) {
+				for (j = 0; j < n; ++j) {
+					for (i = j + 1; i < n; ++i) {
+						int idx_ik = k * n + i;
+						int idx_kj = j * n + k;
+						const unsigned sum = adj[idx_ik] + adj[idx_kj];
+						int idx_ij = j * n + i;
+						if (adj[idx_ij] > sum) {
+							int idx_ji = j * n + i;
+							adj[idx_ij] = adj[idx_ji] = sum;
+						}
+					}
+				}
+			}
+		}
+
+		// Build links.
+		for (j = 0; j < n; ++j) {
+			for (i = j + 1; i < n; ++i) {
+				int idx_ij = j * n + i;
+				if (adj[idx_ij] == (unsigned)p_distance) {
+					append_link(i, j);
+				}
+			}
+		}
+		memdelete_arr(adj);
+	}
+}
+
+//===================================================================
+//
+//
+// This function takes in a list of interdependent Links and tries
+// to maximize the distance between calculation
+// of dependent links.  This increases the amount of parallelism that can
+// be exploited by out-of-order instruction processors with large but
+// (inevitably) finite instruction windows.
+//
+//===================================================================
+
+// 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
+};
+typedef LinkDeps *LinkDepsPtr;
+
+void SoftBody3DSW::reoptimize_link_order() {
+	const int reop_not_dependent = -1;
+	const int reop_node_complete = -2;
+
+	uint32_t i, link_count = links.size(), node_count = nodes.size();
+	Link *lr;
+	int ar, br;
+	Node *node0 = &(nodes[0]);
+	Node *node1 = &(nodes[1]);
+	LinkDepsPtr link_dep;
+	int ready_list_head, ready_list_tail, link_num, link_dep_frees, dep_link;
+
+	// Allocate temporary buffers.
+	int *node_written_at = memnew_arr(int, node_count + 1); // What link calculation produced this node's current values?
+	int *link_dep_A = memnew_arr(int, link_count); // Link calculation input is dependent upon prior calculation #N
+	int *link_dep_B = memnew_arr(int, link_count);
+	int *ready_list = memnew_arr(int, link_count); // List of ready-to-process link calculations (# of links, maximum)
+	LinkDeps *link_dep_free_list = memnew_arr(LinkDeps, 2 * link_count); // Dependent-on-me list elements (2x# of links, maximum)
+	LinkDepsPtr *link_dep_list_starts = memnew_arr(LinkDepsPtr, link_count); // Start nodes of dependent-on-me lists, one for each link
+
+	// Copy the original, unsorted links to a side buffer.
+	Link *link_buffer = memnew_arr(Link, link_count);
+	memcpy(link_buffer, &(links[0]), sizeof(Link) * link_count);
+
+	// Clear out the node setup and ready list.
+	for (i = 0; i < node_count + 1; i++) {
+		node_written_at[i] = reop_not_dependent;
+	}
+	for (i = 0; i < link_count; i++) {
+		link_dep_list_starts[i] = nullptr;
+	}
+	ready_list_head = ready_list_tail = link_dep_frees = 0;
+
+	// Initial link analysis to set up data structures.
+	for (i = 0; i < link_count; i++) {
+		// Note which prior link calculations we are dependent upon & build up dependence lists.
+		lr = &(links[i]);
+		ar = (lr->n[0] - node0) / (node1 - node0);
+		br = (lr->n[1] - node0) / (node1 - node0);
+		if (node_written_at[ar] > reop_not_dependent) {
+			link_dep_A[i] = node_written_at[ar];
+			link_dep = &link_dep_free_list[link_dep_frees++];
+			link_dep->value = i;
+			link_dep->next = link_dep_list_starts[node_written_at[ar]];
+			link_dep_list_starts[node_written_at[ar]] = link_dep;
+		} else {
+			link_dep_A[i] = reop_not_dependent;
+		}
+		if (node_written_at[br] > reop_not_dependent) {
+			link_dep_B[i] = node_written_at[br];
+			link_dep = &link_dep_free_list[link_dep_frees++];
+			link_dep->value = -(int)(i + 1);
+			link_dep->next = link_dep_list_starts[node_written_at[br]];
+			link_dep_list_starts[node_written_at[br]] = link_dep;
+		} else {
+			link_dep_B[i] = reop_not_dependent;
+		}
+
+		// Add this link to the initial ready list, if it is not dependent on any other links.
+		if ((link_dep_A[i] == reop_not_dependent) && (link_dep_B[i] == reop_not_dependent)) {
+			ready_list[ready_list_tail++] = i;
+			link_dep_A[i] = link_dep_B[i] = reop_node_complete; // Probably not needed now.
+		}
+
+		// Update the nodes to mark which ones are calculated by this link.
+		node_written_at[ar] = node_written_at[br] = i;
+	}
+
+	// Process the ready list and create the sorted list of links:
+	// -- By treating the ready list as a queue, we maximize the distance between any
+	//    inter-dependent node calculations.
+	// -- All other (non-related) nodes in the ready list will automatically be inserted
+	//    in between each set of inter-dependent link calculations by this loop.
+	i = 0;
+	while (ready_list_head != ready_list_tail) {
+		// Use ready list to select the next link to process.
+		link_num = ready_list[ready_list_head++];
+		// Copy the next-to-calculate link back into the original link array.
+		links[i++] = link_buffer[link_num];
+
+		// Free up any link inputs that are dependent on this one.
+		link_dep = link_dep_list_starts[link_num];
+		while (link_dep) {
+			dep_link = link_dep->value;
+			if (dep_link >= 0) {
+				link_dep_A[dep_link] = reop_not_dependent;
+			} else {
+				dep_link = -dep_link - 1;
+				link_dep_B[dep_link] = reop_not_dependent;
+			}
+			// Add this dependent link calculation to the ready list if *both* inputs are clear.
+			if ((link_dep_A[dep_link] == reop_not_dependent) && (link_dep_B[dep_link] == reop_not_dependent)) {
+				ready_list[ready_list_tail++] = dep_link;
+				link_dep_A[dep_link] = link_dep_B[dep_link] = reop_node_complete; // Probably not needed now.
+			}
+			link_dep = link_dep->next;
+		}
+	}
+
+	// Delete the temporary buffers.
+	memdelete_arr(node_written_at);
+	memdelete_arr(link_dep_A);
+	memdelete_arr(link_dep_B);
+	memdelete_arr(ready_list);
+	memdelete_arr(link_dep_free_list);
+	memdelete_arr(link_dep_list_starts);
+	memdelete_arr(link_buffer);
+}
+
+void SoftBody3DSW::append_link(uint32_t p_node1, uint32_t p_node2) {
+	if (p_node1 == p_node2) {
+		return;
+	}
+
+	Node *node1 = &nodes[p_node1];
+	Node *node2 = &nodes[p_node2];
+
+	Link link;
+	link.n[0] = node1;
+	link.n[1] = node2;
+	link.rl = (node1->x - node2->x).length();
+
+	links.push_back(link);
+}
+
+void SoftBody3DSW::append_face(uint32_t p_node1, uint32_t p_node2, uint32_t p_node3) {
+	if (p_node1 == p_node2) {
+		return;
+	}
+	if (p_node1 == p_node3) {
+		return;
+	}
+	if (p_node2 == p_node3) {
+		return;
+	}
+
+	Node *node1 = &nodes[p_node1];
+	Node *node2 = &nodes[p_node2];
+	Node *node3 = &nodes[p_node3];
+
+	Face face;
+	face.n[0] = node1;
+	face.n[1] = node2;
+	face.n[2] = node3;
+
+	face.index = faces.size();
+
+	faces.push_back(face);
+}
+
+void SoftBody3DSW::set_iteration_count(int p_val) {
+	iteration_count = p_val;
+}
+
+void SoftBody3DSW::set_total_mass(real_t p_val) {
+	ERR_FAIL_COND(p_val < 0.0);
+
+	inv_total_mass = 1.0 / p_val;
+	real_t mass_factor = total_mass * inv_total_mass;
+	total_mass = p_val;
+
+	uint32_t node_count = nodes.size();
+	for (uint32_t node_index = 0; node_index < node_count; ++node_index) {
+		Node &node = nodes[node_index];
+		node.im *= mass_factor;
+	}
+
+	update_constants();
+}
+
+void SoftBody3DSW::set_collision_margin(real_t p_val) {
+	collision_margin = p_val;
+}
+
+void SoftBody3DSW::set_linear_stiffness(real_t p_val) {
+	linear_stiffness = p_val;
+}
+
+void SoftBody3DSW::set_pressure_coefficient(real_t p_val) {
+	pressure_coefficient = p_val;
+}
+
+void SoftBody3DSW::set_damping_coefficient(real_t p_val) {
+	damping_coefficient = p_val;
+}
+
+void SoftBody3DSW::set_drag_coefficient(real_t p_val) {
+	drag_coefficient = p_val;
+}
+
+void SoftBody3DSW::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) {
+			node.v += p_velocity;
+		}
+	}
+}
+
+void SoftBody3DSW::apply_forces() {
+	if (pressure_coefficient < CMP_EPSILON) {
+		return;
+	}
+
+	if (nodes.is_empty()) {
+		return;
+	}
+
+	uint32_t i, ni;
+
+	// Calculate volume.
+	real_t volume = 0.0;
+	const Vector3 &org = nodes[0].x;
+	for (i = 0, ni = faces.size(); i < ni; ++i) {
+		const Face &face = faces[i];
+		volume += vec3_dot(face.n[0]->x - org, vec3_cross(face.n[1]->x - org, face.n[2]->x - org));
+	}
+	volume /= 6.0;
+
+	// Apply per node forces.
+	real_t ivolumetp = 1.0 / Math::abs(volume) * pressure_coefficient;
+	for (i = 0, ni = nodes.size(); i < ni; ++i) {
+		Node &node = nodes[i];
+		if (node.im > 0) {
+			node.f += node.n * (node.area * ivolumetp);
+		}
+	}
+}
+
+void SoftBody3DSW::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);
+
+	// Apply forces.
+	Vector3 gravity = def_area->get_gravity_vector() * def_area->get_gravity();
+	add_velocity(gravity * p_delta);
+	apply_forces();
+
+	// Avoid soft body from 'exploding' so use some upper threshold of maximum motion
+	// that a node can travel per frame.
+	const real_t max_displacement = 1000.0;
+	real_t clamp_delta_v = max_displacement * inv_delta;
+
+	// Integrate.
+	uint32_t i, ni;
+	for (i = 0, ni = nodes.size(); i < ni; ++i) {
+		Node &node = nodes[i];
+		node.q = node.x;
+		Vector3 delta_v = node.f * node.im * p_delta;
+		for (int c = 0; c < 3; c++) {
+			delta_v[c] = CLAMP(delta_v[c], -clamp_delta_v, clamp_delta_v);
+		}
+		node.v += delta_v;
+		node.x += node.v * p_delta;
+		node.f = Vector3();
+	}
+
+	// Bounds and tree update.
+	update_bounds();
+
+	// Node tree update.
+	for (i = 0, ni = nodes.size(); i < ni; ++i) {
+		const Node &node = nodes[i];
+
+		AABB node_aabb(node.x, Vector3());
+		node_aabb.expand_to(node.x + node.v * p_delta);
+		node_aabb.grow_by(collision_margin);
+
+		node_tree.update(node.leaf, node_aabb);
+	}
+
+	// Face tree update.
+	if (!face_tree.is_empty()) {
+		update_face_tree(p_delta);
+	}
+
+	// Optimize node tree.
+	node_tree.optimize_incremental(1);
+	face_tree.optimize_incremental(1);
+}
+
+void SoftBody3DSW::solve_constraints(real_t p_delta) {
+	const real_t inv_delta = 1.0 / p_delta;
+
+	uint32_t i, ni;
+
+	for (i = 0, ni = links.size(); i < ni; ++i) {
+		Link &link = links[i];
+		link.c3 = link.n[1]->q - link.n[0]->q;
+		link.c2 = 1 / (link.c3.length_squared() * link.c0);
+	}
+
+	// Solve velocities.
+	for (i = 0, ni = nodes.size(); i < ni; ++i) {
+		Node &node = nodes[i];
+		node.x = node.q + node.v * p_delta;
+	}
+
+	// Solve positions.
+	for (int isolve = 0; isolve < iteration_count; ++isolve) {
+		const real_t ti = isolve / (real_t)iteration_count;
+		solve_links(1.0, ti);
+	}
+	const real_t vc = (1.0 - damping_coefficient) * inv_delta;
+	for (i = 0, ni = nodes.size(); i < ni; ++i) {
+		Node &node = nodes[i];
+
+		node.x += node.bv * p_delta;
+		node.bv = Vector3();
+
+		node.v = (node.x - node.q) * vc;
+
+		node.q = node.x;
+	}
+
+	update_normals();
+}
+
+void SoftBody3DSW::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) {
+			Node &node_a = *link.n[0];
+			Node &node_b = *link.n[1];
+			const Vector3 del = node_b.x - node_a.x;
+			const real_t len = del.length_squared();
+			if (link.c1 + len > CMP_EPSILON) {
+				const real_t k = ((link.c1 - len) / (link.c0 * (link.c1 + len))) * kst;
+				node_a.x -= del * (k * node_a.im);
+				node_b.x += del * (k * node_b.im);
+			}
+		}
+	}
+}
+
+struct AABBQueryResult {
+	const SoftBody3DSW *soft_body = nullptr;
+	void *userdata = nullptr;
+	SoftBody3DSW::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) {
+	AABBQueryResult query_result;
+	query_result.soft_body = this;
+	query_result.result_callback = p_result_callback;
+	query_result.userdata = p_userdata;
+
+	node_tree.aabb_query(p_aabb, query_result);
+}
+
+struct RayQueryResult {
+	const SoftBody3DSW *soft_body = nullptr;
+	void *userdata = nullptr;
+	SoftBody3DSW::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) {
+	if (face_tree.is_empty()) {
+		initialize_face_tree();
+	}
+
+	RayQueryResult query_result;
+	query_result.soft_body = this;
+	query_result.result_callback = p_result_callback;
+	query_result.userdata = p_userdata;
+
+	face_tree.ray_query(p_from, p_to, query_result);
+}
+
+void SoftBody3DSW::initialize_face_tree() {
+	face_tree.clear();
+	for (uint32_t i = 0; i < faces.size(); ++i) {
+		Face &face = faces[i];
+
+		AABB face_aabb;
+
+		face_aabb.position = face.n[0]->x;
+		face_aabb.expand_to(face.n[1]->x);
+		face_aabb.expand_to(face.n[2]->x);
+
+		face_aabb.grow_by(collision_margin);
+
+		face.leaf = face_tree.insert(face_aabb, &face);
+	}
+}
+
+void SoftBody3DSW::update_face_tree(real_t p_delta) {
+	for (uint32_t i = 0; i < faces.size(); ++i) {
+		const Face &face = faces[i];
+
+		AABB face_aabb;
+
+		const Node *node0 = face.n[0];
+		face_aabb.position = node0->x;
+		face_aabb.expand_to(node0->x + node0->v * p_delta);
+
+		const Node *node1 = face.n[1];
+		face_aabb.expand_to(node1->x);
+		face_aabb.expand_to(node1->x + node1->v * p_delta);
+
+		const Node *node2 = face.n[2];
+		face_aabb.expand_to(node2->x);
+		face_aabb.expand_to(node2->x + node2->v * p_delta);
+
+		face_aabb.grow_by(collision_margin);
+
+		face_tree.update(face.leaf, face_aabb);
+	}
+}
+
+void SoftBody3DSW::initialize_shape(bool p_force_move) {
+	if (get_shape_count() == 0) {
+		SoftBodyShape3DSW *soft_body_shape = memnew(SoftBodyShape3DSW(this));
+		add_shape(soft_body_shape);
+	} else if (p_force_move) {
+		SoftBodyShape3DSW *soft_body_shape = static_cast<SoftBodyShape3DSW *>(get_shape(0));
+		soft_body_shape->update_bounds();
+	}
+}
+
+void SoftBody3DSW::deinitialize_shape() {
+	if (get_shape_count() > 0) {
+		Shape3DSW *shape = get_shape(0);
+		remove_shape(shape);
+		memdelete(shape);
+	}
+}
+
+void SoftBody3DSW::destroy() {
+	map_visual_to_physics.clear();
+
+	node_tree.clear();
+	face_tree.clear();
+
+	nodes.clear();
+	links.clear();
+	faces.clear();
+
+	bounds = AABB();
+	deinitialize_shape();
+}
+
+void SoftBodyShape3DSW::update_bounds() {
+	ERR_FAIL_COND(!soft_body);
+
+	AABB collision_aabb = soft_body->get_bounds();
+	collision_aabb.grow_by(soft_body->get_collision_margin());
+	configure(collision_aabb);
+}
+
+SoftBodyShape3DSW::SoftBodyShape3DSW(SoftBody3DSW *p_soft_body) {
+	soft_body = p_soft_body;
+	update_bounds();
+}
+
+struct _SoftBodyIntersectSegmentInfo {
+	const SoftBody3DSW *soft_body = nullptr;
+	Vector3 from;
+	Vector3 dir;
+	Vector3 hit_position;
+	uint32_t hit_face_index = -1;
+	real_t hit_dist_sq = Math_INF;
+
+	static bool process_hit(uint32_t p_face_index, void *p_userdata) {
+		_SoftBodyIntersectSegmentInfo &query_info = *(_SoftBodyIntersectSegmentInfo *)(p_userdata);
+
+		Vector3 points[3];
+		query_info.soft_body->get_face_points(p_face_index, points[0], points[1], points[2]);
+
+		Vector3 result;
+		if (Geometry3D::ray_intersects_triangle(query_info.from, query_info.dir, points[0], points[1], points[2], &result)) {
+			real_t dist_sq = query_info.from.distance_squared_to(result);
+			if (dist_sq < query_info.hit_dist_sq) {
+				query_info.hit_dist_sq = dist_sq;
+				query_info.hit_position = result;
+				query_info.hit_face_index = p_face_index;
+			}
+		}
+
+		// Continue with the query.
+		return false;
+	}
+};
+
+bool SoftBodyShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const {
+	_SoftBodyIntersectSegmentInfo query_info;
+	query_info.soft_body = soft_body;
+	query_info.from = p_begin;
+	query_info.dir = (p_end - p_begin).normalized();
+
+	soft_body->query_ray(p_begin, p_end, _SoftBodyIntersectSegmentInfo::process_hit, &query_info);
+
+	if (query_info.hit_dist_sq != Math_INF) {
+		r_result = query_info.hit_position;
+		r_normal = soft_body->get_face_normal(query_info.hit_face_index);
+		return true;
+	}
+
+	return false;
+}
+
+bool SoftBodyShape3DSW::intersect_point(const Vector3 &p_point) const {
+	return false;
+}
+
+Vector3 SoftBodyShape3DSW::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/soft_body_3d_sw.h
new file mode 100644
index 0000000000..6c0451ff45
--- /dev/null
+++ b/servers/physics_3d/soft_body_3d_sw.h
@@ -0,0 +1,247 @@
+/*************************************************************************/
+/*  soft_body_3d_sw.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 SOFT_BODY_3D_SW_H
+#define SOFT_BODY_3D_SW_H
+
+#include "collision_object_3d_sw.h"
+
+#include "core/math/aabb.h"
+#include "core/math/dynamic_bvh.h"
+#include "core/math/vector3.h"
+#include "core/templates/local_vector.h"
+#include "core/templates/set.h"
+#include "core/templates/vset.h"
+#include "scene/resources/mesh.h"
+
+class Constraint3DSW;
+
+class SoftBody3DSW : public CollisionObject3DSW {
+	Ref<Mesh> soft_mesh;
+
+	struct Node {
+		Vector3 s; // Source position
+		Vector3 x; // Position
+		Vector3 q; // Previous step position/Test position
+		Vector3 f; // Force accumulator
+		Vector3 v; // Velocity
+		Vector3 bv; // Biased Velocity
+		Vector3 n; // Normal
+		real_t area = 0.0; // Area
+		real_t im = 0.0; // 1/mass
+		DynamicBVH::ID leaf; // Leaf data
+		uint32_t index = 0;
+	};
+
+	struct Link {
+		Vector3 c3; // gradient
+		Node *n[2] = { nullptr, nullptr }; // Node pointers
+		real_t rl = 0.0; // Rest length
+		real_t c0 = 0.0; // (ima+imb)*kLST
+		real_t c1 = 0.0; // rl^2
+		real_t c2 = 0.0; // |gradient|^2/c0
+	};
+
+	struct Face {
+		Node *n[3] = { nullptr, nullptr, nullptr }; // Node pointers
+		Vector3 normal; // Normal
+		real_t ra = 0.0; // Rest area
+		DynamicBVH::ID leaf; // Leaf data
+		uint32_t index = 0;
+	};
+
+	LocalVector<Node> nodes;
+	LocalVector<Link> links;
+	LocalVector<Face> faces;
+
+	DynamicBVH node_tree;
+	DynamicBVH face_tree;
+
+	LocalVector<uint32_t> map_visual_to_physics;
+
+	AABB bounds;
+
+	real_t collision_margin = 0.05;
+
+	real_t total_mass = 1.0;
+	real_t inv_total_mass = 1.0;
+
+	int iteration_count = 5;
+	real_t linear_stiffness = 0.5; // [0,1]
+	real_t pressure_coefficient = 0.0; // [-inf,+inf]
+	real_t damping_coefficient = 0.01; // [0,1]
+	real_t drag_coefficient = 0.0; // [0,1]
+	LocalVector<int> pinned_vertices;
+
+	SelfList<SoftBody3DSW> active_list;
+
+	Set<Constraint3DSW *> constraints;
+
+	VSet<RID> exceptions;
+
+public:
+	SoftBody3DSW();
+
+	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 clear_constraints() { constraints.clear(); }
+
+	_FORCE_INLINE_ void add_exception(const RID &p_exception) { exceptions.insert(p_exception); }
+	_FORCE_INLINE_ void remove_exception(const RID &p_exception) { exceptions.erase(p_exception); }
+	_FORCE_INLINE_ bool has_exception(const RID &p_exception) const { return exceptions.has(p_exception); }
+	_FORCE_INLINE_ const VSet<RID> &get_exceptions() const { return exceptions; }
+
+	virtual void set_space(Space3DSW *p_space);
+
+	void set_mesh(const Ref<Mesh> &p_mesh);
+
+	void update_rendering_server(RenderingServerHandler *p_rendering_server_handler);
+
+	Vector3 get_vertex_position(int p_index) const;
+	void set_vertex_position(int p_index, const Vector3 &p_position);
+
+	void pin_vertex(int p_index);
+	void unpin_vertex(int p_index);
+	void unpin_all_vertices();
+	bool is_vertex_pinned(int p_index) const;
+
+	uint32_t get_node_count() const;
+	real_t get_node_inv_mass(uint32_t p_node_index) const;
+	Vector3 get_node_position(uint32_t p_node_index) const;
+	Vector3 get_node_velocity(uint32_t p_node_index) const;
+	Vector3 get_node_biased_velocity(uint32_t p_node_index) const;
+	void apply_node_impulse(uint32_t p_node_index, const Vector3 &p_impulse);
+	void apply_node_bias_impulse(uint32_t p_node_index, const Vector3 &p_impulse);
+
+	uint32_t get_face_count() const;
+	void get_face_points(uint32_t p_face_index, Vector3 &r_point_1, Vector3 &r_point_2, Vector3 &r_point_3) const;
+	Vector3 get_face_normal(uint32_t p_face_index) const;
+
+	void set_iteration_count(int p_val);
+	_FORCE_INLINE_ real_t get_iteration_count() const { return iteration_count; }
+
+	void set_total_mass(real_t p_val);
+	_FORCE_INLINE_ real_t get_total_mass() const { return total_mass; }
+	_FORCE_INLINE_ real_t get_total_inv_mass() const { return inv_total_mass; }
+
+	void set_collision_margin(real_t p_val);
+	_FORCE_INLINE_ real_t get_collision_margin() const { return collision_margin; }
+
+	void set_linear_stiffness(real_t p_val);
+	_FORCE_INLINE_ real_t get_linear_stiffness() const { return linear_stiffness; }
+
+	void set_pressure_coefficient(real_t p_val);
+	_FORCE_INLINE_ real_t get_pressure_coefficient() const { return pressure_coefficient; }
+
+	void set_damping_coefficient(real_t p_val);
+	_FORCE_INLINE_ real_t get_damping_coefficient() const { return damping_coefficient; }
+
+	void set_drag_coefficient(real_t p_val);
+	_FORCE_INLINE_ real_t get_drag_coefficient() const { return drag_coefficient; }
+
+	void predict_motion(real_t p_delta);
+	void solve_constraints(real_t p_delta);
+
+	_FORCE_INLINE_ uint32_t get_node_index(void *p_node) const { return ((Node *)p_node)->index; }
+	_FORCE_INLINE_ uint32_t get_face_index(void *p_face) const { return ((Face *)p_face)->index; }
+
+	// Return true to stop the query.
+	// p_index is the node index for AABB query, face index for Ray query.
+	typedef bool (*QueryResultCallback)(uint32_t p_index, void *p_userdata);
+
+	void query_aabb(const AABB &p_aabb, QueryResultCallback p_result_callback, void *p_userdata);
+	void query_ray(const Vector3 &p_from, const Vector3 &p_to, QueryResultCallback p_result_callback, void *p_userdata);
+
+protected:
+	virtual void _shapes_changed();
+
+private:
+	void update_normals();
+	void update_bounds();
+	void update_constants();
+	void update_area();
+	void reset_link_rest_lengths();
+	void update_link_constants();
+
+	void apply_nodes_transform(const Transform &p_transform);
+
+	void add_velocity(const Vector3 &p_velocity);
+
+	void apply_forces();
+
+	bool create_from_trimesh(const Vector<int> &p_indices, const Vector<Vector3> &p_vertices);
+	void generate_bending_constraints(int p_distance);
+	void reoptimize_link_order();
+	void append_link(uint32_t p_node1, uint32_t p_node2);
+	void append_face(uint32_t p_node1, uint32_t p_node2, uint32_t p_node3);
+
+	void solve_links(real_t kst, real_t ti);
+
+	void initialize_face_tree();
+	void update_face_tree(real_t p_delta);
+
+	void initialize_shape(bool p_force_move = true);
+	void deinitialize_shape();
+
+	void destroy();
+};
+
+class SoftBodyShape3DSW : public Shape3DSW {
+	SoftBody3DSW *soft_body = nullptr;
+
+public:
+	SoftBody3DSW *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 Transform &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 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 void set_data(const Variant &p_data) {}
+	virtual Variant get_data() const { return Variant(); }
+
+	void update_bounds();
+
+	SoftBodyShape3DSW(SoftBody3DSW *p_soft_body);
+	~SoftBodyShape3DSW() {}
+};
+
+#endif // SOFT_BODY_3D_SW_H
diff --git a/servers/physics_3d/space_3d_sw.cpp b/servers/physics_3d/space_3d_sw.cpp
index c8741dc930..c2110a7a44 100644
--- a/servers/physics_3d/space_3d_sw.cpp
+++ b/servers/physics_3d/space_3d_sw.cpp
@@ -47,6 +47,10 @@ _FORCE_INLINE_ static bool _can_collide_with(CollisionObject3DSW *p_object, uint
 		return false;
 	}
 
+	if (p_object->get_type() == CollisionObject3DSW::TYPE_SOFT_BODY && !p_collide_with_bodies) {
+		return false;
+	}
+
 	return true;
 }
 
@@ -407,7 +411,7 @@ struct _RestCallbackData {
 	real_t min_allowed_depth;
 };
 
-static void _rest_cbk_result(const Vector3 &p_point_A, const Vector3 &p_point_B, void *p_userdata) {
+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) {
 	_RestCallbackData *rd = (_RestCallbackData *)p_userdata;
 
 	Vector3 contact_rel = p_point_B - p_point_A;
@@ -544,6 +548,8 @@ 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_AREA) {
 			keep = false;
+		} else if (intersection_query_results[i]->get_type() == CollisionObject3DSW::TYPE_SOFT_BODY) {
+			keep = false;
 		} else if ((static_cast<Body3DSW *>(intersection_query_results[i])->test_collision_mask(p_body)) == 0) {
 			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())) {
@@ -1054,14 +1060,23 @@ void *Space3DSW::_broadphase_pair(CollisionObject3DSW *A, int p_subindex_A, Coll
 			Area3DSW *area_b = static_cast<Area3DSW *>(B);
 			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.
 		} else {
 			Body3DSW *body = static_cast<Body3DSW *>(B);
 			AreaPair3DSW *area_pair = memnew(AreaPair3DSW(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));
+			return soft_pair;
+		} else {
+			BodyPair3DSW *b = memnew(BodyPair3DSW((Body3DSW *)A, p_subindex_A, (Body3DSW *)B, p_subindex_B));
+			return b;
+		}
 	} else {
-		BodyPair3DSW *b = memnew(BodyPair3DSW((Body3DSW *)A, p_subindex_A, (Body3DSW *)B, p_subindex_B));
-		return b;
+		// Soft Body/Soft Body, not supported.
 	}
 
 	return nullptr;
@@ -1144,6 +1159,18 @@ const SelfList<Area3DSW>::List &Space3DSW::get_moved_area_list() const {
 	return area_moved_list;
 }
 
+const SelfList<SoftBody3DSW>::List &Space3DSW::get_active_soft_body_list() const {
+	return active_soft_body_list;
+}
+
+void Space3DSW::soft_body_add_to_active_list(SelfList<SoftBody3DSW> *p_soft_body) {
+	active_soft_body_list.add(p_soft_body);
+}
+
+void Space3DSW::soft_body_remove_from_active_list(SelfList<SoftBody3DSW> *p_soft_body) {
+	active_soft_body_list.remove(p_soft_body);
+}
+
 void Space3DSW::call_queries() {
 	while (state_query_list.first()) {
 		Body3DSW *b = state_query_list.first()->self();
diff --git a/servers/physics_3d/space_3d_sw.h b/servers/physics_3d/space_3d_sw.h
index eed3d86a72..3a8f452e54 100644
--- a/servers/physics_3d/space_3d_sw.h
+++ b/servers/physics_3d/space_3d_sw.h
@@ -40,6 +40,7 @@
 #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);
@@ -82,6 +83,7 @@ private:
 	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;
 
 	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);
@@ -145,6 +147,10 @@ public:
 	void area_remove_from_moved_list(SelfList<Area3DSW> *p_area);
 	const SelfList<Area3DSW>::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);
+
 	BroadPhase3DSW *get_broadphase();
 
 	void add_object(CollisionObject3DSW *p_object);
diff --git a/servers/physics_3d/step_3d_sw.cpp b/servers/physics_3d/step_3d_sw.cpp
index d9370de6a3..9a73e11562 100644
--- a/servers/physics_3d/step_3d_sw.cpp
+++ b/servers/physics_3d/step_3d_sw.cpp
@@ -146,6 +146,8 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
 
 	const SelfList<Body3DSW>::List *body_list = &p_space->get_active_body_list();
 
+	const SelfList<SoftBody3DSW>::List *soft_body_list = &p_space->get_active_soft_body_list();
+
 	/* INTEGRATE FORCES */
 
 	uint64_t profile_begtime = OS::get_singleton()->get_ticks_usec();
@@ -160,6 +162,15 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
 		active_count++;
 	}
 
+	/* UPDATE SOFT BODY MOTION */
+
+	const SelfList<SoftBody3DSW> *sb = soft_body_list->first();
+	while (sb) {
+		sb->self()->predict_motion(p_delta);
+		sb = sb->next();
+		active_count++;
+	}
+
 	p_space->set_active_objects(active_count);
 
 	{ //profile
@@ -214,6 +225,20 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
 		p_space->area_remove_from_moved_list((SelfList<Area3DSW> *)aml.first()); //faster to remove here
 	}
 
+	sb = soft_body_list->first();
+	while (sb) {
+		for (const Set<Constraint3DSW *>::Element *E = sb->self()->get_constraints().front(); E; E = E->next()) {
+			Constraint3DSW *c = E->get();
+			if (c->get_island_step() == _step)
+				continue;
+			c->set_island_step(_step);
+			c->set_island_next(NULL);
+			c->set_island_list_next(constraint_island_list);
+			constraint_island_list = c;
+		}
+		sb = sb->next();
+	}
+
 	{ //profile
 		profile_endtime = OS::get_singleton()->get_ticks_usec();
 		p_space->set_elapsed_time(Space3DSW::ELAPSED_TIME_GENERATE_ISLANDS, profile_endtime - profile_begtime);
@@ -272,6 +297,14 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
 		}
 	}
 
+	/* UPDATE SOFT BODY CONSTRAINTS */
+
+	sb = soft_body_list->first();
+	while (sb) {
+		sb->self()->solve_constraints(p_delta);
+		sb = sb->next();
+	}
+
 	{ //profile
 		profile_endtime = OS::get_singleton()->get_ticks_usec();
 		p_space->set_elapsed_time(Space3DSW::ELAPSED_TIME_INTEGRATE_VELOCITIES, profile_endtime - profile_begtime);
diff --git a/servers/physics_server_3d.cpp b/servers/physics_server_3d.cpp
index af25029f04..586845de99 100644
--- a/servers/physics_server_3d.cpp
+++ b/servers/physics_server_3d.cpp
@@ -556,6 +556,10 @@ void PhysicsServer3D::_bind_methods() {
 
 	ClassDB::bind_method(D_METHOD("body_get_direct_state", "body"), &PhysicsServer3D::body_get_direct_state);
 
+	/* SOFT BODY API */
+
+	ClassDB::bind_method(D_METHOD("soft_body_get_bounds", "body"), &PhysicsServer3D::soft_body_get_bounds);
+
 	/* JOINT API */
 
 	ClassDB::bind_method(D_METHOD("joint_create"), &PhysicsServer3D::joint_create);
@@ -693,6 +697,7 @@ void PhysicsServer3D::_bind_methods() {
 	BIND_ENUM_CONSTANT(SHAPE_CONVEX_POLYGON);
 	BIND_ENUM_CONSTANT(SHAPE_CONCAVE_POLYGON);
 	BIND_ENUM_CONSTANT(SHAPE_HEIGHTMAP);
+	BIND_ENUM_CONSTANT(SHAPE_SOFT_BODY);
 	BIND_ENUM_CONSTANT(SHAPE_CUSTOM);
 
 	BIND_ENUM_CONSTANT(AREA_PARAM_GRAVITY);
diff --git a/servers/physics_server_3d.h b/servers/physics_server_3d.h
index e16857192c..69f5c1c0ad 100644
--- a/servers/physics_server_3d.h
+++ b/servers/physics_server_3d.h
@@ -216,6 +216,15 @@ public:
 	PhysicsShapeQueryResult3D();
 };
 
+class RenderingServerHandler {
+public:
+	virtual void set_vertex(int p_vertex_id, const void *p_vector3) = 0;
+	virtual void set_normal(int p_vertex_id, const void *p_vector3) = 0;
+	virtual void set_aabb(const AABB &p_aabb) = 0;
+
+	virtual ~RenderingServerHandler() {}
+};
+
 class PhysicsServer3D : public Object {
 	GDCLASS(PhysicsServer3D, Object);
 
@@ -237,6 +246,7 @@ public:
 		SHAPE_CONVEX_POLYGON, ///< array of planes:"planes"
 		SHAPE_CONCAVE_POLYGON, ///< vector3 array:"triangles" , or Dictionary with "indices" (int array) and "triangles" (Vector3 array)
 		SHAPE_HEIGHTMAP, ///< dict( int:"width", int:"depth",float:"cell_size", float_array:"heights"
+		SHAPE_SOFT_BODY, ///< Used internally, can't be created from the physics server.
 		SHAPE_CUSTOM, ///< Server-Implementation based custom shape, calling shape_create() with this value will result in an error
 	};
 
@@ -522,13 +532,15 @@ public:
 
 	virtual RID soft_body_create() = 0;
 
-	virtual void soft_body_update_rendering_server(RID p_body, class SoftBodyRenderingServerHandler *p_rendering_server_handler) = 0;
+	virtual void soft_body_update_rendering_server(RID p_body, RenderingServerHandler *p_rendering_server_handler) = 0;
 
 	virtual void soft_body_set_space(RID p_body, RID p_space) = 0;
 	virtual RID soft_body_get_space(RID p_body) const = 0;
 
 	virtual void soft_body_set_mesh(RID p_body, const REF &p_mesh) = 0;
 
+	virtual AABB soft_body_get_bounds(RID p_body) const = 0;
+
 	virtual void soft_body_set_collision_layer(RID p_body, uint32_t p_layer) = 0;
 	virtual uint32_t soft_body_get_collision_layer(RID p_body) const = 0;
 
@@ -543,7 +555,6 @@ public:
 	virtual Variant soft_body_get_state(RID p_body, BodyState p_state) const = 0;
 
 	virtual void soft_body_set_transform(RID p_body, const Transform &p_transform) = 0;
-	virtual Vector3 soft_body_get_vertex_position(RID p_body, int vertex_index) const = 0;
 
 	virtual void soft_body_set_ray_pickable(RID p_body, bool p_enable) = 0;
 
@@ -556,18 +567,9 @@ public:
 	virtual void soft_body_set_linear_stiffness(RID p_body, real_t p_stiffness) = 0;
 	virtual real_t soft_body_get_linear_stiffness(RID p_body) const = 0;
 
-	virtual void soft_body_set_angular_stiffness(RID p_body, real_t p_stiffness) = 0;
-	virtual real_t soft_body_get_angular_stiffness(RID p_body) const = 0;
-
-	virtual void soft_body_set_volume_stiffness(RID p_body, real_t p_stiffness) = 0;
-	virtual real_t soft_body_get_volume_stiffness(RID p_body) const = 0;
-
 	virtual void soft_body_set_pressure_coefficient(RID p_body, real_t p_pressure_coefficient) = 0;
 	virtual real_t soft_body_get_pressure_coefficient(RID p_body) const = 0;
 
-	virtual void soft_body_set_pose_matching_coefficient(RID p_body, real_t p_pose_matching_coefficient) = 0;
-	virtual real_t soft_body_get_pose_matching_coefficient(RID p_body) const = 0;
-
 	virtual void soft_body_set_damping_coefficient(RID p_body, real_t p_damping_coefficient) = 0;
 	virtual real_t soft_body_get_damping_coefficient(RID p_body) const = 0;
 
@@ -577,8 +579,6 @@ public:
 	virtual void soft_body_move_point(RID p_body, int p_point_index, const Vector3 &p_global_position) = 0;
 	virtual Vector3 soft_body_get_point_global_position(RID p_body, int p_point_index) const = 0;
 
-	virtual Vector3 soft_body_get_point_offset(RID p_body, int p_point_index) const = 0;
-
 	virtual void soft_body_remove_all_pinned_points(RID p_body) = 0;
 	virtual void soft_body_pin_point(RID p_body, int p_point_index, bool p_pin) = 0;
 	virtual bool soft_body_is_point_pinned(RID p_body, int p_point_index) const = 0;