Implemented Soft body

- Soft Body Physics node
- Soft Body Rendering
- Soft body Editor
- Soft body importer

8 files changed, 603 insertions, 211 deletions

diff --git a/modules/bullet/bullet_physics_server.cpp b/modules/bullet/bullet_physics_server.cpp
index 77484c9efc..2390c71b0a 100644
--- a/modules/bullet/bullet_physics_server.cpp
+++ b/modules/bullet/bullet_physics_server.cpp
@@ -169,7 +169,7 @@ real_t BulletPhysicsServer::shape_get_custom_solver_bias(RID p_shape) const {
 }
 
 RID BulletPhysicsServer::space_create() {
-	SpaceBullet *space = bulletnew(SpaceBullet(false));
+	SpaceBullet *space = bulletnew(SpaceBullet);
 	CreateThenReturnRID(space_owner, space);
 }
 
@@ -567,9 +567,6 @@ void BulletPhysicsServer::body_clear_shapes(RID p_body) {
 
 void BulletPhysicsServer::body_attach_object_instance_id(RID p_body, uint32_t p_ID) {
 	CollisionObjectBullet *body = get_collisin_object(p_body);
-	if (!body) {
-		body = soft_body_owner.get(p_body);
-	}
 	ERR_FAIL_COND(!body);
 
 	body->set_instance_id(p_ID);
@@ -867,6 +864,13 @@ RID BulletPhysicsServer::soft_body_create(bool p_init_sleeping) {
 	CreateThenReturnRID(soft_body_owner, body);
 }
 
+void BulletPhysicsServer::soft_body_update_visual_server(RID p_body, class SoftBodyVisualServerHandler *p_visual_server_handler) {
+	SoftBodyBullet *body = soft_body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->update_visual_server(p_visual_server_handler);
+}
+
 void BulletPhysicsServer::soft_body_set_space(RID p_body, RID p_space) {
 	SoftBodyBullet *body = soft_body_owner.get(p_body);
 	ERR_FAIL_COND(!body);
@@ -893,11 +897,11 @@ RID BulletPhysicsServer::soft_body_get_space(RID p_body) const {
 	return space->get_self();
 }
 
-void BulletPhysicsServer::soft_body_set_trimesh_body_shape(RID p_body, PoolVector<int> p_indices, PoolVector<Vector3> p_vertices, int p_triangles_num) {
+void BulletPhysicsServer::soft_body_set_mesh(RID p_body, const REF &p_mesh) {
 	SoftBodyBullet *body = soft_body_owner.get(p_body);
 	ERR_FAIL_COND(!body);
 
-	body->set_trimesh_body_shape(p_indices, p_vertices, p_triangles_num);
+	body->set_soft_mesh(p_mesh);
 }
 
 void BulletPhysicsServer::soft_body_set_collision_layer(RID p_body, uint32_t p_layer) {
@@ -975,14 +979,16 @@ void BulletPhysicsServer::soft_body_set_transform(RID p_body, const Transform &p
 	SoftBodyBullet *body = soft_body_owner.get(p_body);
 	ERR_FAIL_COND(!body);
 
-	body->set_transform(p_transform);
+	body->set_soft_transform(p_transform);
 }
 
-Transform BulletPhysicsServer::soft_body_get_transform(RID p_body) const {
+Vector3 BulletPhysicsServer::soft_body_get_vertex_position(RID p_body, int vertex_index) const {
 	const SoftBodyBullet *body = soft_body_owner.get(p_body);
-	ERR_FAIL_COND_V(!body, Transform());
+	Vector3 pos;
+	ERR_FAIL_COND_V(!body, pos);
 
-	return body->get_transform();
+	body->get_node_position(vertex_index, pos);
+	return pos;
 }
 
 void BulletPhysicsServer::soft_body_set_ray_pickable(RID p_body, bool p_enable) {
@@ -997,6 +1003,154 @@ bool BulletPhysicsServer::soft_body_is_ray_pickable(RID p_body) const {
 	return body->is_ray_pickable();
 }
 
+void BulletPhysicsServer::soft_body_set_simulation_precision(RID p_body, int p_simulation_precision) {
+	SoftBodyBullet *body = soft_body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+	body->set_simulation_precision(p_simulation_precision);
+}
+
+int BulletPhysicsServer::soft_body_get_simulation_precision(RID p_body) {
+	SoftBodyBullet *body = soft_body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body, 0.f);
+	return body->get_simulation_precision();
+}
+
+void BulletPhysicsServer::soft_body_set_total_mass(RID p_body, real_t p_total_mass) {
+	SoftBodyBullet *body = soft_body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+	body->set_total_mass(p_total_mass);
+}
+
+real_t BulletPhysicsServer::soft_body_get_total_mass(RID p_body) {
+	SoftBodyBullet *body = soft_body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body, 0.f);
+	return body->get_total_mass();
+}
+
+void BulletPhysicsServer::soft_body_set_linear_stiffness(RID p_body, real_t p_stiffness) {
+	SoftBodyBullet *body = soft_body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+	body->set_linear_stiffness(p_stiffness);
+}
+
+real_t BulletPhysicsServer::soft_body_get_linear_stiffness(RID p_body) {
+	SoftBodyBullet *body = soft_body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body, 0.f);
+	return body->get_linear_stiffness();
+}
+
+void BulletPhysicsServer::soft_body_set_areaAngular_stiffness(RID p_body, real_t p_stiffness) {
+	SoftBodyBullet *body = soft_body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+	body->set_areaAngular_stiffness(p_stiffness);
+}
+
+real_t BulletPhysicsServer::soft_body_get_areaAngular_stiffness(RID p_body) {
+	SoftBodyBullet *body = soft_body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body, 0.f);
+	return body->get_areaAngular_stiffness();
+}
+
+void BulletPhysicsServer::soft_body_set_volume_stiffness(RID p_body, real_t p_stiffness) {
+	SoftBodyBullet *body = soft_body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+	body->set_volume_stiffness(p_stiffness);
+}
+
+real_t BulletPhysicsServer::soft_body_get_volume_stiffness(RID p_body) {
+	SoftBodyBullet *body = soft_body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body, 0.f);
+	return body->get_volume_stiffness();
+}
+
+void BulletPhysicsServer::soft_body_set_pressure_coefficient(RID p_body, real_t p_pressure_coefficient) {
+	SoftBodyBullet *body = soft_body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+	body->set_pressure_coefficient(p_pressure_coefficient);
+}
+
+real_t BulletPhysicsServer::soft_body_get_pressure_coefficient(RID p_body) {
+	SoftBodyBullet *body = soft_body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body, 0.f);
+	return body->get_pressure_coefficient();
+}
+
+void BulletPhysicsServer::soft_body_set_pose_matching_coefficient(RID p_body, real_t p_pose_matching_coefficient) {
+	SoftBodyBullet *body = soft_body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+	return body->set_pose_matching_coefficient(p_pose_matching_coefficient);
+}
+
+real_t BulletPhysicsServer::soft_body_get_pose_matching_coefficient(RID p_body) {
+	SoftBodyBullet *body = soft_body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body, 0.f);
+	return body->get_pose_matching_coefficient();
+}
+
+void BulletPhysicsServer::soft_body_set_damping_coefficient(RID p_body, real_t p_damping_coefficient) {
+	SoftBodyBullet *body = soft_body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+	body->set_damping_coefficient(p_damping_coefficient);
+}
+
+real_t BulletPhysicsServer::soft_body_get_damping_coefficient(RID p_body) {
+	SoftBodyBullet *body = soft_body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body, 0.f);
+	return body->get_damping_coefficient();
+}
+
+void BulletPhysicsServer::soft_body_set_drag_coefficient(RID p_body, real_t p_drag_coefficient) {
+	SoftBodyBullet *body = soft_body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+	body->set_drag_coefficient(p_drag_coefficient);
+}
+
+real_t BulletPhysicsServer::soft_body_get_drag_coefficient(RID p_body) {
+	SoftBodyBullet *body = soft_body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body, 0.f);
+	return body->get_drag_coefficient();
+}
+
+void BulletPhysicsServer::soft_body_move_point(RID p_body, int p_point_index, const Vector3 &p_global_position) {
+	SoftBodyBullet *body = soft_body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+	body->set_node_position(p_point_index, p_global_position);
+}
+
+Vector3 BulletPhysicsServer::soft_body_get_point_global_position(RID p_body, int p_point_index) {
+	SoftBodyBullet *body = soft_body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body, Vector3(0., 0., 0.));
+	Vector3 pos;
+	body->get_node_position(p_point_index, pos);
+	return pos;
+}
+
+Vector3 BulletPhysicsServer::soft_body_get_point_offset(RID p_body, int p_point_index) const {
+	SoftBodyBullet *body = soft_body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body, Vector3());
+	Vector3 res;
+	body->get_node_offset(p_point_index, res);
+	return res;
+}
+
+void BulletPhysicsServer::soft_body_remove_all_pinned_points(RID p_body) {
+	SoftBodyBullet *body = soft_body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+	body->reset_all_node_mass();
+}
+
+void BulletPhysicsServer::soft_body_pin_point(RID p_body, int p_point_index, bool p_pin) {
+	SoftBodyBullet *body = soft_body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+	body->set_node_mass(p_point_index, p_pin ? 0 : 1);
+}
+
+bool BulletPhysicsServer::soft_body_is_point_pinned(RID p_body, int p_point_index) {
+	SoftBodyBullet *body = soft_body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body, 0.f);
+	return body->get_node_mass(p_point_index);
+}
+
 PhysicsServer::JointType BulletPhysicsServer::joint_get_type(RID p_joint) const {
 	JointBullet *joint = joint_owner.get(p_joint);
 	ERR_FAIL_COND_V(!joint, JOINT_PIN);
diff --git a/modules/bullet/bullet_physics_server.h b/modules/bullet/bullet_physics_server.h
index 06769106c3..2165845529 100644
--- a/modules/bullet/bullet_physics_server.h
+++ b/modules/bullet/bullet_physics_server.h
@@ -262,10 +262,12 @@ public:
 
 	virtual RID soft_body_create(bool p_init_sleeping = false);
 
+	virtual void soft_body_update_visual_server(RID p_body, class SoftBodyVisualServerHandler *p_visual_server_handler);
+
 	virtual void soft_body_set_space(RID p_body, RID p_space);
 	virtual RID soft_body_get_space(RID p_body) const;
 
-	virtual void soft_body_set_trimesh_body_shape(RID p_body, PoolVector<int> p_indices, PoolVector<Vector3> p_vertices, int p_triangles_num);
+	virtual void soft_body_set_mesh(RID p_body, const REF &p_mesh);
 
 	virtual void soft_body_set_collision_layer(RID p_body, uint32_t p_layer);
 	virtual uint32_t soft_body_get_collision_layer(RID p_body) const;
@@ -280,12 +282,49 @@ public:
 	virtual void soft_body_set_state(RID p_body, BodyState p_state, const Variant &p_variant);
 	virtual Variant soft_body_get_state(RID p_body, BodyState p_state) const;
 
+	/// Special function. This function has bad performance
 	virtual void soft_body_set_transform(RID p_body, const Transform &p_transform);
-	virtual Transform soft_body_get_transform(RID p_body) const;
+	virtual Vector3 soft_body_get_vertex_position(RID p_body, int vertex_index) const;
 
 	virtual void soft_body_set_ray_pickable(RID p_body, bool p_enable);
 	virtual bool soft_body_is_ray_pickable(RID p_body) const;
 
+	virtual void soft_body_set_simulation_precision(RID p_body, int p_simulation_precision);
+	virtual int soft_body_get_simulation_precision(RID p_body);
+
+	virtual void soft_body_set_total_mass(RID p_body, real_t p_total_mass);
+	virtual real_t soft_body_get_total_mass(RID p_body);
+
+	virtual void soft_body_set_linear_stiffness(RID p_body, real_t p_stiffness);
+	virtual real_t soft_body_get_linear_stiffness(RID p_body);
+
+	virtual void soft_body_set_areaAngular_stiffness(RID p_body, real_t p_stiffness);
+	virtual real_t soft_body_get_areaAngular_stiffness(RID p_body);
+
+	virtual void soft_body_set_volume_stiffness(RID p_body, real_t p_stiffness);
+	virtual real_t soft_body_get_volume_stiffness(RID p_body);
+
+	virtual void soft_body_set_pressure_coefficient(RID p_body, real_t p_pressure_coefficient);
+	virtual real_t soft_body_get_pressure_coefficient(RID p_body);
+
+	virtual void soft_body_set_pose_matching_coefficient(RID p_body, real_t p_pose_matching_coefficient);
+	virtual real_t soft_body_get_pose_matching_coefficient(RID p_body);
+
+	virtual void soft_body_set_damping_coefficient(RID p_body, real_t p_damping_coefficient);
+	virtual real_t soft_body_get_damping_coefficient(RID p_body);
+
+	virtual void soft_body_set_drag_coefficient(RID p_body, real_t p_drag_coefficient);
+	virtual real_t soft_body_get_drag_coefficient(RID p_body);
+
+	virtual void soft_body_move_point(RID p_body, int p_point_index, const Vector3 &p_global_position);
+	virtual Vector3 soft_body_get_point_global_position(RID p_body, int p_point_index);
+
+	virtual Vector3 soft_body_get_point_offset(RID p_body, int p_point_index) const;
+
+	virtual void soft_body_remove_all_pinned_points(RID p_body);
+	virtual void soft_body_pin_point(RID p_body, int p_point_index, bool p_pin);
+	virtual bool soft_body_is_point_pinned(RID p_body, int p_point_index);
+
 	/* JOINT API */
 
 	virtual JointType joint_get_type(RID p_joint) const;
diff --git a/modules/bullet/collision_object_bullet.cpp b/modules/bullet/collision_object_bullet.cpp
index 57e4db708e..1d63318fd7 100644
--- a/modules/bullet/collision_object_bullet.cpp
+++ b/modules/bullet/collision_object_bullet.cpp
@@ -111,6 +111,8 @@ void CollisionObjectBullet::setupBulletCollisionObject(btCollisionObject *p_coll
 
 void CollisionObjectBullet::add_collision_exception(const CollisionObjectBullet *p_ignoreCollisionObject) {
 	exceptions.insert(p_ignoreCollisionObject->get_self());
+	if (!bt_collision_object)
+		return;
 	bt_collision_object->setIgnoreCollisionCheck(p_ignoreCollisionObject->bt_collision_object, true);
 	if (space)
 		space->get_broadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bt_collision_object->getBroadphaseHandle(), space->get_dispatcher());
diff --git a/modules/bullet/register_types.cpp b/modules/bullet/register_types.cpp
index b75f7464ab..a76b0438b4 100644
--- a/modules/bullet/register_types.cpp
+++ b/modules/bullet/register_types.cpp
@@ -32,6 +32,7 @@
 
 #include "bullet_physics_server.h"
 #include "class_db.h"
+#include "project_settings.h"
 
 /**
 	@author AndreaCatania
@@ -47,6 +48,9 @@ void register_bullet_types() {
 #ifndef _3D_DISABLED
 	PhysicsServerManager::register_server("Bullet", &_createBulletPhysicsCallback);
 	PhysicsServerManager::set_default_server("Bullet", 1);
+
+	GLOBAL_DEF("physics/3d/active_soft_world", true);
+	ProjectSettings::get_singleton()->set_custom_property_info("physics/3d/active_soft_world", PropertyInfo(Variant::BOOL, "physics/3d/active_soft_world"));
 #endif
 }
 
diff --git a/modules/bullet/soft_body_bullet.cpp b/modules/bullet/soft_body_bullet.cpp
index 5c20eb73f1..b3680d58db 100644
--- a/modules/bullet/soft_body_bullet.cpp
+++ b/modules/bullet/soft_body_bullet.cpp
@@ -32,42 +32,24 @@
 
 #include "bullet_types_converter.h"
 #include "bullet_utilities.h"
-#include "scene/3d/immediate_geometry.h"
+#include "scene/3d/soft_body.h"
 #include "space_bullet.h"
 
-/**
-	@author AndreaCatania
-*/
-
 SoftBodyBullet::SoftBodyBullet() :
 		CollisionObjectBullet(CollisionObjectBullet::TYPE_SOFT_BODY),
-		mass(1),
+		total_mass(1),
 		simulation_precision(5),
-		stiffness(0.5f),
-		pressure_coefficient(50),
-		damping_coefficient(0.005),
-		drag_coefficient(0.005),
+		linear_stiffness(0.5),
+		areaAngular_stiffness(0.5),
+		volume_stiffness(0.5),
+		pressure_coefficient(0.),
+		pose_matching_coefficient(0.),
+		damping_coefficient(0.01),
+		drag_coefficient(0.),
 		bt_soft_body(NULL),
-		soft_shape_type(SOFT_SHAPETYPE_NONE),
-		isScratched(false),
-		soft_body_shape_data(NULL) {
-
-	test_geometry = memnew(ImmediateGeometry);
-
-	red_mat = Ref<SpatialMaterial>(memnew(SpatialMaterial));
-	red_mat->set_flag(SpatialMaterial::FLAG_UNSHADED, true);
-	red_mat->set_line_width(20.0);
-	red_mat->set_feature(SpatialMaterial::FEATURE_TRANSPARENT, true);
-	red_mat->set_flag(SpatialMaterial::FLAG_ALBEDO_FROM_VERTEX_COLOR, true);
-	red_mat->set_flag(SpatialMaterial::FLAG_SRGB_VERTEX_COLOR, true);
-	red_mat->set_albedo(Color(1, 0, 0, 1));
-	test_geometry->set_material_override(red_mat);
-
-	test_is_in_scene = false;
-}
+		isScratched(false) {}
 
 SoftBodyBullet::~SoftBodyBullet() {
-	bulletdelete(soft_body_shape_data);
 }
 
 void SoftBodyBullet::reload_body() {
@@ -80,8 +62,6 @@ void SoftBodyBullet::reload_body() {
 void SoftBodyBullet::set_space(SpaceBullet *p_space) {
 	if (space) {
 		isScratched = false;
-
-		// Remove this object from the physics world
 		space->remove_soft_body(this);
 	}
 
@@ -90,86 +70,181 @@ void SoftBodyBullet::set_space(SpaceBullet *p_space) {
 	if (space) {
 		space->add_soft_body(this);
 	}
-
-	reload_soft_body();
 }
 
-void SoftBodyBullet::dispatch_callbacks() {
-	if (!bt_soft_body) {
+void SoftBodyBullet::dispatch_callbacks() {}
+
+void SoftBodyBullet::on_collision_filters_change() {}
+
+void SoftBodyBullet::on_collision_checker_start() {}
+
+void SoftBodyBullet::on_enter_area(AreaBullet *p_area) {}
+
+void SoftBodyBullet::on_exit_area(AreaBullet *p_area) {}
+
+void SoftBodyBullet::update_visual_server(SoftBodyVisualServerHandler *p_visual_server_handler) {
+	if (!bt_soft_body)
 		return;
+
+	/// Update visual server vertices
+	const btSoftBody::tNodeArray &nodes(bt_soft_body->m_nodes);
+	const int nodes_count = nodes.size();
+
+	Vector<int> *vs_indices;
+	const void *vertex_position;
+	const void *vertex_normal;
+
+	for (int vertex_index = 0; vertex_index < nodes_count; ++vertex_index) {
+		vertex_position = reinterpret_cast<const void *>(&nodes[vertex_index].m_x);
+		vertex_normal = reinterpret_cast<const void *>(&nodes[vertex_index].m_n);
+
+		vs_indices = &indices_table[vertex_index];
+
+		const int vs_indices_size(vs_indices->size());
+		for (int x = 0; x < vs_indices_size; ++x) {
+			p_visual_server_handler->set_vertex((*vs_indices)[x], vertex_position);
+			p_visual_server_handler->set_normal((*vs_indices)[x], vertex_normal);
+		}
 	}
 
-	if (!test_is_in_scene) {
-		test_is_in_scene = true;
-		SceneTree::get_singleton()->get_current_scene()->add_child(test_geometry);
+	/// Generate AABB
+	btVector3 aabb_min;
+	btVector3 aabb_max;
+	bt_soft_body->getAabb(aabb_min, aabb_max);
+
+	btVector3 size(aabb_max - aabb_min);
+
+	AABB aabb;
+	B_TO_G(aabb_min, aabb.position);
+	B_TO_G(size, aabb.size);
+
+	p_visual_server_handler->set_aabb(aabb);
+}
+
+void SoftBodyBullet::set_soft_mesh(const Ref<Mesh> &p_mesh) {
+
+	if (p_mesh.is_null() || !p_mesh->surface_is_softbody_friendly(0))
+		soft_mesh.unref();
+	else
+		soft_mesh = p_mesh;
+
+	if (soft_mesh.is_null()) {
+
+		destroy_soft_body();
+		return;
 	}
 
-	test_geometry->clear();
-	test_geometry->begin(Mesh::PRIMITIVE_LINES, NULL);
-	bool first = true;
-	Vector3 pos;
-	for (int i = 0; i < bt_soft_body->m_nodes.size(); ++i) {
-		const btSoftBody::Node &n = bt_soft_body->m_nodes[i];
-		B_TO_G(n.m_x, pos);
-		test_geometry->add_vertex(pos);
-		if (!first) {
-			test_geometry->add_vertex(pos);
-		} else {
-			first = false;
-		}
+	Array arrays = soft_mesh->surface_get_arrays(0);
+	ERR_FAIL_COND(!(soft_mesh->surface_get_format(0) & VS::ARRAY_FORMAT_INDEX));
+	set_trimesh_body_shape(arrays[VS::ARRAY_INDEX], arrays[VS::ARRAY_VERTEX]);
+}
+
+void SoftBodyBullet::destroy_soft_body() {
+
+	if (!bt_soft_body)
+		return;
+
+	if (space) {
+		/// Remove from world before deletion
+		space->remove_soft_body(this);
 	}
-	test_geometry->end();
+
+	destroyBulletCollisionObject();
+	bt_soft_body = NULL;
+}
+
+void SoftBodyBullet::set_soft_transform(const Transform &p_transform) {
+	reset_all_node_positions();
+	move_all_nodes(p_transform);
 }
 
-void SoftBodyBullet::on_collision_filters_change() {
+void SoftBodyBullet::move_all_nodes(const Transform &p_transform) {
+	if (!bt_soft_body)
+		return;
+	btTransform bt_transf;
+	G_TO_B(p_transform, bt_transf);
+	bt_soft_body->transform(bt_transf);
 }
 
-void SoftBodyBullet::on_collision_checker_start() {
+void SoftBodyBullet::set_node_position(int p_node_index, const Vector3 &p_global_position) {
+	btVector3 bt_pos;
+	G_TO_B(p_global_position, bt_pos);
+	set_node_position(p_node_index, bt_pos);
 }
 
-void SoftBodyBullet::on_enter_area(AreaBullet *p_area) {
+void SoftBodyBullet::set_node_position(int p_node_index, const btVector3 &p_global_position) {
+	if (bt_soft_body) {
+		bt_soft_body->m_nodes[p_node_index].m_x = p_global_position;
+	}
 }
 
-void SoftBodyBullet::on_exit_area(AreaBullet *p_area) {
+void SoftBodyBullet::get_node_position(int p_node_index, Vector3 &r_position) const {
+	if (bt_soft_body) {
+		B_TO_G(bt_soft_body->m_nodes[p_node_index].m_x, r_position);
+	}
 }
 
-void SoftBodyBullet::set_trimesh_body_shape(PoolVector<int> p_indices, PoolVector<Vector3> p_vertices, int p_triangles_num) {
+void SoftBodyBullet::get_node_offset(int p_node_index, Vector3 &r_offset) const {
+	if (soft_mesh.is_null())
+		return;
+
+	Array arrays = soft_mesh->surface_get_arrays(0);
+	PoolVector<Vector3> vertices(arrays[VS::ARRAY_VERTEX]);
 
-	TrimeshSoftShapeData *shape_data = bulletnew(TrimeshSoftShapeData);
-	shape_data->m_triangles_indices = p_indices;
-	shape_data->m_vertices = p_vertices;
-	shape_data->m_triangles_num = p_triangles_num;
+	if (0 <= p_node_index && vertices.size() > p_node_index) {
+		r_offset = vertices[p_node_index];
+	}
+}
 
-	set_body_shape_data(shape_data, SOFT_SHAPE_TYPE_TRIMESH);
-	reload_soft_body();
+void SoftBodyBullet::get_node_offset(int p_node_index, btVector3 &r_offset) const {
+	Vector3 off;
+	get_node_offset(p_node_index, off);
+	G_TO_B(off, r_offset);
 }
 
-void SoftBodyBullet::set_body_shape_data(SoftShapeData *p_soft_shape_data, SoftShapeType p_type) {
-	bulletdelete(soft_body_shape_data);
-	soft_body_shape_data = p_soft_shape_data;
-	soft_shape_type = p_type;
+void SoftBodyBullet::set_node_mass(int node_index, btScalar p_mass) {
+	if (0 >= p_mass) {
+		pin_node(node_index);
+	} else {
+		unpin_node(node_index);
+	}
+	if (bt_soft_body) {
+		bt_soft_body->setMass(node_index, p_mass);
+	}
 }
 
-void SoftBodyBullet::set_transform(const Transform &p_transform) {
-	transform = p_transform;
+btScalar SoftBodyBullet::get_node_mass(int node_index) const {
 	if (bt_soft_body) {
-		// TODO the softbody set new transform considering the current transform as center of world
-		// like if it's local transform, so I must fix this by setting nwe transform considering the old
-		btTransform bt_trans;
-		G_TO_B(transform, bt_trans);
-		//bt_soft_body->transform(bt_trans);
+		return bt_soft_body->getMass(node_index);
+	} else {
+		return -1 == search_node_pinned(node_index) ? 1 : 0;
 	}
 }
 
-const Transform &SoftBodyBullet::get_transform() const {
-	return transform;
+void SoftBodyBullet::reset_all_node_mass() {
+	if (bt_soft_body) {
+		for (int i = pinned_nodes.size() - 1; 0 <= i; --i) {
+			bt_soft_body->setMass(pinned_nodes[i], 1);
+		}
+	}
+	pinned_nodes.resize(0);
 }
 
-void SoftBodyBullet::get_first_node_origin(btVector3 &p_out_origin) const {
-	if (bt_soft_body && bt_soft_body->m_nodes.size()) {
-		p_out_origin = bt_soft_body->m_nodes[0].m_x;
-	} else {
-		p_out_origin.setZero();
+void SoftBodyBullet::reset_all_node_positions() {
+	if (soft_mesh.is_null())
+		return;
+
+	Array arrays = soft_mesh->surface_get_arrays(0);
+	PoolVector<Vector3> vs_vertices(arrays[VS::ARRAY_VERTEX]);
+	PoolVector<Vector3>::Read vs_vertices_read = vs_vertices.read();
+
+	for (int vertex_index = bt_soft_body->m_nodes.size() - 1; 0 <= vertex_index; --vertex_index) {
+
+		G_TO_B(vs_vertices_read[indices_table[vertex_index][0]], bt_soft_body->m_nodes[vertex_index].m_x);
+
+		bt_soft_body->m_nodes[vertex_index].m_q = bt_soft_body->m_nodes[vertex_index].m_x;
+		bt_soft_body->m_nodes[vertex_index].m_v = btVector3(0, 0, 0);
+		bt_soft_body->m_nodes[vertex_index].m_f = btVector3(0, 0, 0);
 	}
 }
 
@@ -181,22 +256,34 @@ void SoftBodyBullet::set_activation_state(bool p_active) {
 	}
 }
 
-void SoftBodyBullet::set_mass(real_t p_val) {
+void SoftBodyBullet::set_total_mass(real_t p_val) {
 	if (0 >= p_val) {
 		p_val = 1;
 	}
-	mass = p_val;
+	total_mass = p_val;
 	if (bt_soft_body) {
-		bt_soft_body->setTotalMass(mass);
+		bt_soft_body->setTotalMass(total_mass);
 	}
 }
 
-void SoftBodyBullet::set_stiffness(real_t p_val) {
-	stiffness = p_val;
+void SoftBodyBullet::set_linear_stiffness(real_t p_val) {
+	linear_stiffness = p_val;
 	if (bt_soft_body) {
-		mat0->m_kAST = stiffness;
-		mat0->m_kLST = stiffness;
-		mat0->m_kVST = stiffness;
+		mat0->m_kLST = linear_stiffness;
+	}
+}
+
+void SoftBodyBullet::set_areaAngular_stiffness(real_t p_val) {
+	areaAngular_stiffness = p_val;
+	if (bt_soft_body) {
+		mat0->m_kAST = areaAngular_stiffness;
+	}
+}
+
+void SoftBodyBullet::set_volume_stiffness(real_t p_val) {
+	volume_stiffness = p_val;
+	if (bt_soft_body) {
+		mat0->m_kVST = volume_stiffness;
 	}
 }
 
@@ -204,6 +291,9 @@ void SoftBodyBullet::set_simulation_precision(int p_val) {
 	simulation_precision = p_val;
 	if (bt_soft_body) {
 		bt_soft_body->m_cfg.piterations = simulation_precision;
+		bt_soft_body->m_cfg.viterations = simulation_precision;
+		bt_soft_body->m_cfg.diterations = simulation_precision;
+		bt_soft_body->m_cfg.citerations = simulation_precision;
 	}
 }
 
@@ -214,6 +304,13 @@ void SoftBodyBullet::set_pressure_coefficient(real_t p_val) {
 	}
 }
 
+void SoftBodyBullet::set_pose_matching_coefficient(real_t p_val) {
+	pose_matching_coefficient = p_val;
+	if (bt_soft_body) {
+		bt_soft_body->m_cfg.kMT = pose_matching_coefficient;
+	}
+}
+
 void SoftBodyBullet::set_damping_coefficient(real_t p_val) {
 	damping_coefficient = p_val;
 	if (bt_soft_body) {
@@ -228,89 +325,156 @@ void SoftBodyBullet::set_drag_coefficient(real_t p_val) {
 	}
 }
 
-void SoftBodyBullet::reload_soft_body() {
-
+void SoftBodyBullet::set_trimesh_body_shape(PoolVector<int> p_indices, PoolVector<Vector3> p_vertices) {
+	/// Assert the current soft body is destroyed
 	destroy_soft_body();
-	create_soft_body();
 
-	if (bt_soft_body) {
+	/// Parse visual server indices to physical indices.
+	/// Merge all overlapping vertices and create a map of physical vertices to visual server
 
-		// TODO the softbody set new transform considering the current transform as center of world
-		// like if it's local transform, so I must fix this by setting nwe transform considering the old
-		btTransform bt_trans;
-		G_TO_B(transform, bt_trans);
-		bt_soft_body->transform(bt_trans);
+	{
+		/// This is the map of visual server indices to physics indices (So it's the inverse of idices_map), Thanks to it I don't need make a heavy search in the indices_map
+		Vector<int> vs_indices_to_physics_table;
 
-		bt_soft_body->generateBendingConstraints(2, mat0);
-		mat0->m_kAST = stiffness;
-		mat0->m_kLST = stiffness;
-		mat0->m_kVST = stiffness;
+		{ // Map vertices
+			indices_table.resize(0);
 
-		bt_soft_body->m_cfg.piterations = simulation_precision;
-		bt_soft_body->m_cfg.kDP = damping_coefficient;
-		bt_soft_body->m_cfg.kDG = drag_coefficient;
-		bt_soft_body->m_cfg.kPR = pressure_coefficient;
-		bt_soft_body->setTotalMass(mass);
-	}
-	if (space) {
-		// TODO remove this please
-		space->add_soft_body(this);
-	}
-}
+			int index = 0;
+			Map<Vector3, int> unique_vertices;
 
-void SoftBodyBullet::create_soft_body() {
-	if (!space || !soft_body_shape_data) {
-		return;
-	}
-	ERR_FAIL_COND(!space->is_using_soft_world());
-	switch (soft_shape_type) {
-		case SOFT_SHAPE_TYPE_TRIMESH: {
-			TrimeshSoftShapeData *trimesh_data = static_cast<TrimeshSoftShapeData *>(soft_body_shape_data);
-
-			Vector<int> indices;
-			Vector<btScalar> vertices;
-
-			int i;
-			const int indices_size = trimesh_data->m_triangles_indices.size();
-			const int vertices_size = trimesh_data->m_vertices.size();
-			indices.resize(indices_size);
-			vertices.resize(vertices_size * 3);
-
-			PoolVector<int>::Read i_r = trimesh_data->m_triangles_indices.read();
-			for (i = 0; i < indices_size; ++i) {
-				indices[i] = i_r[i];
+			const int vs_vertices_size(p_vertices.size());
+
+			PoolVector<Vector3>::Read p_vertices_read = p_vertices.read();
+
+			for (int vs_vertex_index = 0; vs_vertex_index < vs_vertices_size; ++vs_vertex_index) {
+
+				Map<Vector3, int>::Element *e = unique_vertices.find(p_vertices_read[vs_vertex_index]);
+				int vertex_id;
+				if (e) {
+					// Already rxisting
+					vertex_id = e->value();
+				} else {
+					// Create new one
+					unique_vertices[p_vertices_read[vs_vertex_index]] = vertex_id = index++;
+					indices_table.push_back(Vector<int>());
+				}
+
+				indices_table[vertex_id].push_back(vs_vertex_index);
+				vs_indices_to_physics_table.push_back(vertex_id);
+			}
+		}
+
+		const int indices_map_size(indices_table.size());
+
+		Vector<btScalar> bt_vertices;
+
+		{ // Parse vertices to bullet
+
+			bt_vertices.resize(indices_map_size * 3);
+			PoolVector<Vector3>::Read p_vertices_read = p_vertices.read();
+
+			for (int i = 0; i < indices_map_size; ++i) {
+				bt_vertices[3 * i + 0] = p_vertices_read[indices_table[i][0]].x;
+				bt_vertices[3 * i + 1] = p_vertices_read[indices_table[i][0]].y;
+				bt_vertices[3 * i + 2] = p_vertices_read[indices_table[i][0]].z;
 			}
-			i_r = PoolVector<int>::Read();
+		}
+
+		Vector<int> bt_triangles;
+		const int triangles_size(p_indices.size() / 3);
+
+		{ // Parse indices
+
+			bt_triangles.resize(triangles_size * 3);
+
+			PoolVector<int>::Read p_indices_read = p_indices.read();
 
-			PoolVector<Vector3>::Read f_r = trimesh_data->m_vertices.read();
-			for (int j = i = 0; i < vertices_size; ++i, j += 3) {
-				vertices[j + 0] = f_r[i][0];
-				vertices[j + 1] = f_r[i][1];
-				vertices[j + 2] = f_r[i][2];
+			for (int i = 0; i < triangles_size; ++i) {
+				bt_triangles[3 * i + 0] = vs_indices_to_physics_table[p_indices_read[3 * i + 2]];
+				bt_triangles[3 * i + 1] = vs_indices_to_physics_table[p_indices_read[3 * i + 1]];
+				bt_triangles[3 * i + 2] = vs_indices_to_physics_table[p_indices_read[3 * i + 0]];
 			}
-			f_r = PoolVector<Vector3>::Read();
+		}
 
-			bt_soft_body = btSoftBodyHelpers::CreateFromTriMesh(*space->get_soft_body_world_info(), vertices.ptr(), indices.ptr(), trimesh_data->m_triangles_num);
-		} break;
-		default:
-			ERR_PRINT("Shape type not supported");
-			return;
+		btSoftBodyWorldInfo fake_world_info;
+		bt_soft_body = btSoftBodyHelpers::CreateFromTriMesh(fake_world_info, &bt_vertices[0], &bt_triangles[0], triangles_size, false);
+		setup_soft_body();
 	}
+}
+
+void SoftBodyBullet::setup_soft_body() {
+
+	if (!bt_soft_body)
+		return;
 
+	// Soft body setup
 	setupBulletCollisionObject(bt_soft_body);
-	bt_soft_body->getCollisionShape()->setMargin(0.001f);
+	bt_soft_body->m_worldInfo = NULL; // Remove fake world info
+	bt_soft_body->getCollisionShape()->setMargin(0.01);
 	bt_soft_body->setCollisionFlags(bt_soft_body->getCollisionFlags() & (~(btCollisionObject::CF_KINEMATIC_OBJECT | btCollisionObject::CF_STATIC_OBJECT)));
+
+	// Space setup
+	if (space) {
+		space->add_soft_body(this);
+	}
+
 	mat0 = bt_soft_body->appendMaterial();
+
+	// Assign soft body data
+	bt_soft_body->generateBendingConstraints(2, mat0);
+
+	mat0->m_kLST = linear_stiffness;
+	mat0->m_kAST = areaAngular_stiffness;
+	mat0->m_kVST = volume_stiffness;
+
+	// Clusters allow to have Soft vs Soft collision but doesn't work well right now
+
+	//bt_soft_body->m_cfg.kSRHR_CL = 1;// Soft vs rigid hardness [0,1] (cluster only)
+	//bt_soft_body->m_cfg.kSKHR_CL = 1;// Soft vs kinematic hardness [0,1] (cluster only)
+	//bt_soft_body->m_cfg.kSSHR_CL = 1;// Soft vs soft hardness [0,1] (cluster only)
+	//bt_soft_body->m_cfg.kSR_SPLT_CL = 1; // Soft vs rigid impulse split [0,1] (cluster only)
+	//bt_soft_body->m_cfg.kSK_SPLT_CL = 1; // Soft vs kinematic impulse split [0,1] (cluster only)
+	//bt_soft_body->m_cfg.kSS_SPLT_CL = 1; // Soft vs Soft impulse split [0,1] (cluster only)
+	//bt_soft_body->m_cfg.collisions = btSoftBody::fCollision::CL_SS + btSoftBody::fCollision::CL_RS + btSoftBody::fCollision::VF_SS;
+	//bt_soft_body->generateClusters(64);
+
+	bt_soft_body->m_cfg.piterations = simulation_precision;
+	bt_soft_body->m_cfg.viterations = simulation_precision;
+	bt_soft_body->m_cfg.diterations = simulation_precision;
+	bt_soft_body->m_cfg.citerations = simulation_precision;
+	bt_soft_body->m_cfg.kDP = damping_coefficient;
+	bt_soft_body->m_cfg.kDG = drag_coefficient;
+	bt_soft_body->m_cfg.kPR = pressure_coefficient;
+	bt_soft_body->m_cfg.kMT = pose_matching_coefficient;
+	bt_soft_body->setTotalMass(total_mass);
+
+	btSoftBodyHelpers::ReoptimizeLinkOrder(bt_soft_body);
+	bt_soft_body->updateBounds();
+
+	// Set pinned nodes
+	for (int i = pinned_nodes.size() - 1; 0 <= i; --i) {
+		bt_soft_body->setMass(pinned_nodes[i], 0);
+	}
 }
 
-void SoftBodyBullet::destroy_soft_body() {
-	if (space) {
-		/// This step is required to assert that the body is not into the world during deletion
-		/// This step is required since to change the body shape the body must be re-created.
-		/// Here is handled the case when the body is assigned into a world and the body
-		/// shape is changed.
-		space->remove_soft_body(this);
+void SoftBodyBullet::pin_node(int p_node_index) {
+	if (-1 == search_node_pinned(p_node_index)) {
+		pinned_nodes.push_back(p_node_index);
 	}
-	destroyBulletCollisionObject();
-	bt_soft_body = NULL;
+}
+
+void SoftBodyBullet::unpin_node(int p_node_index) {
+	const int id = search_node_pinned(p_node_index);
+	if (-1 != id) {
+		pinned_nodes.remove(id);
+	}
+}
+
+int SoftBodyBullet::search_node_pinned(int p_node_index) const {
+	for (int i = pinned_nodes.size() - 1; 0 <= i; --i) {
+		if (p_node_index == pinned_nodes[i]) {
+			return i;
+		}
+	}
+	return -1;
 }
diff --git a/modules/bullet/soft_body_bullet.h b/modules/bullet/soft_body_bullet.h
index 9895643b84..c775193584 100644
--- a/modules/bullet/soft_body_bullet.h
+++ b/modules/bullet/soft_body_bullet.h
@@ -40,7 +40,10 @@
 #define x11_None 0L
 #endif
 
-#include <BulletSoftBody/btSoftBodyHelpers.h>
+#include "BulletSoftBody/btSoftBodyHelpers.h"
+#include "collision_object_bullet.h"
+#include "scene/resources/mesh.h"
+#include "servers/physics_server.h"
 
 #ifdef x11_None
 /// This is required to re add the macro None defined by x11 compiler
@@ -52,39 +55,34 @@
 	@author AndreaCatania
 */
 
-struct SoftShapeData {};
-struct TrimeshSoftShapeData : public SoftShapeData {
-	PoolVector<int> m_triangles_indices;
-	PoolVector<Vector3> m_vertices;
-	int m_triangles_num;
-};
-
 class SoftBodyBullet : public CollisionObjectBullet {
-public:
-	enum SoftShapeType {
-		SOFT_SHAPETYPE_NONE = 0,
-		SOFT_SHAPE_TYPE_TRIMESH
-	};
 
 private:
 	btSoftBody *bt_soft_body;
+	Vector<Vector<int> > indices_table;
 	btSoftBody::Material *mat0; // This is just a copy of pointer managed by btSoftBody
-	SoftShapeType soft_shape_type;
 	bool isScratched;
 
-	SoftShapeData *soft_body_shape_data;
+	Ref<Mesh> soft_mesh;
 
-	Transform transform;
 	int simulation_precision;
-	real_t mass;
-	real_t stiffness; // [0,1]
+	real_t total_mass;
+	real_t linear_stiffness; // [0,1]
+	real_t areaAngular_stiffness; // [0,1]
+	real_t volume_stiffness; // [0,1]
 	real_t pressure_coefficient; // [-inf,+inf]
+	real_t pose_matching_coefficient; // [0,1]
 	real_t damping_coefficient; // [0,1]
 	real_t drag_coefficient; // [0,1]
+	Vector<int> pinned_nodes;
 
-	class ImmediateGeometry *test_geometry; // TODO remove this please
-	Ref<SpatialMaterial> red_mat; // TODO remove this please
-	bool test_is_in_scene; // TODO remove this please
+	// Other property to add
+	//btScalar				kVC;			// Volume conversation coefficient [0,+inf]
+	//btScalar				kDF;			// Dynamic friction coefficient [0,1]
+	//btScalar				kMT;			// Pose matching coefficient [0,1]
+	//btScalar				kCHR;			// Rigid contacts hardness [0,1]
+	//btScalar				kKHR;			// Kinetic contacts hardness [0,1]
+	//btScalar				kSHR;			// Soft contacts hardness [0,1]
 
 public:
 	SoftBodyBullet();
@@ -101,39 +99,64 @@ public:
 
 	_FORCE_INLINE_ btSoftBody *get_bt_soft_body() const { return bt_soft_body; }
 
-	void set_trimesh_body_shape(PoolVector<int> p_indices, PoolVector<Vector3> p_vertices, int p_triangles_num);
-	void set_body_shape_data(SoftShapeData *p_soft_shape_data, SoftShapeType p_type);
+	void update_visual_server(class SoftBodyVisualServerHandler *p_visual_server_handler);
 
-	void set_transform(const Transform &p_transform);
-	/// This function doesn't return the exact COM transform.
-	/// It returns the origin only of first node (vertice) of current soft body
-	/// ---
-	/// The soft body doesn't have a fixed center of mass, but is a group of nodes (vertices)
-	/// that each has its own position in the world.
-	/// For this reason return the correct COM is not so simple and must be calculate
-	/// Check this to improve this function http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=8803
-	const Transform &get_transform() const;
-	void get_first_node_origin(btVector3 &p_out_origin) const;
+	void set_soft_mesh(const Ref<Mesh> &p_mesh);
+	void destroy_soft_body();
+
+	// Special function. This function has bad performance
+	void set_soft_transform(const Transform &p_transform);
+
+	void move_all_nodes(const Transform &p_transform);
+	void set_node_position(int node_index, const Vector3 &p_global_position);
+	void set_node_position(int node_index, const btVector3 &p_global_position);
+	void get_node_position(int node_index, Vector3 &r_position) const;
+	// Heavy function, Please cache this info
+	void get_node_offset(int node_index, Vector3 &r_offset) const;
+	// Heavy function, Please cache this info
+	void get_node_offset(int node_index, btVector3 &r_offset) const;
+
+	void set_node_mass(int node_index, btScalar p_mass);
+	btScalar get_node_mass(int node_index) const;
+	void reset_all_node_mass();
+	void reset_all_node_positions();
 
 	void set_activation_state(bool p_active);
 
-	void set_mass(real_t p_val);
-	_FORCE_INLINE_ real_t get_mass() const { return mass; }
-	void set_stiffness(real_t p_val);
-	_FORCE_INLINE_ real_t get_stiffness() const { return stiffness; }
+	void set_total_mass(real_t p_val);
+	_FORCE_INLINE_ real_t get_total_mass() const { return total_mass; }
+
+	void set_linear_stiffness(real_t p_val);
+	_FORCE_INLINE_ real_t get_linear_stiffness() const { return linear_stiffness; }
+
+	void set_areaAngular_stiffness(real_t p_val);
+	_FORCE_INLINE_ real_t get_areaAngular_stiffness() const { return areaAngular_stiffness; }
+
+	void set_volume_stiffness(real_t p_val);
+	_FORCE_INLINE_ real_t get_volume_stiffness() const { return volume_stiffness; }
+
 	void set_simulation_precision(int p_val);
 	_FORCE_INLINE_ int get_simulation_precision() const { return simulation_precision; }
+
 	void set_pressure_coefficient(real_t p_val);
 	_FORCE_INLINE_ real_t get_pressure_coefficient() const { return pressure_coefficient; }
+
+	void set_pose_matching_coefficient(real_t p_val);
+	_FORCE_INLINE_ real_t get_pose_matching_coefficient() const { return pose_matching_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; }
 
 private:
-	void reload_soft_body();
-	void create_soft_body();
-	void destroy_soft_body();
+	void set_trimesh_body_shape(PoolVector<int> p_indices, PoolVector<Vector3> p_vertices);
+	void setup_soft_body();
+
+	void pin_node(int p_node_index);
+	void unpin_node(int p_node_index);
+	int search_node_pinned(int p_node_index) const;
 };
 
 #endif // SOFT_BODY_BULLET_H
diff --git a/modules/bullet/space_bullet.cpp b/modules/bullet/space_bullet.cpp
index 331c4a5eba..132c3739d6 100644
--- a/modules/bullet/space_bullet.cpp
+++ b/modules/bullet/space_bullet.cpp
@@ -36,6 +36,7 @@
 #include "constraint_bullet.h"
 #include "godot_collision_configuration.h"
 #include "godot_collision_dispatcher.h"
+#include "project_settings.h"
 #include "rigid_body_bullet.h"
 #include "servers/physics_server.h"
 #include "soft_body_bullet.h"
@@ -325,7 +326,7 @@ Vector3 BulletPhysicsDirectSpaceState::get_closest_point_to_object_volume(RID p_
 	}
 }
 
-SpaceBullet::SpaceBullet(bool p_create_soft_world) :
+SpaceBullet::SpaceBullet() :
 		broadphase(NULL),
 		dispatcher(NULL),
 		solver(NULL),
@@ -338,7 +339,7 @@ SpaceBullet::SpaceBullet(bool p_create_soft_world) :
 		gravityMagnitude(10),
 		contactDebugCount(0) {
 
-	create_empty_world(p_create_soft_world);
+	create_empty_world(GLOBAL_DEF("physics/3d/active_soft_world", true));
 	direct_access = memnew(BulletPhysicsDirectSpaceState(this));
 }
 
@@ -355,6 +356,7 @@ void SpaceBullet::flush_queries() {
 }
 
 void SpaceBullet::step(real_t p_delta_time) {
+	delta_time = p_delta_time;
 	dynamicsWorld->stepSimulation(p_delta_time, 0, 0);
 }
 
@@ -483,6 +485,7 @@ void SpaceBullet::reload_collision_filters(RigidBodyBullet *p_body) {
 void SpaceBullet::add_soft_body(SoftBodyBullet *p_body) {
 	if (is_using_soft_world()) {
 		if (p_body->get_bt_soft_body()) {
+			p_body->get_bt_soft_body()->m_worldInfo = get_soft_body_world_info();
 			static_cast<btSoftRigidDynamicsWorld *>(dynamicsWorld)->addSoftBody(p_body->get_bt_soft_body(), p_body->get_collision_layer(), p_body->get_collision_mask());
 		}
 	} else {
@@ -494,6 +497,7 @@ void SpaceBullet::remove_soft_body(SoftBodyBullet *p_body) {
 	if (is_using_soft_world()) {
 		if (p_body->get_bt_soft_body()) {
 			static_cast<btSoftRigidDynamicsWorld *>(dynamicsWorld)->removeSoftBody(p_body->get_bt_soft_body());
+			p_body->get_bt_soft_body()->m_worldInfo = NULL;
 		}
 	}
 }
diff --git a/modules/bullet/space_bullet.h b/modules/bullet/space_bullet.h
index a6c2786878..006c6462cf 100644
--- a/modules/bullet/space_bullet.h
+++ b/modules/bullet/space_bullet.h
@@ -84,7 +84,7 @@ public:
 };
 
 class SpaceBullet : public RIDBullet {
-private:
+
 	friend class AreaBullet;
 	friend void onBulletTickCallback(btDynamicsWorld *world, btScalar timeStep);
 	friend class BulletPhysicsDirectSpaceState;
@@ -109,12 +109,14 @@ private:
 
 	Vector<Vector3> contactDebug;
 	int contactDebugCount;
+	real_t delta_time;
 
 public:
-	SpaceBullet(bool p_create_soft_world);
+	SpaceBullet();
 	virtual ~SpaceBullet();
 
 	void flush_queries();
+	real_t get_delta_time() { return delta_time; }
 	void step(real_t p_delta_time);
 
 	_FORCE_INLINE_ btBroadphaseInterface *get_broadphase() { return broadphase; }